omnitiles/drivers/
lsm6dsv16x.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! ST LSM6DSV16X 6-axis IMU (accelerometer + gyroscope) SPI driver.
5//!
6//! Wired on SPI4 CS2. Uses 4-wire SPI mode 0 (CPOL=0, CPHA=0), matching the bus
7//! config used by the DWM tag on CS1. The `read_sample()` method returns a single
8//! synchronized accelerometer + gyroscope reading converted to SI units (m/s^2 and
9//! rad/s) so the tilt control loop on the STM32 can consume it directly.
10
11use crate::hw::{spi::CsControl, SpiBus};
12use core::f32::consts::PI;
13use stm32f7xx_hal::spi;
14
15/// Expected WHO_AM_I register value for the LSM6DSV16X.
16const WHO_AM_I_VALUE: u8 = 0x70;
17
18/// Standard gravity (m/s^2) for converting raw accel LSB to SI units.
19const G: f32 = 9.80665;
20
21mod reg {
22    pub const WHO_AM_I: u8 = 0x0F;
23    pub const CTRL1: u8 = 0x10;
24    pub const CTRL2: u8 = 0x11;
25    pub const CTRL3: u8 = 0x12;
26    pub const CTRL6: u8 = 0x15;
27    pub const CTRL8: u8 = 0x17;
28    pub const OUTX_L_G: u8 = 0x22;
29}
30
31/// SPI read bit (OR'd into the register address byte).
32const SPI_READ: u8 = 0x80;
33
34#[derive(Debug)]
35pub enum Error {
36    Spi(spi::Error),
37    InvalidDevice,
38}
39
40impl From<spi::Error> for Error {
41    fn from(e: spi::Error) -> Self {
42        Error::Spi(e)
43    }
44}
45
46/// A single synchronized IMU sample in SI units.
47#[derive(Debug, Clone, Copy, Default)]
48pub struct ImuSample {
49    /// Linear acceleration in m/s^2 (sensor frame).
50    pub ax: f32,
51    pub ay: f32,
52    pub az: f32,
53    /// Angular rate in rad/s (sensor frame).
54    pub gx: f32,
55    pub gy: f32,
56    pub gz: f32,
57}
58
59/// LSM6DSV16X driver. Does not own the SPI bus or CS line — they are passed in
60/// per-call so that SPI4 can be shared with other devices (DWM tag on CS1).
61pub struct Lsm6dsv16x {
62    accel_scale: f32,
63    gyro_scale: f32,
64}
65
66impl Lsm6dsv16x {
67    /// Probe WHO_AM_I, soft-reset, and configure accel + gyro for ±2 g / ±2000 dps
68    /// at 120 Hz ODR in high-performance mode.
69    pub fn new<I, PINS, CS>(spi: &mut SpiBus<I, PINS>, cs: &mut CS) -> Result<Self, Error>
70    where
71        I: spi::Instance,
72        PINS: spi::Pins<I>,
73        CS: CsControl,
74    {
75        let mut dev = Self {
76            accel_scale: (2.0 * G) / 32768.0,            // ±2 g full scale
77            gyro_scale: (2000.0 * PI / 180.0) / 32768.0, // ±2000 dps full scale
78        };
79
80        if dev.read_reg(spi, cs, reg::WHO_AM_I)? != WHO_AM_I_VALUE {
81            return Err(Error::InvalidDevice);
82        }
83
84        // CTRL3: BOOT=0, BDU=1 (bit6), IF_INC=1 (bit2), SW_RESET=1 (bit0).
85        dev.write_reg(spi, cs, reg::CTRL3, 0x45)?;
86        // Wait for SW_RESET to clear (datasheet: ~50 µs). Poll a few times.
87        for _ in 0..1000 {
88            let v = dev.read_reg(spi, cs, reg::CTRL3)?;
89            if (v & 0x01) == 0 {
90                break;
91            }
92        }
93        // Ensure BDU + IF_INC are set after reset.
94        dev.write_reg(spi, cs, reg::CTRL3, 0x44)?;
95
96        // CTRL8: FS_XL[1:0] = 00 → ±2 g.
97        dev.write_reg(spi, cs, reg::CTRL8, 0x00)?;
98        // CTRL6: FS_G[3:0] = 0100 → ±2000 dps.
99        dev.write_reg(spi, cs, reg::CTRL6, 0x04)?;
100
101        // CTRL1: OP_MODE_XL[6:4] = 000 (high-perf), ODR_XL[3:0] = 0110 (120 Hz).
102        dev.write_reg(spi, cs, reg::CTRL1, 0x06)?;
103        // CTRL2: OP_MODE_G[6:4]  = 000 (high-perf), ODR_G[3:0]  = 0110 (120 Hz).
104        dev.write_reg(spi, cs, reg::CTRL2, 0x06)?;
105
106        Ok(dev)
107    }
108
109    /// Burst-read gyro XYZ and accel XYZ (12 bytes starting at OUTX_L_G) and convert
110    /// to SI units. Gyro comes first in the register map, then accel.
111    pub fn read_sample<I, PINS, CS>(
112        &mut self,
113        spi: &mut SpiBus<I, PINS>,
114        cs: &mut CS,
115    ) -> Result<ImuSample, Error>
116    where
117        I: spi::Instance,
118        PINS: spi::Pins<I>,
119        CS: CsControl,
120    {
121        let mut buf = [0u8; 13];
122        buf[0] = reg::OUTX_L_G | SPI_READ;
123
124        cs.select();
125        let res = spi.transfer_in_place(&mut buf);
126        cs.deselect();
127        res?;
128
129        let gx_raw = i16::from_le_bytes([buf[1], buf[2]]);
130        let gy_raw = i16::from_le_bytes([buf[3], buf[4]]);
131        let gz_raw = i16::from_le_bytes([buf[5], buf[6]]);
132        let ax_raw = i16::from_le_bytes([buf[7], buf[8]]);
133        let ay_raw = i16::from_le_bytes([buf[9], buf[10]]);
134        let az_raw = i16::from_le_bytes([buf[11], buf[12]]);
135
136        Ok(ImuSample {
137            ax: ax_raw as f32 * self.accel_scale,
138            ay: ay_raw as f32 * self.accel_scale,
139            az: az_raw as f32 * self.accel_scale,
140            gx: gx_raw as f32 * self.gyro_scale,
141            gy: gy_raw as f32 * self.gyro_scale,
142            gz: gz_raw as f32 * self.gyro_scale,
143        })
144    }
145
146    fn read_reg<I, PINS, CS>(
147        &mut self,
148        spi: &mut SpiBus<I, PINS>,
149        cs: &mut CS,
150        addr: u8,
151    ) -> Result<u8, Error>
152    where
153        I: spi::Instance,
154        PINS: spi::Pins<I>,
155        CS: CsControl,
156    {
157        let mut buf = [addr | SPI_READ, 0];
158        cs.select();
159        let res = spi.transfer_in_place(&mut buf);
160        cs.deselect();
161        res?;
162        Ok(buf[1])
163    }
164
165    fn write_reg<I, PINS, CS>(
166        &mut self,
167        spi: &mut SpiBus<I, PINS>,
168        cs: &mut CS,
169        addr: u8,
170        value: u8,
171    ) -> Result<(), Error>
172    where
173        I: spi::Instance,
174        PINS: spi::Pins<I>,
175        CS: CsControl,
176    {
177        let mut buf = [addr & 0x7F, value];
178        cs.select();
179        let res = spi.transfer_in_place(&mut buf);
180        cs.deselect();
181        res?;
182        Ok(())
183    }
184}
omnitiles/drivers/lsm6dsv16x.rs

omnitiles/drivers/
lsm6dsv16x.rs
