diff --git a/hardware/soc/scala/fpga/VCU118Peripherals.scala b/hardware/soc/scala/fpga/VCU118Peripherals.scala index 7ea0f75..9c8ce2a 100644 --- a/hardware/soc/scala/fpga/VCU118Peripherals.scala +++ b/hardware/soc/scala/fpga/VCU118Peripherals.scala @@ -196,6 +196,8 @@ class TLFpgaSPI(params: FpgaSPIParams, beatBytes: Int)(implicit p: Parameters) e val csn = RegInit(true.B) val sck = RegInit(false.B) val quad = RegInit(false.B) + val cpol = RegInit(false.B) + val cpha = RegInit(false.B) val txShift = RegInit(0.U(32.W)) val rxShift = RegInit(0.U(32.W)) val bitsLeft = RegInit(0.U(6.W)) @@ -211,13 +213,29 @@ class TLFpgaSPI(params: FpgaSPIParams, beatBytes: Int)(implicit p: Parameters) e when(div === (params.clockDiv - 1).U) { div := 0.U sck := ~sck - when(sck) { - val step = Mux(quad, 4.U, 1.U) + val step = Mux(quad, 4.U, 1.U) + /* + * `sck` is the pre-toggle level. The edge away from CPOL is the + * leading edge; CPHA selects leading (0) or trailing (1) sampling. + * TX advances only after the active sample edge, so all four standard + * SPI modes retain a full half-cycle of setup time. + */ + val leadingEdge = sck === cpol + val sampleEdge = Mux(cpha, !leadingEdge, leadingEdge) + when(sampleEdge) { rxShift := Mux(quad, Cat(rxShift(27, 0), dq_i), Cat(rxShift(30, 0), dq_i(1))) + } + + /* + * CPHA=0 completes on the trailing edge after the final leading-edge + * sample. CPHA=1 samples and completes on the final trailing edge. + */ + val advanceEdge = Mux(cpha, sampleEdge, !sampleEdge) + when(advanceEdge) { txShift := Mux(quad, Cat(txShift(27, 0), 0.U(4.W)), Cat(txShift(30, 0), 0.U(1.W))) when(bitsLeft <= step) { busy := false.B - sck := false.B + sck := cpol bitsLeft := 0.U }.otherwise { bitsLeft := bitsLeft - step @@ -229,12 +247,15 @@ class TLFpgaSPI(params: FpgaSPIParams, beatBytes: Int)(implicit p: Parameters) e } val ctrl = Wire(UInt(32.W)) - ctrl := Cat(0.U(29.W), quad, csn, busy) + ctrl := Cat(0.U(27.W), cpha, cpol, quad, csn, busy) val ctrlWrite = RegWriteFn { (valid, data) => when(valid && !busy) { csn := data(1) quad := data(2) + cpol := data(3) + cpha := data(4) + sck := data(3) } true.B } @@ -246,13 +267,13 @@ class TLFpgaSPI(params: FpgaSPIParams, beatBytes: Int)(implicit p: Parameters) e bitsLeft := Mux(quad, 32.U, 8.U) busy := true.B div := 0.U - sck := false.B + sck := cpol } true.B } node.regmap( - 0x00 -> Seq(RegField(32, ctrl, ctrlWrite, RegFieldDesc("ctrl", "bit0 busy RO, bit1 csn RW, bit2 quad RW"))), + 0x00 -> Seq(RegField(32, ctrl, ctrlWrite, RegFieldDesc("ctrl", "bit0 busy RO, bit1 csn RW, bit2 quad RW, bit3 CPOL RW, bit4 CPHA RW"))), 0x04 -> Seq(RegField.w(32, txWrite, RegFieldDesc("tx", "start transfer"))), 0x08 -> Seq(RegField.r(32, rxShift, RegFieldDesc("rx", "last received shift register"))), 0x0c -> Seq(RegField.r(1, !busy, RegFieldDesc("ready", "controller ready"))) diff --git a/soc/software/Makefile b/soc/software/Makefile index dab5124..35b3c12 100644 --- a/soc/software/Makefile +++ b/soc/software/Makefile @@ -20,7 +20,7 @@ OBJCOPY := $(TOOLCHAIN)/riscv64-unknown-elf-objcopy READELF := $(TOOLCHAIN)/riscv64-unknown-elf-readelf MAKE_FLASH_IMAGE := $(SOC_DIR)/software/tools/make_flash_image.py -.PHONY: help all app bootloader hello pmod-spi-test gps-system flash-image clean +.PHONY: help all app bootloader hello pmod-spi-test steval-mki217v1-test gps-system flash-image clean help: @echo "Common Rocket bare-metal software targets:" @@ -29,6 +29,7 @@ help: @echo " make bootloader # build only BootROM flash bootloader" @echo " make hello # build only hello UART app" @echo " make pmod-spi-test # build PMOD SPI smoke test app" + @echo " make steval-mki217v1-test # build STEVAL-MKI217V1 LSM6DSOX polling app" @echo " make gps-system # build imported GPS bare-metal app" @echo " make flash-image APP=name # build bootloader-readable app flash image" @echo " make clean # remove software build products" @@ -51,6 +52,9 @@ hello: pmod-spi-test: @make -C pmod_spi_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/pmod_spi_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all +steval-mki217v1-test: + @make -C steval_mki217v1_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/steval_mki217v1_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all + gps-system: @make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all diff --git a/soc/software/bsp/drivers/pmod_spi.c b/soc/software/bsp/drivers/pmod_spi.c index a0e0a25..09d595d 100644 --- a/soc/software/bsp/drivers/pmod_spi.c +++ b/soc/software/bsp/drivers/pmod_spi.c @@ -11,6 +11,11 @@ #define PMOD_SPI_CTRL_BUSY 0x1u #define PMOD_SPI_CTRL_CSN 0x2u #define PMOD_SPI_CTRL_QUAD 0x4u +#define PMOD_SPI_CTRL_CPOL 0x8u +#define PMOD_SPI_CTRL_CPHA 0x10u +#define PMOD_SPI_CTRL_WRITABLE \ + (PMOD_SPI_CTRL_CSN | PMOD_SPI_CTRL_QUAD | \ + PMOD_SPI_CTRL_CPOL | PMOD_SPI_CTRL_CPHA) static uint32_t pmod_spi_ctrl_shadow = PMOD_SPI_CTRL_CSN; static uint32_t pmod_spi_cs_setup_us; @@ -42,7 +47,7 @@ static void pmod_spi_wait_ready(void) static void pmod_spi_write_ctrl(uint32_t ctrl) { pmod_spi_wait_ready(); - pmod_spi_ctrl_shadow = ctrl & (PMOD_SPI_CTRL_CSN | PMOD_SPI_CTRL_QUAD); + pmod_spi_ctrl_shadow = ctrl & PMOD_SPI_CTRL_WRITABLE; bsp_write32(PMOD_SPI_CTRL, pmod_spi_ctrl_shadow); } @@ -72,6 +77,44 @@ void bsp_pmod_spi_set_quad(int enabled) bsp_pmod_spi_set_mode(enabled ? BSP_PMOD_SPI_MODE_QUAD : BSP_PMOD_SPI_MODE_SINGLE); } +void bsp_pmod_spi_set_clock_mode(bsp_pmod_spi_clock_mode_t mode) +{ + uint32_t ctrl = + pmod_spi_ctrl_shadow & ~(PMOD_SPI_CTRL_CPOL | PMOD_SPI_CTRL_CPHA); + + switch (mode) { + case BSP_PMOD_SPI_CLOCK_MODE1: + ctrl |= PMOD_SPI_CTRL_CPHA; + break; + case BSP_PMOD_SPI_CLOCK_MODE2: + ctrl |= PMOD_SPI_CTRL_CPOL; + break; + case BSP_PMOD_SPI_CLOCK_MODE3: + ctrl |= PMOD_SPI_CTRL_CPOL | PMOD_SPI_CTRL_CPHA; + break; + case BSP_PMOD_SPI_CLOCK_MODE0: + default: + break; + } + pmod_spi_write_ctrl(ctrl); +} + +bsp_pmod_spi_clock_mode_t bsp_pmod_spi_clock_mode(void) +{ + const uint32_t bits = + pmod_spi_ctrl_shadow & (PMOD_SPI_CTRL_CPOL | PMOD_SPI_CTRL_CPHA); + if (bits == (PMOD_SPI_CTRL_CPOL | PMOD_SPI_CTRL_CPHA)) { + return BSP_PMOD_SPI_CLOCK_MODE3; + } + if (bits == PMOD_SPI_CTRL_CPOL) { + return BSP_PMOD_SPI_CLOCK_MODE2; + } + if (bits == PMOD_SPI_CTRL_CPHA) { + return BSP_PMOD_SPI_CLOCK_MODE1; + } + return BSP_PMOD_SPI_CLOCK_MODE0; +} + void bsp_pmod_spi_set_timing(uint32_t cs_setup_us, uint32_t inter_byte_us, uint32_t cs_hold_us, diff --git a/soc/software/bsp/include/bsp/pmod_spi.h b/soc/software/bsp/include/bsp/pmod_spi.h index 7f860d9..ed37383 100644 --- a/soc/software/bsp/include/bsp/pmod_spi.h +++ b/soc/software/bsp/include/bsp/pmod_spi.h @@ -9,11 +9,25 @@ typedef enum { BSP_PMOD_SPI_MODE_QUAD = 1 } bsp_pmod_spi_mode_t; +typedef enum { + BSP_PMOD_SPI_CLOCK_MODE0 = 0, + BSP_PMOD_SPI_CLOCK_MODE1 = 1, + BSP_PMOD_SPI_CLOCK_MODE2 = 2, + BSP_PMOD_SPI_CLOCK_MODE3 = 3 +} bsp_pmod_spi_clock_mode_t; + void bsp_pmod_spi_init(void); void bsp_pmod_spi_set_mode(bsp_pmod_spi_mode_t mode); bsp_pmod_spi_mode_t bsp_pmod_spi_mode(void); void bsp_pmod_spi_set_quad(int enabled); +/* + * Select the standard CPOL/CPHA SPI clock mode independently of data width. + * Mode 0 is hardware-validated; modes 1-3 require target/device validation. + */ +void bsp_pmod_spi_set_clock_mode(bsp_pmod_spi_clock_mode_t mode); +bsp_pmod_spi_clock_mode_t bsp_pmod_spi_clock_mode(void); + /* * Configure delays used by the chip-select-managed transaction helpers. * All values are in microseconds. Zero disables the corresponding delay. diff --git a/soc/software/fusion/fusion_eskf.py b/soc/software/fusion/fusion_eskf.py index 5eb7f5a..17cd9dc 100644 --- a/soc/software/fusion/fusion_eskf.py +++ b/soc/software/fusion/fusion_eskf.py @@ -1,9 +1,12 @@ #!/usr/bin/env python3 -"""Offline GPS/ICM-4268x fusion for the stationary Genesys2 capture. +"""Offline GPS/IMU fusion for the stationary Genesys2 capture. The two UART logs are captured sequentially, so their elapsed-time axes are replayed together. That is valid for the explicitly stationary experiment, but is not a substitute for a synchronous moving-platform capture. + +The IMU parser accepts both the original averaged ICM-4268x report and the +timestamped raw LSM6DSOX stream from ``steval_mki217v1_test``. """ from __future__ import annotations @@ -98,8 +101,69 @@ def parse_gps(path: Path) -> list[GpsSample]: def parse_imu(path: Path) -> list[ImuSample]: samples: list[ImuSample] = [] + raw_meta: dict[str, float] | None = None + first_raw_tick: int | None = None for row in read_uart_csv(path): - match = IMU_RE.search(row["uart_line"]) + line = row["uart_line"] + + if line.startswith("IMU_META,LSM6DSOX,"): + fields = line.split(",")[2:] + values: dict[str, float] = {} + for field in fields: + key, separator, value = field.partition("=") + if not separator: + continue + values[key] = float(value) + required = { + "tick_hz", + "odr_hz", + "accel_ug_lsb", + "gyro_udps_lsb", + "temp_lsb_c", + "temp_offset_millic", + } + missing = required.difference(values) + if missing: + raise ValueError( + f"LSM6DSOX metadata missing {sorted(missing)} in {path}" + ) + if values["tick_hz"] <= 0.0 or values["temp_lsb_c"] <= 0.0: + raise ValueError(f"invalid LSM6DSOX metadata in {path}") + raw_meta = values + continue + + if line.startswith("IMU_RAW,LSM6DSOX,"): + if raw_meta is None: + raise ValueError(f"LSM6DSOX raw sample precedes metadata in {path}") + fields = line.split(",") + if len(fields) != 11: + raise ValueError(f"malformed LSM6DSOX raw sample in {path}: {line}") + tick = int(fields[3]) + if first_raw_tick is None: + first_raw_tick = tick + raw_accel = np.array([int(x) for x in fields[4:7]], dtype=float) + raw_gyro = np.array([int(x) for x in fields[7:10]], dtype=float) + raw_temperature = int(fields[10]) + samples.append( + ImuSample( + t=(tick - first_raw_tick) / raw_meta["tick_hz"], + unix_s=float(row["host_unix_s"]), + samples=1, + temperature_c=( + raw_meta["temp_offset_millic"] / 1000.0 + + raw_temperature / raw_meta["temp_lsb_c"] + ), + accel_mps2=( + G * raw_accel * raw_meta["accel_ug_lsb"] * 1e-6 + ), + gyro_rps=np.deg2rad( + raw_gyro * raw_meta["gyro_udps_lsb"] * 1e-6 + ), + ) + ) + continue + + match = IMU_RE.search(line) if not match: continue samples.append( @@ -119,9 +183,10 @@ def parse_imu(path: Path) -> list[ImuSample]: ) if len(samples) < 4: raise ValueError(f"only {len(samples)} IMU windows found in {path}") - t0 = samples[0].t - for sample in samples: - sample.t -= t0 + if first_raw_tick is None: + t0 = samples[0].t + for sample in samples: + sample.t -= t0 return samples diff --git a/soc/software/gps_system/src/tdk_icm42688.c b/soc/software/gps_system/src/tdk_icm42688.c index 9733e03..834e5fb 100644 --- a/soc/software/gps_system/src/tdk_icm42688.c +++ b/soc/software/gps_system/src/tdk_icm42688.c @@ -221,6 +221,7 @@ bool tdk_icm42688_init(const tdk_icm42688_config_t* config) 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); bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u); delay_us(1000u); diagnostics.who_am_i = read_register(ICM_WHO_AM_I); diff --git a/soc/software/pmod_spi_test/pmod_spi_test.c b/soc/software/pmod_spi_test/pmod_spi_test.c index bbdc553..484758c 100644 --- a/soc/software/pmod_spi_test/pmod_spi_test.c +++ b/soc/software/pmod_spi_test/pmod_spi_test.c @@ -406,6 +406,7 @@ int main(void) 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); bsp_pmod_spi_set_timing(10u, 10u, 10u, 10u); delay_us(1000u); diff --git a/soc/software/steval_mki217v1_test/Makefile b/soc/software/steval_mki217v1_test/Makefile new file mode 100644 index 0000000..43b059d --- /dev/null +++ b/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) diff --git a/soc/software/steval_mki217v1_test/README.md b/soc/software/steval_mki217v1_test/README.md new file mode 100644 index 0000000..a76be5e --- /dev/null +++ b/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. diff --git a/soc/software/steval_mki217v1_test/header/lsm6dsox.h b/soc/software/steval_mki217v1_test/header/lsm6dsox.h new file mode 100644 index 0000000..cf5a372 --- /dev/null +++ b/soc/software/steval_mki217v1_test/header/lsm6dsox.h @@ -0,0 +1,36 @@ +#ifndef LSM6DSOX_H +#define LSM6DSOX_H + +#include + +#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 diff --git a/soc/software/steval_mki217v1_test/main.c b/soc/software/steval_mki217v1_test/main.c new file mode 100644 index 0000000..b9da82c --- /dev/null +++ b/soc/software/steval_mki217v1_test/main.c @@ -0,0 +1,106 @@ +#include +#include + +#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; +} diff --git a/soc/software/steval_mki217v1_test/src/lsm6dsox.c b/soc/software/steval_mki217v1_test/src/lsm6dsox.c new file mode 100644 index 0000000..6fd6540 --- /dev/null +++ b/soc/software/steval_mki217v1_test/src/lsm6dsox.c @@ -0,0 +1,195 @@ +#include "lsm6dsox.h" + +#include + +#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; +}