107 lines
3.2 KiB
C
107 lines
3.2 KiB
C
#include <stdint.h>
|
|
#include <stdio.h>
|
|
|
|
#include "bsp/mmio.h"
|
|
#include "bsp/soc.h"
|
|
#include "bsp/uart.h"
|
|
#include "lsm6dsox.h"
|
|
|
|
#ifndef LSM6DSOX_ODR_HZ
|
|
#define LSM6DSOX_ODR_HZ 104u
|
|
#endif
|
|
|
|
#ifndef CAPTURE_DURATION_S
|
|
#define CAPTURE_DURATION_S 60u
|
|
#endif
|
|
|
|
static uint64_t time_now(void)
|
|
{
|
|
return bsp_read64(BSP_CLINT_MTIME);
|
|
}
|
|
|
|
static void print_identity_probe(void)
|
|
{
|
|
printf("WHO_AM_I probes:");
|
|
for (unsigned i = 0u; i < 8u; ++i) {
|
|
printf(" %02x", lsm6dsox_read_who_am_i());
|
|
}
|
|
printf(" (expected 6c)\n");
|
|
}
|
|
|
|
int main(void)
|
|
{
|
|
printf("\nSTEVAL-MKI217V1 / LSM6DSOX SPI polling test\n");
|
|
printf("Genesys2 JC: CS=JC1 SDA/MOSI=JC2 SDO/MISO=JC3 SCL/SCK=JC4\n");
|
|
printf("Power: VDD=3.3V VDDIO=3.3V common GND; INT is not used\n");
|
|
printf("SPI MMIO=0x%08lx CLINT=%lu Hz UART=%lu baud\n",
|
|
(unsigned long)BSP_PMOD_SPI_BASE,
|
|
(unsigned long)BSP_CLINT_TIMEBASE_HZ,
|
|
(unsigned long)bsp_uart_baudrate());
|
|
|
|
if (!lsm6dsox_init(LSM6DSOX_ODR_HZ)) {
|
|
print_identity_probe();
|
|
printf("INIT FAIL: check CS, MOSI, MISO, SCK, 3.3V and common GND.\n");
|
|
printf("0x00 usually means MISO held low; 0xff usually means no selected device.\n");
|
|
bsp_uart_wait_tx_idle();
|
|
return 1;
|
|
}
|
|
|
|
print_identity_probe();
|
|
printf("CONFIG CTRL1_XL=%02x CTRL2_G=%02x CTRL3_C=%02x STATUS=%02x\n",
|
|
lsm6dsox_read_register(0x10u),
|
|
lsm6dsox_read_register(0x11u),
|
|
lsm6dsox_read_register(0x12u),
|
|
lsm6dsox_read_register(0x1eu));
|
|
|
|
const lsm6dsox_scale_t *scale = lsm6dsox_scale();
|
|
printf("IMU_META,LSM6DSOX,tick_hz=%lu,odr_hz=%lu,"
|
|
"accel_ug_lsb=%lu,gyro_udps_lsb=%lu,temp_lsb_c=%lu,"
|
|
"temp_offset_millic=%ld\n",
|
|
(unsigned long)BSP_CLINT_TIMEBASE_HZ,
|
|
(unsigned long)scale->odr_hz,
|
|
(unsigned long)scale->accel_ug_per_lsb,
|
|
(unsigned long)scale->gyro_udps_per_lsb,
|
|
(unsigned long)scale->temperature_lsb_per_c,
|
|
(long)scale->temperature_offset_millic);
|
|
printf("IMU_COLUMNS,sensor,seq,tick,ax_raw,ay_raw,az_raw,"
|
|
"gx_raw,gy_raw,gz_raw,temp_raw\n");
|
|
|
|
const uint64_t start = time_now();
|
|
const uint64_t duration_ticks =
|
|
(uint64_t)BSP_CLINT_TIMEBASE_HZ * CAPTURE_DURATION_S;
|
|
uint32_t samples = 0u;
|
|
uint64_t previous_tick = 0u;
|
|
uint64_t maximum_gap = 0u;
|
|
|
|
while ((time_now() - start) < duration_ticks) {
|
|
lsm6dsox_sample_t sample;
|
|
if (!lsm6dsox_poll_sample(&sample)) {
|
|
continue;
|
|
}
|
|
|
|
if (previous_tick != 0u) {
|
|
const uint64_t gap = sample.timestamp_ticks - previous_tick;
|
|
if (gap > maximum_gap) {
|
|
maximum_gap = gap;
|
|
}
|
|
}
|
|
previous_tick = sample.timestamp_ticks;
|
|
|
|
printf("IMU_RAW,LSM6DSOX,%lu,%lu,%d,%d,%d,%d,%d,%d,%d\n",
|
|
(unsigned long)samples,
|
|
(unsigned long)sample.timestamp_ticks,
|
|
(int)sample.accel[0], (int)sample.accel[1],
|
|
(int)sample.accel[2], (int)sample.gyro[0],
|
|
(int)sample.gyro[1], (int)sample.gyro[2],
|
|
(int)sample.temperature);
|
|
samples++;
|
|
}
|
|
|
|
lsm6dsox_power_down();
|
|
printf("IMU_DONE,samples=%lu,duration_s=%u,max_gap_ticks=%lu\n",
|
|
(unsigned long)samples, (unsigned)CAPTURE_DURATION_S,
|
|
(unsigned long)maximum_gap);
|
|
bsp_uart_wait_tx_idle();
|
|
return 0;
|
|
}
|