onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add generic IMU fusion and PMOD IRQ verification

Browse Source
This commit is contained in:
onur
2026-07-14 11:03:13 +03:00
parent e77651dac3
commit dd6569c025
35 changed files with 2680 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/steval_mki217v1_test/src/lsm6dsox.c
+109 -13
View File
@@ -9,12 +9,17 @@
#define LSM6DSOX_SPI_READ 0x80u
#define LSM6DSOX_SPI_DUMMY 0xffu
#define LSM6DSOX_COUNTER_BDR_REG1 0x0bu
#define LSM6DSOX_INT1_CTRL 0x0du
#define LSM6DSOX_INT2_CTRL 0x0eu
#define LSM6DSOX_CTRL1_XL 0x10u
#define LSM6DSOX_CTRL2_G 0x11u
#define LSM6DSOX_CTRL3_C 0x12u
#define LSM6DSOX_STATUS_REG 0x1eu
#define LSM6DSOX_TAP_CFG2 0x58u
#define LSM6DSOX_OUT_TEMP_L 0x20u
#define LSM6DSOX_OUTX_L_G 0x22u
#define LSM6DSOX_OUTX_L_A 0x28u
#define LSM6DSOX_WHO_AM_I 0x0fu
#define LSM6DSOX_CTRL3_SW_RESET 0x01u
@@ -22,6 +27,9 @@
#define LSM6DSOX_CTRL3_BDU 0x40u
#define LSM6DSOX_STATUS_XLDA 0x01u
#define LSM6DSOX_STATUS_GDA 0x02u
#define LSM6DSOX_INT1_DRDY_XL 0x01u
#define LSM6DSOX_INTERRUPTS_ENABLE 0x80u
#define LSM6DSOX_GYRO_READY_TIMEOUT_US 100u
/* CTRL1_XL/CTRL2_G ODR fields (bits 7:4). */
#define LSM6DSOX_ODR_52 0x30u
@@ -147,21 +155,9 @@ int lsm6dsox_init(uint32_t odr_hz)
return 1;
}
int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
static void read_output_registers(lsm6dsox_sample_t *sample)
{
uint8_t raw[14];
if (sample == NULL) {
return 0;
}
const uint8_t status = read_reg(LSM6DSOX_STATUS_REG);
if ((status & (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) !=
(LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) {
return 0;
}
/* Timestamp the measurement before the foreground SPI burst. */
sample->timestamp_ticks = time_now();
bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY,
raw,
@@ -180,11 +176,111 @@ int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
sample->accel[0] = little_endian_i16(&raw[8]);
sample->accel[1] = little_endian_i16(&raw[10]);
sample->accel[2] = little_endian_i16(&raw[12]);
}
static void read_accel_registers(lsm6dsox_sample_t *sample)
{
uint8_t raw[6];
bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_A | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, raw, sizeof(raw));
sample->accel[0] = little_endian_i16(&raw[0]);
sample->accel[1] = little_endian_i16(&raw[2]);
sample->accel[2] = little_endian_i16(&raw[4]);
}
static void read_gyro_and_temperature_registers(lsm6dsox_sample_t *sample)
{
uint8_t raw[8];
bsp_pmod_spi_read_regs(LSM6DSOX_OUT_TEMP_L | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, raw, 2u);
bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
LSM6DSOX_SPI_DUMMY, &raw[2], 6u);
sample->temperature = little_endian_i16(&raw[0]);
sample->gyro[0] = little_endian_i16(&raw[2]);
sample->gyro[1] = little_endian_i16(&raw[4]);
sample->gyro[2] = little_endian_i16(&raw[6]);
}
int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample)
{
if (sample == NULL) {
return 0;
}
const uint8_t status = read_reg(LSM6DSOX_STATUS_REG);
if ((status & (LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) !=
(LSM6DSOX_STATUS_XLDA | LSM6DSOX_STATUS_GDA)) {
return 0;
}
/* Timestamp the measurement before the foreground SPI burst. */
sample->timestamp_ticks = time_now();
read_output_registers(sample);
return 1;
}
int lsm6dsox_enable_data_ready_interrupt(void)
{
/* Explicitly select latched data-ready. Reading the associated output
* register clears INT1; this matches the level-sensitive Rocket PLIC. */
write_reg(LSM6DSOX_INT1_CTRL, 0u);
write_reg(LSM6DSOX_INT2_CTRL, 0u);
write_reg(LSM6DSOX_TAP_CFG2,
read_reg(LSM6DSOX_TAP_CFG2) &
(uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
write_reg(LSM6DSOX_COUNTER_BDR_REG1, 0u);
if ((read_reg(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_XLDA) != 0u) {
lsm6dsox_sample_t stale;
read_output_registers(&stale);
}
write_reg(LSM6DSOX_INT1_CTRL, LSM6DSOX_INT1_DRDY_XL);
write_reg(LSM6DSOX_TAP_CFG2,
read_reg(LSM6DSOX_TAP_CFG2) | LSM6DSOX_INTERRUPTS_ENABLE);
return (read_reg(LSM6DSOX_INT1_CTRL) == LSM6DSOX_INT1_DRDY_XL) &&
(read_reg(LSM6DSOX_INT2_CTRL) == 0u) &&
((read_reg(LSM6DSOX_TAP_CFG2) & LSM6DSOX_INTERRUPTS_ENABLE) != 0u) &&
((read_reg(LSM6DSOX_COUNTER_BDR_REG1) & 0x80u) == 0u);
}
void lsm6dsox_disable_data_ready_interrupt(void)
{
write_reg(LSM6DSOX_INT1_CTRL, 0u);
write_reg(LSM6DSOX_INT2_CTRL, 0u);
write_reg(LSM6DSOX_TAP_CFG2,
read_reg(LSM6DSOX_TAP_CFG2) &
(uint8_t)~LSM6DSOX_INTERRUPTS_ENABLE);
}
int lsm6dsox_read_interrupt_sample(lsm6dsox_sample_t *sample,
uint64_t interrupt_timestamp_ticks)
{
if (sample == NULL) {
return 0;
}
const uint8_t status = read_reg(LSM6DSOX_STATUS_REG);
sample->timestamp_ticks = interrupt_timestamp_ticks;
/* Accel DRDY is the routed source, so clear its entire output block first.
* Then wait only long enough to form a coherent same-ODR gyro pair. */
read_accel_registers(sample);
if ((status & LSM6DSOX_STATUS_XLDA) == 0u) {
return 0;
}
const uint64_t wait_start = time_now();
const uint64_t timeout_ticks =
((uint64_t)BSP_CLINT_TIMEBASE_HZ * LSM6DSOX_GYRO_READY_TIMEOUT_US +
999999u) / 1000000u;
while ((read_reg(LSM6DSOX_STATUS_REG) & LSM6DSOX_STATUS_GDA) == 0u) {
if ((time_now() - wait_start) >= timeout_ticks) {
return 0;
}
}
read_gyro_and_temperature_registers(sample);
return 1;
}
void lsm6dsox_power_down(void)
{
lsm6dsox_disable_data_ready_interrupt();
write_reg(LSM6DSOX_CTRL1_XL, 0u);
write_reg(LSM6DSOX_CTRL2_G, 0u);
}