diff --git a/Cargo.lock b/Cargo.lock index 8f52765..3b49a9d 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1405,6 +1405,8 @@ dependencies = [ "embassy-sync", "embassy-time", "embedded-alloc", + "embedded-hal 1.0.0", + "embedded-hal-async", "fixed", "heapless", "libm", diff --git a/Cargo.toml b/Cargo.toml index 018783b..e83f316 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -58,6 +58,7 @@ defmt = "0.3.0" defmt-rtt = "0.4.0" embedded-storage = "0.3.0" embedded-hal = "1.0.0" +embedded-hal-async = { version = "1.0.0" } embedded-alloc = "0.6.0" nrf-softdevice = { version = "0.1.0", features = [ "defmt", diff --git a/memory.x b/memory.x index 7a7d5e0..7328124 100644 --- a/memory.x +++ b/memory.x @@ -1,5 +1,7 @@ MEMORY { + /* https://infocenter.nordicsemi.com/index.jsp?topic=%2Fsds_s140%2FSDS%2Fs1xx%2Fmem_usage%2Fmem_resource_reqs.html&cp=5_7_4_0_13_0_0 */ + /* Need to leave space for the SoftDevice These values are confirmed working for S140 7.3.0 diff --git a/tflite_demo/Cargo.toml b/tflite_demo/Cargo.toml index 90c7091..88b2378 100644 --- a/tflite_demo/Cargo.toml +++ b/tflite_demo/Cargo.toml @@ -19,6 +19,8 @@ lsm6ds3tr.workspace = true defmt.workspace = true defmt-rtt.workspace = true embedded-alloc.workspace = true +embedded-hal.workspace = true +embedded-hal-async.workspace = true nrf-softdevice.workspace = true nrf-softdevice-s140.workspace = true embassy-nrf.workspace = true diff --git a/tflite_demo/lsm6ds3tr/consts.rs b/tflite_demo/lsm6ds3tr/consts.rs new file mode 100644 index 0000000..829e53e --- /dev/null +++ b/tflite_demo/lsm6ds3tr/consts.rs @@ -0,0 +1,8 @@ +/// LSM6DS3TR-C's ID +pub const LSM6DS3TR_ID: u8 = 0x6A; + +/// Temperature scale = 256 LSB/°C +pub const TEMP_SCALE: f32 = 256.0; + +/// The output of the temperature sensor is 0 (typ.) at 25 °C +pub const TEMP_BIAS: f32 = 25.0; diff --git a/tflite_demo/lsm6ds3tr/data.rs b/tflite_demo/lsm6ds3tr/data.rs new file mode 100644 index 0000000..f6e8b5c --- /dev/null +++ b/tflite_demo/lsm6ds3tr/data.rs @@ -0,0 +1,28 @@ +use core::fmt::{Display, Result}; + +#[allow(clippy::upper_case_acronyms)] +#[derive(Default)] +pub struct XYZ { + pub x: T, + pub y: T, + pub z: T, +} + +impl Display for XYZ +where + T: Display, +{ + fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> Result { + write!(f, "X:{} Y:{} Z:{}", self.x, self.y, self.z) + } +} + +#[cfg(feature = "defmt")] +impl defmt::Format for XYZ +where + T: defmt::Format, +{ + fn format(&self, f: defmt::Formatter) { + defmt::write!(f, "X:{} Y:{} Z:{}", self.x, self.y, self.z) + } +} diff --git a/tflite_demo/lsm6ds3tr/interface/i2c.rs b/tflite_demo/lsm6ds3tr/interface/i2c.rs new file mode 100644 index 0000000..7485eea --- /dev/null +++ b/tflite_demo/lsm6ds3tr/interface/i2c.rs @@ -0,0 +1,41 @@ +use super::Interface; +use embedded_hal_async::i2c::I2c; + +const I2C_ADDRESS: u8 = 0x6A; + +#[derive(Debug)] +pub enum InterfaceE { + Comm(CommE), +} + +/// I2C communication interface +pub struct I2cInterface { + i2c: I2C, +} + +impl I2cInterface { + pub fn new(i2c: I2C) -> Self { + Self { i2c } + } +} + +impl Interface for I2cInterface +where + I2C: I2c, +{ + type Error = InterfaceE; + + async fn write(&mut self, addr: u8, value: u8) -> Result<(), Self::Error> { + self.i2c + .write(I2C_ADDRESS, &[addr, value]) + .await + .map_err(InterfaceE::Comm) + } + + async fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error> { + self.i2c + .write_read(I2C_ADDRESS, &[addr], buffer) + .await + .map_err(InterfaceE::Comm) + } +} diff --git a/tflite_demo/lsm6ds3tr/interface/mod.rs b/tflite_demo/lsm6ds3tr/interface/mod.rs new file mode 100644 index 0000000..2e97499 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/interface/mod.rs @@ -0,0 +1,10 @@ +pub mod i2c; + +/// Communication interface +pub trait Interface { + type Error; + + async fn write(&mut self, addr: u8, value: u8) -> Result<(), Self::Error>; + + async fn read(&mut self, addr: u8, buffer: &mut [u8]) -> Result<(), Self::Error>; +} diff --git a/tflite_demo/lsm6ds3tr/mod.rs b/tflite_demo/lsm6ds3tr/mod.rs new file mode 100644 index 0000000..1ee5455 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/mod.rs @@ -0,0 +1,232 @@ +mod consts; +mod data; +pub mod interface; +pub(crate) mod registers; +pub(crate) mod settings; + +use consts::*; +pub use data::XYZ; +use interface::Interface; +pub use registers::{AccelSampleRate, AccelScale}; +use registers::{ + Ctrl3C, GyroSampleRate, GyroScale, RegisterAddress, RegisterBits, RegisterConfig, + RegisterValue, TapSrc, WakeUpSrc, +}; +pub use settings::{ + irq::{InterruptRoute, TapIrqSettings, TapRecognitionMode}, + AccelSettings, GyroSettings, IrqSettings, LsmSettings, +}; + +/// Device driver +pub struct LSM6DS3TR +where + I: Interface, +{ + interface: I, + pub settings: LsmSettings, +} + +impl LSM6DS3TR +where + I: Interface, +{ + /// Returns uninitialized device object with default settings + pub fn new(interface: I) -> Self { + Self { + interface, + settings: Default::default(), + } + } + + /// Returns uninitialized device object with provided settings + pub fn with_settings(mut self, settings: LsmSettings) -> Self { + self.settings = settings; + self + } + + /// Initializes device with stored settings + pub async fn init(&mut self) -> Result<(), I::Error> { + self.init_accel().await?; + self.init_gyro().await?; + self.init_irqs().await?; + self.init_other().await?; + Ok(()) + } + + /// Returns if device is reachable + pub async fn is_reachable(&mut self) -> Result { + Ok(self + .read_register(RegisterAddress::WHO_AM_I.address()) + .await? + == LSM6DS3TR_ID) + } + + /// Performs a software reset + pub async fn software_reset(&mut self) -> Result<(), I::Error> { + let ctrl3_c = Ctrl3C { + software_reset: RegisterBits::new(1), + ..Default::default() + }; + self.write_register_config(ctrl3_c.config()).await?; + Ok(()) + } + + /// Initializes accelerometer with stored settings + pub async fn init_accel(&mut self) -> Result<(), I::Error> { + self.write_register_config(self.settings.accel.config()) + .await?; + Ok(()) + } + + /// Initializes gyroscope with stored settings + pub async fn init_gyro(&mut self) -> Result<(), I::Error> { + self.write_register_config(self.settings.gyro.config()) + .await?; + Ok(()) + } + + /// Initializes interrupts with stored settings + pub async fn init_irqs(&mut self) -> Result<(), I::Error> { + for config in self.settings.irq.configs() { + self.write_register_config(config).await?; + } + Ok(()) + } + + /// Initializes other options with stored settings + pub async fn init_other(&mut self) -> Result<(), I::Error> { + if self.settings.low_performance_mode { + self.write_register(RegisterAddress::CTRL6_C.address(), 1 << 4) + .await?; // TODO make it right like the others + self.write_register(RegisterAddress::CTRL7_G.address(), 1 << 7) + .await?; // TODO make it right like the others + } + Ok(()) + } + + /// Returns accelerometer raw readings + pub async fn read_accel_raw(&mut self) -> Result, I::Error> { + self.read_sensor_raw(RegisterAddress::OUTX_L_XL.address()) + .await + } + + /// Returns accelerometer scaled readings \[g] + pub async fn read_accel(&mut self) -> Result, I::Error> { + let xyz = self.read_accel_raw().await?; + let sensitivity = self.settings.accel.scale.sensitivity(); + Ok(XYZ { + x: xyz.x as f32 * sensitivity, + y: xyz.y as f32 * sensitivity, + z: xyz.z as f32 * sensitivity, + }) + } + + /// Returns gyroscope raw readings + pub async fn read_gyro_raw(&mut self) -> Result, I::Error> { + self.read_sensor_raw(RegisterAddress::OUTX_L_G.address()) + .await + } + + /// Returns gyroscope scaled readings [°/s] + pub async fn read_gyro(&mut self) -> Result, I::Error> { + let xyz = self.read_gyro_raw().await?; + let sensitivity = self.settings.gyro.scale.sensitivity(); + Ok(XYZ { + x: xyz.x as f32 * sensitivity, + y: xyz.y as f32 * sensitivity, + z: xyz.z as f32 * sensitivity, + }) + } + + /// Returns temperature sensor raw reading + pub async fn read_temp_raw(&mut self) -> Result { + let mut bytes = [0u8; 2]; + self.interface + .read(RegisterAddress::OUT_TEMP_L.address(), &mut bytes) + .await?; + let temp: i16 = (bytes[1] as i16) << 8 | bytes[0] as i16; + Ok(temp) + } + + /// Returns temperature sensor scaled reading [°C] + pub async fn read_temp(&mut self) -> Result { + let temp = self.read_temp_raw().await?; + Ok(temp as f32 / TEMP_SCALE + TEMP_BIAS) + } + + /// Returns last interrupt sources + /// TODO + // pub async fn read_interrupt_sources(&mut self) -> Result, I::Error> { + // let mut wake_up_src = WakeUpSrc::default(); + // let mut tap_src = TapSrc::default(); + // // TODO add FUNC_SRC1 reading + // // TODO add FUNC_SRC2 reading + // wake_up_src = self.read_register(wake_up_src.address()).await?.into(); + // tap_src = self.read_register(tap_src.address()).await?.into(); + // let mut irq_sources = Vec::new(); + // for source in wake_up_src.get_irq_sources() { + // irq_sources.push(source.clone()); + // } + // for source in tap_src.get_irq_sources() { + // irq_sources.push(source.clone()); + // } + // Ok(irq_sources) + // } + + async fn read_sensor_raw(&mut self, addr: u8) -> Result, I::Error> { + let mut bytes = [0u8; 6]; + self.interface.read(addr, &mut bytes).await?; + let x: i16 = (bytes[1] as i16) << 8 | bytes[0] as i16; + let y: i16 = (bytes[3] as i16) << 8 | bytes[2] as i16; + let z: i16 = (bytes[5] as i16) << 8 | bytes[4] as i16; + Ok(XYZ { x, y, z }) + } + + async fn read_register(&mut self, address: u8) -> Result { + let mut value = [0u8]; + self.interface.read(address, &mut value).await?; + Ok(value[0]) + } + + async fn write_register(&mut self, address: u8, value: u8) -> Result<(), I::Error> { + self.interface.write(address, value).await?; + Ok(()) + } + + async fn read_register_config(&mut self, address: u8) -> Result { + let mut value = [0u8]; + self.interface.read(address, &mut value).await?; + let value = value[0]; + Ok(RegisterConfig { address, value }) + } + + async fn write_register_config( + &mut self, + register_config: RegisterConfig, + ) -> Result<(), I::Error> { + self.write_register(register_config.address, register_config.value) + .await?; + Ok(()) + } +} + +/// Interrupt sources +#[cfg_attr(feature = "defmt", derive(defmt::Format))] +#[derive(Clone, Debug)] +pub enum IrqSource { + FreeFall, + Sleep, + WakeUp, + WakeUpOnX, + WakeUpOnY, + WakeUpOnZ, + Tap, + SingleTap, + DoubleTap, + TapOnX, + TapOnY, + TapOnZ, +} + +// TODO +// pub struct IrqSources(pub Vec); diff --git a/tflite_demo/lsm6ds3tr/registers/addresses.rs b/tflite_demo/lsm6ds3tr/registers/addresses.rs new file mode 100644 index 0000000..5f54c9d --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/addresses.rs @@ -0,0 +1,540 @@ +/// Registers list +#[allow(non_camel_case_types)] +#[derive(Clone, Copy)] +pub enum RegisterAddress { + // RESERVED = 0x00, + /// Embedded functions configuration register + FUNC_CFG_ACCESS = 0x01, + // RESERVED = [0x02..0x03], + /// Sensor sync configuration register + SENSOR_SYNC_TIME_FRAME = 0x04, + /// Sensor sync configuration register + SENSOR_SYNC_RES_RATIO = 0x05, + /// FIFO configuration registers + FIFO_CTRL1 = 0x06, + /// FIFO configuration registers + FIFO_CTRL2 = 0x07, + /// FIFO configuration registers + FIFO_CTRL3 = 0x08, + /// FIFO configuration registers + FIFO_CTRL4 = 0x09, + /// FIFO configuration registers + FIFO_CTRL5 = 0x0A, + /// (no comment) + DRDY_PULSE_CFG_G = 0x0B, + // RESERVED = 0x0C, + /// INT1 pin control + INT1_CTRL = 0x0D, + /// INT2 pin control + INT2_CTRL = 0x0E, + /// Who I am ID + WHO_AM_I = 0x0F, + /// Accelerometer and gyroscope control registers + CTRL1_XL = 0x10, + /// Accelerometer and gyroscope control registers + CTRL2_G = 0x11, + /// Accelerometer and gyroscope control registers + CTRL3_C = 0x12, + /// Accelerometer and gyroscope control registers + CTRL4_C = 0x13, + /// Accelerometer and gyroscope control registers + CTRL5_C = 0x14, + /// Accelerometer and gyroscope control registers + CTRL6_C = 0x15, + /// Accelerometer and gyroscope control registers + CTRL7_G = 0x16, + /// Accelerometer and gyroscope control registers + CTRL8_XL = 0x17, + /// Accelerometer and gyroscope control registers + CTRL9_XL = 0x18, + /// Accelerometer and gyroscope control registers + CTRL10_C = 0x19, + /// I2C master configuration register + MASTER_CONFIG = 0x1A, + /// Interrupt registers + WAKE_UP_SRC = 0x1B, + /// Interrupt registers + TAP_SRC = 0x1C, + /// Interrupt registers + D6D_SRC = 0x1D, + /// Status data register for user interface + STATUS_REG = 0x1E, + // RESERVED = 0x1F, + /// Temperature output data registers + OUT_TEMP_L = 0x20, + /// Temperature output data registers + OUT_TEMP_H = 0x21, + /// Gyroscope output registers for user interface + OUTX_L_G = 0x22, + /// Gyroscope output registers for user interface + OUTX_H_G = 0x23, + /// Gyroscope output registers for user interface + OUTY_L_G = 0x24, + /// Gyroscope output registers for user interface + OUTY_H_G = 0x25, + /// Gyroscope output registers for user interface + OUTZ_L_G = 0x26, + /// Gyroscope output registers for user interface + OUTZ_H_G = 0x27, + /// Accelerometer output registers + OUTX_L_XL = 0x28, + /// Accelerometer output registers + OUTX_H_XL = 0x29, + /// Accelerometer output registers + OUTY_L_XL = 0x2A, + /// Accelerometer output registers + OUTY_H_XL = 0x2B, + /// Accelerometer output registers + OUTZ_L_XL = 0x2C, + /// Accelerometer output registers + OUTZ_H_XL = 0x2D, + /// Sensor hub output registers + SENSORHUB1_REG = 0x2E, + /// Sensor hub output registers + SENSORHUB2_REG = 0x2F, + /// Sensor hub output registers + SENSORHUB3_REG = 0x30, + /// Sensor hub output registers + SENSORHUB4_REG = 0x31, + /// Sensor hub output registers + SENSORHUB5_REG = 0x32, + /// Sensor hub output registers + SENSORHUB6_REG = 0x33, + /// Sensor hub output registers + SENSORHUB7_REG = 0x34, + /// Sensor hub output registers + SENSORHUB8_REG = 0x35, + /// Sensor hub output registers + SENSORHUB9_REG = 0x36, + /// Sensor hub output registers + SENSORHUB10_REG = 0x37, + /// Sensor hub output registers + SENSORHUB11_REG = 0x38, + /// Sensor hub output registers + SENSORHUB12_REG = 0x39, + /// FIFO status registers + FIFO_STATUS1 = 0x3A, + /// FIFO status registers + FIFO_STATUS2 = 0x3B, + /// FIFO status registers + FIFO_STATUS3 = 0x3C, + /// FIFO status registers + FIFO_STATUS4 = 0x3D, + /// FIFO data output registers + FIFO_DATA_OUT_L = 0x3E, + /// FIFO data output registers + FIFO_DATA_OUT_H = 0x3F, + /// Timestamp output registers + TIMESTAMP0_REG = 0x40, + /// Timestamp output registers + TIMESTAMP1_REG = 0x41, + /// Timestamp output registers + TIMESTAMP2_REG = 0x42, + // RESERVED = [0x43..0x48], + /// Step counter timestamp registers + STEP_TIMESTAMP_L = 0x49, + /// Step counter timestamp registers + STEP_TIMESTAMP_H = 0x4A, + /// Step counter output registers + STEP_COUNTER_L = 0x4B, + /// Step counter output registers + STEP_COUNTER_H = 0x4C, + /// Sensor hub output registers + SENSORHUB13_REG = 0x4D, + /// Sensor hub output registers + SENSORHUB14_REG = 0x4E, + /// Sensor hub output registers + SENSORHUB15_REG = 0x4F, + /// Sensor hub output registers + SENSORHUB16_REG = 0x50, + /// Sensor hub output registers + SENSORHUB17_REG = 0x51, + /// Sensor hub output registers + SENSORHUB18_REG = 0x52, + /// Interrupt registers + FUNC_SRC1 = 0x53, + /// Interrupt registers + FUNC_SRC2 = 0x54, + /// Interrupt register + WRIST_TILT_IA = 0x55, + // RESERVED = [0x56..0x57], + /// Interrupt registers + TAP_CFG = 0x58, + /// Interrupt registers + TAP_THS_6D = 0x59, + /// Interrupt registers + INT_DUR2 = 0x5A, + /// Interrupt registers + WAKE_UP_THS = 0x5B, + /// Interrupt registers + WAKE_UP_DUR = 0x5C, + /// Interrupt registers + FREE_FALL = 0x5D, + /// Interrupt registers + MD1_CFG = 0x5E, + /// Interrupt registers + MD2_CFG = 0x5F, + /// (no comment) + MASTER_CMD_CODE = 0x60, + /// (no comment) + SENS_SYNC_SPI_ERROR_CODE = 0x61, + // RESERVED = [0x62..0x65], + /// External magnetometer raw data output registers + OUT_MAG_RAW_X_L = 0x66, + /// External magnetometer raw data output registers + OUT_MAG_RAW_X_H = 0x67, + /// External magnetometer raw data output registers + OUT_MAG_RAW_Y_L = 0x68, + /// External magnetometer raw data output registers + OUT_MAG_RAW_Y_H = 0x69, + /// External magnetometer raw data output registers + OUT_MAG_RAW_Z_L = 0x6A, + /// External magnetometer raw data output registers + OUT_MAG_RAW_Z_H = 0x6B, + // RESERVED = [0x6C..0x72], + /// Accelerometer user offset correction + X_OFS_USR = 0x73, + /// Accelerometer user offset correction + Y_OFS_USR = 0x74, + /// Accelerometer user offset correction + Z_OFS_USR = 0x75, + // RESERVED = [0x76..0x7F], +} + +impl RegisterAddress { + pub fn address(self) -> u8 { + self as u8 + } +} + +/// Register address+value container +pub struct RegisterConfig { + pub address: u8, + pub value: u8, +} + +/// Simple bit-field structure +#[derive(Default)] +pub struct RegisterBits { + value: u8, +} + +impl RegisterBits { + /// New object with (unshifted) value set + pub fn new(value: u8) -> Self { + Self::from(value) + } + + /// New object with value set by extracting (shifting) relevant bits from register value + pub fn from_reg(value: u8) -> Self { + let mut s = Self::default(); + s.set_from_reg(value); + s + } + + /// Sets value by extracting (shifting) relevant bits from register value + pub fn set_from_reg(&mut self, value: u8) { + self.value = (value >> BITS_POS) & Self::bit_mask(); + } + + /// Returns unshifted value + pub fn value(&self) -> u8 { + self.value + } + + /// Returns unshifted bit mask + pub fn bit_mask() -> u8 { + (1 << BITS_NUM) - 1 + } + + /// Returns shifted bit mask + pub fn bit_shifted_mask() -> u8 { + Self::bit_mask() << BITS_POS + } +} + +pub trait RegisterValue { + fn shifted(&self) -> u8; +} + +impl RegisterValue for RegisterBits { + /// Returns shifted value to be OR-ed into register value + fn shifted(&self) -> u8 { + (self.value & Self::bit_mask()) << BITS_POS + } +} + +impl From for RegisterBits { + fn from(value: u8) -> Self { + Self { + value: value & Self::bit_mask(), + } + } +} + +impl From for RegisterBits { + fn from(value: bool) -> Self { + Self::from(value as u8) + } +} + +#[cfg(test)] +mod tests { + use super::{RegisterBits, RegisterValue}; + + #[test] + fn new_1_0() { + const BITS: u8 = 1; + const POS: u8 = 0; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b1); + } + + #[test] + fn new_1_1() { + const BITS: u8 = 1; + const POS: u8 = 1; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b1); + } + + #[test] + fn new_1_2() { + const BITS: u8 = 1; + const POS: u8 = 2; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b1); + } + + #[test] + fn new_2_0() { + const BITS: u8 = 2; + const POS: u8 = 0; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b11); + } + + #[test] + fn new_2_1() { + const BITS: u8 = 2; + const POS: u8 = 1; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b11); + } + + #[test] + fn new_2_2() { + const BITS: u8 = 2; + const POS: u8 = 2; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b11); + } + + #[test] + fn shifted_1_0() { + const BITS: u8 = 1; + const POS: u8 = 0; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b1); + } + + #[test] + fn shifted_1_1() { + const BITS: u8 = 1; + const POS: u8 = 1; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b10); + } + + #[test] + fn shifted_1_2() { + const BITS: u8 = 1; + const POS: u8 = 2; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b100); + } + + #[test] + fn shifted_2_1() { + const BITS: u8 = 2; + const POS: u8 = 1; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b110); + } + + #[test] + fn shifted_2_2() { + const BITS: u8 = 2; + const POS: u8 = 2; + let rb = RegisterBits::::new(0b11111111); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b1100); + } + + #[test] + fn new_from_reg_1_0() { + const BITS: u8 = 1; + const POS: u8 = 0; + let rb = RegisterBits::::from_reg(0b11111111); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b1); + } + + #[test] + fn new_from_reg_1_1() { + const BITS: u8 = 1; + const POS: u8 = 1; + let rb = RegisterBits::::from_reg(0b11111111); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b10); + } + + #[test] + fn new_from_reg_2_0() { + const BITS: u8 = 2; + const POS: u8 = 0; + let rb = RegisterBits::::from_reg(0b11111111); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b11); + } + + #[test] + fn new_from_reg_2_1() { + const BITS: u8 = 2; + const POS: u8 = 1; + let rb = RegisterBits::::from_reg(0b11111111); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b110); + } + + #[test] + fn new_from_reg_2_2() { + const BITS: u8 = 2; + const POS: u8 = 2; + let rb = RegisterBits::::from_reg(0b11111111); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b1100); + } + + #[test] + fn from_reg_1_0() { + const BITS: u8 = 1; + const POS: u8 = 0; + let mut rb = RegisterBits::::default(); + let reg: u8 = 0b11111111; + rb.set_from_reg(reg); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b1); + } + + #[test] + fn from_reg_1_1() { + const BITS: u8 = 1; + const POS: u8 = 1; + let mut rb = RegisterBits::::default(); + let reg: u8 = 0b11111111; + rb.set_from_reg(reg); + assert_eq!(rb.value, 0b1); + assert_eq!(rb.shifted(), 0b10); + } + + #[test] + fn from_reg_2_0() { + const BITS: u8 = 2; + const POS: u8 = 0; + let mut rb = RegisterBits::::default(); + let reg: u8 = 0b11111111; + rb.set_from_reg(reg); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b11); + } + + #[test] + fn from_reg_2_1() { + const BITS: u8 = 2; + const POS: u8 = 1; + let mut rb = RegisterBits::::default(); + let reg: u8 = 0b11111111; + rb.set_from_reg(reg); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b110); + } + + #[test] + fn from_reg_2_2() { + const BITS: u8 = 2; + const POS: u8 = 2; + let mut rb = RegisterBits::::default(); + let reg: u8 = 0b11111111; + rb.set_from_reg(reg); + assert_eq!(rb.value, 0b11); + assert_eq!(rb.shifted(), 0b1100); + } + + #[test] + fn bit_mask_1_0() { + const BITS: u8 = 1; + const POS: u8 = 0; + assert_eq!(RegisterBits::::bit_mask(), 0b1); + } + + #[test] + fn bit_mask_2_0() { + const BITS: u8 = 2; + const POS: u8 = 0; + assert_eq!(RegisterBits::::bit_mask(), 0b11); + } + + #[test] + fn bit_mask_3_0() { + const BITS: u8 = 3; + const POS: u8 = 0; + assert_eq!(RegisterBits::::bit_mask(), 0b111); + } + + #[test] + fn bit_shifted_mask_1_0() { + const BITS: u8 = 1; + const POS: u8 = 0; + assert_eq!(RegisterBits::::bit_shifted_mask(), 0b1); + } + + #[test] + fn bit_shifted_mask_1_1() { + const BITS: u8 = 1; + const POS: u8 = 1; + assert_eq!(RegisterBits::::bit_shifted_mask(), 0b10); + } + + #[test] + fn bit_shifted_mask_2_0() { + const BITS: u8 = 2; + const POS: u8 = 0; + assert_eq!(RegisterBits::::bit_shifted_mask(), 0b11); + } + + #[test] + fn bit_shifted_mask_2_1() { + const BITS: u8 = 2; + const POS: u8 = 1; + assert_eq!(RegisterBits::::bit_shifted_mask(), 0b110); + } + + #[test] + fn bit_shifted_mask_3_0() { + const BITS: u8 = 3; + const POS: u8 = 0; + assert_eq!(RegisterBits::::bit_shifted_mask(), 0b111); + } + + #[test] + fn bit_shifted_mask_3_1() { + const BITS: u8 = 3; + const POS: u8 = 1; + assert_eq!(RegisterBits::::bit_shifted_mask(), 0b1110); + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/ctrl1_xl.rs b/tflite_demo/lsm6ds3tr/registers/ctrl1_xl.rs new file mode 100644 index 0000000..089b202 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/ctrl1_xl.rs @@ -0,0 +1,103 @@ +//! Linear acceleration sensor control register 1 (r/w). + +#![allow(non_camel_case_types)] + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Linear acceleration sensor control register 1 (r/w). +#[derive(Default)] +pub struct Ctrl1Xl { + /// Output data rate and power mode selection. Default value: 0000 (see Table 52). + pub sample_rate: AccelSampleRate, + /// Accelerometer full-scale selection. Default value: 00. + /// (00: ±2 g; 01: ±16 g; 10: ±4 g; 11: ±8 g) + pub scale: AccelScale, + /// Accelerometer digital LPF (LPF1) bandwidth selection. For bandwidth selection refer to CTRL8_XL (17h). + pub low_pass_filter: RegisterBits<1, 6>, // TODO + /// Accelerometer analog chain bandwidth selection (only for accelerometer ODR ≥ 1.67 kHz). + /// (0: BW @ 1.5 kHz; 1: BW @ 400 Hz) + pub analog_chain_bandwidth: RegisterBits<1, 7>, // TODO +} + +impl Ctrl1Xl { + pub fn address(&self) -> u8 { + RegisterAddress::CTRL1_XL.address() + } + + pub fn value(&self) -> u8 { + self.sample_rate.shifted() + | self.scale.shifted() + | self.low_pass_filter.shifted() + | self.analog_chain_bandwidth.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} + +#[derive(Default, Clone, Copy)] +pub enum AccelScale { + #[default] + _2G = 0b00, + _16G = 0b01, + _4G = 0b10, + _8G = 0b11, +} + +impl AccelScale { + pub fn sensitivity(self) -> f32 { + use AccelScale::*; + match self { + _2G => 0.000_061, + _4G => 0.000_122, + _8G => 0.000_244, + _16G => 0.000_488, + } + } +} + +impl RegisterValue for AccelScale { + fn shifted(&self) -> u8 { + (*self as u8) << 2 + } +} + +#[derive(Default, Clone, Copy)] +pub enum AccelSampleRate { + /// Power Down (disabled) + #[default] + PowerDown = 0b0000, + /// 12.5 Hz + _12_5Hz = 0b0001, + /// 26 Hz + _26Hz = 0b0010, + /// 52 Hz + _52Hz = 0b0011, + /// 104 Hz + _104Hz = 0b0100, + /// 208 Hz + _208Hz = 0b0101, + /// 416 Hz + _416Hz = 0b0110, + /// 833 Hz + _833Hz = 0b0111, + /// 1.66 kHz + _1660Hz = 0b1000, + /// 3.33 kHz + _3330Hz = 0b1001, + /// 6.66 kHz + _6660Hz = 0b1010, + /// 1.6 Hz in low power mode; 12.5 Hz in high performance mode + _1_6Hz_LP_or_12_5Hz_HP = 0b1011, + // Not allowed = [0b1100..0b1111] +} + +impl RegisterValue for AccelSampleRate { + fn shifted(&self) -> u8 { + (*self as u8) << 4 + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/ctrl2_g.rs b/tflite_demo/lsm6ds3tr/registers/ctrl2_g.rs new file mode 100644 index 0000000..3eab9a0 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/ctrl2_g.rs @@ -0,0 +1,98 @@ +//! Angular rate sensor control register 2 (r/w). + +#![allow(non_camel_case_types)] + +use crate::lsm6ds3tr::registers::RegisterConfig; +use crate::lsm6ds3tr::RegisterAddress; + +use super::RegisterValue; + +#[derive(Default)] +/// Angular rate sensor control register 2 (r/w). +pub struct Ctrl2G { + /// Gyroscope output data rate selection. Default value: 0000 (Refer to Table 55) + pub sample_rate: GyroSampleRate, + /// Gyroscope full-scale selection. Default value: 00 (00: 245 dps; 01: 500 dps; 10: 1000 dps; 11: 2000 dps) + /// Gyroscope full-scale at 125 dps. Default value: 0 (0: disabled; 1: enabled) + pub scale: GyroScale, +} + +impl Ctrl2G { + pub fn address(&self) -> u8 { + RegisterAddress::CTRL2_G.address() + } + + pub fn value(&self) -> u8 { + self.sample_rate.shifted() | self.scale.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} + +#[derive(Default, Debug, Clone, Copy)] +pub enum GyroScale { + _125DPS = 0b001, + #[default] + _250DPS = 0b000, + _500DPS = 0b010, + _1000DPS = 0b100, + _2000DPS = 0b110, +} + +impl GyroScale { + pub fn sensitivity(self) -> f32 { + use GyroScale::*; + match self { + _125DPS => 0.004_375, + _250DPS => 0.008_750, + _500DPS => 0.017_500, + _1000DPS => 0.035_000, + _2000DPS => 0.070_000, + } + } +} + +impl RegisterValue for GyroScale { + fn shifted(&self) -> u8 { + (*self as u8) << 1 + } +} + +#[derive(Default, Debug, Clone, Copy)] +pub enum GyroSampleRate { + /// Power Down (disabled) + #[default] + PowerDown = 0b0000, + /// 12.5 Hz + _12_5Hz = 0b0001, + /// 26 Hz + _26Hz = 0b0010, + /// 52 Hz + _52Hz = 0b0011, + /// 104 Hz + _104Hz = 0b0100, + /// 208 Hz + _208Hz = 0b0101, + /// 416 Hz + _416Hz = 0b0110, + /// 833 Hz + _833Hz = 0b0111, + /// 1.66 kHz + _1660Hz = 0b1000, + /// 3.33 kHz + _3330Hz = 0b1001, + /// 6.66 kHz + _6660Hz = 0b1010, + // Not allowed = [0b1011..0b1111] +} + +impl RegisterValue for GyroSampleRate { + fn shifted(&self) -> u8 { + (*self as u8) << 4 + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/ctrl3_c.rs b/tflite_demo/lsm6ds3tr/registers/ctrl3_c.rs new file mode 100644 index 0000000..a2b65f1 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/ctrl3_c.rs @@ -0,0 +1,73 @@ +//! Control register 3 (r/w). + +#![allow(non_camel_case_types)] + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Control register 3 (r/w). +pub struct Ctrl3C { + /// Reboot memory content. Default value: 0 + /// (0: normal mode; 1: reboot memory content) + pub reboot_memory_content: RegisterBits<1, 7>, + /// Block Data Update. Default value: 0 + /// (0: continuous update; 1: output registers not updated until MSB and LSB have been read) + pub block_data_update: RegisterBits<1, 6>, + /// Interrupt activation level. Default value: 0 + /// (0: interrupt output pads active high; 1: interrupt output pads active low) + pub high_low_active: RegisterBits<1, 5>, + /// Push-pull/open-drain selection on INT1 and INT2 pads. Default value: 0 + /// (0: push-pull mode; 1: open-drain mode) + pub push_pull_open_drain: RegisterBits<1, 4>, + /// SPI Serial Interface Mode selection. Default value: 0 + /// (0: 4-wire interface; 1: 3-wire interface). + pub spi_interface_mode: RegisterBits<1, 3>, + /// Register address automatically incremented during a multiple byte access with a serial interface (I2C or SPI). Default value: 1 + /// (0: disabled; 1: enabled) + pub interface_auto_address_increment: RegisterBits<1, 2>, + /// Big/Little Endian Data selection. Default value 0 + /// (0: data LSB @ lower address; 1: data MSB @ lower address) + pub big_little_endian: RegisterBits<1, 1>, + /// Software reset. Default value: 0 + /// (0: normal mode; 1: reset device) + /// This bit is automatically cleared. + pub software_reset: RegisterBits<1, 0>, +} + +impl Default for Ctrl3C { + fn default() -> Self { + Self { + interface_auto_address_increment: RegisterBits::new(1), + reboot_memory_content: RegisterBits::default(), + block_data_update: RegisterBits::default(), + high_low_active: RegisterBits::default(), + push_pull_open_drain: RegisterBits::default(), + spi_interface_mode: RegisterBits::default(), + big_little_endian: RegisterBits::default(), + software_reset: RegisterBits::default(), + } + } +} + +impl Ctrl3C { + pub fn address(&self) -> u8 { + RegisterAddress::CTRL3_C.address() + } + + pub fn value(&self) -> u8 { + self.reboot_memory_content.shifted() + | self.block_data_update.shifted() + | self.high_low_active.shifted() + | self.push_pull_open_drain.shifted() + | self.spi_interface_mode.shifted() + | self.interface_auto_address_increment.shifted() + | self.big_little_endian.shifted() + | self.software_reset.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/ctrl4_c.rs b/tflite_demo/lsm6ds3tr/registers/ctrl4_c.rs new file mode 100644 index 0000000..edcfe19 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/ctrl4_c.rs @@ -0,0 +1,56 @@ +//! Control register 4 (r/w). + +#![allow(non_camel_case_types)] + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Control register 4 (r/w). +#[derive(Default)] +pub struct Ctrl4C { + /// Extend DEN functionality to accelerometer sensor. Default value: 0 + /// (0: disabled; 1: enabled) + pub extend_den: RegisterBits<1, 7>, + /// Gyroscope sleep mode enable. Default value: 0 + /// (0: disabled; 1: enabled) + pub gyroscope_sleep: RegisterBits<1, 6>, + /// DEN DRDY signal on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub den_data_ready_int1: RegisterBits<1, 5>, + /// All interrupt signals available on INT1 pad enable. Default value: 0 + /// (0: interrupt signals divided between INT1 and INT2 pads; + /// 1: all interrupt signals in logic or on INT1 pad) + pub int2_on_int1: RegisterBits<1, 4>, + /// Configuration 1 data available enable bit. Default value: 0 + /// (0: DA timer disabled; 1: DA timer enabled) + pub data_ready_mask: RegisterBits<1, 3>, + /// Disable I2C interface. Default value: 0 + /// (0: both I2C and SPI enabled; 1: I2C disabled, SPI only enabled) + pub i2c_disable: RegisterBits<1, 2>, + /// Enable gyroscope digital LPF1. The bandwidth can be selected through + /// FTYPE[1\:0] in FUNC_CFG_ACCESS (01h). + /// (0: disabled; 1: enabled) + pub gyroscope_low_pass_filter_selection: RegisterBits<1, 1>, +} + +impl Ctrl4C { + pub fn address(&self) -> u8 { + RegisterAddress::CTRL4_C.address() + } + + pub fn value(&self) -> u8 { + self.extend_den.shifted() + | self.gyroscope_sleep.shifted() + | self.den_data_ready_int1.shifted() + | self.int2_on_int1.shifted() + | self.data_ready_mask.shifted() + | self.i2c_disable.shifted() + | self.gyroscope_low_pass_filter_selection.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/drdy_pulse_cfg_g.rs b/tflite_demo/lsm6ds3tr/registers/drdy_pulse_cfg_g.rs new file mode 100644 index 0000000..3971daf --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/drdy_pulse_cfg_g.rs @@ -0,0 +1,34 @@ +//! DataReady configuration register (r/w). + +#![allow(non_camel_case_types)] + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// DataReady configuration register (r/w). +#[derive(Default)] +pub struct DrdyPulseCfgG { + /// Enable pulsed DataReady mode. Default value: 0 + /// (0: DataReady latched mode. Returns to 0 only after output data have been read; + /// 1: DataReady pulsed mode. The DataReady pulses are 75 μs long.) + pub pulsed_data_ready: RegisterBits<1, 7>, + /// Wrist tilt interrupt on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub int2_wrist_tilt: RegisterBits<1, 0>, +} + +impl DrdyPulseCfgG { + pub fn address(&self) -> u8 { + RegisterAddress::DRDY_PULSE_CFG_G.address() + } + + pub fn value(&self) -> u8 { + self.pulsed_data_ready.shifted() | self.int2_wrist_tilt.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/free_fall.rs b/tflite_demo/lsm6ds3tr/registers/free_fall.rs new file mode 100644 index 0000000..4cbc0eb --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/free_fall.rs @@ -0,0 +1,52 @@ +//! Free-fall function duration setting register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Free-fall function duration setting register (r/w). +#[derive(Default)] +pub struct FreeFall { + /// Free-fall duration event. Default: 0 + /// For the complete configuration of the free fall duration, refer to FF_DUR5 in WAKE_UP_DUR (5Ch) configuration + pub duration_event: RegisterBits<5, 3>, + /// Free fall threshold setting. Default: 000 + /// For details refer to Table 196. + pub threshold: FreeFallThreshold, +} + +impl FreeFall { + pub fn address(&self) -> u8 { + RegisterAddress::FREE_FALL.address() + } + + pub fn value(&self) -> u8 { + self.duration_event.shifted() | self.threshold.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} + +#[allow(non_camel_case_types)] +#[repr(u8)] +#[derive(Default, Clone, Copy)] +pub enum FreeFallThreshold { + #[default] + _156_mg = 0b000, + _219_mg = 0b001, + _250_mg = 0b010, + _312_mg = 0b011, + _344_mg = 0b100, + _406_mg = 0b101, + _469_mg = 0b110, + _500_mg = 0b111, +} + +impl RegisterValue for FreeFallThreshold { + fn shifted(&self) -> u8 { + *self as u8 + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/int1_ctrl.rs b/tflite_demo/lsm6ds3tr/registers/int1_ctrl.rs new file mode 100644 index 0000000..4685b43 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/int1_ctrl.rs @@ -0,0 +1,57 @@ +//! INT1 pad control register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// INT1 pad control register (r/w). +/// Each bit in this register enables a signal to be carried through INT1. The pad’s output will supply the OR combination of the selected signals. +#[derive(Default)] +pub struct Int1Ctrl { + /// Pedometer step recognition interrupt enable on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub pedometer_step_recognition: RegisterBits<1, 7>, + /// Significant motion interrupt enable on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub significant_motion: RegisterBits<1, 6>, + /// FIFO full flag interrupt enable on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub fifo_full: RegisterBits<1, 5>, + /// FIFO overrun interrupt on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub fifo_overrun: RegisterBits<1, 4>, + /// FIFO threshold interrupt on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub fifo_threshold: RegisterBits<1, 3>, + /// Boot status available on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub boot_status_available: RegisterBits<1, 2>, + /// Gyroscope Data Ready on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub gyroscope_data_ready: RegisterBits<1, 1>, + /// Accelerometer Data Ready on INT1 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub accelerometer_data_ready: RegisterBits<1, 0>, +} + +impl Int1Ctrl { + pub fn address(&self) -> u8 { + RegisterAddress::INT1_CTRL.address() + } + + pub fn value(&self) -> u8 { + self.pedometer_step_recognition.shifted() + | self.significant_motion.shifted() + | self.fifo_full.shifted() + | self.fifo_overrun.shifted() + | self.fifo_threshold.shifted() + | self.boot_status_available.shifted() + | self.gyroscope_data_ready.shifted() + | self.accelerometer_data_ready.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/int2_ctrl.rs b/tflite_demo/lsm6ds3tr/registers/int2_ctrl.rs new file mode 100644 index 0000000..76ff4db --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/int2_ctrl.rs @@ -0,0 +1,57 @@ +//! INT2 pad control register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// INT2 pad control register (r/w). +/// Each bit in this register enables a signal to be carried through INT2. The pad’s output will supply the OR combination of the selected signals. +#[derive(Default)] +pub struct Int2Ctrl { + /// Pedometer step recognition interrupt on delta time(1) enable on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub pedometer_step_recognition_delta_time: RegisterBits<1, 7>, + /// Step counter overflow interrupt enable on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub step_counter_overflow: RegisterBits<1, 6>, + /// FIFO full flag interrupt enable on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub fifo_full: RegisterBits<1, 5>, + /// FIFO overrun interrupt on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub fifo_overrun: RegisterBits<1, 4>, + /// FIFO threshold interrupt on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub fifo_threshold: RegisterBits<1, 3>, + /// Temperature Data Ready on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub temperature_data_ready: RegisterBits<1, 2>, + /// Gyroscope Data Ready on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub gyroscope_data_ready: RegisterBits<1, 1>, + /// Accelerometer Data Ready on INT2 pad. Default value: 0 + /// (0: disabled; 1: enabled) + pub accelerometer_data_ready: RegisterBits<1, 0>, +} + +impl Int2Ctrl { + pub fn address(&self) -> u8 { + RegisterAddress::INT2_CTRL.address() + } + + pub fn value(&self) -> u8 { + self.pedometer_step_recognition_delta_time.shifted() + | self.step_counter_overflow.shifted() + | self.fifo_full.shifted() + | self.fifo_overrun.shifted() + | self.fifo_threshold.shifted() + | self.temperature_data_ready.shifted() + | self.gyroscope_data_ready.shifted() + | self.accelerometer_data_ready.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/int_dur2.rs b/tflite_demo/lsm6ds3tr/registers/int_dur2.rs new file mode 100644 index 0000000..ba41496 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/int_dur2.rs @@ -0,0 +1,34 @@ +//! Tap recognition function setting register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Tap recognition function setting register (r/w). +#[derive(Default)] +pub struct IntDur2 { + /// Duration of maximum time gap for double tap recognition. Default: 0000 + /// When double tap recognition is enabled, this register expresses the maximum time between two consecutive detected taps to determine a double tap event. The default value of these bits is 0000b which corresponds to 16*ODR_XL time. If the DUR[3\:0] bits are set to a different value, 1LSB corresponds to 32*ODR_XL time. + pub duration: RegisterBits<4, 4>, + /// Expected quiet time after a tap detection. Default value: 00 + /// Quiet time is the time after the first detected tap in which there must not be any overthreshold event. The default value of these bits is 00b which corresponds to 2*ODR_XL time. If the QUIET[1\:0] bits are set to a different value, 1LSB corresponds to 4*ODR_XL time. + pub quiet: RegisterBits<2, 2>, + /// Maximum duration of overthreshold event. Default value: 00 + /// Maximum duration is the maximum time of an overthreshold signal detection to be recognized as a tap event. The default value of these bits is 00b which corresponds to 4*ODR_XL time. If the SHOCK[1\:0] bits are set to a different value, 1LSB corresponds to 8*ODR_XL time. + pub shock: RegisterBits<2, 0>, +} + +impl IntDur2 { + pub fn address(&self) -> u8 { + RegisterAddress::INT_DUR2.address() + } + + pub fn value(&self) -> u8 { + self.duration.shifted() | self.quiet.shifted() | self.shock.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/md1_cfg.rs b/tflite_demo/lsm6ds3tr/registers/md1_cfg.rs new file mode 100644 index 0000000..9de39f9 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/md1_cfg.rs @@ -0,0 +1,61 @@ +//! Function routing on INT1 register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Function routing on INT1 register (r/w). +#[derive(Default)] +pub struct Md1Cfg { + /// Routing on INT1 of inactivity mode. Default: 0 + /// (0: routing on INT1 of inactivity disabled; 1: routing on INT1 of inactivity enabled) + pub inactivity_event: RegisterBits<1, 7>, + /// Single-tap recognition routing on INT1. Default: 0 + /// (0: routing of single-tap event on INT1 disabled; + /// 1: routing of single-tap event on INT1 enabled) + pub single_tap_event: RegisterBits<1, 6>, + /// Routing of wakeup event on INT1. Default value: 0 + /// (0: routing of wakeup event on INT1 disabled; + /// 1: routing of wakeup event on INT1 enabled) + pub wake_up_event: RegisterBits<1, 5>, + /// Routing of free-fall event on INT1. Default value: 0 + /// (0: routing of free-fall event on INT1 disabled; + /// 1: routing of free-fall event on INT1 enabled) + pub free_fall_event: RegisterBits<1, 4>, + /// Routing of tap event on INT1. Default value: 0 + /// (0: routing of double-tap event on INT1 disabled; + /// 1: routing of double-tap event on INT1 enabled) + pub double_tap_event: RegisterBits<1, 3>, + /// Routing of 6D event on INT1. Default value: 0 + /// (0: routing of 6D event on INT1 disabled; 1: routing of 6D event on INT1 enabled) + pub six_degrees_event: RegisterBits<1, 2>, + /// Routing of tilt event on INT1. Default value: 0 + /// (0: routing of tilt event on INT1 disabled; 1: routing of tilt event on INT1 enabled) + pub tilt_event: RegisterBits<1, 1>, + /// Routing of end counter event of timer on INT1. Default value: 0 + /// (0: routing of end counter event of timer on INT1 disabled; + /// 1: routing of end counter event of timer event on INT1 enabled) + pub timer_end_counter_event: RegisterBits<1, 0>, +} + +impl Md1Cfg { + pub fn address(&self) -> u8 { + RegisterAddress::MD1_CFG.address() + } + + pub fn value(&self) -> u8 { + self.inactivity_event.shifted() + | self.single_tap_event.shifted() + | self.wake_up_event.shifted() + | self.free_fall_event.shifted() + | self.double_tap_event.shifted() + | self.six_degrees_event.shifted() + | self.tilt_event.shifted() + | self.timer_end_counter_event.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/md2_cfg.rs b/tflite_demo/lsm6ds3tr/registers/md2_cfg.rs new file mode 100644 index 0000000..dd4b0f7 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/md2_cfg.rs @@ -0,0 +1,61 @@ +//! Function routing on INT2 register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Function routing on INT2 register (r/w). +#[derive(Default)] +pub struct Md2Cfg { + /// Routing on INT2 of inactivity mode. Default: 0 + /// (0: routing on INT2 of inactivity disabled; 1: routing on INT2 of inactivity enabled) + pub inactivity_event: RegisterBits<1, 7>, + /// Single-tap recognition routing on INT2. Default: 0 + /// (0: routing of single-tap event on INT2 disabled; + /// 1: routing of single-tap event on INT2 enabled) + pub single_tap_event: RegisterBits<1, 6>, + /// Routing of wakeup event on INT2. Default value: 0 + /// (0: routing of wakeup event on INT2 disabled; + /// 1: routing of wake-up event on INT2 enabled) + pub wake_up_event: RegisterBits<1, 5>, + /// Routing of free-fall event on INT2. Default value: 0 + /// (0: routing of free-fall event on INT2 disabled; + /// 1: routing of free-fall event on INT2 enabled) + pub free_fall_event: RegisterBits<1, 4>, + /// Routing of tap event on INT2. Default value: 0 + /// (0: routing of double-tap event on INT2 disabled; + /// 1: routing of double-tap event on INT2 enabled) + pub double_tap_event: RegisterBits<1, 3>, + /// Routing of 6D event on INT2. Default value: 0 + /// (0: routing of 6D event on INT2 disabled; 1: routing of 6D event on INT2 enabled) + pub six_degrees_event: RegisterBits<1, 2>, + /// Routing of tilt event on INT2. Default value: 0 + /// (0: routing of tilt event on INT2 disabled; 1: routing of tilt event on INT2 enabled) + pub tilt_event: RegisterBits<1, 1>, + /// Routing of soft-iron/hard-iron algorithm end event on INT2. Default value: 0 + /// (0: routing of soft-iron/hard-iron algorithm end event on INT2 disabled; + /// 1: routing of soft-iron/hard-iron algorithm end event on INT2 enabled) + pub soft_hard_iron_algorithm_end_event: RegisterBits<1, 0>, +} + +impl Md2Cfg { + pub fn address(&self) -> u8 { + RegisterAddress::MD2_CFG.address() + } + + pub fn value(&self) -> u8 { + self.inactivity_event.shifted() + | self.single_tap_event.shifted() + | self.wake_up_event.shifted() + | self.free_fall_event.shifted() + | self.double_tap_event.shifted() + | self.six_degrees_event.shifted() + | self.tilt_event.shifted() + | self.soft_hard_iron_algorithm_end_event.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/mod.rs b/tflite_demo/lsm6ds3tr/registers/mod.rs new file mode 100644 index 0000000..53d21fe --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/mod.rs @@ -0,0 +1,37 @@ +pub mod addresses; +pub mod ctrl1_xl; +pub mod ctrl2_g; +pub mod ctrl3_c; +pub mod ctrl4_c; +pub mod drdy_pulse_cfg_g; +pub mod free_fall; +pub mod int1_ctrl; +pub mod int2_ctrl; +pub mod int_dur2; +pub mod md1_cfg; +pub mod md2_cfg; +pub mod tap_cfg; +pub mod tap_src; +pub mod tap_ths_6d; +pub mod wake_up_dur; +pub mod wake_up_src; +pub mod wake_up_ths; + +pub use addresses::*; +pub use ctrl1_xl::*; +pub use ctrl2_g::*; +pub use ctrl3_c::*; +pub use ctrl4_c::*; +pub use drdy_pulse_cfg_g::*; +pub use free_fall::*; +pub use int1_ctrl::*; +pub use int2_ctrl::*; +pub use int_dur2::*; +pub use md1_cfg::*; +pub use md2_cfg::*; +pub use tap_cfg::*; +pub use tap_src::*; +pub use tap_ths_6d::*; +pub use wake_up_dur::*; +pub use wake_up_src::*; +pub use wake_up_ths::*; diff --git a/tflite_demo/lsm6ds3tr/registers/tap_cfg.rs b/tflite_demo/lsm6ds3tr/registers/tap_cfg.rs new file mode 100644 index 0000000..572cd95 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/tap_cfg.rs @@ -0,0 +1,85 @@ +//! Enables interrupt and inactivity functions, configuration of filtering and tap recognition functions (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Enables interrupt and inactivity functions, configuration of filtering and tap recognition functions (r/w). +#[derive(Default)] +pub struct TapCfg { + /// Enable basic interrupts (6D/4D, free-fall, wake-up, tap, inactivity). Default 0. + /// (0: interrupt disabled; 1: interrupt enabled) + pub enable_basic_interrupts: RegisterBits<1, 7>, + /// Enable inactivity function. Default value: 00 + /// (00: disabled + /// 01: sets accelerometer ODR to 12.5 Hz (low-power mode), gyro does not change; + /// 10: sets accelerometer ODR to 12.5 Hz (low-power mode), gyro to sleep mode; + /// 11: sets accelerometer ODR to 12.5 Hz (low-power mode), gyro to power-down mode) + pub enable_inactivity_function: InactivityFunctionMode, + /// HPF or SLOPE filter selection on wake-up and Activity/Inactivity functions. Refer to Figure 8. Default value: 0 + /// (0: SLOPE filter applied; 1: HPF applied) + pub slope_fds: FilterSelected, + /// Enable X direction in tap recognition. Default value: 0 + /// (0: X direction disabled; 1: X direction enabled) + pub enable_x_direction_tap_recognition: RegisterBits<1, 3>, + /// Enable Y direction in tap recognition. Default value: 0 + /// (0: Y direction disabled; 1: Y direction enabled) + pub enable_y_direction_tap_recognition: RegisterBits<1, 2>, + /// Enable Z direction in tap recognition. Default value: 0 + /// (0: Z direction disabled; 1: Z direction enabled) + pub enable_z_direction_tap_recognition: RegisterBits<1, 1>, + /// Latched Interrupt. Default value: 0 + /// (0: interrupt request not latched; 1: interrupt request latched) + pub latched_interrupt: RegisterBits<1, 0>, +} + +impl TapCfg { + pub fn address(&self) -> u8 { + RegisterAddress::TAP_CFG.address() + } + + pub fn value(&self) -> u8 { + self.enable_basic_interrupts.shifted() + | self.enable_inactivity_function.shifted() + | self.slope_fds.shifted() + | self.enable_x_direction_tap_recognition.shifted() + | self.enable_y_direction_tap_recognition.shifted() + | self.enable_z_direction_tap_recognition.shifted() + | self.latched_interrupt.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} + +#[repr(u8)] +#[derive(Default, Clone, Copy)] +pub enum InactivityFunctionMode { + #[default] + Disabled = 0b00, + AccelLowPowerGyroUnchanged = 0b01, + AccelLowPowerGyroSleepMode = 0b10, + AccelLowPowerGyroPowerDown = 0b11, +} + +impl RegisterValue for InactivityFunctionMode { + fn shifted(&self) -> u8 { + (*self as u8) << 5 + } +} + +#[repr(u8)] +#[derive(Default, Clone, Copy)] +pub enum FilterSelected { + #[default] + Slope = 0b0, + HighPassFilter = 0b1, +} + +impl RegisterValue for FilterSelected { + fn shifted(&self) -> u8 { + (*self as u8) << 4 + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/tap_src.rs b/tflite_demo/lsm6ds3tr/registers/tap_src.rs new file mode 100644 index 0000000..01c8528 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/tap_src.rs @@ -0,0 +1,73 @@ +//! Tap source register (r). + +use crate::lsm6ds3tr::{IrqSource, RegisterAddress, RegisterBits}; + +/// Tap source register (r). +#[derive(Default)] +pub struct TapSrc { + /// Tap event detection status. Default: 0 + /// (0: tap event not detected; 1: tap event detected) + pub tap_event: RegisterBits<1, 6>, + /// Single-tap event status. Default value: 0 + /// (0: single tap event not detected; 1: single tap event detected) + pub single_tap_event: RegisterBits<1, 5>, + /// Double-tap event detection status. Default value: 0 + /// (0: double-tap event not detected; 1: double-tap event detected.) + pub double_tap_event: RegisterBits<1, 4>, + /// Sign of acceleration detected by tap event. Default: 0 + /// (0: positive sign of acceleration detected by tap event; + /// 1: negative sign of acceleration detected by tap event) + pub tap_sign_acceleration: RegisterBits<1, 3>, + /// Tap event detection status on X-axis. Default value: 0 + /// (0: tap event on X-axis not detected; 1: tap event on X-axis detected) + pub tap_x_axis: RegisterBits<1, 2>, + /// Tap event detection status on Y-axis. Default value: 0 + /// (0: tap event on Y-axis not detected; 1: tap event on Y-axis detected) + pub tap_y_axis: RegisterBits<1, 1>, + /// Tap event detection status on Z-axis. Default value: 0 + /// (0: tap event on Z-axis not detected; 1: tap event on Z-axis detected) + pub tap_z_axis: RegisterBits<1, 0>, +} + +impl TapSrc { + pub fn address(&self) -> u8 { + RegisterAddress::TAP_SRC.address() + } + + // pub fn get_irq_sources(&self) -> Vec { + // let mut v: Vec = Default::default(); + // if self.tap_event.value() != 0 { + // v.push(IrqSource::Tap); + // } + // if self.single_tap_event.value() != 0 { + // v.push(IrqSource::SingleTap); + // } + // if self.double_tap_event.value() != 0 { + // v.push(IrqSource::DoubleTap); + // } + // if self.tap_x_axis.value() != 0 { + // v.push(IrqSource::TapOnX); + // } + // if self.tap_y_axis.value() != 0 { + // v.push(IrqSource::TapOnY); + // } + // if self.tap_z_axis.value() != 0 { + // v.push(IrqSource::TapOnZ); + // } + // v + // } +} + +impl From for TapSrc { + fn from(value: u8) -> Self { + let mut s = Self::default(); + s.tap_event.set_from_reg(value); + s.single_tap_event.set_from_reg(value); + s.double_tap_event.set_from_reg(value); + s.tap_sign_acceleration.set_from_reg(value); + s.tap_x_axis.set_from_reg(value); + s.tap_y_axis.set_from_reg(value); + s.tap_z_axis.set_from_reg(value); + s + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/tap_ths_6d.rs b/tflite_demo/lsm6ds3tr/registers/tap_ths_6d.rs new file mode 100644 index 0000000..1a14745 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/tap_ths_6d.rs @@ -0,0 +1,36 @@ +//! Portrait/landscape position and tap function threshold register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Portrait/landscape position and tap function threshold register (r/w). +#[derive(Default)] +pub struct TapThs6d { + /// 4D orientation detection enable. Z-axis position detection is disabled. Default value: 0 + /// (0: enabled; 1: disabled) + pub four_degrees_detection_enable: RegisterBits<1, 7>, + /// Threshold for 4D/6D function. Default value: 00 + /// For details, refer to Table 187. + pub six_degrees_threshold: RegisterBits<2, 5>, + /// Threshold for tap recognition. Default value: 00000 + /// 1 LSb corresponds to FS_XL/2^5 + pub tap_threshold: RegisterBits<5, 0>, +} + +impl TapThs6d { + pub fn address(&self) -> u8 { + RegisterAddress::TAP_THS_6D.address() + } + + pub fn value(&self) -> u8 { + self.four_degrees_detection_enable.shifted() + | self.six_degrees_threshold.shifted() + | self.tap_threshold.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/wake_up_dur.rs b/tflite_demo/lsm6ds3tr/registers/wake_up_dur.rs new file mode 100644 index 0000000..c0cbfbb --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/wake_up_dur.rs @@ -0,0 +1,41 @@ +//! Free-fall, wakeup, timestamp and sleep mode functions duration setting register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Free-fall, wakeup, timestamp and sleep mode functions duration setting register (r/w). +#[derive(Default)] +pub struct WakeUpDur { + /// Free fall duration event. Default: 0 + /// For the complete configuration of the free-fall duration, refer to FF_DUR[4\:0] in FREE_FALL (5Dh) configuration. + /// 1 LSB = 1 ODR_time + pub free_fall_duration_event: RegisterBits<1, 7>, + /// Wake up duration event. Default: 00 + /// 1LSB = 1 ODR_time + pub wake_up_duration_event: RegisterBits<2, 5>, + /// Timestamp register resolution setting. Default value: 0 + /// (0: 1LSB = 6.4 ms; 1: 1LSB = 25 μs) + pub timestamp_resolution: RegisterBits<1, 4>, + /// Duration to go in sleep mode. Default value: 0000 (this corresponds to 16 ODR) + /// 1 LSB = 512 ODR + pub sleep_duration_event: RegisterBits<4, 0>, +} + +impl WakeUpDur { + pub fn address(&self) -> u8 { + RegisterAddress::WAKE_UP_DUR.address() + } + + pub fn value(&self) -> u8 { + self.free_fall_duration_event.shifted() + | self.wake_up_duration_event.shifted() + | self.timestamp_resolution.shifted() + | self.sleep_duration_event.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/wake_up_src.rs b/tflite_demo/lsm6ds3tr/registers/wake_up_src.rs new file mode 100644 index 0000000..8d94ca0 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/wake_up_src.rs @@ -0,0 +1,69 @@ +//! Wake up interrupt source register (r). + +use crate::lsm6ds3tr::{IrqSource, RegisterAddress, RegisterBits}; + +/// Wake up interrupt source register (r). +#[derive(Default)] +pub struct WakeUpSrc { + /// Free-fall event detection status. Default: 0 + /// (0: free-fall event not detected; 1: free-fall event detected) + pub free_fall_event: RegisterBits<1, 5>, + /// Sleep event status. Default value: 0 + /// (0: sleep event not detected; 1: sleep event detected) + pub sleep_event: RegisterBits<1, 4>, + /// Wakeup event detection status. Default value: 0 + /// (0: wakeup event not detected; 1: wakeup event detected.) + pub wake_up_event: RegisterBits<1, 3>, + /// Wakeup event detection status on X-axis. Default value: 0 + /// (0: wakeup event on X-axis not detected; 1: wakeup event on X-axis detected) + pub wake_up_event_x: RegisterBits<1, 2>, + /// Wakeup event detection status on Y-axis. Default value: 0 + /// (0: wakeup event on Y-axis not detected; 1: wakeup event on Y-axis detected) + pub wake_up_event_y: RegisterBits<1, 1>, + /// Wakeup event detection status on Z-axis. Default value: 0 + /// (0: wakeup event on Z-axis not detected; 1: wakeup event on Z-axis detected) + pub wake_up_event_z: RegisterBits<1, 0>, +} + +impl WakeUpSrc { + pub fn address(&self) -> u8 { + RegisterAddress::WAKE_UP_SRC.address() + } + + // TODO + // pub fn get_irq_sources(&self) -> Vec { + // let mut v = Vec::new(); + // if self.free_fall_event.value() != 0 { + // v.push(IrqSource::FreeFall); + // } + // if self.sleep_event.value() != 0 { + // v.push(IrqSource::Sleep); + // } + // if self.wake_up_event.value() != 0 { + // v.push(IrqSource::WakeUp); + // } + // if self.wake_up_event_x.value() != 0 { + // v.push(IrqSource::WakeUpOnX); + // } + // if self.wake_up_event_y.value() != 0 { + // v.push(IrqSource::WakeUpOnY); + // } + // if self.wake_up_event_z.value() != 0 { + // v.push(IrqSource::WakeUpOnZ); + // } + // v + // } +} + +impl From for WakeUpSrc { + fn from(value: u8) -> Self { + let mut s = Self::default(); + s.free_fall_event.set_from_reg(value); + s.sleep_event.set_from_reg(value); + s.wake_up_event.set_from_reg(value); + s.wake_up_event_x.set_from_reg(value); + s.wake_up_event_y.set_from_reg(value); + s.wake_up_event_z.set_from_reg(value); + s + } +} diff --git a/tflite_demo/lsm6ds3tr/registers/wake_up_ths.rs b/tflite_demo/lsm6ds3tr/registers/wake_up_ths.rs new file mode 100644 index 0000000..44bce44 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/registers/wake_up_ths.rs @@ -0,0 +1,32 @@ +//! Single and double-tap function threshold register (r/w). + +use crate::lsm6ds3tr::{RegisterAddress, RegisterBits, RegisterConfig, RegisterValue}; + +/// Single and double-tap function threshold register (r/w). +#[derive(Default)] +pub struct WakeUpThs { + /// Single/double-tap event enable. Default: 0 + /// (0: only single-tap event enabled; + /// 1: both single and double-tap events enabled) + pub single_double_tap_enabled: RegisterBits<1, 7>, + /// Threshold for wakeup. Default value: 000000 + /// 1 LSb corresponds to FS_XL/2^6 + pub wake_up_threshold: RegisterBits<6, 0>, +} + +impl WakeUpThs { + pub fn address(&self) -> u8 { + RegisterAddress::WAKE_UP_THS.address() + } + + pub fn value(&self) -> u8 { + self.single_double_tap_enabled.shifted() | self.wake_up_threshold.shifted() + } + + pub fn config(&self) -> RegisterConfig { + RegisterConfig { + address: self.address(), + value: self.value(), + } + } +} diff --git a/tflite_demo/lsm6ds3tr/settings/accel.rs b/tflite_demo/lsm6ds3tr/settings/accel.rs new file mode 100644 index 0000000..dcfb470 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/settings/accel.rs @@ -0,0 +1,37 @@ +use crate::lsm6ds3tr::registers::Ctrl1Xl; +use crate::lsm6ds3tr::{AccelSampleRate, AccelScale, RegisterConfig}; + +/// Accelerometer settings +#[derive(Default)] +pub struct AccelSettings { + pub sample_rate: AccelSampleRate, + pub scale: AccelScale, +} + +impl AccelSettings { + pub fn new() -> Self { + Self::default() + .with_sample_rate(AccelSampleRate::_416Hz) + .with_scale(AccelScale::_2G) + } + + pub fn with_sample_rate(mut self, sample_rate: AccelSampleRate) -> Self { + self.sample_rate = sample_rate; + self + } + + pub fn with_scale(mut self, scale: AccelScale) -> Self { + self.scale = scale; + self + } + + /// Returns accelerometer-related register config to be written + pub fn config(&self) -> RegisterConfig { + Ctrl1Xl { + sample_rate: self.sample_rate, + scale: self.scale, + ..Default::default() + } + .config() + } +} diff --git a/tflite_demo/lsm6ds3tr/settings/gyro.rs b/tflite_demo/lsm6ds3tr/settings/gyro.rs new file mode 100644 index 0000000..3bc5602 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/settings/gyro.rs @@ -0,0 +1,36 @@ +use crate::lsm6ds3tr::registers::Ctrl2G; +use crate::lsm6ds3tr::{GyroSampleRate, GyroScale, RegisterConfig}; + +/// Gyroscope settings +#[derive(Default)] +pub struct GyroSettings { + pub scale: GyroScale, + pub sample_rate: GyroSampleRate, +} + +impl GyroSettings { + pub fn new() -> Self { + Self::default() + .with_sample_rate(GyroSampleRate::_416Hz) + .with_scale(GyroScale::_250DPS) + } + + pub fn with_sample_rate(mut self, sample_rate: GyroSampleRate) -> Self { + self.sample_rate = sample_rate; + self + } + + pub fn with_scale(mut self, scale: GyroScale) -> Self { + self.scale = scale; + self + } + + /// Returns gyroscope-related register config to be written + pub fn config(&self) -> RegisterConfig { + Ctrl2G { + sample_rate: self.sample_rate, + scale: self.scale, + } + .config() + } +} diff --git a/tflite_demo/lsm6ds3tr/settings/irq.rs b/tflite_demo/lsm6ds3tr/settings/irq.rs new file mode 100644 index 0000000..575c0d5 --- /dev/null +++ b/tflite_demo/lsm6ds3tr/settings/irq.rs @@ -0,0 +1,277 @@ +use { + crate::lsm6ds3tr::data::XYZ, + crate::lsm6ds3tr::registers::{ + FreeFall, FreeFallThreshold, Int1Ctrl, Int2Ctrl, IntDur2, Md1Cfg, Md2Cfg, RegisterConfig, + TapCfg, TapThs6d, WakeUpDur, WakeUpThs, + }, +}; + +/// Interrupt pin/pad routing selection +#[derive(Default, Clone, Copy)] +pub enum InterruptRoute { + #[default] + None, + Int1, + Int2, + Both, +} + +#[derive(Default, Clone, Copy)] +pub enum TapRecognitionMode { + #[default] + None, + Single, + Double, + Both, +} + +#[derive(Default, Clone, Copy)] +pub struct FreeFallIrqSettings { + pub interrupt_route: InterruptRoute, + pub threshold: FreeFallThreshold, + pub duration_samples: u8, +} + +#[derive(Default)] +pub struct WakeUpIrqSettings { + pub interrupt_route: InterruptRoute, + pub threshold: u8, +} + +#[derive(Default)] +pub struct OrientationDetectionIrqSettings { + pub interrupt_route: InterruptRoute, + // TODO +} + +#[derive(Default)] +pub struct TapIrqSettings { + pub interrupt_route: InterruptRoute, + pub recognition_mode: TapRecognitionMode, + pub direction_enable: XYZ, + pub threshold: u8, + pub shock_samples: u8, + pub quiet_samples: u8, + pub duration_samples: u8, +} + +#[derive(Default)] +pub struct ActivityIrqSettings { + pub interrupt_route: InterruptRoute, + // TODO +} + +/// Interrupt settings +#[derive(Default)] +pub struct IrqSettings { + pub free_fall: FreeFallIrqSettings, + pub wake_up: WakeUpIrqSettings, + pub orientation_detection: OrientationDetectionIrqSettings, + pub tap: TapIrqSettings, + pub activity: ActivityIrqSettings, + registers: IrqRegisters, +} + +#[derive(Default)] +struct IrqRegisters { + int1_ctrl: Int1Ctrl, + int2_ctrl: Int2Ctrl, + md1_cfg: Md1Cfg, + md2_cfg: Md2Cfg, + tap_cfg: TapCfg, + tap_ths_6d: TapThs6d, + free_fall: FreeFall, + wake_up_dur: WakeUpDur, + wake_up_ths: WakeUpThs, + int_dur2: IntDur2, +} + +impl IrqSettings { + /// Enables Free Fall interrupt + pub fn enable_free_fall_irq( + &mut self, + threshold: FreeFallThreshold, + duration_samples: u8, + interrupt_route: InterruptRoute, + latched_irq: bool, + ) { + self.enable_irqs(latched_irq); + + self.free_fall.threshold = threshold; + self.free_fall.duration_samples = duration_samples; + self.free_fall.interrupt_route = interrupt_route; + + self.update_registers(); + } + + /// Enables Wake Up interrupt + pub fn enable_wake_up_irq( + &mut self, + threshold: u8, + interrupt_route: InterruptRoute, + latched_irq: bool, + ) { + self.enable_irqs(latched_irq); + + self.wake_up.threshold = threshold; + self.wake_up.interrupt_route = interrupt_route; + + self.update_registers(); + } + + // TODO provide helper function calculating desired tap time to register values + /// Enables Single/Double Tap interrupt + pub fn enable_tap_irq( + &mut self, + recognition_mode: TapRecognitionMode, + direction_enable: XYZ, + // threshold: u8, + // shock_samples: u8, + // quiet_samples: u8, + // duration_samples: u8, + interrupt_route: InterruptRoute, + latched_irq: bool, + ) { + self.enable_irqs(latched_irq); + + self.tap.recognition_mode = recognition_mode; + self.tap.direction_enable = direction_enable; + // self.tap.threshold = threshold; + // self.tap.shock_samples = shock_samples; + // self.tap.quiet_samples = quiet_samples; + // self.tap.duration_samples = duration_samples; + self.tap.interrupt_route = interrupt_route; + + self.update_registers(); + } + + /// Enables basic interrupts + pub fn enable_irqs(&mut self, latched_irq: bool) { + self.registers.tap_cfg.enable_basic_interrupts = 1.into(); + self.registers.tap_cfg.latched_interrupt = latched_irq.into(); + + self.update_registers(); + } + + /// Disables basic interrupts + pub fn disable_irqs(&mut self) { + self.registers.tap_cfg.enable_basic_interrupts = 0.into(); + + self.update_registers(); + } + + /// Returns interrupt-related registers configs to be written + pub fn configs(&self) -> [RegisterConfig; 10] { + [ + self.registers.int1_ctrl.config(), + self.registers.int2_ctrl.config(), + self.registers.int_dur2.config(), + self.registers.md1_cfg.config(), + self.registers.md2_cfg.config(), + self.registers.tap_cfg.config(), + self.registers.tap_ths_6d.config(), + self.registers.free_fall.config(), + self.registers.wake_up_dur.config(), + self.registers.wake_up_ths.config(), + ] + } + + fn update_registers(&mut self) { + self.update_free_fall_registers(); + self.update_wake_up_registers(); + self.update_tap_registers(); + } + + fn update_free_fall_registers(&mut self) { + ( + self.registers.md1_cfg.free_fall_event, + self.registers.md2_cfg.free_fall_event, + ) = match self.free_fall.interrupt_route { + InterruptRoute::None => (0.into(), 0.into()), + InterruptRoute::Int1 => (1.into(), 0.into()), + InterruptRoute::Int2 => (0.into(), 1.into()), + InterruptRoute::Both => (1.into(), 1.into()), + }; + self.registers.free_fall.threshold = self.free_fall.threshold; + self.registers.wake_up_dur.free_fall_duration_event = + (((self.free_fall.threshold as u8) >> 5) & 0b1).into(); + self.registers.free_fall.duration_event = self.free_fall.duration_samples.into(); + } + + fn update_wake_up_registers(&mut self) { + ( + self.registers.md1_cfg.wake_up_event, + self.registers.md2_cfg.wake_up_event, + ) = match self.wake_up.interrupt_route { + InterruptRoute::None => (0.into(), 0.into()), + InterruptRoute::Int1 => (1.into(), 0.into()), + InterruptRoute::Int2 => (0.into(), 1.into()), + InterruptRoute::Both => (1.into(), 1.into()), + }; + self.registers.wake_up_ths.wake_up_threshold = self.wake_up.threshold.into(); + } + + fn update_tap_registers(&mut self) { + match self.tap.recognition_mode { + TapRecognitionMode::None | TapRecognitionMode::Single => { + self.registers.wake_up_ths.single_double_tap_enabled = 0.into() + } + TapRecognitionMode::Double | TapRecognitionMode::Both => { + self.registers.wake_up_ths.single_double_tap_enabled = 1.into() + } + } + match self.tap.recognition_mode { + TapRecognitionMode::None => { + self.registers.tap_cfg.enable_basic_interrupts = false.into() + } + TapRecognitionMode::Single => { + self.registers.tap_cfg.enable_basic_interrupts = true.into(); + self.update_single_tap_registers() + } + TapRecognitionMode::Double => { + self.registers.tap_cfg.enable_basic_interrupts = true.into(); + self.update_double_tap_registers() + } + TapRecognitionMode::Both => { + self.registers.tap_cfg.enable_basic_interrupts = true.into(); + self.update_single_tap_registers(); + self.update_double_tap_registers(); + } + }; + self.registers.tap_cfg.enable_basic_interrupts = 1.into(); + self.registers.tap_cfg.enable_x_direction_tap_recognition = + self.tap.direction_enable.x.into(); + self.registers.tap_cfg.enable_y_direction_tap_recognition = + self.tap.direction_enable.y.into(); + self.registers.tap_cfg.enable_z_direction_tap_recognition = + self.tap.direction_enable.z.into(); + self.registers.tap_ths_6d.tap_threshold = self.tap.threshold.into(); + self.registers.int_dur2.duration = self.tap.duration_samples.into(); + self.registers.int_dur2.quiet = self.tap.quiet_samples.into(); + self.registers.int_dur2.shock = self.tap.shock_samples.into(); + } + + fn update_single_tap_registers(&mut self) { + ( + self.registers.md1_cfg.single_tap_event, + self.registers.md2_cfg.single_tap_event, + ) = match self.tap.interrupt_route { + InterruptRoute::None => (0.into(), 0.into()), + InterruptRoute::Int1 => (1.into(), 0.into()), + InterruptRoute::Int2 => (0.into(), 1.into()), + InterruptRoute::Both => (1.into(), 1.into()), + }; + } + + fn update_double_tap_registers(&mut self) { + ( + self.registers.md1_cfg.double_tap_event, + self.registers.md2_cfg.double_tap_event, + ) = match self.tap.interrupt_route { + InterruptRoute::None => (0.into(), 0.into()), + InterruptRoute::Int1 => (1.into(), 0.into()), + InterruptRoute::Int2 => (0.into(), 1.into()), + InterruptRoute::Both => (1.into(), 1.into()), + }; + } +} diff --git a/tflite_demo/lsm6ds3tr/settings/mod.rs b/tflite_demo/lsm6ds3tr/settings/mod.rs new file mode 100644 index 0000000..f1bdb3b --- /dev/null +++ b/tflite_demo/lsm6ds3tr/settings/mod.rs @@ -0,0 +1,44 @@ +pub mod accel; +pub mod gyro; +pub mod irq; + +pub use accel::*; +pub use gyro::*; +pub use irq::*; + +/// Device settings +#[derive(Default)] +pub struct LsmSettings { + pub accel: AccelSettings, + pub gyro: GyroSettings, + pub irq: IrqSettings, + pub low_performance_mode: bool, +} + +impl LsmSettings { + pub fn basic() -> Self { + Self::default() + .with_accel(AccelSettings::new()) + .with_gyro(GyroSettings::new()) + } + + pub fn with_accel(mut self, accel_settings: AccelSettings) -> Self { + self.accel = accel_settings; + self + } + + pub fn with_gyro(mut self, gyro_settings: GyroSettings) -> Self { + self.gyro = gyro_settings; + self + } + + pub fn with_irq(mut self, irq_settings: IrqSettings) -> Self { + self.irq = irq_settings; + self + } + + pub fn with_low_performance_mode(mut self) -> Self { + self.low_performance_mode = true; + self + } +} diff --git a/tflite_demo/src/lsm6ds.rs b/tflite_demo/src/lsm6ds.rs new file mode 100644 index 0000000..5fc3ca0 --- /dev/null +++ b/tflite_demo/src/lsm6ds.rs @@ -0,0 +1,658 @@ +// +// lsm6ds3tr +// +use core::convert::TryFrom; +use embedded_hal::i2c::{ErrorKind, ErrorType}; + +use crate::lsm6ds::Error::CommunicationError; +use embedded_hal_async::i2c::I2c; +// use log::info; + +/// Enum containing all possible types of errors when interacting with the IMU +#[derive(Debug)] +pub enum Error { + CommunicationError, + ChipDetectFailed, + RegisterReadFailed, +} + +// Value of the WHO_AM_I register +const CHIP_ID: u8 = 0x6a; + +// Earth gravity constant for acceleration conversion +const EARTH_GRAVITY: f32 = 9.80665; + +/// 6-DoF IMU accelerometer + gyro +pub struct Lsm6ds33 { + i2c: I2C, + addr: u8, + accelerometer_scale: Option, + gyroscope_scale: Option, +} + +impl Lsm6ds33 +where + I2C: I2c, +{ + /// Create an instance of the Lsm6ds33 driver + /// If the device cannot be detected on the bus, an error will be returned + pub async fn new(i2c: I2C, addr: u8) -> Result { + let mut lsm = Lsm6ds33 { + i2c, + addr, + accelerometer_scale: None, + gyroscope_scale: None, + }; + + match lsm.check().await { + Ok(true) => match lsm.set_auto_increment(true).await { + Ok(()) => Ok(lsm), + Err(e) => Err((lsm.release(), e)), + }, + Ok(false) => Err((lsm.release(), Error::ChipDetectFailed)), + Err(e) => Err((lsm.release(), e)), + } + } + + /// Set the accelerometer output rate + pub async fn set_accelerometer_output( + &mut self, + output: AccelerometerOutput, + ) -> Result<(), Error> { + self.write_register_option(Register::Ctrl1XL, output).await + } + + /// Set the accelerometer operating range + pub async fn set_accelerometer_scale( + &mut self, + scale: AccelerometerScale, + ) -> Result<(), Error> { + match self.write_register_option(Register::Ctrl1XL, scale).await { + Ok(()) => { + self.accelerometer_scale = Some(scale); + Ok(()) + } + Err(e) => { + self.accelerometer_scale = None; + Err(e) + } + } + } + + /// Set the accelerometer bandwidth filter + pub async fn set_accelerometer_bandwidth( + &mut self, + bandwidth: AccelerometerBandwidth, + ) -> Result<(), Error> { + self.write_register_option(Register::Ctrl1XL, bandwidth) + .await + } + + /// Set the gyroscope output rate + pub async fn set_gyroscope_output(&mut self, output: GyroscopeOutput) -> Result<(), Error> { + self.write_register_option(Register::Ctrl2G, output).await + } + + /// Set the gyroscope operating range + pub async fn set_gyroscope_scale(&mut self, scale: GyroscopeFullScale) -> Result<(), Error> { + match self.write_register_option(Register::Ctrl2G, scale).await { + Ok(()) => { + self.gyroscope_scale = Some(scale); + Ok(()) + } + Err(e) => { + self.accelerometer_scale = None; + Err(e) + } + } + } + + /// Set the low power mode + pub async fn set_low_power_mode(&mut self, low_power: bool) -> Result<(), Error> { + // N.B. "1" means low-power, "0" means high-performance. + self.write_bit( + Register::Ctrl6C, + low_power as u8, + Ctrl6C::AccelHighPerformanceMode as u8, + ) + .await?; + self.write_bit( + Register::Ctrl7G, + low_power as u8, + Ctrl7G::HighPerformanceMode as u8, + ) + .await + } + + /// Read the gyroscope data for each axis (RAD/s) + pub async fn read_gyro(&mut self) -> Result<(f32, f32, f32), Error> { + let scale = self.read_gyroscope_scale().await?; + self.read_registers(Register::OutXLG) + .await + .map(|res| Self::convert_gyro_data(&res, scale)) + } + + fn convert_gyro_data(data: &[u8; 6], scale: GyroscopeFullScale) -> (f32, f32, f32) { + // Convert raw data to float + let (x, y, z) = Self::u8_to_f32(data); + let scale = scale.scale(); + // Convert to RAD/s (Raw gyro data is in milli-degrees per second per bit) + ( + (x * scale / 1000.0).to_radians(), + (y * scale / 1000.0).to_radians(), + (z * scale / 1000.0).to_radians(), + ) + } + + /// Read the accelerometer data for each axis (m/s^2) + pub async fn read_accelerometer(&mut self) -> Result<(f32, f32, f32), Error> { + let scale = self.read_accelerometer_scale().await?; + self.read_registers(Register::OutXLXL) + .await + .map(|res| Self::convert_accel_data(&res, scale)) + } + + fn convert_accel_data(data: &[u8; 6], scale: AccelerometerScale) -> (f32, f32, f32) { + // Convert raw values to float + let (x, y, z) = Self::u8_to_f32(data); + let scale = scale.scale(); + + // Convert to m/s^2 (Raw value is in mg/bit) + ( + (x * scale / 1000.0) * EARTH_GRAVITY, + (y * scale / 1000.0) * EARTH_GRAVITY, + (z * scale / 1000.0) * EARTH_GRAVITY, + ) + } + + /// Read the temperature (degC) + pub async fn read_temperature(&mut self) -> Result { + let data = self.read_registers::<2>(Register::OutTempL).await?; + Ok(Self::convert_temp_data(&data)) + } + + fn convert_temp_data(data: &[u8; 2]) -> f32 { + let (lo, hi) = (data[0], data[1]); + // Raw temperature as signal 16-bit number + let temperature = ((hi as i16) << 8) | (lo as i16); + + // info!("Temperature: {:x} {:x}", hi, lo); + // info!("Temperature: {:x}", temperature); + + // As float + let temperature = temperature as f32; + + // Converted given the temperature sensitively value 16 bits per C + let temperature = (temperature / 256.0) + 25.0; + + temperature + } + + /// Check if there is new accelerometer data + pub async fn accel_data_available(&mut self) -> Result { + self.read_status().await.map(|status| status & 0b1 != 0) + } + + /// Check if there is new gyro scope data + pub async fn gyro_data_available(&mut self) -> Result { + self.read_status().await.map(|status| status & 0b10 != 0) + } + + /// Read the accelerometer scale value from the configuration register + pub async fn read_accelerometer_scale(&mut self) -> Result { + match self.accelerometer_scale { + Some(v) => Ok(v), + None => { + let scale = self.read_register_option(Register::Ctrl1XL).await?; + self.accelerometer_scale = Some(scale); + Ok(scale) + } + } + } + + /// Read the gyroscope scale value from the configuration register + pub async fn read_gyroscope_scale(&mut self) -> Result { + match self.gyroscope_scale { + Some(v) => Ok(v), + None => { + let scale = self.read_register_option(Register::Ctrl2G).await?; + self.gyroscope_scale = Some(scale); + Ok(scale) + } + } + } + + async fn check(&mut self) -> Result { + self.read_register(Register::WhoAmI).await.map(|chip_id| { + // info!("Chip ID: {:x}", chip_id); + return chip_id == CHIP_ID; + }) + } + + async fn set_auto_increment(&mut self, enabled: bool) -> Result<(), Error> { + self.write_bit(Register::Ctrl3C, enabled as u8, Ctrl3C::AutoIncrement as u8) + .await + } + + async fn read_status(&mut self) -> Result { + self.read_register(Register::StatusReg).await + } + + async fn write_register_option( + &mut self, + register: Register, + ro: RO, + ) -> Result<(), Error> { + self.write_bits(register, ro.value(), RO::mask(), RO::bit_offset()) + .await + } + + async fn read_register_option>( + &mut self, + register: Register, + ) -> Result { + let value = self.read_register(register).await?; + RO::try_from(value).map_err(|_| Error::RegisterReadFailed) + } + + async fn write_bit(&mut self, register: Register, value: u8, shift: u8) -> Result<(), Error> { + self.write_bits(register, value, 0x01, shift).await + } + + async fn write_bits( + &mut self, + register: Register, + new_value: u8, + mask: u8, + shift: u8, + ) -> Result<(), Error> { + let current_value = self.read_register(register).await?; + let modified_value = (current_value & !(mask << shift)) | ((new_value & mask) << shift); + self.write_register(register, modified_value).await + } + + fn u8_to_f32(res: &[u8; 6]) -> (f32, f32, f32) { + let (x, y, z) = ( + (res[0] as i16) | ((res[1] as i16) << 8), + (res[2] as i16) | ((res[3] as i16) << 8), + (res[4] as i16) | ((res[5] as i16) << 8), + ); + (x as f32, y as f32, z as f32) + } + + // Read a byte from the given register + async fn read_register(&mut self, register: Register) -> Result { + let mut res = [0u8]; + if self + .i2c + .write_read(self.addr, &[register.into()], &mut res) + .await + .is_ok() + { + Ok(res[0]) + } else { + Err(CommunicationError) + } + } + + async fn read_registers( + &mut self, + start_reg: Register, + ) -> Result<[u8; N], Error> { + let mut res = [0u8; N]; + if self + .i2c + .write_read(self.addr, &[start_reg.into()], &mut res) + .await + .is_ok() + { + Ok(res) + } else { + Err(CommunicationError) + } + } + + // Write the specified value to the given register + async fn write_register(&mut self, register: Register, value: u8) -> Result<(), Error> { + if self + .i2c + .write(self.addr, &[register.into(), value]) + .await + .is_ok() + { + Ok(()) + } else { + Err(CommunicationError) + } + } + + /// Return the underlying I2C device + pub fn release(self) -> I2C { + self.i2c + } +} + +#[derive(Debug)] +pub enum RegisterError { + ConversionError, +} + +// Device registers +#[derive(Debug, Clone, Copy)] +pub enum Register { + FifoCtrl1 = 0x06, + FifoCtrl2 = 0x07, + FifoCtrl3 = 0x08, + FifoCtrl4 = 0x09, + FifoCtrl5 = 0x0A, + + OrientCfgG = 0x0B, + + Int1Ctrl = 0x0D, + Int2Ctrl = 0x0E, + + WhoAmI = 0x0F, + + Ctrl1XL = 0x10, + Ctrl2G = 0x11, + Ctrl3C = 0x12, + Ctrl4C = 0x13, + Ctrl5C = 0x14, + Ctrl6C = 0x15, + Ctrl7G = 0x16, + Ctrl8Xl = 0x18, + Ctrl10C = 0x19, + + WakeUpSrc = 0x1B, + TapSrc = 0x1C, + D6dSrc = 0x1D, + + StatusReg = 0x1E, + + OutTempL = 0x20, + OutTempH = 0x21, + + OutXLG = 0x22, + OutXHG = 0x23, + OutYLG = 0x24, + OutYHG = 0x25, + OutZLG = 0x26, + OutZHG = 0x27, + OutXLXL = 0x28, + OutXHXL = 0x29, + OutYLXL = 0x2A, + OutYHXL = 0x2B, + OutZLXL = 0x2C, + OutZHXL = 0x2D, + + FifoStatus1 = 0x3A, + FifoStatus2 = 0x3B, + FifoStatus3 = 0x3C, + FifoStatus4 = 0x3D, + + FifoDataOutL = 0x3E, + FifoDataOutH = 0x3F, + + Timestamp1Reg = 0x41, + Timestamp2Reg = 0x42, + + StepTimestampL = 0x49, + StepTimestampH = 0x4A, + + StepCounterL = 0x4B, + StepCounterH = 0x4C, + + FuncSrc = 0x53, + + TapCfg = 0x58, + TapThs6d = 0x59, + + IntDur2 = 0x5A, + + WakeUpThs = 0x5B, + WakeUpDur = 0x5C, + + FreeFall = 0x5D, + + Md1Cfg = 0x5E, + Md2Cfg = 0x5F, +} + +impl From for u8 { + fn from(r: Register) -> u8 { + r as u8 + } +} + +pub trait RegisterOption { + fn value(&self) -> u8; + fn bit_offset() -> u8; + fn mask() -> u8; +} + +// --------------------------------------------------------------------------------------------------------------------- +// --- CTRL1_XL -------------------------------------------------------------------------------------------------------- +// --------------------------------------------------------------------------------------------------------------------- + +#[derive(Debug, Clone, Copy)] +pub enum AccelerometerOutput { + PowerDown = 0b0000, + Rate13 = 0b0001, + Rate26 = 0b0010, + Rate52 = 0b0011, + Rate104 = 0b0100, + Rate208 = 0b0101, + Rate416 = 0b0110, + Rate833 = 0b0111, + Rate1_66k = 0b1000, + Rate3_33k = 0b1001, + Rate6_66k = 0b1010, +} + +impl RegisterOption for AccelerometerOutput { + fn value(&self) -> u8 { + *self as u8 + } + fn mask() -> u8 { + 0xF + } + fn bit_offset() -> u8 { + 4 + } +} + +/// Accelerometer full-scale selection +#[derive(Debug, Clone, Copy)] +pub enum AccelerometerScale { + G02 = 0b00, + G16 = 0b01, + G04 = 0b10, + G08 = 0b11, +} + +impl RegisterOption for AccelerometerScale { + fn value(&self) -> u8 { + *self as u8 + } + fn mask() -> u8 { + 0x03 + } + fn bit_offset() -> u8 { + 2 + } +} + +impl AccelerometerScale { + pub fn scale(&self) -> f32 { + match *self { + Self::G02 => 0.061, + Self::G04 => 0.122, + Self::G08 => 0.244, + Self::G16 => 0.488, + } + } +} + +impl TryFrom for AccelerometerScale { + type Error = RegisterError; + + fn try_from(value: u8) -> Result { + let value = (value >> Self::bit_offset()) & Self::mask(); + match value { + 0b00 => Ok(Self::G02), + 0b01 => Ok(Self::G16), + 0b10 => Ok(Self::G04), + 0b11 => Ok(Self::G08), + _ => Err(RegisterError::ConversionError), + } + } +} + +#[derive(Debug, Clone, Copy)] +pub enum AccelerometerBandwidth { + Freq400 = 0b00, + Freq200 = 0b01, + Freq100 = 0b10, + Freq50 = 0b11, +} + +impl RegisterOption for AccelerometerBandwidth { + fn value(&self) -> u8 { + *self as u8 + } + fn mask() -> u8 { + 0x03 + } + fn bit_offset() -> u8 { + 0 + } +} + +// --------------------------------------------------------------------------------------------------------------------- +// --- CTRL2_G -------------------------------------------------------------------------------------------------------- +// --------------------------------------------------------------------------------------------------------------------- + +#[derive(Debug, Clone, Copy)] +pub enum GyroscopeOutput { + PowerDown = 0b0000, + Rate13 = 0b0001, + Rate26 = 0b0010, + Rate52 = 0b0011, + Rate104 = 0b0100, + Rate208 = 0b0101, + Rate416 = 0b0110, + Rate833 = 0b0111, + Rate1_66k = 0b1000, + Rate3_33k = 0b1001, + Rate6_66k = 0b1010, +} + +impl RegisterOption for GyroscopeOutput { + fn value(&self) -> u8 { + *self as u8 + } + fn mask() -> u8 { + 0xF + } + fn bit_offset() -> u8 { + 4 + } +} + +#[derive(Debug, Clone, Copy)] +pub enum GyroscopeFullScale { + Dps125 = 0b001, + Dps245 = 0b000, + Dps500 = 0b010, + Dps1000 = 0b100, + Dps2000 = 0b110, +} + +impl RegisterOption for GyroscopeFullScale { + fn value(&self) -> u8 { + *self as u8 + } + fn mask() -> u8 { + 0b111 + } + fn bit_offset() -> u8 { + 1 + } +} + +impl GyroscopeFullScale { + pub fn scale(&self) -> f32 { + match *self { + Self::Dps125 => 4.375, + Self::Dps245 => 8.75, + Self::Dps500 => 17.50, + Self::Dps1000 => 35.0, + Self::Dps2000 => 70.0, + } + } +} + +impl TryFrom for GyroscopeFullScale { + type Error = RegisterError; + + fn try_from(value: u8) -> Result { + let value = (value >> Self::bit_offset()) & Self::mask(); + match value { + 0b001 => Ok(GyroscopeFullScale::Dps125), + 0b000 => Ok(GyroscopeFullScale::Dps245), + 0b010 => Ok(GyroscopeFullScale::Dps500), + 0b100 => Ok(GyroscopeFullScale::Dps1000), + 0b110 => Ok(GyroscopeFullScale::Dps2000), + _ => Err(RegisterError::ConversionError), + } + } +} + +// #[derive(Debug, Clone, Copy)] +// pub enum GyroscopeFullScale125Dps { +// Disabled = 0, +// Enabled = 1, +// } + +// impl RegisterOption for GyroscopeFullScale125Dps { +// fn value(&self) -> u8 { +// *self as u8 +// } +// fn mask() -> u8 { +// 0b1 +// } +// fn bit_offset() -> u8 { +// 1 +// } +// } + +// pub fn option_mask(opt: &T) -> u8 { +// 0x01 << opt.bit_offset() +// } + +/// Bit fields for CTRL3_C +pub enum Ctrl3C { + Boot = 7, + BlockDataUpdate = 6, + InterruptActivationLevel = 5, + InterruptPadOutput = 4, + SpiSerialInterfaceMode = 3, + AutoIncrement = 2, + Endian = 1, + SoftwareReset = 0, +} + +/// Bit fields for CTRL6_C +pub enum Ctrl6C { + GyroEdgeTrigge = 7, + GyroLevelTrigger = 6, + GyroLevelLatched = 5, + AccelHighPerformanceMode = 4, +} + +/// Bit fields for CTRL6_G +pub enum Ctrl7G { + HighPerformanceMode = 7, + HighPassFilter = 6, + SourceRegisterRounding = 3, +} diff --git a/tflite_demo/src/main.rs b/tflite_demo/src/main.rs index 75d6c97..6b6be73 100644 --- a/tflite_demo/src/main.rs +++ b/tflite_demo/src/main.rs @@ -2,16 +2,17 @@ #![no_std] mod common; - mod i2c; +mod lsm6ds; use core::fmt::Write; use embedded_alloc::LlffHeap; use libm::sinf; -use lsm6ds3tr::{ - interface::{I2cInterface, SpiInterface}, - LsmSettings, LSM6DS3TR, -}; +use lsm6ds::Lsm6ds33; +// use lsm6ds3tr::{ +// interface::{I2cInterface, SpiInterface}, +// LsmSettings, LSM6DS3TR, +// }; // use lsm6ds3tr::{interface::SpiInterface, AccelScale, LsmSettings, LSM6DS3TR}; use microflow::model; use nalgebra::matrix; @@ -127,17 +128,19 @@ fn main() -> ! { // let mut imu = LSM6DS3TR::new(spi_interface); // imu.init().expect("Couldn't initialize the LSM6 sensor!"); - let scl = port0.p0_27.into_floating_input().degrade(); - let sda = port0.p0_07.into_floating_input().degrade(); + // let scl = port0.p0_27.into_floating_input().degrade(); + // let sda = port0.p0_07.into_floating_input().degrade(); - let pins = twim::Pins { scl: scl, sda: sda }; + // let pins = twim::Pins { scl: scl, sda: sda }; - let i2c = Twim::new(p.TWIM0, pins, twim::Frequency::K100); + // let i2c = Twim::new(p.TWIM0, pins, twim::Frequency::K100); - let i2c_interface = I2cInterface::new(i2c); + // let i2c_interface = I2cInterface::new(i2c); // let mut imu = LSM6DS3TR::new(I2cInterface::new("")); + // let mut imu = Lsm6ds33::new(i2c, addr); + let rtc = Rtc::new(p.RTC0, 0).unwrap(); rtc.enable_counter();