Add STEVAL-MKI217V1 SPI logger and replay

soc/software/steval_mki217v1_test/Makefile

@@ -0,0 +1,20 @@
+TARGET := steval_mki217v1_test
+
+APP_SRCS := \
+	main.c \
+	src/lsm6dsox.c
+
+BSP_SRCS := init.c uart.c pmod_spi.c
+
+# Genesys2 fabric/UART and CLINT defaults. Top-level board builds may override.
+BSP_UART_CLOCK_HZ ?= 50000000
+BSP_CLINT_TIMEBASE_HZ ?= 500000
+
+LSM6DSOX_ODR_HZ ?= 104
+CAPTURE_DURATION_S ?= 60
+
+include ../common/app.mk
+
+CFLAGS += -Iheader
+CFLAGS += -DLSM6DSOX_ODR_HZ=$(LSM6DSOX_ODR_HZ) \
+	-DCAPTURE_DURATION_S=$(CAPTURE_DURATION_S)

soc/software/steval_mki217v1_test/README.md

@@ -0,0 +1,110 @@
+# STEVAL-MKI217V1 polling test (Genesys2)
+
+This standalone bare-metal application reads the LSM6DSOX on the
+STEVAL-MKI217V1 over the existing Genesys2 PMOD SPI peripheral. It does not
+use INT1/INT2. The LIS2MDL magnetometer on the module is not enabled in this
+first electrical/readout smoke test; it is internally reachable through the
+LSM6DSOX sensor hub and will be integrated separately after the IMU link is
+verified.
+
+## Wiring
+
+| STEVAL-MKI217V1 | Genesys2 | Function |
+|---|---|---|
+| CS | JC1 | active-low chip select |
+| SDA | JC2 | SPI MOSI / SDI |
+| SDO | JC3 | SPI MISO |
+| SCL | JC4 | SPI clock / SPC |
+| VDD | 3.3 V | sensor supply |
+| VDDIO | 3.3 V | digital I/O supply |
+| GND | GND | common ground |
+
+`CS` is mandatory for four-wire SPI. Never feed 5 V to VDDIO. JC9 is the
+optional sensor interrupt pin but is deliberately unused here.
+
+## Hardware bring-up results (2026-07-13)
+
+The Genesys2/Rocket ELF was loaded over GDB and the UART output was captured
+on `/dev/ttyUSB0`. All eight identity reads returned `0xff` instead of the
+expected `0x6c`. In that trial SCL, SDA, SDO, VDDIO, VDD and GND were connected
+but CS was not. An all-`0xff` response is therefore the expected open/unselected
+SPI-bus result, not a valid sensor sample.
+
+After CS was connected to JC1, all eight identity reads returned `0x6c` and
+configuration read-back returned `CTRL1_XL=0x40`, `CTRL2_G=0x40`,
+`CTRL3_C=0x44` and `STATUS=0x07`.
+
+The original FPGA SPI implementation sampled MISO on the data-change edge.
+A first 60-second diagnostic log consequently contained about 68.4% bit-shifted
+samples. The generic SPI control register was extended with independent CPOL
+and CPHA bits while retaining the single/quad-width bit. Mode 0 is selected by
+this driver and has been verified on hardware. A Mode-3 probe returned an
+incorrect identity byte (`0x28`), so the CPHA=1 path is not yet production
+validated.
+
+One additional LSM6DSOX-specific issue was found: a 14-byte burst crossing the
+`OUT_TEMP_H` to `OUTX_L_G` internal output-block boundary shifted the remaining
+stream by one bit. Delaying between bytes did not help. The permanent readout
+therefore uses a 2-byte temperature burst followed by one coherent 12-byte
+gyro+accelerometer burst. This is not sample filtering or post-processing; it
+matches the sensor's output block structure and preserves BDU coherence.
+
+The final real-hardware capture is
+`../fusion/logs/steval_mki217v1_clean_60s.csv`:
+
+- 6349 samples over 59.994462 seconds (105.810 Hz measured)
+- sequence discontinuities: 0
+- bit-shifted, saturated or invalid samples: 0
+- maximum timestamp gap: 4742 CLINT ticks (9.484 ms)
+- acceleration norm: 9.94245 +/- 0.00687 m/s^2
+- gyro mean XYZ: +0.20395, -0.07578, -0.27628 deg/s
+- gyro standard deviation XYZ: 0.02600, 0.02379, 0.02630 deg/s
+- temperature: 25.909 +/- 0.056 degC
+
+These measurements are for the stationary board in its test orientation. The
+nonzero gyro mean is startup bias to be estimated by stationary calibration,
+not angular motion.
+
+## Build and load
+
+From the repository root:
+
+```sh
+make FPGA_BOARD=genesys2 fpga-elf-flash-image \
+  APP=steval_mki217v1_test
+
+make FPGA_BOARD=genesys2 fpga-debug-load \
+  APP=steval_mki217v1_test
+```
+
+The default run is 104 Hz for 60 seconds. Alternatives can be passed through
+the application directory build:
+
+```sh
+make -C soc/software/steval_mki217v1_test \
+  BUILD_DIR=../../out/software/steval_mki217v1_test \
+  LSM6DSOX_ODR_HZ=52 CAPTURE_DURATION_S=60
+```
+
+Supported smoke-test ODR values are 52, 104 and 208 Hz. At 115200 baud, use
+52 or 104 Hz for loss-free line-oriented logging. The firmware verifies
+`WHO_AM_I=0x6c`, selects SPI mode 0, resets the device, enables
+block-data-update and register auto-increment, and configures +/-2 g and
++/-250 dps. The FPGA SPI controller exposes CPOL/CPHA independently from its
+single/quad data-width selection.
+
+## UART/replay format
+
+Each sample is timestamped from the local CLINT/CPU time domain before its SPI
+burst and emitted without floating-point formatting:
+
+```text
+IMU_META,LSM6DSOX,tick_hz=500000,odr_hz=104,accel_ug_lsb=61,gyro_udps_lsb=8750,temp_lsb_c=256,temp_offset_millic=25000
+IMU_RAW,LSM6DSOX,0,12345678,ax,ay,az,gx,gy,gz,temp
+```
+
+Scale factors are 61 ug/LSB, 8750 udps/LSB and 256 LSB/degC with a 25 degC
+offset. `fusion/fusion_eskf.py` accepts this raw format directly, so a captured
+log can be replayed with an existing GPS UART capture. Sequential GPS and IMU
+captures are meaningful only for a stationary test. A moving-platform replay
+requires GPS and IMU samples stamped in the same firmware/run.

soc/software/steval_mki217v1_test/header/lsm6dsox.h

@@ -0,0 +1,36 @@
+#ifndef LSM6DSOX_H
+#define LSM6DSOX_H
+
+#include <stdint.h>
+
+#define LSM6DSOX_WHO_AM_I_VALUE 0x6cu
+
+typedef struct {
+  uint64_t timestamp_ticks;
+  int16_t accel[3];
+  int16_t gyro[3];
+  int16_t temperature;
+} lsm6dsox_sample_t;
+
+typedef struct {
+  uint32_t odr_hz;
+  uint32_t accel_ug_per_lsb;
+  uint32_t gyro_udps_per_lsb;
+  uint32_t temperature_lsb_per_c;
+  int32_t temperature_offset_millic;
+} lsm6dsox_scale_t;
+
+/* Initialize the PMOD SPI peripheral, identify and configure the IMU. */
+int lsm6dsox_init(uint32_t odr_hz);
+
+/* Read-only helpers used by the smoke-test diagnostics. */
+uint8_t lsm6dsox_read_who_am_i(void);
+uint8_t lsm6dsox_read_register(uint8_t reg);
+
+/* Return 1 for a new coherent gyro+accelerometer sample, 0 if not ready. */
+int lsm6dsox_poll_sample(lsm6dsox_sample_t *sample);
+
+void lsm6dsox_power_down(void);
+const lsm6dsox_scale_t *lsm6dsox_scale(void);
+
+#endif

soc/software/steval_mki217v1_test/main.c

@@ -0,0 +1,106 @@
+#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;
+}

soc/software/steval_mki217v1_test/src/lsm6dsox.c

@@ -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;
+}