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

Add STEVAL-MKI217V1 SPI logger and replay

Browse Source
This commit is contained in:
onur
2026-07-13 22:16:51 +03:00
parent f16ac8aa80
commit df563aa207
12 changed files with 629 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
hardware/soc/scala/fpga/VCU118Peripherals.scala
+27 -6
View File
@@ -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")))
soc/software/Makefile
+5 -1
View File
@@ -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
soc/software/bsp/drivers/pmod_spi.c
+44 -1
View File
@@ -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,
soc/software/bsp/include/bsp/pmod_spi.h
+14
View File
@@ -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.
soc/software/fusion/fusion_eskf.py
+70 -5
View File
@@ -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
soc/software/gps_system/src/tdk_icm42688.c
+1
View File
@@ -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);
soc/software/pmod_spi_test/pmod_spi_test.c
+1
View File
@@ -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);
soc/software/steval_mki217v1_test/Makefile
+20
View File
@@ -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
+110
View File
@@ -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
+36
View File
@@ -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
+106
View File
@@ -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
+195
View File
@@ -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;
}