From 4c9d80b75ae8c2b0835edaed7877d3db33c214b1 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Thu, 23 Nov 2023 15:03:53 +0100 Subject: [PATCH 01/23] LSM6DSO: Library skeleton and FIFO config --- Cargo.toml | 1 + utils/lsm6dso/Cargo.toml | 15 ++ utils/lsm6dso/src/bounded_int.rs | 69 ++++++++ utils/lsm6dso/src/config.rs | 270 +++++++++++++++++++++++++++++++ utils/lsm6dso/src/lib.rs | 116 +++++++++++++ utils/lsm6dso/src/registers.rs | 133 +++++++++++++++ 6 files changed, 604 insertions(+) create mode 100644 utils/lsm6dso/Cargo.toml create mode 100644 utils/lsm6dso/src/bounded_int.rs create mode 100644 utils/lsm6dso/src/config.rs create mode 100644 utils/lsm6dso/src/lib.rs create mode 100644 utils/lsm6dso/src/registers.rs diff --git a/Cargo.toml b/Cargo.toml index 9c8b17df..7afba60c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,6 +9,7 @@ members = [ "arch/x86/aerugo-x86-hal", "utils/env-parser", "utils/env-parser-tests", + "utils/lsm6dso", "calldwell/calldwell-rs", ] exclude = ["demos", "examples", "testbins", "calldwell/examples"] diff --git a/utils/lsm6dso/Cargo.toml b/utils/lsm6dso/Cargo.toml new file mode 100644 index 00000000..b98f4c2e --- /dev/null +++ b/utils/lsm6dso/Cargo.toml @@ -0,0 +1,15 @@ +[package] +name = "lsm6dso" +version = "1.0.0" +authors.workspace = true +edition.workspace = true +rust-version.workspace = true +repository.workspace = true +license.workspace = true +description = "LSM6DSO driver library" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +embedded-hal = "1.0.0-rc.1" +paste = "1.0.14" diff --git a/utils/lsm6dso/src/bounded_int.rs b/utils/lsm6dso/src/bounded_int.rs new file mode 100644 index 00000000..562da5df --- /dev/null +++ b/utils/lsm6dso/src/bounded_int.rs @@ -0,0 +1,69 @@ +//! Module with implementation of Bounded integer types. +#![allow(dead_code)] +use paste::paste; + +/// Macro creating a generic bounded value type, which allows for storing a value of specific type +/// in specified range. +/// +/// You'd usually want to create a type alias that uses this structure. +macro_rules! generic_bounded_value { + ($underlying_type:ty) => { + paste! { + #[doc = "Generic bounded value that uses `" $underlying_type "` as underlying type. "] + #[doc = "Provided range is inclusive."] + #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] + pub struct []($underlying_type); + + impl []<{ LOW }, { HIGH }> { + #[doc = "Lowest value that an instance of this struct can have."] + pub const LOW: $underlying_type = LOW; + #[doc = "Highest value that an instance of this struct can have."] + pub const HIGH: $underlying_type = HIGH; + + #[doc = "Creates a new bounded value. Returns `None` if the value is out of range."] + pub const fn new(value: $underlying_type) -> Option { + if value >= Self::LOW && value <= Self::HIGH { + Some(Self(value)) + } else { + None + } + } + + #[doc = "Creates a new bounded value. If `value` is out of range, it will be saturated "] + #[doc = "to `LOW` or `HIGH`, depending whether it's too small or large."] + pub const fn new_saturated(value: $underlying_type) -> Self { + Self(match value { + value if value < Self::LOW => Self::LOW, + value if value > Self::HIGH => Self::HIGH, + value => value + }) + } + + #[doc = "Returns the stored value. A `const` alternative to `Deref` trait."] + pub const fn get(&self) -> $underlying_type { + self.0 + } + } + + impl core::ops::Deref for []<{ LOW }, { HIGH }> { + type Target = $underlying_type; + + fn deref(&self) -> &Self::Target { + &self.0 + } + } + } + }; +} + +generic_bounded_value!(u8); +generic_bounded_value!(u16); +generic_bounded_value!(u32); +generic_bounded_value!(u64); +generic_bounded_value!(usize); + +generic_bounded_value!(i8); +generic_bounded_value!(i16); +generic_bounded_value!(i32); +generic_bounded_value!(i64); +generic_bounded_value!(isize); diff --git a/utils/lsm6dso/src/config.rs b/utils/lsm6dso/src/config.rs new file mode 100644 index 00000000..75bb9e32 --- /dev/null +++ b/utils/lsm6dso/src/config.rs @@ -0,0 +1,270 @@ +//! Module with config-related items + +use crate::{ + bounded_int::BoundedU16, + registers::{MultiRegisterConversion, MultiRegisterField, RegisterConversion, RegisterField}, +}; + +/// Type representing FIFO watermark threshold as FIFO records (6 bytes of sensor data + tag) +pub type FifoWatermarkThreshold = BoundedU16<0, 0x1FF>; +impl MultiRegisterField for FifoWatermarkThreshold { + const MASKS: [u8; 2] = [0xFF, 0x01]; + const OFFSETS: [usize; 2] = [0, 0]; +} + +impl MultiRegisterConversion for FifoWatermarkThreshold { + fn to_regs(self) -> [u8; 2] { + self.to_le_bytes() + } + + fn from_regs(regs: &[u8]) -> Self { + assert!(regs.len() >= 2); + let value_lsb = regs[0]; + let value_msb = regs[1] & Self::MASKS[1]; + Self::new(u16::from_le_bytes([value_lsb, value_msb])).unwrap() + } +} + +/// Type representing amount of items in FIFO as FIFO records (6 bytes of sensor data + tag) +pub type FifoDataLength = BoundedU16<0, 0x3FF>; +impl MultiRegisterField for FifoDataLength { + const MASKS: [u8; 2] = [0xFF, 0x03]; + const OFFSETS: [usize; 2] = [0, 0]; +} + +impl MultiRegisterConversion for FifoDataLength { + fn to_regs(self) -> [u8; 2] { + self.to_le_bytes() + } + + fn from_regs(regs: &[u8]) -> Self { + assert!(regs.len() >= 2); + let value_lsb = regs[0]; + let value_msb = regs[1] & Self::MASKS[1]; + Self::new(u16::from_le_bytes([value_lsb, value_msb])).unwrap() + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub enum DataRateChangeBatching { + Enabled = 1, + Disabled = 0, +} +impl RegisterField for DataRateChangeBatching { + const MASK: u8 = 0x10; + const OFFSET: usize = 4; +} +impl RegisterConversion for DataRateChangeBatching { + fn to_reg(self) -> u8 { + (self as u8) << Self::OFFSET + } + + fn from_reg(reg: u8) -> Self { + if reg & Self::MASK != 0 { + DataRateChangeBatching::Enabled + } else { + DataRateChangeBatching::Disabled + } + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub enum StopOnWatermarkThreshold { + Yes = 1, + No = 0, +} +impl RegisterField for StopOnWatermarkThreshold { + const MASK: u8 = 0x80; + const OFFSET: usize = 7; +} +impl RegisterConversion for StopOnWatermarkThreshold { + fn to_reg(self) -> u8 { + (self as u8) << Self::OFFSET + } + + fn from_reg(reg: u8) -> Self { + if reg & Self::MASK != 0 { + StopOnWatermarkThreshold::Yes + } else { + StopOnWatermarkThreshold::No + } + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub enum GyroscopeBatchingRate { + NoBatching = 0b0000, + Batch12_5Hz = 0b0001, + Batch26Hz = 0b0010, + Batch52Hz = 0b0011, + Batch104Hz = 0b0100, + Batch208Hz = 0b0101, + Batch417Hz = 0b0110, + Batch833Hz = 0b0111, + Batch1667Hz = 0b1000, + Batch3333Hz = 0b1001, + Batch6667Hz = 0b1010, + Batch6_5Hz = 0b1011, +} +impl RegisterField for GyroscopeBatchingRate { + const MASK: u8 = 0xF0; + const OFFSET: usize = 4; +} +impl RegisterConversion for GyroscopeBatchingRate { + fn to_reg(self) -> u8 { + (self as u8) << Self::OFFSET + } + + fn from_reg(reg: u8) -> Self { + match (reg & Self::MASK) >> Self::OFFSET { + 0b0000 => GyroscopeBatchingRate::NoBatching, + 0b0001 => GyroscopeBatchingRate::Batch12_5Hz, + 0b0010 => GyroscopeBatchingRate::Batch26Hz, + 0b0011 => GyroscopeBatchingRate::Batch52Hz, + 0b0100 => GyroscopeBatchingRate::Batch104Hz, + 0b0101 => GyroscopeBatchingRate::Batch208Hz, + 0b0110 => GyroscopeBatchingRate::Batch417Hz, + 0b0111 => GyroscopeBatchingRate::Batch833Hz, + 0b1000 => GyroscopeBatchingRate::Batch1667Hz, + 0b1001 => GyroscopeBatchingRate::Batch3333Hz, + 0b1010 => GyroscopeBatchingRate::Batch6667Hz, + 0b1011 => GyroscopeBatchingRate::Batch6_5Hz, + other => { + panic!("Unexpected value tried to be parsed as GyroscopeBatchingRate: {other:2X}"); + } + } + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub enum AccelerometerBatchingRate { + NoBatching = 0b0000, + Batch12_5Hz = 0b0001, + Batch26Hz = 0b0010, + Batch52Hz = 0b0011, + Batch104Hz = 0b0100, + Batch208Hz = 0b0101, + Batch417Hz = 0b0110, + Batch833Hz = 0b0111, + Batch1667Hz = 0b1000, + Batch3333Hz = 0b1001, + Batch6667Hz = 0b1010, + Batch1_6Hz = 0b1011, +} +impl RegisterField for AccelerometerBatchingRate { + const MASK: u8 = 0x0F; + const OFFSET: usize = 0; +} +impl RegisterConversion for AccelerometerBatchingRate { + fn to_reg(self) -> u8 { + (self as u8) << Self::OFFSET + } + + fn from_reg(reg: u8) -> Self { + match (reg & Self::MASK) >> Self::OFFSET { + 0b0000 => AccelerometerBatchingRate::NoBatching, + 0b0001 => AccelerometerBatchingRate::Batch12_5Hz, + 0b0010 => AccelerometerBatchingRate::Batch26Hz, + 0b0011 => AccelerometerBatchingRate::Batch52Hz, + 0b0100 => AccelerometerBatchingRate::Batch104Hz, + 0b0101 => AccelerometerBatchingRate::Batch208Hz, + 0b0110 => AccelerometerBatchingRate::Batch417Hz, + 0b0111 => AccelerometerBatchingRate::Batch833Hz, + 0b1000 => AccelerometerBatchingRate::Batch1667Hz, + 0b1001 => AccelerometerBatchingRate::Batch3333Hz, + 0b1010 => AccelerometerBatchingRate::Batch6667Hz, + 0b1011 => AccelerometerBatchingRate::Batch1_6Hz, + other => { + panic!( + "Unexpected value tried to be parsed as AccelerometerBatchingRate: {other:2X}" + ); + } + } + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub enum FifoMode { + /// FIFO disabled. + Bypass = 0b000, + /// FIFO enabled and sensor stops collecting data when FIFO is full. + Fifo = 0b001, + /// Continuous mode until trigger is deasserted, then FIFO mode. + ContinuousToFifo = 0b011, + /// Bypass mode until trigger is deasserted, then Continuous mode. + BypassToContinuous = 0b101, + /// If FIFO is full, then the new sample overwrites the older one. + Continuous = 0b110, + /// Bypass mode until trigger is deasserted, then FIFO mode. + BypassToFifo = 0b111, +} +impl RegisterField for FifoMode { + const MASK: u8 = 0x07; + const OFFSET: usize = 0; +} +impl RegisterConversion for FifoMode { + fn to_reg(self) -> u8 { + (self as u8) << Self::OFFSET + } + + fn from_reg(reg: u8) -> Self { + match (reg & Self::MASK) >> Self::OFFSET { + 0b000 => FifoMode::Bypass, + 0b001 => FifoMode::Fifo, + 0b011 => FifoMode::ContinuousToFifo, + 0b101 => FifoMode::BypassToContinuous, + 0b110 => FifoMode::Continuous, + 0b111 => FifoMode::BypassToFifo, + other => { + panic!("Unexpected value tried to be parsed as FifoMode: {other:2X}"); + } + } + } +} + +/// Structure representing LSM6DSO FIFO configuration. +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct FifoConfig { + /// Watermark threshold, defines the amount of FIFO items required for watermark flag to rise. + pub watermark_threshold: FifoWatermarkThreshold, + /// State of ODR CHANGE virtual sensor batching in FIFO. + pub odr_change_batched: DataRateChangeBatching, + /// Whether to limit FIFO depth up to watermark threshold, or not. + pub stop_on_watermark: StopOnWatermarkThreshold, + /// Gyroscope batching data rate. + pub gyroscope_batching_rate: GyroscopeBatchingRate, + /// Accelerometer batching data rate + pub accelerometer_batching_rate: AccelerometerBatchingRate, + /// FIFO mode + pub mode: FifoMode, +} + +/// Type representing memory buffer for FIFO configuration. +pub type FifoConfigBuffer = [u8; 4]; + +impl From for FifoConfigBuffer { + fn from(config: FifoConfig) -> Self { + let watermark_threshold_bytes = config.watermark_threshold.to_regs(); + [ + watermark_threshold_bytes[0], + watermark_threshold_bytes[1] + | config.odr_change_batched.to_reg() + | config.stop_on_watermark.to_reg(), + config.accelerometer_batching_rate.to_reg() | config.gyroscope_batching_rate.to_reg(), + config.mode.to_reg(), + ] + } +} + +impl From for FifoConfig { + fn from(regs: FifoConfigBuffer) -> Self { + FifoConfig { + watermark_threshold: FifoWatermarkThreshold::from_regs(®s[0..=1]), + odr_change_batched: DataRateChangeBatching::from_reg(regs[1]), + stop_on_watermark: StopOnWatermarkThreshold::from_reg(regs[1]), + gyroscope_batching_rate: GyroscopeBatchingRate::from_reg(regs[2]), + accelerometer_batching_rate: AccelerometerBatchingRate::from_reg(regs[2]), + mode: FifoMode::from_reg(regs[3]), + } + } +} diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs new file mode 100644 index 00000000..f3dbf4e3 --- /dev/null +++ b/utils/lsm6dso/src/lib.rs @@ -0,0 +1,116 @@ +#![no_std] +//! LSM6DSO driver library. +//! +//! This library is made specifically for usage with Aerugo SAMV71 HAL. For this reason, it +//! requires [`SpiBus`] instead of [`SpiDevice`](embedded_hal::spi::SpiDevice), as SAMV71 HAL +//! currently has only the [`SpiBus`] trait implementation. +//! +//! When SAMV71 HAL will support [`SpiDevice`](embedded_hal::spi::SpiDevice), this library should +//! be refactored to use it, instead of the whole bus. + +extern crate embedded_hal; +extern crate paste; + +mod bounded_int; +pub mod config; +mod registers; + +use config::{FifoConfig, FifoConfigBuffer}; +use registers::Register; + +pub use embedded_hal::spi::SpiBus; +pub use registers::WHO_AM_I_VALUE; + +/// LSM6DSO driver structure. +/// +/// # Generic parameters +/// * `SPI` - An SPI bus instance that LSM6DSO driver will use to communicate with the sensor +pub struct LSM6DSO { + /// SPI instance. + spi: SPI, +} + +/// Constant representing a mask applied to value being read from LSM6DSO via SPI +const READ_REQUEST_MASK: u8 = 0x80; + +impl LSM6DSO { + /// Creates new LSM6DSO instance. Consumes the SPI bus. + /// + /// SPI must be configured with following settings for LSM6DSO to work correctly: + /// * embedded-hal compatible config must be applied (NVIC SPI IRQ off, RX/TX SPI IRQs enabled) + /// * Clock polarity: high when inactive + /// * Clock phase: data changed on leading edge + /// * Chip select behavior: Deactivate after last transfer + /// * Bits per transfer: 8 + /// * Serial clock divider: Serial clock must be less than 10MHz + /// * Delay before first clock and consecutive transfers: 0 + pub fn new(spi: SPI) -> Result { + let mut lsm = Self { spi }; + // First transaction usually fails to communicate with LSM and returns junk, so it's done + // here to prevent failing user operations. + lsm.id()?; + Ok(lsm) + } + + /// Returns the ID read from WHO_AM_I register of the sensor. + /// Should be equal to [`WHO_AM_I_VALUE`]. + pub fn id(&mut self) -> Result { + self.read_register(Register::WHO_AM_I) + } + + /// Returns `true` if sensor responds with valid ID. + pub fn is_alive(&mut self) -> Result { + Ok(self.id()? == WHO_AM_I_VALUE) + } + + pub fn set_fifo_config(&mut self, config: FifoConfig) -> Result<(), SPI::Error> { + let fifo_config_regs: FifoConfigBuffer = config.into(); + self.write_registers(Register::FIFO_CTRL1, &fifo_config_regs)?; + Ok(()) + } + + pub fn get_fifo_config(&mut self) -> Result { + let mut buffer = [0u8, 0, 0, 0]; + self.read_registers(Register::FIFO_CTRL1, &mut buffer)?; + Ok(buffer.into()) + } + + /// Reads the value from a single LSM6DSO register and returns it. + fn read_register(&mut self, register: Register) -> Result { + let mut data_buffer = [READ_REQUEST_MASK | (register as u8), 0]; + self.spi.transfer_in_place(&mut data_buffer)?; + Ok(data_buffer[1]) + } + + /// Read the value from multiple registers, starting with specified one. + /// Amount of read registers depends on the size of the buffer slice. + /// Buffer must be at least 2 bytes long, otherwise this function will panic. + fn read_registers( + &mut self, + first_register: Register, + buffer: &mut [u8], + ) -> Result<(), SPI::Error> { + assert!(buffer.len() >= 2); + buffer[0] = READ_REQUEST_MASK | (first_register as u8); + self.spi.transfer_in_place(buffer)?; + Ok(()) + } + + /// Writes a value to LSM6DSO register. + fn _write_register(&mut self, register: Register, value: u8) -> Result<(), SPI::Error> { + let write_request = [register as u8, value]; + self.spi.write(&write_request)?; + Ok(()) + } + + /// Writes multiple values to LSM6DSO registers. + fn write_registers( + &mut self, + first_register: Register, + values: &[u8], + ) -> Result<(), SPI::Error> { + self.spi.write(&[first_register as u8])?; + self.spi.write(values)?; + Ok(()) + } +} diff --git a/utils/lsm6dso/src/registers.rs b/utils/lsm6dso/src/registers.rs new file mode 100644 index 00000000..b0de337f --- /dev/null +++ b/utils/lsm6dso/src/registers.rs @@ -0,0 +1,133 @@ +//! LSM6DSO registers-related definitions. + +/// LSM6DSO registers and their addresses. +#[allow(non_camel_case_types)] +#[allow(dead_code)] +pub(crate) enum Register { + FUNC_CFG_ACCESS = 0x01, + PIN_CTRL = 0x02, + FIFO_CTRL1 = 0x07, + FIFO_CTRL2 = 0x08, + FIFO_CTRL3 = 0x09, + FIFO_CTRL4 = 0x0A, + COUNTER_BDR_REG1 = 0x0B, + COUNTER_BDR_REG2 = 0x0C, + INT1_CTRL = 0x0D, + INT2_CTRL = 0x0E, + WHO_AM_I = 0x0F, + CTRL1_XL = 0x10, + CTRL2_G = 0x11, + CTRL3_C = 0x12, + CTRL4_C = 0x13, + CTRL5_C = 0x14, + CTRL6_C = 0x15, + CTRL7_G = 0x16, + CTRL8_XL = 0x17, + CTRL9_XL = 0x18, + CTRL10_C = 0x19, + ALL_INT_SRC = 0x1A, + WAKE_UP_SRC = 0x1B, + TAP_SRC = 0x1C, + D6D_SRC = 0x1D, + STATUS_REG = 0x1E, + OUT_TEMP_L = 0x20, + OUT_TEMP_H = 0x21, + OUTX_L_G = 0x22, + OUTX_H_G = 0x23, + OUTY_L_G = 0x24, + OUTY_H_G = 0x25, + OUTZ_L_G = 0x26, + OUTZ_H_G = 0x27, + OUTX_L_A = 0x28, + OUTX_H_A = 0x29, + OUTY_L_A = 0x2A, + OUTY_H_A = 0x2B, + OUTZ_L_A = 0x2C, + OUTZ_H_A = 0x2D, + EMB_FUNC_STATUS_MAINPAGE = 0x35, + FSM_STATUS_A_MAINPAGE = 0x36, + FSM_STATUS_B_MAINPAGE = 0x37, + STATUS_MASTER_MAINPAGE = 0x39, + FIFO_STATUS1 = 0x3A, + FIFO_STATUS2 = 0x3B, + TIMESTAMP0 = 0x40, + TIMESTAMP1 = 0x41, + TIMESTAMP2 = 0x42, + TIMESTAMP3 = 0x43, + TAP_CFG0 = 0x56, + TAP_CFG1 = 0x57, + TAP_CFG2 = 0x58, + TAP_THS_6D = 0x59, + INT_DUR2 = 0x5A, + WAKE_UP_THS = 0x5B, + WAKE_UP_DUR = 0x5C, + FREE_FALL = 0x5D, + MD1_CFG = 0x5E, + MD2_CFG = 0x5F, + I2C_BUS_AVB = 0x62, + INTERNAL_FREQ_FINE = 0x63, + INT_OIS = 0x6F, + CTRL1_OIS = 0x70, + CTRL2_OIS = 0x71, + CTRL3_OIS = 0x72, + X_OFS_USR = 0x73, + Y_OFS_USR = 0x74, + Z_OFS_USR = 0x75, + FIFO_DATA_OUT_TAG = 0x78, + FIFO_DATA_X_L = 0x79, + FIFO_DATA_X_H = 0x7A, + FIFO_DATA_Y_L = 0x7B, + FIFO_DATA_Y_H = 0x7C, + FIFO_DATA_Z_L = 0x7D, + FIFO_DATA_Z_H = 0x7E, +} + +/// Expected value of WHO_AM_I register +pub const WHO_AM_I_VALUE: u8 = 0x6C; + +/// Trait for single-register fields +pub trait RegisterField +where + Self: Copy, +{ + /// Field mask, per datasheet (as-in register). + const MASK: u8; + /// Offset of the field's LSB to register's LSB. + const OFFSET: usize; +} + +/// Trait for fields that span multiple registers. The order of masks and offsets must be defined +/// respective to the order of registers this field spans, smaller address first. +pub trait MultiRegisterField +where + Self: Copy, +{ + /// Field masks, per datasheet (as-in register). + const MASKS: [u8; REGISTER_SPAN]; + /// Offsets of the field's LSB from register's LSB. + const OFFSETS: [usize; REGISTER_SPAN]; +} + +pub trait RegisterConversion +where + Self: Copy + RegisterField, +{ + /// This function should return the value of current register field that can be OR'd with + /// register's content to set it. + fn to_reg(self) -> u8; + + /// This function should extract the field's value from the register and return it. + fn from_reg(reg: u8) -> Self; +} + +pub trait MultiRegisterConversion +where + Self: Copy + MultiRegisterField, +{ + /// This function should return the value of provided registers as an array. Unused bits should + /// remain 0. + fn to_regs(self) -> [u8; REGISTER_SPAN]; + + /// This function should extract the field's value from the register and return it. + fn from_regs(regs: &[u8]) -> Self; +} From 7afa6b86482ecd8b2413c3edfcaaced5ef490456 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Thu, 23 Nov 2023 15:05:32 +0100 Subject: [PATCH 02/23] LSM6DSO: Example created --- examples/samv71-lsm6dso/.cargo/config.toml | 11 +++ examples/samv71-lsm6dso/Cargo.toml | 25 ++++++ examples/samv71-lsm6dso/build.rs | 14 ++++ examples/samv71-lsm6dso/memory.x | 6 ++ .../samv71-lsm6dso/src/hardware_config.rs | 83 +++++++++++++++++++ examples/samv71-lsm6dso/src/main.rs | 54 ++++++++++++ examples/samv71-lsm6dso/src/tasklets.rs | 1 + 7 files changed, 194 insertions(+) create mode 100644 examples/samv71-lsm6dso/.cargo/config.toml create mode 100644 examples/samv71-lsm6dso/Cargo.toml create mode 100644 examples/samv71-lsm6dso/build.rs create mode 100644 examples/samv71-lsm6dso/memory.x create mode 100644 examples/samv71-lsm6dso/src/hardware_config.rs create mode 100644 examples/samv71-lsm6dso/src/main.rs create mode 100644 examples/samv71-lsm6dso/src/tasklets.rs diff --git a/examples/samv71-lsm6dso/.cargo/config.toml b/examples/samv71-lsm6dso/.cargo/config.toml new file mode 100644 index 00000000..be9455f4 --- /dev/null +++ b/examples/samv71-lsm6dso/.cargo/config.toml @@ -0,0 +1,11 @@ +[build] +target = "thumbv7em-none-eabihf" + +[env] +AERUGO_TASKLET_COUNT = { value = "2" } + +[target.thumbv7em-none-eabihf] +rustflags = [ + "-C", "link-arg=--nmagic", # Disable page alignment of sections (to prevent issues with binary size) + "-C", "link-arg=-Tlink.x", # Use cortex-m-rt's linker script +] diff --git a/examples/samv71-lsm6dso/Cargo.toml b/examples/samv71-lsm6dso/Cargo.toml new file mode 100644 index 00000000..c1f40daf --- /dev/null +++ b/examples/samv71-lsm6dso/Cargo.toml @@ -0,0 +1,25 @@ +[package] +name = "samv71-lsm6dso" +authors = ["Wojciech Olech "] +edition = "2021" +version = "0.1.0" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +aerugo = { version = "0.1.0", path = "../..", features = [ + "use-aerugo-cortex-m", + "rt", +] } +cortex-m = { version = "0.7.7", features = ["critical-section-single-core"] } +cortex-m-rt = { version = "0.7.3", features = ["device"] } +lsm6dso = { version = "*", path = "../../utils/lsm6dso" } +panic-rtt-target = { version = "0.1.2", features = ["cortex-m"] } + +[features] +rt = ["aerugo/rt"] + +[profile.release] +codegen-units = 1 +lto = true +debug = true diff --git a/examples/samv71-lsm6dso/build.rs b/examples/samv71-lsm6dso/build.rs new file mode 100644 index 00000000..d26b3397 --- /dev/null +++ b/examples/samv71-lsm6dso/build.rs @@ -0,0 +1,14 @@ +use std::env; +use std::fs::File; +use std::io::Write; +use std::path::PathBuf; + +fn main() { + // Put the linker script somewhere the linker can find it + let out = &PathBuf::from(env::var_os("OUT_DIR").unwrap()); + File::create(out.join("memory.x")) + .unwrap() + .write_all(include_bytes!("memory.x")) + .unwrap(); + println!("cargo:rustc-link-search={}", out.display()); +} diff --git a/examples/samv71-lsm6dso/memory.x b/examples/samv71-lsm6dso/memory.x new file mode 100644 index 00000000..62221839 --- /dev/null +++ b/examples/samv71-lsm6dso/memory.x @@ -0,0 +1,6 @@ +/* Linker script for SAMV71Q21 */ +MEMORY +{ + FLASH (rx) : ORIGIN = 0x00400000, LENGTH = 0x00200000 + RAM (rwx) : ORIGIN = 0x20400000, LENGTH = 0x00060000 +} diff --git a/examples/samv71-lsm6dso/src/hardware_config.rs b/examples/samv71-lsm6dso/src/hardware_config.rs new file mode 100644 index 00000000..709b9568 --- /dev/null +++ b/examples/samv71-lsm6dso/src/hardware_config.rs @@ -0,0 +1,83 @@ +use aerugo::hal::{ + drivers::{ + pio::{ + pin::{InputMode, Peripheral, PeripheralMode}, + Pin, Port, + }, + pmc::{config::PeripheralId, PMC}, + spi::{ + chip_config::{ + BitsPerTransfer, ChipConfig, ChipSelectBehavior, ClockPhase, ClockPolarity, + SerialClockDivider, + }, + config::{MasterConfig, SelectedChip}, + interrupts::Interrupts, + Master, NotConfigured, Spi, + }, + }, + user_peripherals::{PIOD, SPI0}, +}; + +/// LSM6DSO chip select signal. +const LSM6DSO_CHIP: SelectedChip = SelectedChip::Chip1; +/// LSM6DSO SPI clock divider. Default peripheral clock is 12MHz, LSM6DSO can work with up to 10MHz, +/// but for safety a 480kHz clock will be used instead (12MHz/25). +const LSM6DSO_SPI_CLOCK_DIVIDER: u8 = 25; + +pub struct LSM6DSOPins { + pub miso: Pin, + pub mosi: Pin, + pub sck: Pin, + pub cs: Pin, + pub int1: Pin, +} + +pub fn configure_pio(port: Port) -> LSM6DSOPins { + let mut pins = port.into_pins(); + + let miso = pins[20].take().unwrap().into_peripheral_pin(Peripheral::B); + let mosi = pins[21].take().unwrap().into_peripheral_pin(Peripheral::B); + let sck = pins[22].take().unwrap().into_peripheral_pin(Peripheral::B); + let cs = pins[25].take().unwrap().into_peripheral_pin(Peripheral::B); + let int1 = pins[28].take().unwrap().into_input_pin(); + + LSM6DSOPins { + miso, + mosi, + sck, + cs, + int1, + } +} + +pub fn configure_spi(spi: Spi) -> Spi { + let mut spi = spi.into_master(MasterConfig::new(LSM6DSO_CHIP)); + spi.configure_chip( + LSM6DSO_CHIP, + ChipConfig { + clock_polarity: ClockPolarity::HighWhenInactive, + clock_phase: ClockPhase::DataChangedOnLeadingEdge, + chip_select_behavior: ChipSelectBehavior::DeactivateAfterLastTransfer, + bits_per_transfer: BitsPerTransfer::Bits8, + clock_divider: SerialClockDivider::new(LSM6DSO_SPI_CLOCK_DIVIDER).unwrap(), + delay_before_first_clock: 0, + delay_between_consecutive_transfers: 0, + }, + ); + spi.set_interrupts_state(Interrupts { + rx_data_register_full: true, + tx_data_register_empty: true, + mode_fault_error: false, + overrun_error: false, + nss_rising: false, + tx_registers_empty: true, + underrun_error: false, + }); + + spi +} + +pub fn configure_pmc(pmc: &mut PMC) { + pmc.enable_peripheral_clock(PeripheralId::SPI0); + pmc.enable_peripheral_clock(PeripheralId::PIOD); +} diff --git a/examples/samv71-lsm6dso/src/main.rs b/examples/samv71-lsm6dso/src/main.rs new file mode 100644 index 00000000..22215f44 --- /dev/null +++ b/examples/samv71-lsm6dso/src/main.rs @@ -0,0 +1,54 @@ +#![no_std] +#![no_main] + +extern crate cortex_m; +extern crate cortex_m_rt as rt; +extern crate lsm6dso; +extern crate panic_rtt_target; + +use aerugo::{ + hal::drivers::{pio::Port, spi::Spi}, + logln, Aerugo, InitApi, SystemHardwareConfig, +}; +use lsm6dso::{ + config::{ + AccelerometerBatchingRate, DataRateChangeBatching, FifoConfig, FifoMode, + FifoWatermarkThreshold, GyroscopeBatchingRate, StopOnWatermarkThreshold, + }, + LSM6DSO, +}; +use rt::entry; + +mod hardware_config; +mod tasklets; + +#[entry] +fn main() -> ! { + let (aerugo, mut peripherals) = Aerugo::initialize(SystemHardwareConfig::default()); + logln!("Hello, world! Aerugo initialized! Initializing hardware..."); + + let mut pmc = peripherals.pmc.take().unwrap(); + let spi = Spi::new(peripherals.spi_0.take().unwrap()); + let port_d = Port::new(peripherals.pio_d.take().unwrap()); + + hardware_config::configure_pmc(&mut pmc); + let _lsm_pins = hardware_config::configure_pio(port_d); + let lsm_spi = hardware_config::configure_spi(spi); + + let mut lsm = LSM6DSO::new(lsm_spi).unwrap(); + + logln!("LSM id: {:2X?}", lsm.id()); + logln!("Is LSM alive? {:?}", lsm.is_alive()); + logln!("Current LSM config: {:#?}", lsm.get_fifo_config()); + let test_config = FifoConfig { + watermark_threshold: FifoWatermarkThreshold::new(123).unwrap(), + odr_change_batched: DataRateChangeBatching::Enabled, + stop_on_watermark: StopOnWatermarkThreshold::Yes, + gyroscope_batching_rate: GyroscopeBatchingRate::Batch26Hz, + accelerometer_batching_rate: AccelerometerBatchingRate::Batch417Hz, + mode: FifoMode::Fifo, + }; + lsm.set_fifo_config(test_config).unwrap(); + logln!("New LSM config: {:#?}", lsm.get_fifo_config()); + aerugo.start(); +} diff --git a/examples/samv71-lsm6dso/src/tasklets.rs b/examples/samv71-lsm6dso/src/tasklets.rs new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/examples/samv71-lsm6dso/src/tasklets.rs @@ -0,0 +1 @@ + From 40281a49e9739ec09bad89ba23daa9a35b92340a Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Fri, 24 Nov 2023 10:05:09 +0100 Subject: [PATCH 03/23] LSM6DSO: Minor library structure refactor, privatized internal items --- .../lsm6dso/src/{config.rs => config/fifo.rs} | 0 utils/lsm6dso/src/config/mod.rs | 1 + utils/lsm6dso/src/lib.rs | 68 ++++++++++++++----- utils/lsm6dso/src/registers.rs | 6 +- 4 files changed, 55 insertions(+), 20 deletions(-) rename utils/lsm6dso/src/{config.rs => config/fifo.rs} (100%) create mode 100644 utils/lsm6dso/src/config/mod.rs diff --git a/utils/lsm6dso/src/config.rs b/utils/lsm6dso/src/config/fifo.rs similarity index 100% rename from utils/lsm6dso/src/config.rs rename to utils/lsm6dso/src/config/fifo.rs diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs new file mode 100644 index 00000000..2badf406 --- /dev/null +++ b/utils/lsm6dso/src/config/mod.rs @@ -0,0 +1 @@ +pub mod fifo; diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index f3dbf4e3..200d6522 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -15,25 +15,26 @@ mod bounded_int; pub mod config; mod registers; -use config::{FifoConfig, FifoConfigBuffer}; -use registers::Register; +use config::fifo::{FifoConfig, FifoConfigBuffer}; +use registers::{Register, WHO_AM_I_VALUE}; pub use embedded_hal::spi::SpiBus; -pub use registers::WHO_AM_I_VALUE; /// LSM6DSO driver structure. /// /// # Generic parameters /// * `SPI` - An SPI bus instance that LSM6DSO driver will use to communicate with the sensor -pub struct LSM6DSO { +pub struct LSM6DSO { /// SPI instance. spi: SPI, + /// Buffer for internal operations + buffer: [u8; BUFFER_SIZE], } /// Constant representing a mask applied to value being read from LSM6DSO via SPI const READ_REQUEST_MASK: u8 = 0x80; -impl LSM6DSO { +impl LSM6DSO { /// Creates new LSM6DSO instance. Consumes the SPI bus. /// /// SPI must be configured with following settings for LSM6DSO to work correctly: @@ -45,7 +46,14 @@ impl LSM6DSO { /// * Serial clock divider: Serial clock must be less than 10MHz /// * Delay before first clock and consecutive transfers: 0 pub fn new(spi: SPI) -> Result { - let mut lsm = Self { spi }; + assert!( + BUFFER_SIZE >= 2, + "LSM6DSO buffer must be at least 2 bytes long" + ); + let mut lsm = Self { + spi, + buffer: [0; BUFFER_SIZE], + }; // First transaction usually fails to communicate with LSM and returns junk, so it's done // here to prevent failing user operations. lsm.id()?; @@ -82,6 +90,13 @@ impl LSM6DSO { Ok(data_buffer[1]) } + /// Writes a value to LSM6DSO register. + fn _write_register(&mut self, register: Register, value: u8) -> Result<(), SPI::Error> { + let write_request = [register as u8, value]; + self.spi.write(&write_request)?; + Ok(()) + } + /// Read the value from multiple registers, starting with specified one. /// Amount of read registers depends on the size of the buffer slice. /// Buffer must be at least 2 bytes long, otherwise this function will panic. @@ -90,16 +105,23 @@ impl LSM6DSO { first_register: Register, buffer: &mut [u8], ) -> Result<(), SPI::Error> { - assert!(buffer.len() >= 2); - buffer[0] = READ_REQUEST_MASK | (first_register as u8); - self.spi.transfer_in_place(buffer)?; - Ok(()) - } + let user_buffer_length = buffer.len(); + // at least 1 byte for data + assert!(user_buffer_length > 0, "provided buffer is too small"); + // +1 byte for address + assert!( + BUFFER_SIZE > user_buffer_length, + "LSM6DSO buffer is too small for this operation" + ); + + // Prepare transfer + self.buffer[0] = READ_REQUEST_MASK | (first_register as u8); + // Get data from sensor + self.spi + .transfer_in_place(&mut self.buffer[0..user_buffer_length])?; + // Copy to user's buffer + buffer.copy_from_slice(&self.buffer[1..=user_buffer_length]); - /// Writes a value to LSM6DSO register. - fn _write_register(&mut self, register: Register, value: u8) -> Result<(), SPI::Error> { - let write_request = [register as u8, value]; - self.spi.write(&write_request)?; Ok(()) } @@ -109,8 +131,20 @@ impl LSM6DSO { first_register: Register, values: &[u8], ) -> Result<(), SPI::Error> { - self.spi.write(&[first_register as u8])?; - self.spi.write(values)?; + let user_buffer_length = values.len(); + // at least 1 byte for data + assert!(user_buffer_length > 0, "provided buffer is too small"); + // +1 byte for address + assert!( + BUFFER_SIZE > user_buffer_length, + "LSM6DSO buffer is too small for this operation" + ); + + // Prepare transfer + self.buffer[0] = first_register as u8; + self.buffer[1..=user_buffer_length].copy_from_slice(values); + // Write data + self.spi.write(&self.buffer[0..=user_buffer_length])?; Ok(()) } } diff --git a/utils/lsm6dso/src/registers.rs b/utils/lsm6dso/src/registers.rs index b0de337f..c2370af1 100644 --- a/utils/lsm6dso/src/registers.rs +++ b/utils/lsm6dso/src/registers.rs @@ -86,7 +86,7 @@ pub(crate) enum Register { pub const WHO_AM_I_VALUE: u8 = 0x6C; /// Trait for single-register fields -pub trait RegisterField +pub(crate) trait RegisterField where Self: Copy, { @@ -98,7 +98,7 @@ where /// Trait for fields that span multiple registers. The order of masks and offsets must be defined /// respective to the order of registers this field spans, smaller address first. -pub trait MultiRegisterField +pub(crate) trait MultiRegisterField where Self: Copy, { @@ -108,7 +108,7 @@ where const OFFSETS: [usize; REGISTER_SPAN]; } -pub trait RegisterConversion +pub(crate) trait RegisterConversion where Self: Copy + RegisterField, { From 00ebee9c718867235dac68e9615c37c1f7193f40 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Fri, 24 Nov 2023 10:06:13 +0100 Subject: [PATCH 04/23] LSM6DSO: Example updated to refactored library structure --- examples/samv71-lsm6dso/src/hardware_config.rs | 9 ++++----- examples/samv71-lsm6dso/src/main.rs | 10 +++++++--- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/examples/samv71-lsm6dso/src/hardware_config.rs b/examples/samv71-lsm6dso/src/hardware_config.rs index 709b9568..e9367e24 100644 --- a/examples/samv71-lsm6dso/src/hardware_config.rs +++ b/examples/samv71-lsm6dso/src/hardware_config.rs @@ -20,9 +20,8 @@ use aerugo::hal::{ /// LSM6DSO chip select signal. const LSM6DSO_CHIP: SelectedChip = SelectedChip::Chip1; -/// LSM6DSO SPI clock divider. Default peripheral clock is 12MHz, LSM6DSO can work with up to 10MHz, -/// but for safety a 480kHz clock will be used instead (12MHz/25). -const LSM6DSO_SPI_CLOCK_DIVIDER: u8 = 25; +/// LSM6DSO SPI clock divider. Default peripheral clock is 12MHz, LSM6DSO can work with up to 10MHz. +const LSM6DSO_SPI_CLOCK_DIVIDER: u8 = 100; pub struct LSM6DSOPins { pub miso: Pin, @@ -67,8 +66,8 @@ pub fn configure_spi(spi: Spi) -> Spi { spi.set_interrupts_state(Interrupts { rx_data_register_full: true, tx_data_register_empty: true, - mode_fault_error: false, - overrun_error: false, + mode_fault_error: true, + overrun_error: true, nss_rising: false, tx_registers_empty: true, underrun_error: false, diff --git a/examples/samv71-lsm6dso/src/main.rs b/examples/samv71-lsm6dso/src/main.rs index 22215f44..70663e55 100644 --- a/examples/samv71-lsm6dso/src/main.rs +++ b/examples/samv71-lsm6dso/src/main.rs @@ -11,7 +11,7 @@ use aerugo::{ logln, Aerugo, InitApi, SystemHardwareConfig, }; use lsm6dso::{ - config::{ + config::fifo::{ AccelerometerBatchingRate, DataRateChangeBatching, FifoConfig, FifoMode, FifoWatermarkThreshold, GyroscopeBatchingRate, StopOnWatermarkThreshold, }, @@ -35,11 +35,12 @@ fn main() -> ! { let _lsm_pins = hardware_config::configure_pio(port_d); let lsm_spi = hardware_config::configure_spi(spi); - let mut lsm = LSM6DSO::new(lsm_spi).unwrap(); + let mut lsm: LSM6DSO<_, 32> = LSM6DSO::new(lsm_spi).unwrap(); logln!("LSM id: {:2X?}", lsm.id()); logln!("Is LSM alive? {:?}", lsm.is_alive()); logln!("Current LSM config: {:#?}", lsm.get_fifo_config()); + let test_config = FifoConfig { watermark_threshold: FifoWatermarkThreshold::new(123).unwrap(), odr_change_batched: DataRateChangeBatching::Enabled, @@ -49,6 +50,9 @@ fn main() -> ! { mode: FifoMode::Fifo, }; lsm.set_fifo_config(test_config).unwrap(); - logln!("New LSM config: {:#?}", lsm.get_fifo_config()); + let read_config = lsm.get_fifo_config(); + logln!("New LSM config: {:#?}", read_config); + assert_eq!(read_config.unwrap(), test_config); + aerugo.start(); } From 4eeeb9fa4911b9bec9c08fa4c7b2d4774702eb5b Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Fri, 24 Nov 2023 10:09:40 +0100 Subject: [PATCH 05/23] SPI: "Fixed" embedded-hal write implementation --- .../samv71-hal/src/spi/embedded_hal.rs | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/arch/cortex-m/samv71-hal/src/spi/embedded_hal.rs b/arch/cortex-m/samv71-hal/src/spi/embedded_hal.rs index 1ae8d4fb..37fb3047 100644 --- a/arch/cortex-m/samv71-hal/src/spi/embedded_hal.rs +++ b/arch/cortex-m/samv71-hal/src/spi/embedded_hal.rs @@ -83,6 +83,15 @@ impl SpiBus for Spi { /// # Parameters /// * `words` - Slice of words to be transmitted. /// + /// # Remarks + /// This function may behave unexpectedly in some scenarios, as it waits for RX data register + /// full event, instead of TX data register empty event, as the latter doesn't happen after + /// first word is transmitted for unknown reasons in some SPI configurations. The issue may + /// happen if the device expects the write to be a single transaction, and SPI is configured in + /// a way that will make every word to be a separate transaction with CS deactivations in + /// between. Verify if this function works for your scenario, or use `transfer`/`transfer_in_place` + /// for writing. + /// /// # Returns /// Ok(()) on success, [`SpiError`] on SPI error. fn write(&mut self, words: &[u8]) -> Result<(), Self::Error> { @@ -91,10 +100,13 @@ impl SpiBus for Spi { for &word in words { self.transmit_value(word as u16); - status_reader.wait_for_status( - |status| status.interrupts.tx_data_register_empty, - usize::MAX, - ); + // This should, in theory, check the state of TX data register. However, when it does + // that, the transfer hangs at 2nd word, as the flag never rises again. A workaround + // is to wait for RX data flag, as it'll be risen after the word is transmitted, but it's + // not guaranteed to work in every case (as it doesn't guarantee that the transfer will + // be a single SPI transaction, which may sometimes be necessary). + status_reader + .wait_for_status(|status| status.interrupts.rx_data_register_full, usize::MAX); // Dummy read to prevent overrun error self.get_received_data(); } From 4d5bd4c1ccce722077c820f478656dcc411b2a99 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Fri, 24 Nov 2023 12:18:19 +0100 Subject: [PATCH 06/23] LSM6DSO: Added macro for reducing register enum definitions boilerplate + irq config --- utils/lsm6dso/Cargo.toml | 6 + utils/lsm6dso/src/config/control.rs | 0 utils/lsm6dso/src/config/fifo.rs | 147 ++----------------------- utils/lsm6dso/src/config/interrupts.rs | 76 +++++++++++++ utils/lsm6dso/src/config/mod.rs | 2 + utils/lsm6dso/src/config/templates.rs | 31 ++++++ utils/lsm6dso/src/lib.rs | 28 ++++- 7 files changed, 153 insertions(+), 137 deletions(-) create mode 100644 utils/lsm6dso/src/config/control.rs create mode 100644 utils/lsm6dso/src/config/interrupts.rs create mode 100644 utils/lsm6dso/src/config/templates.rs diff --git a/utils/lsm6dso/Cargo.toml b/utils/lsm6dso/Cargo.toml index b98f4c2e..83befacf 100644 --- a/utils/lsm6dso/Cargo.toml +++ b/utils/lsm6dso/Cargo.toml @@ -11,5 +11,11 @@ description = "LSM6DSO driver library" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] +derive_more = { version = "1.0.0-beta.6", features = [ + "try_from", + "try_into", +], default-features = false } embedded-hal = "1.0.0-rc.1" paste = "1.0.14" + +[features] diff --git a/utils/lsm6dso/src/config/control.rs b/utils/lsm6dso/src/config/control.rs new file mode 100644 index 00000000..e69de29b diff --git a/utils/lsm6dso/src/config/fifo.rs b/utils/lsm6dso/src/config/fifo.rs index 75bb9e32..f8afda3f 100644 --- a/utils/lsm6dso/src/config/fifo.rs +++ b/utils/lsm6dso/src/config/fifo.rs @@ -1,7 +1,6 @@ -//! Module with config-related items - use crate::{ bounded_int::BoundedU16, + config::templates, registers::{MultiRegisterConversion, MultiRegisterField, RegisterConversion, RegisterField}, }; @@ -45,54 +44,17 @@ impl MultiRegisterConversion for FifoDataLength { } } -#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub enum DataRateChangeBatching { - Enabled = 1, +templates::register_enum!(DataRateChangeBatching [mask=0x10, offset=4] { Disabled = 0, -} -impl RegisterField for DataRateChangeBatching { - const MASK: u8 = 0x10; - const OFFSET: usize = 4; -} -impl RegisterConversion for DataRateChangeBatching { - fn to_reg(self) -> u8 { - (self as u8) << Self::OFFSET - } - - fn from_reg(reg: u8) -> Self { - if reg & Self::MASK != 0 { - DataRateChangeBatching::Enabled - } else { - DataRateChangeBatching::Disabled - } - } -} + Enabled = 1, +}); -#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub enum StopOnWatermarkThreshold { +templates::register_enum!(StopOnWatermarkThreshold [mask=0x80, offset=7]{ Yes = 1, No = 0, -} -impl RegisterField for StopOnWatermarkThreshold { - const MASK: u8 = 0x80; - const OFFSET: usize = 7; -} -impl RegisterConversion for StopOnWatermarkThreshold { - fn to_reg(self) -> u8 { - (self as u8) << Self::OFFSET - } +}); - fn from_reg(reg: u8) -> Self { - if reg & Self::MASK != 0 { - StopOnWatermarkThreshold::Yes - } else { - StopOnWatermarkThreshold::No - } - } -} - -#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub enum GyroscopeBatchingRate { +templates::register_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { NoBatching = 0b0000, Batch12_5Hz = 0b0001, Batch26Hz = 0b0010, @@ -105,39 +67,9 @@ pub enum GyroscopeBatchingRate { Batch3333Hz = 0b1001, Batch6667Hz = 0b1010, Batch6_5Hz = 0b1011, -} -impl RegisterField for GyroscopeBatchingRate { - const MASK: u8 = 0xF0; - const OFFSET: usize = 4; -} -impl RegisterConversion for GyroscopeBatchingRate { - fn to_reg(self) -> u8 { - (self as u8) << Self::OFFSET - } - - fn from_reg(reg: u8) -> Self { - match (reg & Self::MASK) >> Self::OFFSET { - 0b0000 => GyroscopeBatchingRate::NoBatching, - 0b0001 => GyroscopeBatchingRate::Batch12_5Hz, - 0b0010 => GyroscopeBatchingRate::Batch26Hz, - 0b0011 => GyroscopeBatchingRate::Batch52Hz, - 0b0100 => GyroscopeBatchingRate::Batch104Hz, - 0b0101 => GyroscopeBatchingRate::Batch208Hz, - 0b0110 => GyroscopeBatchingRate::Batch417Hz, - 0b0111 => GyroscopeBatchingRate::Batch833Hz, - 0b1000 => GyroscopeBatchingRate::Batch1667Hz, - 0b1001 => GyroscopeBatchingRate::Batch3333Hz, - 0b1010 => GyroscopeBatchingRate::Batch6667Hz, - 0b1011 => GyroscopeBatchingRate::Batch6_5Hz, - other => { - panic!("Unexpected value tried to be parsed as GyroscopeBatchingRate: {other:2X}"); - } - } - } -} +}); -#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub enum AccelerometerBatchingRate { +templates::register_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { NoBatching = 0b0000, Batch12_5Hz = 0b0001, Batch26Hz = 0b0010, @@ -150,41 +82,9 @@ pub enum AccelerometerBatchingRate { Batch3333Hz = 0b1001, Batch6667Hz = 0b1010, Batch1_6Hz = 0b1011, -} -impl RegisterField for AccelerometerBatchingRate { - const MASK: u8 = 0x0F; - const OFFSET: usize = 0; -} -impl RegisterConversion for AccelerometerBatchingRate { - fn to_reg(self) -> u8 { - (self as u8) << Self::OFFSET - } +}); - fn from_reg(reg: u8) -> Self { - match (reg & Self::MASK) >> Self::OFFSET { - 0b0000 => AccelerometerBatchingRate::NoBatching, - 0b0001 => AccelerometerBatchingRate::Batch12_5Hz, - 0b0010 => AccelerometerBatchingRate::Batch26Hz, - 0b0011 => AccelerometerBatchingRate::Batch52Hz, - 0b0100 => AccelerometerBatchingRate::Batch104Hz, - 0b0101 => AccelerometerBatchingRate::Batch208Hz, - 0b0110 => AccelerometerBatchingRate::Batch417Hz, - 0b0111 => AccelerometerBatchingRate::Batch833Hz, - 0b1000 => AccelerometerBatchingRate::Batch1667Hz, - 0b1001 => AccelerometerBatchingRate::Batch3333Hz, - 0b1010 => AccelerometerBatchingRate::Batch6667Hz, - 0b1011 => AccelerometerBatchingRate::Batch1_6Hz, - other => { - panic!( - "Unexpected value tried to be parsed as AccelerometerBatchingRate: {other:2X}" - ); - } - } - } -} - -#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] -pub enum FifoMode { +templates::register_enum!(FifoMode [mask=0x07, offset=0] { /// FIFO disabled. Bypass = 0b000, /// FIFO enabled and sensor stops collecting data when FIFO is full. @@ -197,30 +97,7 @@ pub enum FifoMode { Continuous = 0b110, /// Bypass mode until trigger is deasserted, then FIFO mode. BypassToFifo = 0b111, -} -impl RegisterField for FifoMode { - const MASK: u8 = 0x07; - const OFFSET: usize = 0; -} -impl RegisterConversion for FifoMode { - fn to_reg(self) -> u8 { - (self as u8) << Self::OFFSET - } - - fn from_reg(reg: u8) -> Self { - match (reg & Self::MASK) >> Self::OFFSET { - 0b000 => FifoMode::Bypass, - 0b001 => FifoMode::Fifo, - 0b011 => FifoMode::ContinuousToFifo, - 0b101 => FifoMode::BypassToContinuous, - 0b110 => FifoMode::Continuous, - 0b111 => FifoMode::BypassToFifo, - other => { - panic!("Unexpected value tried to be parsed as FifoMode: {other:2X}"); - } - } - } -} +}); /// Structure representing LSM6DSO FIFO configuration. #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] diff --git a/utils/lsm6dso/src/config/interrupts.rs b/utils/lsm6dso/src/config/interrupts.rs new file mode 100644 index 00000000..593546f0 --- /dev/null +++ b/utils/lsm6dso/src/config/interrupts.rs @@ -0,0 +1,76 @@ +pub struct INT1Interrupts { + pub data_ready: bool, + pub counter_bdr: bool, + pub fifo_full: bool, + pub fifo_overrun: bool, + pub fifo_threshold: bool, + pub boot: bool, + pub gyro_data_ready: bool, + pub accel_data_ready: bool, +} + +pub struct INT2Interrupts { + pub counter_bdr: bool, + pub fifo_full: bool, + pub fifo_overrun: bool, + pub fifo_threshold: bool, + pub temperature_data_ready: bool, + pub gyro_data_ready: bool, + pub accel_data_ready: bool, +} + +pub type InterruptConfigBuffer = u8; + +impl From for InterruptConfigBuffer { + fn from(config: INT1Interrupts) -> Self { + config.accel_data_ready as u8 + | ((config.gyro_data_ready as u8) << 1) + | ((config.boot as u8) << 2) + | ((config.fifo_threshold as u8) << 3) + | ((config.fifo_overrun as u8) << 4) + | ((config.fifo_full as u8) << 5) + | ((config.counter_bdr as u8) << 6) + | ((config.data_ready as u8) << 7) + } +} + +impl From for InterruptConfigBuffer { + fn from(config: INT2Interrupts) -> Self { + config.accel_data_ready as u8 + | ((config.gyro_data_ready as u8) << 1) + | ((config.temperature_data_ready as u8) << 2) + | ((config.fifo_threshold as u8) << 3) + | ((config.fifo_overrun as u8) << 4) + | ((config.fifo_full as u8) << 5) + | ((config.counter_bdr as u8) << 6) + } +} + +impl From for INT1Interrupts { + fn from(value: InterruptConfigBuffer) -> Self { + INT1Interrupts { + data_ready: value & 0x80 != 0, + counter_bdr: value & 0x40 != 0, + fifo_full: value & 0x20 != 0, + fifo_overrun: value & 0x10 != 0, + fifo_threshold: value & 0x08 != 0, + boot: value & 0x04 != 0, + gyro_data_ready: value & 0x02 != 0, + accel_data_ready: value & 0x01 != 0, + } + } +} + +impl From for INT2Interrupts { + fn from(value: InterruptConfigBuffer) -> Self { + INT2Interrupts { + counter_bdr: value & 0x40 != 0, + fifo_full: value & 0x20 != 0, + fifo_overrun: value & 0x10 != 0, + fifo_threshold: value & 0x08 != 0, + temperature_data_ready: value & 0x04 != 0, + gyro_data_ready: value & 0x02 != 0, + accel_data_ready: value & 0x01 != 0, + } + } +} diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs index 2badf406..a71110f3 100644 --- a/utils/lsm6dso/src/config/mod.rs +++ b/utils/lsm6dso/src/config/mod.rs @@ -1 +1,3 @@ pub mod fifo; +pub mod interrupts; +pub(crate) mod templates; diff --git a/utils/lsm6dso/src/config/templates.rs b/utils/lsm6dso/src/config/templates.rs new file mode 100644 index 00000000..a4622178 --- /dev/null +++ b/utils/lsm6dso/src/config/templates.rs @@ -0,0 +1,31 @@ +/// Macro creating an bitfield enum with methods for converting it from/to register value. +macro_rules! register_enum { + ($name:ident [mask = $mask:literal, offset = $offset:literal] { $( $(#[doc = $doc:expr])? $variant_name:ident = $variant_value:literal,)+ }) => { + #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, derive_more::TryFrom)] + #[try_from(repr)] + #[repr(u8)] + pub enum $name { + $( + $(#[doc = $doc])? + $variant_name = $variant_value, + )+ + } + + impl crate::registers::RegisterField for $name { + const MASK: u8 = $mask; + const OFFSET: usize = $offset; + } + + impl crate::registers::RegisterConversion for $name { + fn to_reg(self) -> u8 { + (self as u8) << Self::OFFSET + } + + fn from_reg(reg: u8) -> Self { + ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() + } + } + }; +} + +pub(crate) use register_enum; diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 200d6522..7acec53c 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -8,14 +8,18 @@ //! When SAMV71 HAL will support [`SpiDevice`](embedded_hal::spi::SpiDevice), this library should //! be refactored to use it, instead of the whole bus. +extern crate derive_more; extern crate embedded_hal; extern crate paste; mod bounded_int; pub mod config; -mod registers; +pub(crate) mod registers; -use config::fifo::{FifoConfig, FifoConfigBuffer}; +use config::{ + fifo::{FifoConfig, FifoConfigBuffer}, + interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, +}; use registers::{Register, WHO_AM_I_VALUE}; pub use embedded_hal::spi::SpiBus; @@ -83,6 +87,26 @@ impl LSM6DSO { Ok(buffer.into()) } + pub fn set_int1_interrupts(&mut self, interrupts: INT1Interrupts) -> Result<(), SPI::Error> { + let config_reg: InterruptConfigBuffer = interrupts.into(); + self._write_register(Register::INT1_CTRL, config_reg)?; + Ok(()) + } + + pub fn get_int1_interrupts(&mut self) -> Result { + Ok(self.read_register(Register::INT1_CTRL)?.into()) + } + + pub fn set_int2_interrupts(&mut self, interrupts: INT2Interrupts) -> Result<(), SPI::Error> { + let config_reg: InterruptConfigBuffer = interrupts.into(); + self._write_register(Register::INT2_CTRL, config_reg)?; + Ok(()) + } + + pub fn get_int2_interrupts(&mut self) -> Result { + Ok(self.read_register(Register::INT2_CTRL)?.into()) + } + /// Reads the value from a single LSM6DSO register and returns it. fn read_register(&mut self, register: Register) -> Result { let mut data_buffer = [READ_REQUEST_MASK | (register as u8), 0]; From 49e8e6ee31cc8933108c9a08ce5716c7efe5c670 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 13:15:53 +0100 Subject: [PATCH 07/23] LSM6DSO: Restored correct items privacy and added accel/gyro config --- utils/lsm6dso/src/config/control.rs | 116 ++++++++++++++++++++++++++ utils/lsm6dso/src/config/fifo.rs | 14 ++-- utils/lsm6dso/src/config/mod.rs | 1 + utils/lsm6dso/src/config/templates.rs | 2 + utils/lsm6dso/src/lib.rs | 57 +++++++++++-- utils/lsm6dso/src/registers.rs | 2 +- 6 files changed, 178 insertions(+), 14 deletions(-) diff --git a/utils/lsm6dso/src/config/control.rs b/utils/lsm6dso/src/config/control.rs index e69de29b..4e2527d8 100644 --- a/utils/lsm6dso/src/config/control.rs +++ b/utils/lsm6dso/src/config/control.rs @@ -0,0 +1,116 @@ +use crate::{config::templates::register_enum, registers::RegisterConversion}; + +register_enum!(AccelerometerDataRate [mask=0xF0, offset=4] { + PowerDown = 0b0000, + /// This is 1.6Hz only in low-power mode. Otherwise, it's 12.5Hz. + Rate1_6Hz = 0b1011, + Rate12_5Hz = 0b0001, + Rate26Hz = 0b0010, + Rate52Hz = 0b0011, + Rate104Hz = 0b0100, + Rate208Hz = 0b0101, + Rate416Hz = 0b0110, + Rate833Hz = 0b0111, + Rate1667Hz = 0b1000, + Rate3333Hz = 0b1001, + Rate6667Hz = 0b1010, +}); + +register_enum!(AccelerometerScale [mask=0x0C, offset=2] { + Scale2g = 0b00, + /// This is 16g only when full-scale mode is active. Otherwise, it's 2g. + Scale16g = 0b01, + Scale4g = 0b10, + Scale8g = 0b11, +}); + +register_enum!(AccelerometerOutputSelection [mask=0x02, offset=1] { + FirstStageFilter = 0, + LPF2SecondFilter = 1, +}); + +register_enum!(GyroscopeDataRate [mask=0xF0, offset=4] { + PowerDown = 0b0000, + Rate12_5Hz = 0b0001, + Rate26Hz = 0b0010, + Rate52Hz = 0b0011, + Rate104Hz = 0b0100, + Rate208Hz = 0b0101, + Rate416Hz = 0b0110, + Rate833Hz = 0b0111, + Rate1667Hz = 0b1000, + Rate3333Hz = 0b1001, + Rate6667Hz = 0b1010, +}); + +register_enum!(GyroscopeScale [mask=0x0E, offset=1] { + Scale125dps = 0b001, + Scale250dps = 0b000, + Scale500dps = 0b010, + Scale1000dps = 0b100, + Scale2000dps = 0b110, +}); + +register_enum!(InterruptPinMode [mask=0x20, offset=5] { + PushPull = 0, + OpenDrain = 1, +}); + +register_enum!(GyroscopeTestMode [mask=0x0C, offset=2] { + Normal = 0b00, + Positive = 0b01, + Negative = 0b11, +}); + +register_enum!(AccelerometerTestMode [mask=0x03, offset=0] { + Normal = 0b00, + Positive = 0b01, + Negative = 0b10, +}); + +pub type AccelerometerConfigBuffer = u8; +pub type GyroscopeConfigBuffer = u8; + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct AccelerometerConfig { + pub data_rate: AccelerometerDataRate, + pub scale: AccelerometerScale, + pub output_selection: AccelerometerOutputSelection, +} + +impl From for AccelerometerConfigBuffer { + fn from(config: AccelerometerConfig) -> Self { + config.output_selection.to_reg() | config.scale.to_reg() | config.data_rate.to_reg() + } +} + +impl From for AccelerometerConfig { + fn from(value: AccelerometerConfigBuffer) -> Self { + AccelerometerConfig { + data_rate: AccelerometerDataRate::from_reg(value), + scale: AccelerometerScale::from_reg(value), + output_selection: AccelerometerOutputSelection::from_reg(value), + } + } +} + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct GyroscopeConfig { + pub data_rate: GyroscopeDataRate, + pub scale: GyroscopeScale, +} + +impl From for GyroscopeConfigBuffer { + fn from(config: GyroscopeConfig) -> Self { + config.scale.to_reg() | config.data_rate.to_reg() + } +} + +impl From for GyroscopeConfig { + fn from(value: GyroscopeConfigBuffer) -> Self { + GyroscopeConfig { + data_rate: GyroscopeDataRate::from_reg(value), + scale: GyroscopeScale::from_reg(value), + } + } +} diff --git a/utils/lsm6dso/src/config/fifo.rs b/utils/lsm6dso/src/config/fifo.rs index f8afda3f..65429825 100644 --- a/utils/lsm6dso/src/config/fifo.rs +++ b/utils/lsm6dso/src/config/fifo.rs @@ -1,7 +1,7 @@ use crate::{ bounded_int::BoundedU16, - config::templates, - registers::{MultiRegisterConversion, MultiRegisterField, RegisterConversion, RegisterField}, + config::templates::register_enum, + registers::{MultiRegisterConversion, MultiRegisterField, RegisterConversion}, }; /// Type representing FIFO watermark threshold as FIFO records (6 bytes of sensor data + tag) @@ -44,17 +44,17 @@ impl MultiRegisterConversion for FifoDataLength { } } -templates::register_enum!(DataRateChangeBatching [mask=0x10, offset=4] { +register_enum!(DataRateChangeBatching [mask=0x10, offset=4] { Disabled = 0, Enabled = 1, }); -templates::register_enum!(StopOnWatermarkThreshold [mask=0x80, offset=7]{ +register_enum!(StopOnWatermarkThreshold [mask=0x80, offset=7]{ Yes = 1, No = 0, }); -templates::register_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { +register_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { NoBatching = 0b0000, Batch12_5Hz = 0b0001, Batch26Hz = 0b0010, @@ -69,7 +69,7 @@ templates::register_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { Batch6_5Hz = 0b1011, }); -templates::register_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { +register_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { NoBatching = 0b0000, Batch12_5Hz = 0b0001, Batch26Hz = 0b0010, @@ -84,7 +84,7 @@ templates::register_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { Batch1_6Hz = 0b1011, }); -templates::register_enum!(FifoMode [mask=0x07, offset=0] { +register_enum!(FifoMode [mask=0x07, offset=0] { /// FIFO disabled. Bypass = 0b000, /// FIFO enabled and sensor stops collecting data when FIFO is full. diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs index a71110f3..02ad31be 100644 --- a/utils/lsm6dso/src/config/mod.rs +++ b/utils/lsm6dso/src/config/mod.rs @@ -1,3 +1,4 @@ +pub mod control; pub mod fifo; pub mod interrupts; pub(crate) mod templates; diff --git a/utils/lsm6dso/src/config/templates.rs b/utils/lsm6dso/src/config/templates.rs index a4622178..3b19e695 100644 --- a/utils/lsm6dso/src/config/templates.rs +++ b/utils/lsm6dso/src/config/templates.rs @@ -18,10 +18,12 @@ macro_rules! register_enum { impl crate::registers::RegisterConversion for $name { fn to_reg(self) -> u8 { + use crate::registers::RegisterField; (self as u8) << Self::OFFSET } fn from_reg(reg: u8) -> Self { + use crate::registers::RegisterField; ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() } } diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 7acec53c..413991ae 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -17,6 +17,9 @@ pub mod config; pub(crate) mod registers; use config::{ + control::{ + AccelerometerConfig, AccelerometerConfigBuffer, GyroscopeConfig, GyroscopeConfigBuffer, + }, fifo::{FifoConfig, FifoConfigBuffer}, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; @@ -89,7 +92,7 @@ impl LSM6DSO { pub fn set_int1_interrupts(&mut self, interrupts: INT1Interrupts) -> Result<(), SPI::Error> { let config_reg: InterruptConfigBuffer = interrupts.into(); - self._write_register(Register::INT1_CTRL, config_reg)?; + self.write_register(Register::INT1_CTRL, config_reg)?; Ok(()) } @@ -99,7 +102,7 @@ impl LSM6DSO { pub fn set_int2_interrupts(&mut self, interrupts: INT2Interrupts) -> Result<(), SPI::Error> { let config_reg: InterruptConfigBuffer = interrupts.into(); - self._write_register(Register::INT2_CTRL, config_reg)?; + self.write_register(Register::INT2_CTRL, config_reg)?; Ok(()) } @@ -107,6 +110,47 @@ impl LSM6DSO { Ok(self.read_register(Register::INT2_CTRL)?.into()) } + pub fn set_accelerometer_config( + &mut self, + config: AccelerometerConfig, + ) -> Result<(), SPI::Error> { + let config_reg: AccelerometerConfigBuffer = config.into(); + self.write_register(Register::CTRL1_XL, config_reg)?; + Ok(()) + } + + pub fn get_accelerometer_config(&mut self) -> Result { + Ok(self.read_register(Register::CTRL1_XL)?.into()) + } + + pub fn set_gyroscope_config(&mut self, config: GyroscopeConfig) -> Result<(), SPI::Error> { + let config_reg: GyroscopeConfigBuffer = config.into(); + self.write_register(Register::CTRL2_G, config_reg)?; + Ok(()) + } + + pub fn get_gyroscope_config(&mut self) -> Result { + Ok(self.read_register(Register::CTRL2_G)?.into()) + } + + pub fn reboot_memory_content(&mut self) -> Result<(), SPI::Error> { + const REBOOT_MEMORY_BIT_MASK: u8 = 0x80; + let ctrl_reg = self.read_register(Register::CTRL3_C)? | REBOOT_MEMORY_BIT_MASK; + self.write_register(Register::CTRL3_C, ctrl_reg)?; + Ok(()) + } + + pub fn software_reset(&mut self) -> Result<(), SPI::Error> { + const RESET_BIT_MASH: u8 = 0x01; + let ctrl_reg = self.read_register(Register::CTRL3_C)? | RESET_BIT_MASH; + self.write_register(Register::CTRL3_C, ctrl_reg)?; + Ok(()) + } + + pub fn get_reg(&mut self) -> u8 { + self.read_register(Register::CTRL1_XL).unwrap() + } + /// Reads the value from a single LSM6DSO register and returns it. fn read_register(&mut self, register: Register) -> Result { let mut data_buffer = [READ_REQUEST_MASK | (register as u8), 0]; @@ -115,9 +159,9 @@ impl LSM6DSO { } /// Writes a value to LSM6DSO register. - fn _write_register(&mut self, register: Register, value: u8) -> Result<(), SPI::Error> { - let write_request = [register as u8, value]; - self.spi.write(&write_request)?; + fn write_register(&mut self, register: Register, value: u8) -> Result<(), SPI::Error> { + let mut write_request = [register as u8, value]; + self.spi.transfer_in_place(&mut write_request)?; Ok(()) } @@ -168,7 +212,8 @@ impl LSM6DSO { self.buffer[0] = first_register as u8; self.buffer[1..=user_buffer_length].copy_from_slice(values); // Write data - self.spi.write(&self.buffer[0..=user_buffer_length])?; + self.spi + .transfer_in_place(&mut self.buffer[0..=user_buffer_length])?; Ok(()) } } diff --git a/utils/lsm6dso/src/registers.rs b/utils/lsm6dso/src/registers.rs index c2370af1..1cc49148 100644 --- a/utils/lsm6dso/src/registers.rs +++ b/utils/lsm6dso/src/registers.rs @@ -120,7 +120,7 @@ where fn from_reg(reg: u8) -> Self; } -pub trait MultiRegisterConversion +pub(crate) trait MultiRegisterConversion where Self: Copy + MultiRegisterField, { From 4fddba1546f9e5e98232d0603d22fdcebcab6c8b Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 13:16:58 +0100 Subject: [PATCH 08/23] LSM6DSO: Example updated --- examples/samv71-lsm6dso/src/main.rs | 41 ++++++++++++++++++----------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/examples/samv71-lsm6dso/src/main.rs b/examples/samv71-lsm6dso/src/main.rs index 70663e55..f56c3e46 100644 --- a/examples/samv71-lsm6dso/src/main.rs +++ b/examples/samv71-lsm6dso/src/main.rs @@ -11,9 +11,9 @@ use aerugo::{ logln, Aerugo, InitApi, SystemHardwareConfig, }; use lsm6dso::{ - config::fifo::{ - AccelerometerBatchingRate, DataRateChangeBatching, FifoConfig, FifoMode, - FifoWatermarkThreshold, GyroscopeBatchingRate, StopOnWatermarkThreshold, + config::control::{ + AccelerometerConfig, AccelerometerDataRate, AccelerometerOutputSelection, + AccelerometerScale, }, LSM6DSO, }; @@ -39,20 +39,29 @@ fn main() -> ! { logln!("LSM id: {:2X?}", lsm.id()); logln!("Is LSM alive? {:?}", lsm.is_alive()); - logln!("Current LSM config: {:#?}", lsm.get_fifo_config()); - - let test_config = FifoConfig { - watermark_threshold: FifoWatermarkThreshold::new(123).unwrap(), - odr_change_batched: DataRateChangeBatching::Enabled, - stop_on_watermark: StopOnWatermarkThreshold::Yes, - gyroscope_batching_rate: GyroscopeBatchingRate::Batch26Hz, - accelerometer_batching_rate: AccelerometerBatchingRate::Batch417Hz, - mode: FifoMode::Fifo, + logln!("Pre-reboot LSM config: {:#?}", lsm.get_fifo_config()); + lsm.software_reset().unwrap(); + lsm.reboot_memory_content().unwrap(); + + let accel_cfg_a = AccelerometerConfig { + data_rate: AccelerometerDataRate::Rate208Hz, + scale: AccelerometerScale::Scale4g, + output_selection: AccelerometerOutputSelection::FirstStageFilter, + }; + let accel_cfg_b = AccelerometerConfig { + data_rate: AccelerometerDataRate::Rate3333Hz, + scale: AccelerometerScale::Scale8g, + output_selection: AccelerometerOutputSelection::LPF2SecondFilter, }; - lsm.set_fifo_config(test_config).unwrap(); - let read_config = lsm.get_fifo_config(); - logln!("New LSM config: {:#?}", read_config); - assert_eq!(read_config.unwrap(), test_config); + + logln!("Original config: {:#?}", lsm.get_accelerometer_config()); + logln!("Register value: {:#02X?}", lsm.get_reg()); + lsm.set_accelerometer_config(accel_cfg_a).unwrap(); + logln!("New config A: {:#?}", lsm.get_accelerometer_config()); + logln!("Register value: {:#02X?}", lsm.get_reg()); + lsm.set_accelerometer_config(accel_cfg_b).unwrap(); + logln!("New config B: {:#?}", lsm.get_accelerometer_config()); + logln!("Register value: {:#02X?}", lsm.get_reg()); aerugo.start(); } From 6c992d1dde2557d6df52eaebaf2932e0f71489b9 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 13:57:09 +0100 Subject: [PATCH 09/23] LSM6DSO: Added remaining basic config functions --- utils/lsm6dso/src/config/control.rs | 20 +++++- utils/lsm6dso/src/config/templates.rs | 5 ++ utils/lsm6dso/src/lib.rs | 88 ++++++++++++++++++++++++--- utils/lsm6dso/src/registers.rs | 3 + 4 files changed, 105 insertions(+), 11 deletions(-) diff --git a/utils/lsm6dso/src/config/control.rs b/utils/lsm6dso/src/config/control.rs index 4e2527d8..f2000fe2 100644 --- a/utils/lsm6dso/src/config/control.rs +++ b/utils/lsm6dso/src/config/control.rs @@ -51,11 +51,29 @@ register_enum!(GyroscopeScale [mask=0x0E, offset=1] { Scale2000dps = 0b110, }); -register_enum!(InterruptPinMode [mask=0x20, offset=5] { +register_enum!(RebootMemoryContent [mask=0x80, offset=7] { + Yes = 1, +}); + +register_enum!(IrqActivationLevel [mask=0x20, offset=5] { + ActiveHigh = 0, + ActiveLow = 1, +}); + +register_enum!(IrqPinMode [mask=0x20, offset=5] { PushPull = 0, OpenDrain = 1, }); +register_enum!(SoftwareReset [mask=0x01, offset=0] { + Yes = 1, +}); + +register_enum!(DataReadyState [mask=0x08, offset=3] { + Disabled = 0, + Enabled = 1, +}); + register_enum!(GyroscopeTestMode [mask=0x0C, offset=2] { Normal = 0b00, Positive = 0b01, diff --git a/utils/lsm6dso/src/config/templates.rs b/utils/lsm6dso/src/config/templates.rs index 3b19e695..365eb00d 100644 --- a/utils/lsm6dso/src/config/templates.rs +++ b/utils/lsm6dso/src/config/templates.rs @@ -26,6 +26,11 @@ macro_rules! register_enum { use crate::registers::RegisterField; ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() } + + fn apply_to_reg(self, reg: u8) -> u8 { + use crate::registers::RegisterField; + (reg & !Self::MASK) | self.to_reg() + } } }; } diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 413991ae..5b32e5c3 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -18,15 +18,19 @@ pub(crate) mod registers; use config::{ control::{ - AccelerometerConfig, AccelerometerConfigBuffer, GyroscopeConfig, GyroscopeConfigBuffer, + AccelerometerConfig, AccelerometerConfigBuffer, AccelerometerTestMode, GyroscopeConfig, + GyroscopeConfigBuffer, GyroscopeTestMode, IrqActivationLevel, IrqPinMode, + RebootMemoryContent, SoftwareReset, }, fifo::{FifoConfig, FifoConfigBuffer}, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; -use registers::{Register, WHO_AM_I_VALUE}; +use registers::{Register, RegisterConversion, WHO_AM_I_VALUE}; pub use embedded_hal::spi::SpiBus; +use crate::config::control::DataReadyState; + /// LSM6DSO driver structure. /// /// # Generic parameters @@ -134,21 +138,85 @@ impl LSM6DSO { } pub fn reboot_memory_content(&mut self) -> Result<(), SPI::Error> { - const REBOOT_MEMORY_BIT_MASK: u8 = 0x80; - let ctrl_reg = self.read_register(Register::CTRL3_C)? | REBOOT_MEMORY_BIT_MASK; - self.write_register(Register::CTRL3_C, ctrl_reg)?; + let ctrl_reg = self.read_register(Register::CTRL3_C)?; + self.write_register( + Register::CTRL3_C, + RebootMemoryContent::Yes.apply_to_reg(ctrl_reg), + )?; + Ok(()) + } + + pub fn set_irq_activation_level( + &mut self, + activation_level: IrqActivationLevel, + ) -> Result<(), SPI::Error> { + let ctrl_reg = self.read_register(Register::CTRL3_C)?; + self.write_register(Register::CTRL3_C, activation_level.apply_to_reg(ctrl_reg))?; Ok(()) } + pub fn get_irq_activation_level(&mut self) -> Result { + Ok(IrqActivationLevel::from_reg( + self.read_register(Register::CTRL3_C)?, + )) + } + + pub fn set_irq_pin_mode(&mut self, pin_mode: IrqPinMode) -> Result<(), SPI::Error> { + let ctrl_reg = self.read_register(Register::CTRL3_C)?; + self.write_register(Register::CTRL3_C, pin_mode.apply_to_reg(ctrl_reg))?; + Ok(()) + } + + pub fn get_irq_pin_mode(&mut self) -> Result { + Ok(IrqPinMode::from_reg(self.read_register(Register::CTRL3_C)?)) + } + pub fn software_reset(&mut self) -> Result<(), SPI::Error> { - const RESET_BIT_MASH: u8 = 0x01; - let ctrl_reg = self.read_register(Register::CTRL3_C)? | RESET_BIT_MASH; - self.write_register(Register::CTRL3_C, ctrl_reg)?; + let ctrl_reg = self.read_register(Register::CTRL3_C)?; + self.write_register(Register::CTRL3_C, SoftwareReset::Yes.apply_to_reg(ctrl_reg))?; + Ok(()) + } + + pub fn set_data_ready_state(&mut self, state: DataReadyState) -> Result<(), SPI::Error> { + let ctrl_reg = self.read_register(Register::CTRL4_C)?; + self.write_register(Register::CTRL4_C, state.apply_to_reg(ctrl_reg))?; + Ok(()) + } + + pub fn get_data_ready_state(&mut self) -> Result { + Ok(DataReadyState::from_reg( + self.read_register(Register::CTRL4_C)?, + )) + } + + pub fn set_accelerometer_test_mode( + &mut self, + test_mode: AccelerometerTestMode, + ) -> Result<(), SPI::Error> { + let ctrl_reg = self.read_register(Register::CTRL5_C)?; + self.write_register(Register::CTRL5_C, test_mode.apply_to_reg(ctrl_reg))?; + Ok(()) + } + + pub fn get_accelerometer_test_mode(&mut self) -> Result { + Ok(AccelerometerTestMode::from_reg( + self.read_register(Register::CTRL5_C)?, + )) + } + + pub fn set_gyroscope_test_mode( + &mut self, + test_mode: GyroscopeTestMode, + ) -> Result<(), SPI::Error> { + let ctrl_reg = self.read_register(Register::CTRL5_C)?; + self.write_register(Register::CTRL5_C, test_mode.apply_to_reg(ctrl_reg))?; Ok(()) } - pub fn get_reg(&mut self) -> u8 { - self.read_register(Register::CTRL1_XL).unwrap() + pub fn get_gyroscope_test_mode(&mut self) -> Result { + Ok(GyroscopeTestMode::from_reg( + self.read_register(Register::CTRL5_C)?, + )) } /// Reads the value from a single LSM6DSO register and returns it. diff --git a/utils/lsm6dso/src/registers.rs b/utils/lsm6dso/src/registers.rs index 1cc49148..d904ac54 100644 --- a/utils/lsm6dso/src/registers.rs +++ b/utils/lsm6dso/src/registers.rs @@ -118,6 +118,9 @@ where /// This function should extract the field's value from the register and return it. fn from_reg(reg: u8) -> Self; + + /// This function modifies existing register's bits and returns it's new value with applied field. + fn apply_to_reg(self, reg: u8) -> u8; } pub(crate) trait MultiRegisterConversion From 324e3621372d2906347fc77a53742ae22819d6ed Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 14:46:16 +0100 Subject: [PATCH 10/23] LSM6DSO: Added data types and status, renamed fifo to fifo_config --- utils/lsm6dso/src/config/control.rs | 5 ++- utils/lsm6dso/src/config/data_types.rs | 32 +++++++++++++++++++ .../src/config/{fifo.rs => fifo_config.rs} | 10 ++++-- utils/lsm6dso/src/config/interrupts.rs | 2 ++ utils/lsm6dso/src/config/mod.rs | 4 ++- utils/lsm6dso/src/config/status.rs | 14 ++++++++ utils/lsm6dso/src/config/templates.rs | 10 ++++-- utils/lsm6dso/src/lib.rs | 4 +-- utils/lsm6dso/src/registers.rs | 19 +++++++++-- 9 files changed, 89 insertions(+), 11 deletions(-) create mode 100644 utils/lsm6dso/src/config/data_types.rs rename utils/lsm6dso/src/config/{fifo.rs => fifo_config.rs} (94%) create mode 100644 utils/lsm6dso/src/config/status.rs diff --git a/utils/lsm6dso/src/config/control.rs b/utils/lsm6dso/src/config/control.rs index f2000fe2..4f2a2a17 100644 --- a/utils/lsm6dso/src/config/control.rs +++ b/utils/lsm6dso/src/config/control.rs @@ -1,4 +1,7 @@ -use crate::{config::templates::register_enum, registers::RegisterConversion}; +use crate::{ + config::templates::register_enum, + registers::{FromRegister, ToRegister}, +}; register_enum!(AccelerometerDataRate [mask=0xF0, offset=4] { PowerDown = 0b0000, diff --git a/utils/lsm6dso/src/config/data_types.rs b/utils/lsm6dso/src/config/data_types.rs new file mode 100644 index 00000000..d13b2cc1 --- /dev/null +++ b/utils/lsm6dso/src/config/data_types.rs @@ -0,0 +1,32 @@ +pub struct Vec3D { + pub x: T, + pub y: T, + pub z: T, +} + +pub type Temperature = i16; +pub type AccelerometerData = Vec3D; +pub type GyroscopeData = Vec3D; + +pub(crate) trait FromBuffer { + fn from_buffer(buffer: &[u8; DATA_LENGTH]) -> Self; +} + +impl FromBuffer<2> for Temperature { + fn from_buffer(buffer: &[u8; 2]) -> Self { + Self::from_le_bytes(*buffer) + } +} + +impl FromBuffer<6> for AccelerometerData { + fn from_buffer(buffer: &[u8; 6]) -> Self { + Self { + x: i16::from_le_bytes([buffer[0], buffer[1]]), + y: i16::from_le_bytes([buffer[2], buffer[3]]), + z: i16::from_le_bytes([buffer[4], buffer[5]]), + } + } +} + +// GyroscopeData implementation is not necessary, as it's the same as AccelerometerData. +// If underlying type ever changes, remember to add the conversion. diff --git a/utils/lsm6dso/src/config/fifo.rs b/utils/lsm6dso/src/config/fifo_config.rs similarity index 94% rename from utils/lsm6dso/src/config/fifo.rs rename to utils/lsm6dso/src/config/fifo_config.rs index 65429825..b19759ec 100644 --- a/utils/lsm6dso/src/config/fifo.rs +++ b/utils/lsm6dso/src/config/fifo_config.rs @@ -1,7 +1,7 @@ use crate::{ bounded_int::BoundedU16, config::templates::register_enum, - registers::{MultiRegisterConversion, MultiRegisterField, RegisterConversion}, + registers::{FromMultiRegister, FromRegister, MultiRegisterField, ToMultiRegister, ToRegister}, }; /// Type representing FIFO watermark threshold as FIFO records (6 bytes of sensor data + tag) @@ -11,11 +11,13 @@ impl MultiRegisterField for FifoWatermarkThreshold { const OFFSETS: [usize; 2] = [0, 0]; } -impl MultiRegisterConversion for FifoWatermarkThreshold { +impl ToMultiRegister for FifoWatermarkThreshold { fn to_regs(self) -> [u8; 2] { self.to_le_bytes() } +} +impl FromMultiRegister for FifoWatermarkThreshold { fn from_regs(regs: &[u8]) -> Self { assert!(regs.len() >= 2); let value_lsb = regs[0]; @@ -31,11 +33,13 @@ impl MultiRegisterField for FifoDataLength { const OFFSETS: [usize; 2] = [0, 0]; } -impl MultiRegisterConversion for FifoDataLength { +impl ToMultiRegister for FifoDataLength { fn to_regs(self) -> [u8; 2] { self.to_le_bytes() } +} +impl FromMultiRegister for FifoDataLength { fn from_regs(regs: &[u8]) -> Self { assert!(regs.len() >= 2); let value_lsb = regs[0]; diff --git a/utils/lsm6dso/src/config/interrupts.rs b/utils/lsm6dso/src/config/interrupts.rs index 593546f0..9e79def9 100644 --- a/utils/lsm6dso/src/config/interrupts.rs +++ b/utils/lsm6dso/src/config/interrupts.rs @@ -1,3 +1,4 @@ +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] pub struct INT1Interrupts { pub data_ready: bool, pub counter_bdr: bool, @@ -9,6 +10,7 @@ pub struct INT1Interrupts { pub accel_data_ready: bool, } +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] pub struct INT2Interrupts { pub counter_bdr: bool, pub fifo_full: bool, diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs index 02ad31be..607aea59 100644 --- a/utils/lsm6dso/src/config/mod.rs +++ b/utils/lsm6dso/src/config/mod.rs @@ -1,4 +1,6 @@ pub mod control; -pub mod fifo; +pub mod data_types; +pub mod fifo_config; pub mod interrupts; +pub mod status; pub(crate) mod templates; diff --git a/utils/lsm6dso/src/config/status.rs b/utils/lsm6dso/src/config/status.rs new file mode 100644 index 00000000..2b04e53b --- /dev/null +++ b/utils/lsm6dso/src/config/status.rs @@ -0,0 +1,14 @@ +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct Status { + pub new_temperature_available: bool, + pub new_gyroscope_data_available: bool, + pub new_accelerometer_data_available: bool, +} + +impl From for u8 { + fn from(value: Status) -> Self { + ((value.new_temperature_available as u8) << 2) + | ((value.new_gyroscope_data_available as u8) << 1) + | (value.new_accelerometer_data_available as u8) + } +} diff --git a/utils/lsm6dso/src/config/templates.rs b/utils/lsm6dso/src/config/templates.rs index 365eb00d..5aa2df22 100644 --- a/utils/lsm6dso/src/config/templates.rs +++ b/utils/lsm6dso/src/config/templates.rs @@ -16,19 +16,25 @@ macro_rules! register_enum { const OFFSET: usize = $offset; } - impl crate::registers::RegisterConversion for $name { + impl crate::registers::ToRegister for $name { fn to_reg(self) -> u8 { use crate::registers::RegisterField; (self as u8) << Self::OFFSET } + } + impl crate::registers::FromRegister for $name { fn from_reg(reg: u8) -> Self { use crate::registers::RegisterField; ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() } + } + impl crate::registers::ApplyToRegister for $name + where Self: crate::registers::ToRegister + { fn apply_to_reg(self, reg: u8) -> u8 { - use crate::registers::RegisterField; + use crate::registers::{RegisterField, ToRegister}; (reg & !Self::MASK) | self.to_reg() } } diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 5b32e5c3..1eea25ab 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -22,10 +22,10 @@ use config::{ GyroscopeConfigBuffer, GyroscopeTestMode, IrqActivationLevel, IrqPinMode, RebootMemoryContent, SoftwareReset, }, - fifo::{FifoConfig, FifoConfigBuffer}, + fifo_config::{FifoConfig, FifoConfigBuffer}, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; -use registers::{Register, RegisterConversion, WHO_AM_I_VALUE}; +use registers::{ApplyToRegister, FromRegister, Register, WHO_AM_I_VALUE}; pub use embedded_hal::spi::SpiBus; diff --git a/utils/lsm6dso/src/registers.rs b/utils/lsm6dso/src/registers.rs index d904ac54..4450ce44 100644 --- a/utils/lsm6dso/src/registers.rs +++ b/utils/lsm6dso/src/registers.rs @@ -108,29 +108,44 @@ where const OFFSETS: [usize; REGISTER_SPAN]; } -pub(crate) trait RegisterConversion +pub(crate) trait ToRegister where Self: Copy + RegisterField, { /// This function should return the value of current register field that can be OR'd with /// register's content to set it. fn to_reg(self) -> u8; +} +pub(crate) trait FromRegister +where + Self: Copy + RegisterField, +{ /// This function should extract the field's value from the register and return it. fn from_reg(reg: u8) -> Self; +} +pub(crate) trait ApplyToRegister +where + Self: Copy + RegisterField, +{ /// This function modifies existing register's bits and returns it's new value with applied field. fn apply_to_reg(self, reg: u8) -> u8; } -pub(crate) trait MultiRegisterConversion +pub(crate) trait ToMultiRegister where Self: Copy + MultiRegisterField, { /// This function should return the value of provided registers as an array. Unused bits should /// remain 0. fn to_regs(self) -> [u8; REGISTER_SPAN]; +} +pub(crate) trait FromMultiRegister +where + Self: Copy + MultiRegisterField, +{ /// This function should extract the field's value from the register and return it. fn from_regs(regs: &[u8]) -> Self; } From 5ec2a0731b9d9eda37dbe737ee5b03a298cb36b2 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 14:53:07 +0100 Subject: [PATCH 11/23] LSM6DSO: Added raw data reading methods --- utils/lsm6dso/src/config/data_types.rs | 6 +++--- utils/lsm6dso/src/lib.rs | 19 +++++++++++++++++++ 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/utils/lsm6dso/src/config/data_types.rs b/utils/lsm6dso/src/config/data_types.rs index d13b2cc1..256ceca0 100644 --- a/utils/lsm6dso/src/config/data_types.rs +++ b/utils/lsm6dso/src/config/data_types.rs @@ -5,8 +5,8 @@ pub struct Vec3D { } pub type Temperature = i16; -pub type AccelerometerData = Vec3D; -pub type GyroscopeData = Vec3D; +pub type LinearAcceleration = Vec3D; +pub type AngularRate = Vec3D; pub(crate) trait FromBuffer { fn from_buffer(buffer: &[u8; DATA_LENGTH]) -> Self; @@ -18,7 +18,7 @@ impl FromBuffer<2> for Temperature { } } -impl FromBuffer<6> for AccelerometerData { +impl FromBuffer<6> for LinearAcceleration { fn from_buffer(buffer: &[u8; 6]) -> Self { Self { x: i16::from_le_bytes([buffer[0], buffer[1]]), diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 1eea25ab..6ae9714d 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -22,6 +22,7 @@ use config::{ GyroscopeConfigBuffer, GyroscopeTestMode, IrqActivationLevel, IrqPinMode, RebootMemoryContent, SoftwareReset, }, + data_types::{AngularRate, FromBuffer, LinearAcceleration, Temperature}, fifo_config::{FifoConfig, FifoConfigBuffer}, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; @@ -219,6 +220,24 @@ impl LSM6DSO { )) } + pub fn get_temperature(&mut self) -> Result { + let mut data_buffer = [0u8; 2]; + self.read_registers(Register::OUT_TEMP_L, &mut data_buffer)?; + Ok(Temperature::from_buffer(&data_buffer)) + } + + pub fn get_angular_rate(&mut self) -> Result { + let mut data_buffer = [0u8; 6]; + self.read_registers(Register::OUTX_L_G, &mut data_buffer)?; + Ok(AngularRate::from_buffer(&data_buffer)) + } + + pub fn get_linear_acceleration(&mut self) -> Result { + let mut data_buffer = [0u8; 6]; + self.read_registers(Register::OUTX_L_A, &mut data_buffer)?; + Ok(AngularRate::from_buffer(&data_buffer)) + } + /// Reads the value from a single LSM6DSO register and returns it. fn read_register(&mut self, register: Register) -> Result { let mut data_buffer = [READ_REQUEST_MASK | (register as u8), 0]; From f77fec3295794c4433be7ba8c1f0316d09bfe6c6 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 15:14:54 +0100 Subject: [PATCH 12/23] LSM6DSO: Added fifo status and timestamp data types --- utils/lsm6dso/src/config/data_types.rs | 10 ++++- utils/lsm6dso/src/config/fifo_status.rs | 53 +++++++++++++++++++++++++ utils/lsm6dso/src/config/mod.rs | 1 + 3 files changed, 62 insertions(+), 2 deletions(-) create mode 100644 utils/lsm6dso/src/config/fifo_status.rs diff --git a/utils/lsm6dso/src/config/data_types.rs b/utils/lsm6dso/src/config/data_types.rs index 256ceca0..0e17c8f7 100644 --- a/utils/lsm6dso/src/config/data_types.rs +++ b/utils/lsm6dso/src/config/data_types.rs @@ -7,6 +7,7 @@ pub struct Vec3D { pub type Temperature = i16; pub type LinearAcceleration = Vec3D; pub type AngularRate = Vec3D; +pub type Timestamp = u32; pub(crate) trait FromBuffer { fn from_buffer(buffer: &[u8; DATA_LENGTH]) -> Self; @@ -18,6 +19,8 @@ impl FromBuffer<2> for Temperature { } } +// GyroscopeData implementation is not necessary, as it's the same as AccelerometerData. +// If underlying type ever changes, remember to add the conversion. impl FromBuffer<6> for LinearAcceleration { fn from_buffer(buffer: &[u8; 6]) -> Self { Self { @@ -28,5 +31,8 @@ impl FromBuffer<6> for LinearAcceleration { } } -// GyroscopeData implementation is not necessary, as it's the same as AccelerometerData. -// If underlying type ever changes, remember to add the conversion. +impl FromBuffer<4> for Timestamp { + fn from_buffer(buffer: &[u8; 4]) -> Self { + u32::from_le_bytes(*buffer) + } +} diff --git a/utils/lsm6dso/src/config/fifo_status.rs b/utils/lsm6dso/src/config/fifo_status.rs new file mode 100644 index 00000000..eb09bfc7 --- /dev/null +++ b/utils/lsm6dso/src/config/fifo_status.rs @@ -0,0 +1,53 @@ +use crate::registers::FromRegister; + +use super::templates::register_enum; + +register_enum!(Watermark [mask=0x80, offset=7] { + NotReached = 0, + Reached = 1, +}); + +register_enum!(Overrun [mask=0x40, offset=6] { + FifoNotFull = 0, + FifoFull = 1, +}); + +register_enum!(SmartStatus [mask=0x20, offset=5] { + FifoNotFull = 0, + FifoWillBeFullAtNextODR = 1, +}); + +register_enum!(CounterThreshold [mask=0x10, offset=4] { + NotReached = 0, + Reached = 1, +}); + +register_enum!(LatchedOverrun [mask=0x08, offset=3] { + FifoNotFull = 0, + FifoFull = 1, +}); + +pub struct FifoStatus { + pub stored_words: u16, + pub latched_overrun: LatchedOverrun, + pub counter_threshold: CounterThreshold, + pub smart_fifo: SmartStatus, + pub overrun: Overrun, + pub watermark: Watermark, +} + +pub(crate) type FifoStatusBuffer = [u8; 2]; + +impl From for FifoStatus { + fn from(buffer: FifoStatusBuffer) -> Self { + const STORED_WORDS_MASK: u8 = 0x03; + Self { + stored_words: u16::from_le_bytes([buffer[0], buffer[1] & STORED_WORDS_MASK]), + latched_overrun: LatchedOverrun::from_reg(buffer[1]), + counter_threshold: CounterThreshold::from_reg(buffer[1]), + smart_fifo: SmartStatus::from_reg(buffer[1]), + overrun: Overrun::from_reg(buffer[1]), + watermark: Watermark::from_reg(buffer[1]), + } + } +} diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs index 607aea59..90606b1f 100644 --- a/utils/lsm6dso/src/config/mod.rs +++ b/utils/lsm6dso/src/config/mod.rs @@ -1,6 +1,7 @@ pub mod control; pub mod data_types; pub mod fifo_config; +pub mod fifo_status; pub mod interrupts; pub mod status; pub(crate) mod templates; From c5285f933ae81e62273699d8916c93c85ab40b77 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Sun, 26 Nov 2023 15:23:56 +0100 Subject: [PATCH 13/23] LSM6DSO: Moved fifo-related items to separate module, added fifo data struct --- .../config/{fifo_config.rs => fifo/config.rs} | 0 utils/lsm6dso/src/config/fifo/data.rs | 40 +++++++++++++++++++ utils/lsm6dso/src/config/fifo/mod.rs | 3 ++ .../config/{fifo_status.rs => fifo/status.rs} | 2 +- utils/lsm6dso/src/config/mod.rs | 3 +- utils/lsm6dso/src/lib.rs | 11 ++++- 6 files changed, 55 insertions(+), 4 deletions(-) rename utils/lsm6dso/src/config/{fifo_config.rs => fifo/config.rs} (100%) create mode 100644 utils/lsm6dso/src/config/fifo/data.rs create mode 100644 utils/lsm6dso/src/config/fifo/mod.rs rename utils/lsm6dso/src/config/{fifo_status.rs => fifo/status.rs} (96%) diff --git a/utils/lsm6dso/src/config/fifo_config.rs b/utils/lsm6dso/src/config/fifo/config.rs similarity index 100% rename from utils/lsm6dso/src/config/fifo_config.rs rename to utils/lsm6dso/src/config/fifo/config.rs diff --git a/utils/lsm6dso/src/config/fifo/data.rs b/utils/lsm6dso/src/config/fifo/data.rs new file mode 100644 index 00000000..1ed5c988 --- /dev/null +++ b/utils/lsm6dso/src/config/fifo/data.rs @@ -0,0 +1,40 @@ +use crate::{ + config::{data_types::FromBuffer, templates::register_enum}, + registers::FromRegister, +}; + +register_enum!(Tag [mask=0xF8, offset=3] { + GyroscopeNC = 0x01, + AccelerometerNC = 0x02, + Temperature = 0x03, + Timestamp = 0x04, + ConfigChange = 0x05, + AccelerometerNCT2 = 0x06, + AccelerometerNCT1 = 0x07, + Accelerometer2xC = 0x08, + Accelerometer3xC = 0x09, + GyroscopeNCT2 = 0x0A, + GyroscopeNCT1 = 0x0B, + Gyroscope2xC = 0x0C, + Gyroscope3xC = 0x0D, + SensorHubSlave0 = 0x0E, + SensorHubSlave1 = 0x0F, + SensorHubSlave2 = 0x10, + SensorHubSlave3 = 0x11, + StepCounter = 0x12, + SensorHubNack = 0x19, +}); + +pub struct FifoWord { + pub tag: Tag, + pub data: [u8; 6], +} + +impl FromBuffer<7> for FifoWord { + fn from_buffer(buffer: &[u8; 7]) -> Self { + FifoWord { + tag: Tag::from_reg(buffer[0]), + data: buffer[1..7].try_into().unwrap(), + } + } +} diff --git a/utils/lsm6dso/src/config/fifo/mod.rs b/utils/lsm6dso/src/config/fifo/mod.rs new file mode 100644 index 00000000..36e48d3b --- /dev/null +++ b/utils/lsm6dso/src/config/fifo/mod.rs @@ -0,0 +1,3 @@ +pub mod config; +pub mod data; +pub mod status; diff --git a/utils/lsm6dso/src/config/fifo_status.rs b/utils/lsm6dso/src/config/fifo/status.rs similarity index 96% rename from utils/lsm6dso/src/config/fifo_status.rs rename to utils/lsm6dso/src/config/fifo/status.rs index eb09bfc7..d4e23cfb 100644 --- a/utils/lsm6dso/src/config/fifo_status.rs +++ b/utils/lsm6dso/src/config/fifo/status.rs @@ -1,6 +1,6 @@ use crate::registers::FromRegister; -use super::templates::register_enum; +use crate::config::templates::register_enum; register_enum!(Watermark [mask=0x80, offset=7] { NotReached = 0, diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs index 90606b1f..9f7f8bad 100644 --- a/utils/lsm6dso/src/config/mod.rs +++ b/utils/lsm6dso/src/config/mod.rs @@ -1,7 +1,6 @@ pub mod control; pub mod data_types; -pub mod fifo_config; -pub mod fifo_status; +pub mod fifo; pub mod interrupts; pub mod status; pub(crate) mod templates; diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 6ae9714d..6c247242 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -23,7 +23,10 @@ use config::{ RebootMemoryContent, SoftwareReset, }, data_types::{AngularRate, FromBuffer, LinearAcceleration, Temperature}, - fifo_config::{FifoConfig, FifoConfigBuffer}, + fifo::{ + config::{FifoConfig, FifoConfigBuffer}, + data::FifoWord, + }, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; use registers::{ApplyToRegister, FromRegister, Register, WHO_AM_I_VALUE}; @@ -238,6 +241,12 @@ impl LSM6DSO { Ok(AngularRate::from_buffer(&data_buffer)) } + pub fn get_next_fifo_word(&mut self) -> Result { + let mut data_buffer = [0u8; 7]; + self.read_registers(Register::FIFO_DATA_OUT_TAG, &mut data_buffer)?; + Ok(FifoWord::from_buffer(&data_buffer)) + } + /// Reads the value from a single LSM6DSO register and returns it. fn read_register(&mut self, register: Register) -> Result { let mut data_buffer = [READ_REQUEST_MASK | (register as u8), 0]; From a1f211647535024cc9d152eff90d945ccaf7684d Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Mon, 27 Nov 2023 14:29:11 +0100 Subject: [PATCH 14/23] LSM6DSO: Implemented proper FIFO reading and data parsing --- utils/lsm6dso/Cargo.toml | 1 + utils/lsm6dso/src/config/data_types.rs | 14 ++-- utils/lsm6dso/src/config/fifo/data.rs | 86 +++++++++++++++++++++++-- utils/lsm6dso/src/config/fifo/status.rs | 1 + utils/lsm6dso/src/lib.rs | 18 +++++- 5 files changed, 106 insertions(+), 14 deletions(-) diff --git a/utils/lsm6dso/Cargo.toml b/utils/lsm6dso/Cargo.toml index 83befacf..8df37687 100644 --- a/utils/lsm6dso/Cargo.toml +++ b/utils/lsm6dso/Cargo.toml @@ -16,6 +16,7 @@ derive_more = { version = "1.0.0-beta.6", features = [ "try_into", ], default-features = false } embedded-hal = "1.0.0-rc.1" +fugit = "0.3.7" paste = "1.0.14" [features] diff --git a/utils/lsm6dso/src/config/data_types.rs b/utils/lsm6dso/src/config/data_types.rs index 0e17c8f7..6a0fdec8 100644 --- a/utils/lsm6dso/src/config/data_types.rs +++ b/utils/lsm6dso/src/config/data_types.rs @@ -1,13 +1,19 @@ +use fugit::TimerInstantU32; + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Hash)] pub struct Vec3D { pub x: T, pub y: T, pub z: T, } +/// Timestamps are provided with 0.25us resolution, therefore it's timer runs @ 4MHz. +const TIMESTAMP_TIMER_FREQUENCY_HZ: u32 = 4_000_000; + pub type Temperature = i16; pub type LinearAcceleration = Vec3D; pub type AngularRate = Vec3D; -pub type Timestamp = u32; +pub type Timestamp = TimerInstantU32<{ TIMESTAMP_TIMER_FREQUENCY_HZ }>; pub(crate) trait FromBuffer { fn from_buffer(buffer: &[u8; DATA_LENGTH]) -> Self; @@ -30,9 +36,3 @@ impl FromBuffer<6> for LinearAcceleration { } } } - -impl FromBuffer<4> for Timestamp { - fn from_buffer(buffer: &[u8; 4]) -> Self { - u32::from_le_bytes(*buffer) - } -} diff --git a/utils/lsm6dso/src/config/fifo/data.rs b/utils/lsm6dso/src/config/fifo/data.rs index 1ed5c988..673fa4eb 100644 --- a/utils/lsm6dso/src/config/fifo/data.rs +++ b/utils/lsm6dso/src/config/fifo/data.rs @@ -1,5 +1,8 @@ use crate::{ - config::{data_types::FromBuffer, templates::register_enum}, + config::{ + data_types::{AngularRate, FromBuffer, LinearAcceleration, Temperature, Timestamp}, + templates::register_enum, + }, registers::FromRegister, }; @@ -25,16 +28,89 @@ register_enum!(Tag [mask=0xF8, offset=3] { SensorHubNack = 0x19, }); -pub struct FifoWord { +pub type RawData = [u8; 6]; + +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] +pub struct FifoWordStruct { pub tag: Tag, - pub data: [u8; 6], + pub data: RawData, } -impl FromBuffer<7> for FifoWord { +impl FromBuffer<7> for FifoWordStruct { fn from_buffer(buffer: &[u8; 7]) -> Self { - FifoWord { + FifoWordStruct { tag: Tag::from_reg(buffer[0]), data: buffer[1..7].try_into().unwrap(), } } } + +#[derive(Debug, Copy, Clone, Eq, PartialEq)] +pub enum FifoWord { + Gyroscope(AngularRate), + Accelerometer(LinearAcceleration), + Temperature(Temperature), + Timestamp(Timestamp), + ConfigChange(RawData), + AccelerometerNCT2(LinearAcceleration), + AccelerometerNCT1(LinearAcceleration), + Accelerometer2xC(LinearAcceleration), + Accelerometer3xC(LinearAcceleration), + GyroscopeNCT2(AngularRate), + GyroscopeNCT1(AngularRate), + Gyroscope2xC(AngularRate), + Gyroscope3xC(AngularRate), + SensorHubSlave0(RawData), + SensorHubSlave1(RawData), + SensorHubSlave2(RawData), + SensorHubSlave3(RawData), + StepCounter(RawData), + SensorHubNack(RawData), +} + +impl From for FifoWord { + fn from(value: FifoWordStruct) -> Self { + match value.tag { + Tag::GyroscopeNC => FifoWord::Gyroscope(AngularRate::from_buffer(&value.data)), + Tag::AccelerometerNC => { + FifoWord::Accelerometer(LinearAcceleration::from_buffer(&value.data)) + } + Tag::Temperature => FifoWord::Temperature(Temperature::from_buffer( + &value.data[4..6].try_into().unwrap(), + )), + Tag::Timestamp => { + let ticks = u32::from_be_bytes(value.data[2..6].try_into().unwrap()); + FifoWord::Timestamp(Timestamp::from_ticks(ticks)) + } + Tag::ConfigChange => FifoWord::ConfigChange(value.data), + Tag::AccelerometerNCT2 => { + FifoWord::AccelerometerNCT2(LinearAcceleration::from_buffer(&value.data)) + } + Tag::AccelerometerNCT1 => { + FifoWord::AccelerometerNCT1(LinearAcceleration::from_buffer(&value.data)) + } + Tag::Accelerometer2xC => { + FifoWord::Accelerometer2xC(LinearAcceleration::from_buffer(&value.data)) + } + Tag::Accelerometer3xC => { + FifoWord::Accelerometer3xC(LinearAcceleration::from_buffer(&value.data)) + } + Tag::GyroscopeNCT2 => FifoWord::GyroscopeNCT2(AngularRate::from_buffer(&value.data)), + Tag::GyroscopeNCT1 => FifoWord::GyroscopeNCT1(AngularRate::from_buffer(&value.data)), + Tag::Gyroscope2xC => FifoWord::Gyroscope2xC(AngularRate::from_buffer(&value.data)), + Tag::Gyroscope3xC => FifoWord::Gyroscope3xC(AngularRate::from_buffer(&value.data)), + Tag::SensorHubSlave0 => FifoWord::SensorHubSlave0(value.data), + Tag::SensorHubSlave1 => FifoWord::SensorHubSlave1(value.data), + Tag::SensorHubSlave2 => FifoWord::SensorHubSlave2(value.data), + Tag::SensorHubSlave3 => FifoWord::SensorHubSlave3(value.data), + Tag::StepCounter => FifoWord::StepCounter(value.data), + Tag::SensorHubNack => FifoWord::SensorHubNack(value.data), + } + } +} + +impl FromBuffer<7> for FifoWord { + fn from_buffer(buffer: &[u8; 7]) -> Self { + FifoWordStruct::from_buffer(buffer).into() + } +} diff --git a/utils/lsm6dso/src/config/fifo/status.rs b/utils/lsm6dso/src/config/fifo/status.rs index d4e23cfb..2f801a78 100644 --- a/utils/lsm6dso/src/config/fifo/status.rs +++ b/utils/lsm6dso/src/config/fifo/status.rs @@ -27,6 +27,7 @@ register_enum!(LatchedOverrun [mask=0x08, offset=3] { FifoFull = 1, }); +#[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] pub struct FifoStatus { pub stored_words: u16, pub latched_overrun: LatchedOverrun, diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 6c247242..63783fc1 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -10,6 +10,7 @@ extern crate derive_more; extern crate embedded_hal; +extern crate fugit; extern crate paste; mod bounded_int; @@ -26,12 +27,14 @@ use config::{ fifo::{ config::{FifoConfig, FifoConfigBuffer}, data::FifoWord, + status::{FifoStatus, FifoStatusBuffer}, }, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; -use registers::{ApplyToRegister, FromRegister, Register, WHO_AM_I_VALUE}; +use registers::{ApplyToRegister, FromRegister, Register}; pub use embedded_hal::spi::SpiBus; +pub use registers::WHO_AM_I_VALUE; use crate::config::control::DataReadyState; @@ -86,18 +89,23 @@ impl LSM6DSO { Ok(self.id()? == WHO_AM_I_VALUE) } + /// Sets FIFO configuration. pub fn set_fifo_config(&mut self, config: FifoConfig) -> Result<(), SPI::Error> { let fifo_config_regs: FifoConfigBuffer = config.into(); self.write_registers(Register::FIFO_CTRL1, &fifo_config_regs)?; Ok(()) } + /// Returns current FIFO configuration. pub fn get_fifo_config(&mut self) -> Result { let mut buffer = [0u8, 0, 0, 0]; self.read_registers(Register::FIFO_CTRL1, &mut buffer)?; Ok(buffer.into()) } + /// Sets which interrupts will trigger INT1 pin. + /// + /// Activation level and pin mode can be set using [`set_irq_activation_level`](LSM6DSO::set_irq_activation_level) pub fn set_int1_interrupts(&mut self, interrupts: INT1Interrupts) -> Result<(), SPI::Error> { let config_reg: InterruptConfigBuffer = interrupts.into(); self.write_register(Register::INT1_CTRL, config_reg)?; @@ -241,6 +249,12 @@ impl LSM6DSO { Ok(AngularRate::from_buffer(&data_buffer)) } + pub fn get_fifo_status(&mut self) -> Result { + let mut data_buffer: FifoStatusBuffer = [0u8; 2]; + self.read_registers(Register::FIFO_STATUS1, &mut data_buffer)?; + Ok(FifoStatus::from(data_buffer)) + } + pub fn get_next_fifo_word(&mut self) -> Result { let mut data_buffer = [0u8; 7]; self.read_registers(Register::FIFO_DATA_OUT_TAG, &mut data_buffer)?; @@ -282,7 +296,7 @@ impl LSM6DSO { self.buffer[0] = READ_REQUEST_MASK | (first_register as u8); // Get data from sensor self.spi - .transfer_in_place(&mut self.buffer[0..user_buffer_length])?; + .transfer_in_place(&mut self.buffer[0..=user_buffer_length])?; // Copy to user's buffer buffer.copy_from_slice(&self.buffer[1..=user_buffer_length]); From 6e6cef5e6fe93e0c5c3ff941ec73f319192988a5 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Mon, 27 Nov 2023 14:29:47 +0100 Subject: [PATCH 15/23] LSM6DSO: Modified example to configure and continuously read data from LSM --- examples/samv71-lsm6dso/src/main.rs | 71 ++++++++++++++++++------- examples/samv71-lsm6dso/src/tasklets.rs | 48 +++++++++++++++++ 2 files changed, 101 insertions(+), 18 deletions(-) diff --git a/examples/samv71-lsm6dso/src/main.rs b/examples/samv71-lsm6dso/src/main.rs index f56c3e46..bb9fb767 100644 --- a/examples/samv71-lsm6dso/src/main.rs +++ b/examples/samv71-lsm6dso/src/main.rs @@ -11,14 +11,23 @@ use aerugo::{ logln, Aerugo, InitApi, SystemHardwareConfig, }; use lsm6dso::{ - config::control::{ - AccelerometerConfig, AccelerometerDataRate, AccelerometerOutputSelection, - AccelerometerScale, + config::{ + control::{ + AccelerometerConfig, AccelerometerDataRate, AccelerometerOutputSelection, + AccelerometerScale, AccelerometerTestMode, GyroscopeConfig, GyroscopeDataRate, + GyroscopeScale, GyroscopeTestMode, + }, + fifo::config::{ + AccelerometerBatchingRate, DataRateChangeBatching, FifoConfig, FifoMode, + FifoWatermarkThreshold, GyroscopeBatchingRate, StopOnWatermarkThreshold, + }, }, LSM6DSO, }; use rt::entry; +use crate::tasklets::init_system; + mod hardware_config; mod tasklets; @@ -39,29 +48,55 @@ fn main() -> ! { logln!("LSM id: {:2X?}", lsm.id()); logln!("Is LSM alive? {:?}", lsm.is_alive()); - logln!("Pre-reboot LSM config: {:#?}", lsm.get_fifo_config()); lsm.software_reset().unwrap(); lsm.reboot_memory_content().unwrap(); - let accel_cfg_a = AccelerometerConfig { - data_rate: AccelerometerDataRate::Rate208Hz, + let fifo_config = FifoConfig { + watermark_threshold: FifoWatermarkThreshold::new(50).unwrap(), + odr_change_batched: DataRateChangeBatching::Enabled, + stop_on_watermark: StopOnWatermarkThreshold::No, + gyroscope_batching_rate: GyroscopeBatchingRate::Batch12_5Hz, + accelerometer_batching_rate: AccelerometerBatchingRate::Batch12_5Hz, + mode: FifoMode::Fifo, + }; + lsm.set_fifo_config(fifo_config).unwrap(); + logln!("New LSM FIFO config: {:#?}", lsm.get_fifo_config()); + + let accelerometer_config = AccelerometerConfig { + data_rate: AccelerometerDataRate::Rate12_5Hz, scale: AccelerometerScale::Scale4g, output_selection: AccelerometerOutputSelection::FirstStageFilter, }; - let accel_cfg_b = AccelerometerConfig { - data_rate: AccelerometerDataRate::Rate3333Hz, - scale: AccelerometerScale::Scale8g, - output_selection: AccelerometerOutputSelection::LPF2SecondFilter, + + let gyroscope_config = GyroscopeConfig { + data_rate: GyroscopeDataRate::Rate12_5Hz, + scale: GyroscopeScale::Scale500dps, }; - logln!("Original config: {:#?}", lsm.get_accelerometer_config()); - logln!("Register value: {:#02X?}", lsm.get_reg()); - lsm.set_accelerometer_config(accel_cfg_a).unwrap(); - logln!("New config A: {:#?}", lsm.get_accelerometer_config()); - logln!("Register value: {:#02X?}", lsm.get_reg()); - lsm.set_accelerometer_config(accel_cfg_b).unwrap(); - logln!("New config B: {:#?}", lsm.get_accelerometer_config()); - logln!("Register value: {:#02X?}", lsm.get_reg()); + lsm.set_accelerometer_test_mode(AccelerometerTestMode::Negative) + .unwrap(); + lsm.set_gyroscope_test_mode(GyroscopeTestMode::Positive) + .unwrap(); + + logln!( + "Accelerometer mode: {:?}, gyroscope mode: {:?}", + lsm.get_accelerometer_test_mode(), + lsm.get_gyroscope_test_mode() + ); + + lsm.set_accelerometer_config(accelerometer_config).unwrap(); + lsm.set_gyroscope_config(gyroscope_config).unwrap(); + logln!( + "New LSM accelerometer config: {:#?}", + lsm.get_accelerometer_config() + ); + logln!( + "New LSM gyroscope config: {:#?}", + lsm.get_gyroscope_config() + ); + + init_system(aerugo, lsm); + logln!("System is starting!"); aerugo.start(); } diff --git a/examples/samv71-lsm6dso/src/tasklets.rs b/examples/samv71-lsm6dso/src/tasklets.rs index 8b137891..bced493a 100644 --- a/examples/samv71-lsm6dso/src/tasklets.rs +++ b/examples/samv71-lsm6dso/src/tasklets.rs @@ -1 +1,49 @@ +use aerugo::{ + hal::{ + drivers::spi::{Master, Spi}, + user_peripherals::SPI0, + }, + logln, Duration, InitApi, RuntimeApi, TaskletConfig, TaskletStorage, +}; +use lsm6dso::LSM6DSO; +static LSM_TASK_STORAGE: TaskletStorage<(), LSMTaskContext, 0> = TaskletStorage::new(); + +type Lsm = LSM6DSO>; + +pub struct LSMTaskContext { + pub lsm: Lsm, +} + +pub fn lsm_task(_: (), context: &mut LSMTaskContext, _: &'static dyn RuntimeApi) { + let fifo_status = context.lsm.get_fifo_status().unwrap(); + for _ in 0..fifo_status.stored_words { + logln!("{:?}", context.lsm.get_next_fifo_word().unwrap()); + } +} + +pub fn init_system(aerugo: &'static impl InitApi, lsm: Lsm) { + logln!("Initializing tasks..."); + + let lsm_task_config = TaskletConfig { + name: "LSMTask", + ..Default::default() + }; + + let lsm_task_context = LSMTaskContext { lsm }; + + aerugo.create_tasklet_with_context( + lsm_task_config, + lsm_task, + lsm_task_context, + &LSM_TASK_STORAGE, + ); + + let lsm_task_handle = LSM_TASK_STORAGE.create_handle().unwrap(); + + logln!("Tasks created, subscribing..."); + + aerugo.subscribe_tasklet_to_cyclic(&lsm_task_handle, Some(Duration::millis(100)), None); + + logln!("Tasks created and subscribed!"); +} From c1e43f854c132b2fe6fa9231d35edfd76a5f13e2 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 09:30:17 +0100 Subject: [PATCH 16/23] Utils: Added bitfield-enum library --- utils/bitfield-enum/Cargo.toml | 17 +++++ utils/bitfield-enum/src/lib.rs | 121 +++++++++++++++++++++++++++++++++ 2 files changed, 138 insertions(+) create mode 100644 utils/bitfield-enum/Cargo.toml create mode 100644 utils/bitfield-enum/src/lib.rs diff --git a/utils/bitfield-enum/Cargo.toml b/utils/bitfield-enum/Cargo.toml new file mode 100644 index 00000000..2417b4ad --- /dev/null +++ b/utils/bitfield-enum/Cargo.toml @@ -0,0 +1,17 @@ +[package] +name = "bitfield-enum" +version = "1.0.0" +authors.workspace = true +edition.workspace = true +rust-version.workspace = true +repository.workspace = true +license.workspace = true +description = "Library with macro for creating bitfield enums with automatic conversion from/to bytes. Also provides traits for bitfield-to-bytes/bytes-to-bitfield conversion" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +derive_more = { version = "1.0.0-beta.6", features = [ + "try_from", + "try_into", +], default-features = false } diff --git a/utils/bitfield-enum/src/lib.rs b/utils/bitfield-enum/src/lib.rs new file mode 100644 index 00000000..b4eff1e9 --- /dev/null +++ b/utils/bitfield-enum/src/lib.rs @@ -0,0 +1,121 @@ +//! You must add `derive_more` (version 1.0.0.beta-6 is recommended) to dependencies if you want to +//! use this library: +//! +//! ``` +//! derive_more = { version = "1.0.0-beta.6", features = [ +//! "try_from", +//! "try_into", +//! ], default-features = false } +//! ``` + +#![no_std] +pub extern crate derive_more; + +#[macro_export] +/// Macro creating an bitfield enum with methods for converting it from/to register value. +macro_rules! bitfield_enum { + ($name:ident [mask = $mask:literal, offset = $offset:literal] { $( $(#[doc = $doc:expr])? $variant_name:ident = $variant_value:literal,)+ }) => { + #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, $crate::derive_more::TryFrom)] + #[try_from(repr)] + #[repr(u8)] + pub enum $name { + $( + $(#[doc = $doc])? + $variant_name = $variant_value, + )+ + } + + impl $crate::RegisterField for $name { + const MASK: u8 = $mask; + const OFFSET: usize = $offset; + } + + impl $crate::ToRegister for $name { + fn to_reg(self) -> u8 { + use $crate::RegisterField; + (self as u8) << Self::OFFSET + } + } + + impl $crate::FromRegister for $name { + fn from_reg(reg: u8) -> Self { + use $crate::RegisterField; + ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() + } + } + + impl $crate::ApplyToRegister for $name + where Self: $crate::ToRegister + { + fn apply_to_reg(self, reg: u8) -> u8 { + use $crate::{RegisterField, ToRegister}; + (reg & !Self::MASK) | self.to_reg() + } + } + }; +} + +/// Trait for single-register fields +pub trait RegisterField +where + Self: Copy, +{ + /// Field mask, per datasheet (as-in register). + const MASK: u8; + /// Offset of the field's LSB to register's LSB. + const OFFSET: usize; +} + +pub trait ToRegister +where + Self: Copy + RegisterField, +{ + /// This function should return the value of current register field that can be OR'd with + /// register's content to set it. + fn to_reg(self) -> u8; +} + +pub trait FromRegister +where + Self: Copy + RegisterField, +{ + /// This function should extract the field's value from the register and return it. + fn from_reg(reg: u8) -> Self; +} + +pub trait ApplyToRegister +where + Self: Copy + RegisterField, +{ + /// This function modifies existing register's bits and returns it's new value with applied field. + fn apply_to_reg(self, reg: u8) -> u8; +} + +/// Trait for fields that span multiple registers. The order of masks and offsets must be defined +/// respective to the order of registers this field spans, smaller address first. +pub trait MultiRegisterField +where + Self: Copy, +{ + /// Field masks, per datasheet (as-in register). + const MASKS: [u8; REGISTER_SPAN]; + /// Offsets of the field's LSB from register's LSB. + const OFFSETS: [usize; REGISTER_SPAN]; +} + +pub trait ToMultiRegister +where + Self: Copy + MultiRegisterField, +{ + /// This function should return the value of provided registers as an array. Unused bits should + /// remain 0. + fn to_regs(self) -> [u8; REGISTER_SPAN]; +} + +pub trait FromMultiRegister +where + Self: Copy + MultiRegisterField, +{ + /// This function should extract the field's value from the register and return it. + fn from_regs(regs: &[u8]) -> Self; +} From 1135b1a66379c29e69ed30bdce8cf56f8404aee2 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 09:30:56 +0100 Subject: [PATCH 17/23] Utils: Added bounded-int library --- utils/bounded-int/Cargo.toml | 14 ++++++++ utils/bounded-int/src/lib.rs | 69 ++++++++++++++++++++++++++++++++++++ 2 files changed, 83 insertions(+) create mode 100644 utils/bounded-int/Cargo.toml create mode 100644 utils/bounded-int/src/lib.rs diff --git a/utils/bounded-int/Cargo.toml b/utils/bounded-int/Cargo.toml new file mode 100644 index 00000000..306527cd --- /dev/null +++ b/utils/bounded-int/Cargo.toml @@ -0,0 +1,14 @@ +[package] +name = "bounded-int" +version = "1.0.0" +authors.workspace = true +edition.workspace = true +rust-version.workspace = true +repository.workspace = true +license.workspace = true +description = "Library with customizable bounded integer types" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +paste = "1.0.14" diff --git a/utils/bounded-int/src/lib.rs b/utils/bounded-int/src/lib.rs new file mode 100644 index 00000000..e022a45f --- /dev/null +++ b/utils/bounded-int/src/lib.rs @@ -0,0 +1,69 @@ +//! Library with implementation of Bounded integer types. +use paste::paste; + +/// Macro creating a generic bounded value type, which allows for storing a value of specific type +/// in specified range. +/// +/// You'd usually want to create a type alias that uses this structure. +#[macro_export] +macro_rules! generic_bounded_value { + ($underlying_type:ty) => { + paste! { + #[doc = "Generic bounded value that uses `" $underlying_type "` as underlying type. "] + #[doc = "Provided range is inclusive."] + #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash)] + pub struct []($underlying_type); + + impl []<{ LOW }, { HIGH }> { + #[doc = "Lowest value that an instance of this struct can have."] + pub const LOW: $underlying_type = LOW; + #[doc = "Highest value that an instance of this struct can have."] + pub const HIGH: $underlying_type = HIGH; + + #[doc = "Creates a new bounded value. Returns `None` if the value is out of range."] + pub const fn new(value: $underlying_type) -> Option { + if value >= Self::LOW && value <= Self::HIGH { + Some(Self(value)) + } else { + None + } + } + + #[doc = "Creates a new bounded value. If `value` is out of range, it will be saturated "] + #[doc = "to `LOW` or `HIGH`, depending whether it's too small or large."] + pub const fn new_saturated(value: $underlying_type) -> Self { + Self(match value { + value if value < Self::LOW => Self::LOW, + value if value > Self::HIGH => Self::HIGH, + value => value + }) + } + + #[doc = "Returns the stored value. A `const` alternative to `Deref` trait."] + pub const fn get(&self) -> $underlying_type { + self.0 + } + } + + impl core::ops::Deref for []<{ LOW }, { HIGH }> { + type Target = $underlying_type; + + fn deref(&self) -> &Self::Target { + &self.0 + } + } + } + }; +} + +generic_bounded_value!(u8); +generic_bounded_value!(u16); +generic_bounded_value!(u32); +generic_bounded_value!(u64); +generic_bounded_value!(usize); + +generic_bounded_value!(i8); +generic_bounded_value!(i16); +generic_bounded_value!(i32); +generic_bounded_value!(i64); +generic_bounded_value!(isize); From ee1c917aef521689fb952253227fafb7eb1131bc Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 09:31:44 +0100 Subject: [PATCH 18/23] Aerugo: Added bitfield-enum and bounded-int to workspace --- Cargo.toml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Cargo.toml b/Cargo.toml index 7afba60c..c3627a6e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -10,6 +10,8 @@ members = [ "utils/env-parser", "utils/env-parser-tests", "utils/lsm6dso", + "utils/bitfield-enum", + "utils/bounded-int", "calldwell/calldwell-rs", ] exclude = ["demos", "examples", "testbins", "calldwell/examples"] From a7dfc1103055c6bf49582366306dfeb8ab85dc9e Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 09:33:14 +0100 Subject: [PATCH 19/23] LSM6DSO: Moved from internal bitfield-enum implementation to external bounded-int implementation was left internally, otherwise it would be impossible to implement traits from bitfield-enum --- utils/lsm6dso/Cargo.toml | 1 + utils/lsm6dso/src/bounded_int.rs | 11 ----- utils/lsm6dso/src/config/control.rs | 29 +++++------ utils/lsm6dso/src/config/fifo/config.rs | 16 +++--- utils/lsm6dso/src/config/fifo/data.rs | 9 ++-- utils/lsm6dso/src/config/fifo/status.rs | 14 +++--- utils/lsm6dso/src/config/mod.rs | 1 - utils/lsm6dso/src/config/templates.rs | 44 ----------------- utils/lsm6dso/src/lib.rs | 5 +- utils/lsm6dso/src/registers.rs | 65 ------------------------- 10 files changed, 35 insertions(+), 160 deletions(-) delete mode 100644 utils/lsm6dso/src/config/templates.rs diff --git a/utils/lsm6dso/Cargo.toml b/utils/lsm6dso/Cargo.toml index 8df37687..16123152 100644 --- a/utils/lsm6dso/Cargo.toml +++ b/utils/lsm6dso/Cargo.toml @@ -11,6 +11,7 @@ description = "LSM6DSO driver library" # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html [dependencies] +bitfield-enum = { path = "../bitfield-enum" } derive_more = { version = "1.0.0-beta.6", features = [ "try_from", "try_into", diff --git a/utils/lsm6dso/src/bounded_int.rs b/utils/lsm6dso/src/bounded_int.rs index 562da5df..43b4b776 100644 --- a/utils/lsm6dso/src/bounded_int.rs +++ b/utils/lsm6dso/src/bounded_int.rs @@ -1,5 +1,4 @@ //! Module with implementation of Bounded integer types. -#![allow(dead_code)] use paste::paste; /// Macro creating a generic bounded value type, which allows for storing a value of specific type @@ -56,14 +55,4 @@ macro_rules! generic_bounded_value { }; } -generic_bounded_value!(u8); generic_bounded_value!(u16); -generic_bounded_value!(u32); -generic_bounded_value!(u64); -generic_bounded_value!(usize); - -generic_bounded_value!(i8); -generic_bounded_value!(i16); -generic_bounded_value!(i32); -generic_bounded_value!(i64); -generic_bounded_value!(isize); diff --git a/utils/lsm6dso/src/config/control.rs b/utils/lsm6dso/src/config/control.rs index 4f2a2a17..bae7db64 100644 --- a/utils/lsm6dso/src/config/control.rs +++ b/utils/lsm6dso/src/config/control.rs @@ -1,9 +1,6 @@ -use crate::{ - config::templates::register_enum, - registers::{FromRegister, ToRegister}, -}; +use bitfield_enum::{bitfield_enum, FromRegister, ToRegister}; -register_enum!(AccelerometerDataRate [mask=0xF0, offset=4] { +bitfield_enum!(AccelerometerDataRate [mask=0xF0, offset=4] { PowerDown = 0b0000, /// This is 1.6Hz only in low-power mode. Otherwise, it's 12.5Hz. Rate1_6Hz = 0b1011, @@ -19,7 +16,7 @@ register_enum!(AccelerometerDataRate [mask=0xF0, offset=4] { Rate6667Hz = 0b1010, }); -register_enum!(AccelerometerScale [mask=0x0C, offset=2] { +bitfield_enum!(AccelerometerScale [mask=0x0C, offset=2] { Scale2g = 0b00, /// This is 16g only when full-scale mode is active. Otherwise, it's 2g. Scale16g = 0b01, @@ -27,12 +24,12 @@ register_enum!(AccelerometerScale [mask=0x0C, offset=2] { Scale8g = 0b11, }); -register_enum!(AccelerometerOutputSelection [mask=0x02, offset=1] { +bitfield_enum!(AccelerometerOutputSelection [mask=0x02, offset=1] { FirstStageFilter = 0, LPF2SecondFilter = 1, }); -register_enum!(GyroscopeDataRate [mask=0xF0, offset=4] { +bitfield_enum!(GyroscopeDataRate [mask=0xF0, offset=4] { PowerDown = 0b0000, Rate12_5Hz = 0b0001, Rate26Hz = 0b0010, @@ -46,7 +43,7 @@ register_enum!(GyroscopeDataRate [mask=0xF0, offset=4] { Rate6667Hz = 0b1010, }); -register_enum!(GyroscopeScale [mask=0x0E, offset=1] { +bitfield_enum!(GyroscopeScale [mask=0x0E, offset=1] { Scale125dps = 0b001, Scale250dps = 0b000, Scale500dps = 0b010, @@ -54,36 +51,36 @@ register_enum!(GyroscopeScale [mask=0x0E, offset=1] { Scale2000dps = 0b110, }); -register_enum!(RebootMemoryContent [mask=0x80, offset=7] { +bitfield_enum!(RebootMemoryContent [mask=0x80, offset=7] { Yes = 1, }); -register_enum!(IrqActivationLevel [mask=0x20, offset=5] { +bitfield_enum!(IrqActivationLevel [mask=0x20, offset=5] { ActiveHigh = 0, ActiveLow = 1, }); -register_enum!(IrqPinMode [mask=0x20, offset=5] { +bitfield_enum!(IrqPinMode [mask=0x20, offset=5] { PushPull = 0, OpenDrain = 1, }); -register_enum!(SoftwareReset [mask=0x01, offset=0] { +bitfield_enum!(SoftwareReset [mask=0x01, offset=0] { Yes = 1, }); -register_enum!(DataReadyState [mask=0x08, offset=3] { +bitfield_enum!(DataReadyState [mask=0x08, offset=3] { Disabled = 0, Enabled = 1, }); -register_enum!(GyroscopeTestMode [mask=0x0C, offset=2] { +bitfield_enum!(GyroscopeTestMode [mask=0x0C, offset=2] { Normal = 0b00, Positive = 0b01, Negative = 0b11, }); -register_enum!(AccelerometerTestMode [mask=0x03, offset=0] { +bitfield_enum!(AccelerometerTestMode [mask=0x03, offset=0] { Normal = 0b00, Positive = 0b01, Negative = 0b10, diff --git a/utils/lsm6dso/src/config/fifo/config.rs b/utils/lsm6dso/src/config/fifo/config.rs index b19759ec..89403688 100644 --- a/utils/lsm6dso/src/config/fifo/config.rs +++ b/utils/lsm6dso/src/config/fifo/config.rs @@ -1,7 +1,9 @@ use crate::{ + bitfield_enum::{ + bitfield_enum, FromMultiRegister, FromRegister, MultiRegisterField, ToMultiRegister, + ToRegister, + }, bounded_int::BoundedU16, - config::templates::register_enum, - registers::{FromMultiRegister, FromRegister, MultiRegisterField, ToMultiRegister, ToRegister}, }; /// Type representing FIFO watermark threshold as FIFO records (6 bytes of sensor data + tag) @@ -48,17 +50,17 @@ impl FromMultiRegister for FifoDataLength { } } -register_enum!(DataRateChangeBatching [mask=0x10, offset=4] { +bitfield_enum!(DataRateChangeBatching [mask=0x10, offset=4] { Disabled = 0, Enabled = 1, }); -register_enum!(StopOnWatermarkThreshold [mask=0x80, offset=7]{ +bitfield_enum!(StopOnWatermarkThreshold [mask=0x80, offset=7]{ Yes = 1, No = 0, }); -register_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { +bitfield_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { NoBatching = 0b0000, Batch12_5Hz = 0b0001, Batch26Hz = 0b0010, @@ -73,7 +75,7 @@ register_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { Batch6_5Hz = 0b1011, }); -register_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { +bitfield_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { NoBatching = 0b0000, Batch12_5Hz = 0b0001, Batch26Hz = 0b0010, @@ -88,7 +90,7 @@ register_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { Batch1_6Hz = 0b1011, }); -register_enum!(FifoMode [mask=0x07, offset=0] { +bitfield_enum!(FifoMode [mask=0x07, offset=0] { /// FIFO disabled. Bypass = 0b000, /// FIFO enabled and sensor stops collecting data when FIFO is full. diff --git a/utils/lsm6dso/src/config/fifo/data.rs b/utils/lsm6dso/src/config/fifo/data.rs index 673fa4eb..d4f49f36 100644 --- a/utils/lsm6dso/src/config/fifo/data.rs +++ b/utils/lsm6dso/src/config/fifo/data.rs @@ -1,12 +1,9 @@ use crate::{ - config::{ - data_types::{AngularRate, FromBuffer, LinearAcceleration, Temperature, Timestamp}, - templates::register_enum, - }, - registers::FromRegister, + bitfield_enum::{bitfield_enum, FromRegister}, + config::data_types::{AngularRate, FromBuffer, LinearAcceleration, Temperature, Timestamp}, }; -register_enum!(Tag [mask=0xF8, offset=3] { +bitfield_enum!(Tag [mask=0xF8, offset=3] { GyroscopeNC = 0x01, AccelerometerNC = 0x02, Temperature = 0x03, diff --git a/utils/lsm6dso/src/config/fifo/status.rs b/utils/lsm6dso/src/config/fifo/status.rs index 2f801a78..b1ae40d0 100644 --- a/utils/lsm6dso/src/config/fifo/status.rs +++ b/utils/lsm6dso/src/config/fifo/status.rs @@ -1,28 +1,26 @@ -use crate::registers::FromRegister; +use bitfield_enum::{bitfield_enum, FromRegister}; -use crate::config::templates::register_enum; - -register_enum!(Watermark [mask=0x80, offset=7] { +bitfield_enum!(Watermark [mask=0x80, offset=7] { NotReached = 0, Reached = 1, }); -register_enum!(Overrun [mask=0x40, offset=6] { +bitfield_enum!(Overrun [mask=0x40, offset=6] { FifoNotFull = 0, FifoFull = 1, }); -register_enum!(SmartStatus [mask=0x20, offset=5] { +bitfield_enum!(SmartStatus [mask=0x20, offset=5] { FifoNotFull = 0, FifoWillBeFullAtNextODR = 1, }); -register_enum!(CounterThreshold [mask=0x10, offset=4] { +bitfield_enum!(CounterThreshold [mask=0x10, offset=4] { NotReached = 0, Reached = 1, }); -register_enum!(LatchedOverrun [mask=0x08, offset=3] { +bitfield_enum!(LatchedOverrun [mask=0x08, offset=3] { FifoNotFull = 0, FifoFull = 1, }); diff --git a/utils/lsm6dso/src/config/mod.rs b/utils/lsm6dso/src/config/mod.rs index 9f7f8bad..439a6554 100644 --- a/utils/lsm6dso/src/config/mod.rs +++ b/utils/lsm6dso/src/config/mod.rs @@ -3,4 +3,3 @@ pub mod data_types; pub mod fifo; pub mod interrupts; pub mod status; -pub(crate) mod templates; diff --git a/utils/lsm6dso/src/config/templates.rs b/utils/lsm6dso/src/config/templates.rs deleted file mode 100644 index 5aa2df22..00000000 --- a/utils/lsm6dso/src/config/templates.rs +++ /dev/null @@ -1,44 +0,0 @@ -/// Macro creating an bitfield enum with methods for converting it from/to register value. -macro_rules! register_enum { - ($name:ident [mask = $mask:literal, offset = $offset:literal] { $( $(#[doc = $doc:expr])? $variant_name:ident = $variant_value:literal,)+ }) => { - #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, derive_more::TryFrom)] - #[try_from(repr)] - #[repr(u8)] - pub enum $name { - $( - $(#[doc = $doc])? - $variant_name = $variant_value, - )+ - } - - impl crate::registers::RegisterField for $name { - const MASK: u8 = $mask; - const OFFSET: usize = $offset; - } - - impl crate::registers::ToRegister for $name { - fn to_reg(self) -> u8 { - use crate::registers::RegisterField; - (self as u8) << Self::OFFSET - } - } - - impl crate::registers::FromRegister for $name { - fn from_reg(reg: u8) -> Self { - use crate::registers::RegisterField; - ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() - } - } - - impl crate::registers::ApplyToRegister for $name - where Self: crate::registers::ToRegister - { - fn apply_to_reg(self, reg: u8) -> u8 { - use crate::registers::{RegisterField, ToRegister}; - (reg & !Self::MASK) | self.to_reg() - } - } - }; -} - -pub(crate) use register_enum; diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index 63783fc1..c687e0cc 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -8,7 +8,7 @@ //! When SAMV71 HAL will support [`SpiDevice`](embedded_hal::spi::SpiDevice), this library should //! be refactored to use it, instead of the whole bus. -extern crate derive_more; +extern crate bitfield_enum; extern crate embedded_hal; extern crate fugit; extern crate paste; @@ -17,6 +17,7 @@ mod bounded_int; pub mod config; pub(crate) mod registers; +use bitfield_enum::{ApplyToRegister, FromRegister}; use config::{ control::{ AccelerometerConfig, AccelerometerConfigBuffer, AccelerometerTestMode, GyroscopeConfig, @@ -31,7 +32,7 @@ use config::{ }, interrupts::{INT1Interrupts, INT2Interrupts, InterruptConfigBuffer}, }; -use registers::{ApplyToRegister, FromRegister, Register}; +use registers::Register; pub use embedded_hal::spi::SpiBus; pub use registers::WHO_AM_I_VALUE; diff --git a/utils/lsm6dso/src/registers.rs b/utils/lsm6dso/src/registers.rs index 4450ce44..738ca97c 100644 --- a/utils/lsm6dso/src/registers.rs +++ b/utils/lsm6dso/src/registers.rs @@ -84,68 +84,3 @@ pub(crate) enum Register { /// Expected value of WHO_AM_I register pub const WHO_AM_I_VALUE: u8 = 0x6C; - -/// Trait for single-register fields -pub(crate) trait RegisterField -where - Self: Copy, -{ - /// Field mask, per datasheet (as-in register). - const MASK: u8; - /// Offset of the field's LSB to register's LSB. - const OFFSET: usize; -} - -/// Trait for fields that span multiple registers. The order of masks and offsets must be defined -/// respective to the order of registers this field spans, smaller address first. -pub(crate) trait MultiRegisterField -where - Self: Copy, -{ - /// Field masks, per datasheet (as-in register). - const MASKS: [u8; REGISTER_SPAN]; - /// Offsets of the field's LSB from register's LSB. - const OFFSETS: [usize; REGISTER_SPAN]; -} - -pub(crate) trait ToRegister -where - Self: Copy + RegisterField, -{ - /// This function should return the value of current register field that can be OR'd with - /// register's content to set it. - fn to_reg(self) -> u8; -} - -pub(crate) trait FromRegister -where - Self: Copy + RegisterField, -{ - /// This function should extract the field's value from the register and return it. - fn from_reg(reg: u8) -> Self; -} - -pub(crate) trait ApplyToRegister -where - Self: Copy + RegisterField, -{ - /// This function modifies existing register's bits and returns it's new value with applied field. - fn apply_to_reg(self, reg: u8) -> u8; -} - -pub(crate) trait ToMultiRegister -where - Self: Copy + MultiRegisterField, -{ - /// This function should return the value of provided registers as an array. Unused bits should - /// remain 0. - fn to_regs(self) -> [u8; REGISTER_SPAN]; -} - -pub(crate) trait FromMultiRegister -where - Self: Copy + MultiRegisterField, -{ - /// This function should extract the field's value from the register and return it. - fn from_regs(regs: &[u8]) -> Self; -} From 55e27f3982ed6291f2bc8c196584fa77b900d7f1 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 09:47:24 +0100 Subject: [PATCH 20/23] Utils: Added `no_std` to bounded-int --- utils/bounded-int/src/lib.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/utils/bounded-int/src/lib.rs b/utils/bounded-int/src/lib.rs index e022a45f..05614a08 100644 --- a/utils/bounded-int/src/lib.rs +++ b/utils/bounded-int/src/lib.rs @@ -1,3 +1,4 @@ +#![no_std] //! Library with implementation of Bounded integer types. use paste::paste; From 31d2c09e61c46f251bb453f1937a76e3c11bafb0 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 10:28:22 +0100 Subject: [PATCH 21/23] Bitfield-enum: Changed naming convention --- utils/bitfield-enum/src/lib.rs | 91 ++++++++++++------------- utils/lsm6dso/src/config/control.rs | 16 ++--- utils/lsm6dso/src/config/fifo/config.rs | 58 ++++++++-------- utils/lsm6dso/src/config/fifo/data.rs | 4 +- utils/lsm6dso/src/config/fifo/status.rs | 12 ++-- utils/lsm6dso/src/lib.rs | 31 +++++---- 6 files changed, 108 insertions(+), 104 deletions(-) diff --git a/utils/bitfield-enum/src/lib.rs b/utils/bitfield-enum/src/lib.rs index b4eff1e9..8bcf80f0 100644 --- a/utils/bitfield-enum/src/lib.rs +++ b/utils/bitfield-enum/src/lib.rs @@ -12,7 +12,7 @@ pub extern crate derive_more; #[macro_export] -/// Macro creating an bitfield enum with methods for converting it from/to register value. +/// Macro creating an bitfield enum with methods for converting it from/to bitfield value. macro_rules! bitfield_enum { ($name:ident [mask = $mask:literal, offset = $offset:literal] { $( $(#[doc = $doc:expr])? $variant_name:ident = $variant_value:literal,)+ }) => { #[derive(Debug, Copy, Clone, Eq, PartialEq, Ord, PartialOrd, Hash, $crate::derive_more::TryFrom)] @@ -25,97 +25,96 @@ macro_rules! bitfield_enum { )+ } - impl $crate::RegisterField for $name { + impl $crate::BitField for $name { const MASK: u8 = $mask; const OFFSET: usize = $offset; } - impl $crate::ToRegister for $name { - fn to_reg(self) -> u8 { - use $crate::RegisterField; + impl $crate::BitFieldToByte for $name { + fn to_byte(self) -> u8 { + use $crate::BitField; (self as u8) << Self::OFFSET } } - impl $crate::FromRegister for $name { - fn from_reg(reg: u8) -> Self { - use $crate::RegisterField; - ((reg & Self::MASK) >> Self::OFFSET).try_into().unwrap() + impl $crate::BitFieldFromByte for $name { + fn from_byte(byte: u8) -> Self { + use $crate::BitField; + ((byte & Self::MASK) >> Self::OFFSET).try_into().unwrap() } } - impl $crate::ApplyToRegister for $name - where Self: $crate::ToRegister + impl $crate::ApplyBitFieldToByte for $name + where Self: $crate::BitField { - fn apply_to_reg(self, reg: u8) -> u8 { - use $crate::{RegisterField, ToRegister}; - (reg & !Self::MASK) | self.to_reg() + fn apply_to_byte(self, byte: u8) -> u8 { + use $crate::{BitField, BitFieldToByte}; + (byte & !Self::MASK) | self.to_byte() } } }; } -/// Trait for single-register fields -pub trait RegisterField +/// Trait for single-byte bitfields +pub trait BitField where Self: Copy, { - /// Field mask, per datasheet (as-in register). + // Bitfield mask. const MASK: u8; - /// Offset of the field's LSB to register's LSB. + /// Offset of the field's LSB. const OFFSET: usize; } -pub trait ToRegister +pub trait BitFieldToByte where - Self: Copy + RegisterField, + Self: Copy + BitField, { - /// This function should return the value of current register field that can be OR'd with - /// register's content to set it. - fn to_reg(self) -> u8; + /// This function should return the value of current bitfield with unused bits set to 0. + fn to_byte(self) -> u8; } -pub trait FromRegister +pub trait BitFieldFromByte where - Self: Copy + RegisterField, + Self: Copy + BitField, { - /// This function should extract the field's value from the register and return it. - fn from_reg(reg: u8) -> Self; + /// This function should extract the field's value from the bitfield and return it. + fn from_byte(byte: u8) -> Self; } -pub trait ApplyToRegister +pub trait ApplyBitFieldToByte where - Self: Copy + RegisterField, + Self: Copy + BitField, { - /// This function modifies existing register's bits and returns it's new value with applied field. - fn apply_to_reg(self, reg: u8) -> u8; + /// This function modifies existing byte's bits and returns it's new value with applied bitfield. + fn apply_to_byte(self, byte: u8) -> u8; } -/// Trait for fields that span multiple registers. The order of masks and offsets must be defined -/// respective to the order of registers this field spans, smaller address first. -pub trait MultiRegisterField +/// Trait for bitfields that span multiple bytes. The order of masks and offsets must be defined +/// respective to the order of bytes this field spans. +pub trait MultiByteBitField where Self: Copy, { - /// Field masks, per datasheet (as-in register). - const MASKS: [u8; REGISTER_SPAN]; - /// Offsets of the field's LSB from register's LSB. - const OFFSETS: [usize; REGISTER_SPAN]; + /// Field masks. + const MASKS: [u8; BITFIELD_SPAN]; + /// Offsets of the field's LSB. + const OFFSETS: [usize; BITFIELD_SPAN]; } -pub trait ToMultiRegister +pub trait MultiByteBitFieldToBytes where - Self: Copy + MultiRegisterField, + Self: Copy + MultiByteBitField, { - /// This function should return the value of provided registers as an array. Unused bits should + /// This function should return the value of provided bitfield as an array. Unused bits should /// remain 0. - fn to_regs(self) -> [u8; REGISTER_SPAN]; + fn to_bytes(self) -> [u8; BITFIELD_SPAN]; } -pub trait FromMultiRegister +pub trait MultiByteBitFieldFromBytes where - Self: Copy + MultiRegisterField, + Self: Copy + MultiByteBitField, { - /// This function should extract the field's value from the register and return it. - fn from_regs(regs: &[u8]) -> Self; + /// This function should extract the field's value from the bitfield and return it. + fn from_bytes(bytes: &[u8]) -> Self; } diff --git a/utils/lsm6dso/src/config/control.rs b/utils/lsm6dso/src/config/control.rs index bae7db64..1bc388e5 100644 --- a/utils/lsm6dso/src/config/control.rs +++ b/utils/lsm6dso/src/config/control.rs @@ -1,4 +1,4 @@ -use bitfield_enum::{bitfield_enum, FromRegister, ToRegister}; +use bitfield_enum::{bitfield_enum, BitFieldFromByte, BitFieldToByte}; bitfield_enum!(AccelerometerDataRate [mask=0xF0, offset=4] { PowerDown = 0b0000, @@ -98,16 +98,16 @@ pub struct AccelerometerConfig { impl From for AccelerometerConfigBuffer { fn from(config: AccelerometerConfig) -> Self { - config.output_selection.to_reg() | config.scale.to_reg() | config.data_rate.to_reg() + config.output_selection.to_byte() | config.scale.to_byte() | config.data_rate.to_byte() } } impl From for AccelerometerConfig { fn from(value: AccelerometerConfigBuffer) -> Self { AccelerometerConfig { - data_rate: AccelerometerDataRate::from_reg(value), - scale: AccelerometerScale::from_reg(value), - output_selection: AccelerometerOutputSelection::from_reg(value), + data_rate: AccelerometerDataRate::from_byte(value), + scale: AccelerometerScale::from_byte(value), + output_selection: AccelerometerOutputSelection::from_byte(value), } } } @@ -120,15 +120,15 @@ pub struct GyroscopeConfig { impl From for GyroscopeConfigBuffer { fn from(config: GyroscopeConfig) -> Self { - config.scale.to_reg() | config.data_rate.to_reg() + config.scale.to_byte() | config.data_rate.to_byte() } } impl From for GyroscopeConfig { fn from(value: GyroscopeConfigBuffer) -> Self { GyroscopeConfig { - data_rate: GyroscopeDataRate::from_reg(value), - scale: GyroscopeScale::from_reg(value), + data_rate: GyroscopeDataRate::from_byte(value), + scale: GyroscopeScale::from_byte(value), } } } diff --git a/utils/lsm6dso/src/config/fifo/config.rs b/utils/lsm6dso/src/config/fifo/config.rs index 89403688..331742ef 100644 --- a/utils/lsm6dso/src/config/fifo/config.rs +++ b/utils/lsm6dso/src/config/fifo/config.rs @@ -1,51 +1,51 @@ use crate::{ bitfield_enum::{ - bitfield_enum, FromMultiRegister, FromRegister, MultiRegisterField, ToMultiRegister, - ToRegister, + bitfield_enum, BitFieldFromByte, BitFieldToByte, MultiByteBitField, + MultiByteBitFieldFromBytes, MultiByteBitFieldToBytes, }, bounded_int::BoundedU16, }; /// Type representing FIFO watermark threshold as FIFO records (6 bytes of sensor data + tag) pub type FifoWatermarkThreshold = BoundedU16<0, 0x1FF>; -impl MultiRegisterField for FifoWatermarkThreshold { +impl MultiByteBitField for FifoWatermarkThreshold { const MASKS: [u8; 2] = [0xFF, 0x01]; const OFFSETS: [usize; 2] = [0, 0]; } -impl ToMultiRegister for FifoWatermarkThreshold { - fn to_regs(self) -> [u8; 2] { +impl MultiByteBitFieldToBytes for FifoWatermarkThreshold { + fn to_bytes(self) -> [u8; 2] { self.to_le_bytes() } } -impl FromMultiRegister for FifoWatermarkThreshold { - fn from_regs(regs: &[u8]) -> Self { - assert!(regs.len() >= 2); - let value_lsb = regs[0]; - let value_msb = regs[1] & Self::MASKS[1]; +impl MultiByteBitFieldFromBytes for FifoWatermarkThreshold { + fn from_bytes(bytes: &[u8]) -> Self { + assert!(bytes.len() >= 2); + let value_lsb = bytes[0]; + let value_msb = bytes[1] & Self::MASKS[1]; Self::new(u16::from_le_bytes([value_lsb, value_msb])).unwrap() } } /// Type representing amount of items in FIFO as FIFO records (6 bytes of sensor data + tag) pub type FifoDataLength = BoundedU16<0, 0x3FF>; -impl MultiRegisterField for FifoDataLength { +impl MultiByteBitField for FifoDataLength { const MASKS: [u8; 2] = [0xFF, 0x03]; const OFFSETS: [usize; 2] = [0, 0]; } -impl ToMultiRegister for FifoDataLength { - fn to_regs(self) -> [u8; 2] { +impl MultiByteBitFieldToBytes for FifoDataLength { + fn to_bytes(self) -> [u8; 2] { self.to_le_bytes() } } -impl FromMultiRegister for FifoDataLength { - fn from_regs(regs: &[u8]) -> Self { - assert!(regs.len() >= 2); - let value_lsb = regs[0]; - let value_msb = regs[1] & Self::MASKS[1]; +impl MultiByteBitFieldFromBytes for FifoDataLength { + fn from_bytes(bytes: &[u8]) -> Self { + assert!(bytes.len() >= 2); + let value_lsb = bytes[0]; + let value_msb = bytes[1] & Self::MASKS[1]; Self::new(u16::from_le_bytes([value_lsb, value_msb])).unwrap() } } @@ -127,14 +127,14 @@ pub type FifoConfigBuffer = [u8; 4]; impl From for FifoConfigBuffer { fn from(config: FifoConfig) -> Self { - let watermark_threshold_bytes = config.watermark_threshold.to_regs(); + let watermark_threshold_bytes = config.watermark_threshold.to_bytes(); [ watermark_threshold_bytes[0], watermark_threshold_bytes[1] - | config.odr_change_batched.to_reg() - | config.stop_on_watermark.to_reg(), - config.accelerometer_batching_rate.to_reg() | config.gyroscope_batching_rate.to_reg(), - config.mode.to_reg(), + | config.odr_change_batched.to_byte() + | config.stop_on_watermark.to_byte(), + config.accelerometer_batching_rate.to_byte() | config.gyroscope_batching_rate.to_byte(), + config.mode.to_byte(), ] } } @@ -142,12 +142,12 @@ impl From for FifoConfigBuffer { impl From for FifoConfig { fn from(regs: FifoConfigBuffer) -> Self { FifoConfig { - watermark_threshold: FifoWatermarkThreshold::from_regs(®s[0..=1]), - odr_change_batched: DataRateChangeBatching::from_reg(regs[1]), - stop_on_watermark: StopOnWatermarkThreshold::from_reg(regs[1]), - gyroscope_batching_rate: GyroscopeBatchingRate::from_reg(regs[2]), - accelerometer_batching_rate: AccelerometerBatchingRate::from_reg(regs[2]), - mode: FifoMode::from_reg(regs[3]), + watermark_threshold: FifoWatermarkThreshold::from_bytes(®s[0..=1]), + odr_change_batched: DataRateChangeBatching::from_byte(regs[1]), + stop_on_watermark: StopOnWatermarkThreshold::from_byte(regs[1]), + gyroscope_batching_rate: GyroscopeBatchingRate::from_byte(regs[2]), + accelerometer_batching_rate: AccelerometerBatchingRate::from_byte(regs[2]), + mode: FifoMode::from_byte(regs[3]), } } } diff --git a/utils/lsm6dso/src/config/fifo/data.rs b/utils/lsm6dso/src/config/fifo/data.rs index d4f49f36..8b8284af 100644 --- a/utils/lsm6dso/src/config/fifo/data.rs +++ b/utils/lsm6dso/src/config/fifo/data.rs @@ -1,5 +1,5 @@ use crate::{ - bitfield_enum::{bitfield_enum, FromRegister}, + bitfield_enum::{bitfield_enum, BitFieldFromByte}, config::data_types::{AngularRate, FromBuffer, LinearAcceleration, Temperature, Timestamp}, }; @@ -36,7 +36,7 @@ pub struct FifoWordStruct { impl FromBuffer<7> for FifoWordStruct { fn from_buffer(buffer: &[u8; 7]) -> Self { FifoWordStruct { - tag: Tag::from_reg(buffer[0]), + tag: Tag::from_byte(buffer[0]), data: buffer[1..7].try_into().unwrap(), } } diff --git a/utils/lsm6dso/src/config/fifo/status.rs b/utils/lsm6dso/src/config/fifo/status.rs index b1ae40d0..f774c1b1 100644 --- a/utils/lsm6dso/src/config/fifo/status.rs +++ b/utils/lsm6dso/src/config/fifo/status.rs @@ -1,4 +1,4 @@ -use bitfield_enum::{bitfield_enum, FromRegister}; +use bitfield_enum::{bitfield_enum, BitFieldFromByte}; bitfield_enum!(Watermark [mask=0x80, offset=7] { NotReached = 0, @@ -42,11 +42,11 @@ impl From for FifoStatus { const STORED_WORDS_MASK: u8 = 0x03; Self { stored_words: u16::from_le_bytes([buffer[0], buffer[1] & STORED_WORDS_MASK]), - latched_overrun: LatchedOverrun::from_reg(buffer[1]), - counter_threshold: CounterThreshold::from_reg(buffer[1]), - smart_fifo: SmartStatus::from_reg(buffer[1]), - overrun: Overrun::from_reg(buffer[1]), - watermark: Watermark::from_reg(buffer[1]), + latched_overrun: LatchedOverrun::from_byte(buffer[1]), + counter_threshold: CounterThreshold::from_byte(buffer[1]), + smart_fifo: SmartStatus::from_byte(buffer[1]), + overrun: Overrun::from_byte(buffer[1]), + watermark: Watermark::from_byte(buffer[1]), } } } diff --git a/utils/lsm6dso/src/lib.rs b/utils/lsm6dso/src/lib.rs index c687e0cc..5ae285b0 100644 --- a/utils/lsm6dso/src/lib.rs +++ b/utils/lsm6dso/src/lib.rs @@ -17,7 +17,7 @@ mod bounded_int; pub mod config; pub(crate) mod registers; -use bitfield_enum::{ApplyToRegister, FromRegister}; +use bitfield_enum::{ApplyBitFieldToByte, BitFieldFromByte}; use config::{ control::{ AccelerometerConfig, AccelerometerConfigBuffer, AccelerometerTestMode, GyroscopeConfig, @@ -154,7 +154,7 @@ impl LSM6DSO { let ctrl_reg = self.read_register(Register::CTRL3_C)?; self.write_register( Register::CTRL3_C, - RebootMemoryContent::Yes.apply_to_reg(ctrl_reg), + RebootMemoryContent::Yes.apply_to_byte(ctrl_reg), )?; Ok(()) } @@ -164,40 +164,45 @@ impl LSM6DSO { activation_level: IrqActivationLevel, ) -> Result<(), SPI::Error> { let ctrl_reg = self.read_register(Register::CTRL3_C)?; - self.write_register(Register::CTRL3_C, activation_level.apply_to_reg(ctrl_reg))?; + self.write_register(Register::CTRL3_C, activation_level.apply_to_byte(ctrl_reg))?; Ok(()) } pub fn get_irq_activation_level(&mut self) -> Result { - Ok(IrqActivationLevel::from_reg( + Ok(IrqActivationLevel::from_byte( self.read_register(Register::CTRL3_C)?, )) } pub fn set_irq_pin_mode(&mut self, pin_mode: IrqPinMode) -> Result<(), SPI::Error> { let ctrl_reg = self.read_register(Register::CTRL3_C)?; - self.write_register(Register::CTRL3_C, pin_mode.apply_to_reg(ctrl_reg))?; + self.write_register(Register::CTRL3_C, pin_mode.apply_to_byte(ctrl_reg))?; Ok(()) } pub fn get_irq_pin_mode(&mut self) -> Result { - Ok(IrqPinMode::from_reg(self.read_register(Register::CTRL3_C)?)) + Ok(IrqPinMode::from_byte( + self.read_register(Register::CTRL3_C)?, + )) } pub fn software_reset(&mut self) -> Result<(), SPI::Error> { let ctrl_reg = self.read_register(Register::CTRL3_C)?; - self.write_register(Register::CTRL3_C, SoftwareReset::Yes.apply_to_reg(ctrl_reg))?; + self.write_register( + Register::CTRL3_C, + SoftwareReset::Yes.apply_to_byte(ctrl_reg), + )?; Ok(()) } pub fn set_data_ready_state(&mut self, state: DataReadyState) -> Result<(), SPI::Error> { let ctrl_reg = self.read_register(Register::CTRL4_C)?; - self.write_register(Register::CTRL4_C, state.apply_to_reg(ctrl_reg))?; + self.write_register(Register::CTRL4_C, state.apply_to_byte(ctrl_reg))?; Ok(()) } pub fn get_data_ready_state(&mut self) -> Result { - Ok(DataReadyState::from_reg( + Ok(DataReadyState::from_byte( self.read_register(Register::CTRL4_C)?, )) } @@ -207,12 +212,12 @@ impl LSM6DSO { test_mode: AccelerometerTestMode, ) -> Result<(), SPI::Error> { let ctrl_reg = self.read_register(Register::CTRL5_C)?; - self.write_register(Register::CTRL5_C, test_mode.apply_to_reg(ctrl_reg))?; + self.write_register(Register::CTRL5_C, test_mode.apply_to_byte(ctrl_reg))?; Ok(()) } pub fn get_accelerometer_test_mode(&mut self) -> Result { - Ok(AccelerometerTestMode::from_reg( + Ok(AccelerometerTestMode::from_byte( self.read_register(Register::CTRL5_C)?, )) } @@ -222,12 +227,12 @@ impl LSM6DSO { test_mode: GyroscopeTestMode, ) -> Result<(), SPI::Error> { let ctrl_reg = self.read_register(Register::CTRL5_C)?; - self.write_register(Register::CTRL5_C, test_mode.apply_to_reg(ctrl_reg))?; + self.write_register(Register::CTRL5_C, test_mode.apply_to_byte(ctrl_reg))?; Ok(()) } pub fn get_gyroscope_test_mode(&mut self) -> Result { - Ok(GyroscopeTestMode::from_reg( + Ok(GyroscopeTestMode::from_byte( self.read_register(Register::CTRL5_C)?, )) } From 2758c3eaa3ee105f9d200db18c315272001b30a1 Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Tue, 28 Nov 2023 11:45:44 +0100 Subject: [PATCH 22/23] Bitfield-enum: Added TryFrom equivalents --- utils/bitfield-enum/src/lib.rs | 53 +++++++++++++++++++++++++++------- 1 file changed, 43 insertions(+), 10 deletions(-) diff --git a/utils/bitfield-enum/src/lib.rs b/utils/bitfield-enum/src/lib.rs index 8bcf80f0..c7cf76bb 100644 --- a/utils/bitfield-enum/src/lib.rs +++ b/utils/bitfield-enum/src/lib.rs @@ -37,13 +37,6 @@ macro_rules! bitfield_enum { } } - impl $crate::BitFieldFromByte for $name { - fn from_byte(byte: u8) -> Self { - use $crate::BitField; - ((byte & Self::MASK) >> Self::OFFSET).try_into().unwrap() - } - } - impl $crate::ApplyBitFieldToByte for $name where Self: $crate::BitField { @@ -52,6 +45,25 @@ macro_rules! bitfield_enum { (byte & !Self::MASK) | self.to_byte() } } + + impl $crate::BitFieldFromByte for $name { + /// Implementation of this function uses `BitFieldTryFromByte` as base, and performs + /// `unwrap`. This should be used only when it's guaranteed that bitfield will have + /// valid value. + fn from_byte(byte: u8) -> Self { + use $crate::BitFieldTryFromByte; + Self::try_from_byte(byte).unwrap() + } + } + + impl $crate::BitFieldTryFromByte for $name { + type Error = $crate::derive_more::TryFromReprError; + + fn try_from_byte(byte: u8) -> Result { + use $crate::BitField; + ((byte & Self::MASK) >> Self::OFFSET).try_into() + } + } }; } @@ -74,20 +86,30 @@ where fn to_byte(self) -> u8; } +pub trait ApplyBitFieldToByte +where + Self: Copy + BitField, +{ + /// This function modifies existing byte's bits and returns it's new value with applied bitfield. + fn apply_to_byte(self, byte: u8) -> u8; +} + pub trait BitFieldFromByte where Self: Copy + BitField, { /// This function should extract the field's value from the bitfield and return it. + /// If this extraction can fail, use [`BitFieldTryFromByte`]. fn from_byte(byte: u8) -> Self; } -pub trait ApplyBitFieldToByte +pub trait BitFieldTryFromByte where Self: Copy + BitField, { - /// This function modifies existing byte's bits and returns it's new value with applied bitfield. - fn apply_to_byte(self, byte: u8) -> u8; + type Error; + /// This function should try to extract the field's value from the bitfield and return it. + fn try_from_byte(byte: u8) -> Result; } /// Trait for bitfields that span multiple bytes. The order of masks and offsets must be defined @@ -116,5 +138,16 @@ where Self: Copy + MultiByteBitField, { /// This function should extract the field's value from the bitfield and return it. + /// If this extraction can fail, use [`MultiByteBitFieldTryFromBytes`]. fn from_bytes(bytes: &[u8]) -> Self; } + +pub trait MultiByteBitFieldTryFromBytes +where + Self: Copy + MultiByteBitField, +{ + type Error; + /// This function should extract the field's value from the bitfield and return it. + /// If this extraction can fail, use [`MultiByteBitFieldTryFromBytes`]. + fn try_from_bytes(bytes: &[u8]) -> Result; +} From b75fd2a710311e0e3cf8e2ca3e269e533fb461cb Mon Sep 17 00:00:00 2001 From: Wojciech Olech Date: Wed, 29 Nov 2023 10:30:51 +0100 Subject: [PATCH 23/23] LSM6DSO: Renamed batching rate frequency to be same as data rate --- utils/lsm6dso/src/config/fifo/config.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utils/lsm6dso/src/config/fifo/config.rs b/utils/lsm6dso/src/config/fifo/config.rs index 331742ef..f449568a 100644 --- a/utils/lsm6dso/src/config/fifo/config.rs +++ b/utils/lsm6dso/src/config/fifo/config.rs @@ -67,7 +67,7 @@ bitfield_enum!(GyroscopeBatchingRate [mask=0xF0, offset=4] { Batch52Hz = 0b0011, Batch104Hz = 0b0100, Batch208Hz = 0b0101, - Batch417Hz = 0b0110, + Batch416Hz = 0b0110, Batch833Hz = 0b0111, Batch1667Hz = 0b1000, Batch3333Hz = 0b1001, @@ -82,7 +82,7 @@ bitfield_enum!(AccelerometerBatchingRate [mask=0x0F, offset=0] { Batch52Hz = 0b0011, Batch104Hz = 0b0100, Batch208Hz = 0b0101, - Batch417Hz = 0b0110, + Batch416Hz = 0b0110, Batch833Hz = 0b0111, Batch1667Hz = 0b1000, Batch3333Hz = 0b1001,