Improved initialization and config upload

README.md

@@ -8,32 +8,39 @@ This is an [embedded-hal](https://github.com/rust-embedded/embedded-hal) driver
 
 ```rust
 // ...
-
 use bmi2::Bmi2;
 use bmi2::config;
 use bmi2::{types::Burst, I2cAddr, types::PwrCtrl};
+use embedded_hal::delay::DelayNs;
+
+// Specify your delay type, for example:
+let delay = MyDelay::new(); // Replace with your actual delay implementation
+
+// Choose a buffer size (e.g., 512 bytes), needs to be >= max data burst
+const BUFFER_SIZE: usize = 512;
 
 /// Create a new Bmi2 device using I2C with its alternative address (0x69).
 /// Configure the max data burst to 255 bytes:
 /// - used for the upload of the configuration during initialization.
-/// - This is a limitation from your device or its HAL. 
-let mut bmi = Bmi2::new_i2c(i2c, I2cAddr::Alternative, Burst::Other(255));
+/// - This is a limitation from your device or its HAL.
+let mut bmi = Bmi2::<_, _, BUFFER_SIZE>::new_i2c(
+    i2c, 
+    delay,
+    I2cAddr::Alternative, 
+    Burst::new(255),
+);
 
 /// Get the chip id. Should be 0x24 or 36 in decimal
 let chip_id = bmi.get_chip_id().unwrap();
-
 /// Initialize the senor.
 /// During this process a configuration of > 8kB is uploaded to the sensor.
 /// Alternatively, for the BMI260 call its dedicated config:
 /// bmi.init(&config::BMI260_CONFIG_FILE).unwrap();
 bmi.init(&config::BMI270_CONFIG_FILE).unwrap();
-
 /// Enable power for the accelerometer and the gyroscope.
 let pwr_ctrl = PwrCtrl { aux_en: false, gyr_en: true, acc_en: true, temp_en: false };
 bmi.set_pwr_ctrl(pwr_ctrl).unwrap();
-
 /// Read the raw data
 let data = bmi.get_data().unwrap();
-
 // ...
 ```

examples/nrf52840/basic_read/src/main.rs

@@ -11,6 +11,7 @@ use nrf52840_hal::{
     pac,
     timer::Timer,
     twim::{self, Twim},
+    Delay,
 };
 
 use defmt_rtt as _;
@@ -22,6 +23,7 @@ use bmi2::{types::Burst, types::PwrCtrl, I2cAddr};
 #[entry]
 fn main() -> ! {
     let p = pac::Peripherals::take().unwrap();
+    let core = pac::CorePeripherals::take().unwrap();
 
     let mut timer = Timer::new(p.TIMER0);
     let port0 = p0::Parts::new(p.P0);
@@ -33,7 +35,11 @@ fn main() -> ! {
 
     let i2c = Twim::new(p.TWIM0, twim_pins, twim::Frequency::K100);
 
-    let mut bmi = Bmi2::new_i2c(i2c, I2cAddr::Alternative, Burst::Other(255));
+    let delay = Delay::new(core.SYST);
+
+    const BUFFER_SIZE: usize = 256;
+
+    let mut bmi = Bmi2::<_, _, BUFFER_SIZE>::new_i2c(i2c, delay, I2cAddr::Alternative, Burst::new(255));
     let chip_id = bmi.get_chip_id().unwrap();
     defmt::info!("chip id: {}", chip_id);
 

src/bmi2.rs

@@ -1,30 +1,29 @@
 use fixedvec::FixedVec;
+use embedded_hal::delay::DelayNs;
 
 use crate::interface::{I2cAddr, I2cInterface, ReadData, SpiInterface, WriteData};
 use crate::registers::Registers;
 
 use crate::types::{
-    AccConf, AccOffsets, AccRange, AccSelfTest, AuxConf, AuxData, AuxIfConf, AxisData, Burst, Cmd,
-    Data, Drv, Error, ErrorReg, ErrorRegMsk, Event, FifoConf, FifoDowns, GyrConf, GyrCrtConf,
-    GyrOffsets, GyrRange, GyrSelfTest, IfConf, IntIoCtrl, IntLatch, IntMapData, IntMapFeat,
-    InternalError, InternalStatus, InterruptStatus, NvConf, PullUpConf, PwrConf, PwrCtrl,
-    Saturation, Status, WristGestureActivity, FIFO_LENGTH_1_MASK,
+    AccConf, AccOffsets, AccRange, AccSelfTest, AuxConf, AuxData, AuxIfConf, AxisData, Burst, Cmd, Data, Drv, Error, ErrorReg, ErrorRegMsk, Event, FifoConf, FifoDowns, GyrConf, GyrCrtConf, GyrOffsets, GyrRange, GyrSelfTest, IfConf, IntIoCtrl, IntLatch, IntMapData, IntMapFeat, InternalError, InternalStatus, InterruptStatus, NvConf, PullUpConf, PwrConf, PwrCtrl, Saturation, Status, WristGestureActivity, BMI160_CHIP_ID, BMI260_CHIP_ID, BMI270_CHIP_ID, FIFO_LENGTH_1_MASK,
 };
 
-pub struct Bmi2<I> {
+pub struct Bmi2<I, D, const N: usize> {
     iface: I,
     max_burst: u16,
+    delay: D,
 }
 
-impl<I2C> Bmi2<I2cInterface<I2C>> {
+impl<I2C, D, const N: usize> Bmi2<I2cInterface<I2C>, D, N> {
     /// Create a new Bmi270 device with I2C communication.
-    pub fn new_i2c(i2c: I2C, address: I2cAddr, burst: Burst) -> Self {
+    pub fn new_i2c(i2c: I2C, delay: D, address: I2cAddr, burst: Burst) -> Self {
         Bmi2 {
             iface: I2cInterface {
                 i2c,
                 address: address.addr(),
             },
             max_burst: burst.val(),
+            delay,
         }
     }
 
@@ -34,12 +33,16 @@ impl<I2C> Bmi2<I2cInterface<I2C>> {
     }
 }
 
-impl<SPI> Bmi2<SpiInterface<SPI>> {
+impl<SPI, D, const N: usize> Bmi2<SpiInterface<SPI>, D, N> 
+where 
+    D: DelayNs
+{
     /// Create a new Bmi270 device with SPI communication.
-    pub fn new_spi(spi: SPI, burst: Burst) -> Self {
+    pub fn new_spi(spi: SPI, delay: D, burst: Burst) -> Self {
         Bmi2 {
             iface: SpiInterface { spi },
             max_burst: burst.val(),
+            delay,
         }
     }
 
@@ -49,9 +52,10 @@ impl<SPI> Bmi2<SpiInterface<SPI>> {
     }
 }
 
-impl<I, CommE> Bmi2<I>
+impl<I, D, CommE, const N: usize> Bmi2<I, D, N>
 where
     I: ReadData<Error = Error<CommE>> + WriteData<Error = Error<CommE>>,
+    D: embedded_hal::delay::DelayNs, // Add constraint for D
 {
     /// Get the chip id.
     pub fn get_chip_id(&mut self) -> Result<u8, Error<CommE>> {
@@ -703,46 +707,109 @@ where
         Ok(())
     }
 
+    /// Disable power save mode.
+    pub fn disable_power_save(&mut self) -> Result<(), Error<CommE>> {
+        let mut pwr_conf = self.get_pwr_conf()?;
+        pwr_conf.power_save = false;
+        self.set_pwr_conf(pwr_conf)?;
+        // Critical delay after disabling power save
+        self.delay.delay_us(450);
+        Ok(())
+    }
+
+    /// Enable power save mode.
+    pub fn enable_power_save(&mut self) -> Result<(), Error<CommE>> {
+        let mut pwr_conf = self.get_pwr_conf()?;
+        pwr_conf.power_save = true;
+        self.set_pwr_conf(pwr_conf)?;
+        // Critical delay after enabling power save
+        self.delay.delay_us(450);
+        Ok(())
+    }
+
+
     /// Initialize sensor.
     pub fn init(&mut self, config_file: &[u8]) -> Result<(), Error<CommE>> {
+        // Verify chip ID first
+        let chip_id = self.iface.read_reg(Registers::CHIP_ID)?;
+        if !(chip_id == BMI160_CHIP_ID || chip_id == BMI260_CHIP_ID || chip_id == BMI270_CHIP_ID) {
+            return Err(Error::InvalidChipId); 
+        }
+
+        // Reset Chip, mandatory per datasheet
+        self.send_cmd(Cmd::SoftReset)?;
+        self.delay.delay_us(2000);
 
         // Disable advanced power mode
-        let mut pwr_conf = self.get_pwr_conf()?;
-        pwr_conf.power_save = false;
-        self.set_pwr_conf(pwr_conf)?;
+        self.disable_power_save()?;
+
+        // Verify buffer size
+        if self.max_burst as usize > N {
+            return Err(Error::<CommE>::BufferTooSmall);
+        }
 
-        // TODO allow config of pre alloc
-        let mut preallocated_space = alloc_stack!([u8; 512]);
+        let mut preallocated_space = alloc_stack!([u8; N]);
         let mut vec = FixedVec::new(&mut preallocated_space);
 
+        // Offset and burst calculation
         let mut offset = 0u16;
         let max_len = config_file.len() as u16;
-        let burst = self.max_burst - 1; // Remove 1 for address byte
-
-        self.set_init_ctrl(0)?;
+        let burst = if self.max_burst % 2 == 0 {
+            self.max_burst - 1  // Address byte + even number of data bytes
+        } else {
+            self.max_burst - 2  // Make sure we have even data bytes
+        };
+
+        let init_ctrl= self.get_init_ctrl()?;
+        self.set_init_ctrl(init_ctrl & 0b1111_1110)?;
+        self.delay.delay_us(450); 
 
         while offset < max_len {
-            self.set_init_addr(offset / 2)?;
-
-            let end = if (offset + burst) > max_len {
-                max_len
+            // INIT_ADDR should point to 16-bit words
+            self.set_init_addr(offset / 2)?;  // needs to be divided by 2 because offset is in bytes
+
+            // Ensure we're writing complete 16-bit words
+            let mut chunk_size = burst;
+            if (chunk_size % 2) != 0 {
+                // If burst size would result in odd number of bytes, reduce by 1
+                chunk_size -= 1;
+            }
+
+            let end = if (offset + chunk_size) > max_len {
+                // For the last chunk, ensure we still write complete 16-bit words
+                let remaining = max_len - offset;
+                offset + (remaining - (remaining % 2))
             } else {
-                offset + burst
+                offset + chunk_size
             };
-
+
+            vec.clear();
             vec.push(Registers::INIT_DATA)
-                .map_err(|_| Error::<CommE>::Alloc)?;
-
+                .map_err(|_| Error::Alloc)?;
+            
             vec.push_all(&config_file[offset as usize..end as usize])
-                .map_err(|_| Error::<CommE>::Alloc)?;
-
-            self.iface.write(&mut vec.as_mut_slice())?;
-
-            offset += burst;
-            vec.clear();
+                .map_err(|_| Error::Alloc)?;
+            
+            self.iface.write(vec.as_mut_slice())?;
+            
+            offset += chunk_size;
+            self.delay.delay_us(2);
         }
 
+        // This operation must not be performed more than once after POR or soft reset.
         self.set_init_ctrl(1)?;
+        self.delay.delay_us(2);
+
+        self.enable_power_save()?;
+
+        // Initialization takes at most 20ms per datasheet
+        self.delay.delay_us(20_000);
+
+        let internal_status = self.iface.read_reg(Registers::INTERNAL_STATUS)?;
+
+        if internal_status & 0b0000_0001 != 1 {
+            return Err(Error::InitFailed);
+        }
 
         Ok(())
     }

src/interface.rs

@@ -75,18 +75,18 @@ where
         payload[0] += 0x80;
 
         // `write` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.write(&payload).map_err(Error::Comm);
+
 
-        res
+        self.spi.write(payload).map_err(Error::Comm)
     }
 
     fn write_reg(&mut self, register: u8, data: u8) -> Result<(), Self::Error> {
         let payload: [u8; 2] = [register + 0x80, data];
 
         // `write` asserts and deasserts CS for us. No need to do it manually!
-        let res = self.spi.write(&payload).map_err(Error::Comm);
+
 
-        res
+        self.spi.write(&payload).map_err(Error::Comm)
     }
 }
 

src/registers.rs

@@ -15,6 +15,7 @@ impl Registers {
     pub const INTERNAL_STATUS: u8 = 0x21;
     pub const TEMPERATURE_0: u8 = 0x22;
     pub const FIFO_LENGTH_0: u8 = 0x24;
+    #[allow(dead_code)]
     pub const FIFO_DATA: u8 = 0x26;
     pub const ACC_CONF: u8 = 0x40;
     pub const ACC_RANGE: u8 = 0x41;

src/types.rs

@@ -1,33 +1,66 @@
+pub const BMI160_CHIP_ID: u8 = 0xD1;
+pub const BMI260_CHIP_ID: u8 = 0x27;
+pub const BMI270_CHIP_ID: u8 = 0x24;
+
 /// The possible errors that could be encountered.
 #[derive(Debug)]
 pub enum Error<CommE> {
     /// Communication error over I2C or SPI.
     Comm(CommE),
     /// Memory allocation error during initialization.
     Alloc,
+    /// Invalid Chip Id.
+    InvalidChipId,
+    /// Buffer too small.
+    BufferTooSmall,
+    /// Initialization Failed.
+    InitFailed,
 }
 
 /// Data burst.
-pub enum Burst {
-    /// Burst of 512 bytes.
+pub struct Burst {
+    max: u16,
+    kind: BurstKind,
+}
+
+enum BurstKind {
     Max,
-    /// An other burst amount under 512 bytes.
     Other(u16),
 }
 
-impl Default for Burst {
+impl Default for BurstKind {
     fn default() -> Self {
-        Burst::Max
+        Self::Max
     }
 }
 
 impl Burst {
-    pub fn val(self) -> u16 {
-        match self {
-            Burst::Max => 512,
-            Burst::Other(v) => v % 512,
+    pub fn new(max: u16) -> Self {
+        Self {
+            max,
+            kind: BurstKind::default(),
         }
     }
+
+    pub fn custom(max: u16, value: u16) -> Self {
+        Self {
+            max,
+            kind: BurstKind::Other(value),
+        }
+    }
+
+    pub fn val(&self) -> u16 {
+        match self.kind {
+            BurstKind::Max => self.max,
+            BurstKind::Other(v) => v.min(self.max),
+        }
+    }
+}
+
+impl Default for Burst {
+    fn default() -> Self {
+        Self::new(512)
+    }
 }
 
 pub struct ErrRegMask;