Add STEVAL-MKI217V1 SPI logger and replay

2026-07-13 22:16:51 +03:00
parent f16ac8aa80
commit df563aa207
12 changed files with 629 additions and 13 deletions
@@ -0,0 +1,195 @@
+#include "lsm6dsox.h"
+
+#include <stddef.h>
+
+#include "bsp/mmio.h"
+#include "bsp/pmod_spi.h"
+#include "bsp/soc.h"
+
+#define LSM6DSOX_SPI_READ 0x80u
+#define LSM6DSOX_SPI_DUMMY 0xffu
+
+#define LSM6DSOX_CTRL1_XL 0x10u
+#define LSM6DSOX_CTRL2_G 0x11u
+#define LSM6DSOX_CTRL3_C 0x12u
+#define LSM6DSOX_STATUS_REG 0x1eu
+#define LSM6DSOX_OUT_TEMP_L 0x20u
+#define LSM6DSOX_OUTX_L_G 0x22u
+#define LSM6DSOX_WHO_AM_I 0x0fu
+
+#define LSM6DSOX_CTRL3_SW_RESET 0x01u
+#define LSM6DSOX_CTRL3_IF_INC 0x04u
+#define LSM6DSOX_CTRL3_BDU 0x40u
+#define LSM6DSOX_STATUS_XLDA 0x01u
+#define LSM6DSOX_STATUS_GDA 0x02u
+
+/* CTRL1_XL/CTRL2_G ODR fields (bits 7:4). */
+#define LSM6DSOX_ODR_52 0x30u
+#define LSM6DSOX_ODR_104 0x40u
+#define LSM6DSOX_ODR_208 0x50u
+
+/* This first integration uses +-2 g and +-250 dps. */
+static const lsm6dsox_scale_t sensor_scale = {
+  0u,
+  61u,       /* 0.061 mg/LSB = 61 ug/LSB. */
+  8750u,     /* 8.75 mdps/LSB = 8750 udps/LSB. */
+  256u,
+  25000
+};
+
+static lsm6dsox_scale_t configured_scale;
+
+static uint64_t time_now(void)
+{
+  return bsp_read64(BSP_CLINT_MTIME);
+}
+
+static void delay_ms(uint32_t ms)
+{
+  const uint64_t ticks =
+      ((uint64_t)BSP_CLINT_TIMEBASE_HZ * ms + 999u) / 1000u;
+  const uint64_t start = time_now();
+  while ((time_now() - start) < ticks) {
+  }
+}
+
+static void write_reg(uint8_t reg, uint8_t value)
+{
+  bsp_pmod_spi_write_reg8(reg & (uint8_t)~LSM6DSOX_SPI_READ, value);
+}
+
+static uint8_t read_reg(uint8_t reg)
+{
+  return bsp_pmod_spi_read_reg8(reg | LSM6DSOX_SPI_READ,
+                                LSM6DSOX_SPI_DUMMY);
+}
+
+static int16_t little_endian_i16(const uint8_t *bytes)
+{
+  return (int16_t)((uint16_t)bytes[0] | ((uint16_t)bytes[1] << 8));
+}
+
+static int odr_register_value(uint32_t odr_hz, uint8_t *value)
+{
+  switch (odr_hz) {
+    case 52u:
+      *value = LSM6DSOX_ODR_52;
+      return 1;
+    case 104u:
+      *value = LSM6DSOX_ODR_104;
+      return 1;
+    case 208u:
+      *value = LSM6DSOX_ODR_208;
+      return 1;
+    default:
+      return 0;
+  }
+}
+
+uint8_t lsm6dsox_read_who_am_i(void)
+{
+  return read_reg(LSM6DSOX_WHO_AM_I);
+}
+
+uint8_t lsm6dsox_read_register(uint8_t reg)
+{
+  return read_reg(reg);
+}
+
+int lsm6dsox_init(uint32_t odr_hz)
+{
+  uint8_t odr_value;
+  if (!odr_register_value(odr_hz, &odr_value)) {
+    return 0;
+  }
+
+  bsp_pmod_spi_init();
+  bsp_pmod_spi_set_mode(BSP_PMOD_SPI_MODE_SINGLE);
+  bsp_pmod_spi_set_clock_mode(BSP_PMOD_SPI_CLOCK_MODE0);
+  /* Conservative bring-up timing; SPI SCK remains controlled by the FPGA IP. */
+  bsp_pmod_spi_set_timing(2u, 2u, 2u, 5u);
+  delay_ms(20u);
+
+  if (read_reg(LSM6DSOX_WHO_AM_I) != LSM6DSOX_WHO_AM_I_VALUE) {
+    return 0;
+  }
+
+  write_reg(LSM6DSOX_CTRL3_C, LSM6DSOX_CTRL3_SW_RESET);
+  const uint64_t reset_start = time_now();
+  const uint64_t reset_timeout =
+      ((uint64_t)BSP_CLINT_TIMEBASE_HZ * 100u + 999u) / 1000u;
+  while ((read_reg(LSM6DSOX_CTRL3_C) & LSM6DSOX_CTRL3_SW_RESET) != 0u) {
+    if ((time_now() - reset_start) >= reset_timeout) {
+      return 0;
+    }
+  }
+  delay_ms(10u);
+
+  /* Block partial output updates and auto-increment during burst reads. */
+  write_reg(LSM6DSOX_CTRL3_C,
+            LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC);
+
+  /* ODR plus FS=00: +-2 g accelerometer, +-250 dps gyroscope. */
+  write_reg(LSM6DSOX_CTRL1_XL, odr_value);
+  write_reg(LSM6DSOX_CTRL2_G, odr_value);
+  delay_ms(100u);
+
+  if ((read_reg(LSM6DSOX_CTRL3_C) !=
+       (LSM6DSOX_CTRL3_BDU | LSM6DSOX_CTRL3_IF_INC)) ||
+      (read_reg(LSM6DSOX_CTRL1_XL) != odr_value) ||
+      (read_reg(LSM6DSOX_CTRL2_G) != odr_value)) {
+    lsm6dsox_power_down();
+    return 0;
+  }
+
+  configured_scale = sensor_scale;
+  configured_scale.odr_hz = odr_hz;
+  return 1;
+}
+
+int lsm6dsox_poll_sample(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,
+                         2u);
+  /* Do not cross the temperature-to-gyroscope internal output-block boundary
+   * in one SPI burst. Gyroscope and accelerometer form one 12-byte block. */
+  bsp_pmod_spi_read_regs(LSM6DSOX_OUTX_L_G | LSM6DSOX_SPI_READ,
+                         LSM6DSOX_SPI_DUMMY,
+                         &raw[2],
+                         sizeof(raw) - 2u);
+
+  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]);
+  sample->accel[0] = little_endian_i16(&raw[8]);
+  sample->accel[1] = little_endian_i16(&raw[10]);
+  sample->accel[2] = little_endian_i16(&raw[12]);
+  return 1;
+}
+
+void lsm6dsox_power_down(void)
+{
+  write_reg(LSM6DSOX_CTRL1_XL, 0u);
+  write_reg(LSM6DSOX_CTRL2_G, 0u);
+}
+
+const lsm6dsox_scale_t *lsm6dsox_scale(void)
+{
+  return &configured_scale;
+}