SNS - I2C ToF

boards/stm32f767zi/src/io/i2c.rs

@@ -18,6 +18,22 @@ impl<'d> HypedI2c for Stm32f767ziI2c<'d> {
         }
     }
 
+    /// Read a byte from a register with a 16-bit address on a device
+    fn read_byte_16(&mut self, device_address: u8, register_address: u16) -> Option<u8> {
+        let register_addr_hi = (register_address >> 8) as u8 & 0xFF;
+        let register_addr_lo = register_address as u8 & 0xFF;
+        let mut read = [0];
+        let result = self.i2c.blocking_write_read(
+            device_address,
+            [register_addr_hi, register_addr_lo].as_ref(),
+            &mut read,
+        );
+        match result {
+            Ok(_) => Some(read[0]),
+            Err(_) => None,
+        }
+    }
+
     /// Write a byte to a register on a device
     fn write_byte_to_register(
         &mut self,
@@ -41,6 +57,33 @@ impl<'d> HypedI2c for Stm32f767ziI2c<'d> {
             }),
         }
     }
+
+    // Write a byte to a register with a 16-bit address on a device
+    fn write_byte_to_register_16(
+        &mut self,
+        device_address: u8,
+        register_address: u16,
+        data: u8,
+    ) -> Result<(), I2cError> {
+        let register_addr_hi = (register_address >> 8) as u8;
+        let register_addr_lo = register_address as u8;
+        let result = self.i2c.blocking_write(
+            device_address,
+            [register_addr_hi, register_addr_lo, data].as_ref(),
+        );
+        match result {
+            Ok(_) => Ok(()),
+            Err(e) => Err(match e {
+                embassy_stm32::i2c::Error::Bus => I2cError::Bus,
+                embassy_stm32::i2c::Error::Arbitration => I2cError::Arbitration,
+                embassy_stm32::i2c::Error::Nack => I2cError::Nack,
+                embassy_stm32::i2c::Error::Timeout => I2cError::Timeout,
+                embassy_stm32::i2c::Error::Crc => I2cError::Crc,
+                embassy_stm32::i2c::Error::Overrun => I2cError::Overrun,
+                embassy_stm32::i2c::Error::ZeroLengthTransfer => I2cError::ZeroLengthTransfer,
+            }),
+        }
+    }
 }
 
 impl<'d> Stm32f767ziI2c<'d> {

boards/stm32l476rg/src/bin/tof_test.rs

@@ -0,0 +1,16 @@
+#![no_std]
+#![no_main]
+
+use embassy_executor::Spawner;
+use embassy_time::{Duration, Timer};
+use hyped_boards_stm32l476rg::tasks::tof::read_tof_range;
+use {defmt_rtt as _, panic_probe as _};
+
+#[embassy_executor::main]
+async fn main(spawner: Spawner) -> ! {
+    spawner.spawn(read_tof_range()).unwrap();
+
+    loop {
+        Timer::after(Duration::from_secs(1)).await;
+    }
+}

boards/stm32l476rg/src/io/i2c.rs

@@ -18,6 +18,22 @@ impl<'d> HypedI2c for Stm32l476rgI2c<'d> {
         }
     }
 
+    /// Read a byte from a register with a 16-bit address on a device
+    fn read_byte_16(&mut self, device_address: u8, register_address: u16) -> Option<u8> {
+        let register_addr_hi = (register_address >> 8) as u8 & 0xFF;
+        let register_addr_lo = register_address as u8 & 0xFF;
+        let mut read = [0];
+        let result = self.i2c.blocking_write_read(
+            device_address,
+            [register_addr_hi, register_addr_lo].as_ref(),
+            &mut read,
+        );
+        match result {
+            Ok(_) => Some(read[0]),
+            Err(_) => None,
+        }
+    }
+
     /// Write a byte to a register on a device
     fn write_byte_to_register(
         &mut self,
@@ -41,6 +57,33 @@ impl<'d> HypedI2c for Stm32l476rgI2c<'d> {
             }),
         }
     }
+
+    // Write a byte to a register with a 16-bit address on a device
+    fn write_byte_to_register_16(
+        &mut self,
+        device_address: u8,
+        register_address: u16,
+        data: u8,
+    ) -> Result<(), I2cError> {
+        let register_addr_hi = (register_address >> 8) as u8;
+        let register_addr_lo = register_address as u8;
+        let result = self.i2c.blocking_write(
+            device_address,
+            [register_addr_hi, register_addr_lo, data].as_ref(),
+        );
+        match result {
+            Ok(_) => Ok(()),
+            Err(e) => Err(match e {
+                embassy_stm32::i2c::Error::Bus => I2cError::Bus,
+                embassy_stm32::i2c::Error::Arbitration => I2cError::Arbitration,
+                embassy_stm32::i2c::Error::Nack => I2cError::Nack,
+                embassy_stm32::i2c::Error::Timeout => I2cError::Timeout,
+                embassy_stm32::i2c::Error::Crc => I2cError::Crc,
+                embassy_stm32::i2c::Error::Overrun => I2cError::Overrun,
+                embassy_stm32::i2c::Error::ZeroLengthTransfer => I2cError::ZeroLengthTransfer,
+            }),
+        }
+    }
 }
 
 impl<'d> Stm32l476rgI2c<'d> {

boards/stm32l476rg/src/tasks.rs

@@ -1,2 +1,3 @@
 pub mod keyence;
 pub mod temperature;
+pub mod tof;

boards/stm32l476rg/src/tasks/tof.rs

@@ -0,0 +1,34 @@
+use crate::io::i2c::Stm32l476rgI2c;
+use defmt_rtt as _;
+use embassy_stm32::i2c::I2c;
+use embassy_stm32::time::Hertz;
+use hyped_sensors::tof::{TimeOfFlight, ToFAddresses};
+
+// Test task that continuously starts a single shot measurement and reads the result
+#[embassy_executor::task]
+pub async fn read_tof_range() -> ! {
+    let p = embassy_stm32::init(Default::default());
+    let i2c = I2c::new_blocking(p.I2C1, p.PB8, p.PB9, Hertz(100_000), Default::default());
+    let mut hyped_i2c = Stm32l476rgI2c::new(i2c);
+
+    let mut tof_sensor = TimeOfFlight::new(&mut hyped_i2c, ToFAddresses::Address29).expect(
+        "Failed to create Time of Flight sensor. Check the wiring and I2C address of the sensor.",
+    );
+
+    loop {
+        defmt::info!("Starting single shot measurement");
+        tof_sensor.start_ss_measure();
+        defmt::info!("Polling for range");
+        tof_sensor.poll_range().await;
+        defmt::info!("Reading range");
+
+        match tof_sensor.read_range() {
+            Some(range) => {
+                defmt::info!("Range: {:?}", range)
+            }
+            None => {
+                defmt::info!("Failed to read range")
+            }
+        }
+    }
+}

lib/io/src/i2c.rs

@@ -15,19 +15,26 @@ pub enum I2cError {
 /// I2C trait used to abstract the I2C peripheral
 pub trait HypedI2c {
     fn read_byte(&mut self, device_address: u8, register_address: u8) -> Option<u8>;
+    fn read_byte_16(&mut self, device_address: u8, register_address: u16) -> Option<u8>;
     fn write_byte_to_register(
         &mut self,
         device_address: u8,
         register_address: u8,
         data: u8,
     ) -> Result<(), I2cError>;
+    fn write_byte_to_register_16(
+        &mut self,
+        device_address: u8,
+        register_address: u16,
+        data: u8,
+    ) -> Result<(), I2cError>;
 }
 
 pub mod mock_i2c {
     use heapless::FnvIndexMap;
 
     /// A fixed-size map of I2C values, indexed by device address and register address
-    type I2cValues = FnvIndexMap<(u8, u8), Option<u8>, 16>;
+    type I2cValues = FnvIndexMap<(u8, u16), Option<u8>, 64>;
 
     /// A mock I2C instance which can be used for testing
     pub struct MockI2c {
@@ -38,18 +45,42 @@ pub mod mock_i2c {
     impl crate::i2c::HypedI2c for MockI2c {
         /// Reads a byte by looking up the device address and register address in the map
         fn read_byte(&mut self, device_address: u8, register_address: u8) -> Option<u8> {
+            self.values
+                .get(&(device_address, register_address.into()))
+                .copied()
+                .unwrap()
+        }
+
+        /// Reads a byte by looking up the device address and register address in the map
+        fn read_byte_16(&mut self, device_address: u8, register_address: u16) -> Option<u8> {
             self.values
                 .get(&(device_address, register_address))
                 .copied()
                 .unwrap()
         }
 
-        /// Writes a byte to the map so that it can be read later to check the value
+        /// Writes a byte to the map 9with 8-bit register address) so that it can be read later to check the value
         fn write_byte_to_register(
             &mut self,
             device_address: u8,
             register_address: u8,
             data: u8,
+        ) -> Result<(), super::I2cError> {
+            match self
+                .writes
+                .insert((device_address, register_address.into()), Some(data))
+            {
+                Ok(_) => Ok(()),
+                Err(_) => Err(super::I2cError::Unknown),
+            }
+        }
+
+        /// Writes a byte to the map (with 16-bit register address) so that it can be read later to check the value
+        fn write_byte_to_register_16(
+            &mut self,
+            device_address: u8,
+            register_address: u16,
+            data: u8,
         ) -> Result<(), super::I2cError> {
             match self
                 .writes

lib/sensors/Cargo.toml

@@ -5,6 +5,8 @@ edition = "2021"
 
 [dependencies]
 heapless = "0.8.0"
+defmt = "0.3.8"
+embassy-time = { version = "0.3.1", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"], git = "https://github.com/embassy-rs/embassy", rev = "1c466b81e6af6b34b1f706318cc0870a459550b7"}
 
 hyped_core = { path = "../core" }
 hyped_io = { path = "../io" }

lib/sensors/src/lib.rs

@@ -2,3 +2,4 @@
 
 pub mod keyence;
 pub mod temperature;
+pub mod tof;

lib/sensors/src/temperature.rs

@@ -137,7 +137,7 @@ mod tests {
         let _ = Temperature::new(&mut i2c, TemperatureAddresses::Address3f);
         assert_eq!(
             i2c.get_writes()
-                .get(&(TemperatureAddresses::Address3f as u8, STTS22H_CTRL)),
+                .get(&(TemperatureAddresses::Address3f as u8, STTS22H_CTRL.into())),
             Some(&Some(STTS22H_CONFIG_SETTINGS))
         );
     }
@@ -146,11 +146,17 @@ mod tests {
     fn test_temperature_read_0() {
         let mut i2c_values = FnvIndexMap::new();
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_DATA_TEMP_H),
+            (
+                TemperatureAddresses::Address3f as u8,
+                STTS22H_DATA_TEMP_H as u16,
+            ),
             Some(0x00),
         );
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_DATA_TEMP_L),
+            (
+                TemperatureAddresses::Address3f as u8,
+                STTS22H_DATA_TEMP_L as u16,
+            ),
             Some(0x00),
         );
         let mut i2c = MockI2c::new(i2c_values);
@@ -162,11 +168,17 @@ mod tests {
     fn test_temperature_read_25() {
         let mut i2c_values = FnvIndexMap::new();
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_DATA_TEMP_H),
+            (
+                TemperatureAddresses::Address3f as u8,
+                STTS22H_DATA_TEMP_H as u16,
+            ),
             Some(0x09),
         );
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_DATA_TEMP_L),
+            (
+                TemperatureAddresses::Address3f as u8,
+                STTS22H_DATA_TEMP_L as u16,
+            ),
             Some(0xc4),
         );
         let mut i2c = MockI2c::new(i2c_values);
@@ -178,11 +190,17 @@ mod tests {
     fn test_temperature_read_minus_10() {
         let mut i2c_values = FnvIndexMap::new();
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_DATA_TEMP_H),
+            (
+                TemperatureAddresses::Address3f as u8,
+                STTS22H_DATA_TEMP_H as u16,
+            ),
             Some(0xfc),
         );
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_DATA_TEMP_L),
+            (
+                TemperatureAddresses::Address3f as u8,
+                STTS22H_DATA_TEMP_L as u16,
+            ),
             Some(0x18),
         );
         let mut i2c = MockI2c::new(i2c_values);
@@ -194,7 +212,7 @@ mod tests {
     fn test_temperature_status_busy() {
         let mut i2c_values = FnvIndexMap::new();
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_STATUS),
+            (TemperatureAddresses::Address3f as u8, STTS22H_STATUS.into()),
             Some(STTS22H_STATUS_BUSY),
         );
         let mut i2c = MockI2c::new(i2c_values);
@@ -206,7 +224,7 @@ mod tests {
     fn test_temperature_status_temp_over_upper_limit() {
         let mut i2c_values = FnvIndexMap::new();
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_STATUS),
+            (TemperatureAddresses::Address3f as u8, STTS22H_STATUS.into()),
             Some(STTS22H_TEMP_OVER_UPPER_LIMIT),
         );
         let mut i2c = MockI2c::new(i2c_values);
@@ -218,7 +236,7 @@ mod tests {
     fn test_temperature_status_temp_under_lower_limit() {
         let mut i2c_values = FnvIndexMap::new();
         let _ = i2c_values.insert(
-            (TemperatureAddresses::Address3f as u8, STTS22H_STATUS),
+            (TemperatureAddresses::Address3f as u8, STTS22H_STATUS.into()),
             Some(STTS22H_TEMP_UNDER_LOWER_LIMIT),
         );
         let mut i2c = MockI2c::new(i2c_values);