Add accelerometer as optional

firmware/src/accel.rs

@@ -12,38 +12,66 @@ pub struct Accelerometer {
     lsm: Lsm6ds33<twim::Twim<'static, TWISPI0>>,
 }
 
+pub enum AccelError {
+    Init,
+}
+
 impl Accelerometer {
-    pub fn new(twi: TWISPI0, sda: P0_12, scl: P0_11) -> Self {
+    pub fn new(twi: TWISPI0, sda: P0_12, scl: P0_11) -> Result<Self, AccelError> {
         let mut config = twim::Config::default();
         config.frequency = twim::Frequency::K100;
         let irq = interrupt::take!(SPIM0_SPIS0_TWIM0_TWIS0_SPI0_TWI0);
         let i2c = twim::Twim::new(twi, irq, sda, scl, config);
 
         //let mut adxl = adxl343::Adxl343::new(i2c).unwrap();
-        let mut lsm = lsm6ds33::Lsm6ds33::new(i2c, 0x6A).unwrap();
+        let mut lsm = lsm6ds33::Lsm6ds33::new(i2c, 0x6A).map_err(|_| AccelError::Init)?;
         lsm.set_accelerometer_output(lsm6ds33::AccelerometerOutput::Rate13)
-            .unwrap();
+            .map_err(|_| AccelError::Init)?;
         lsm.set_accelerometer_scale(lsm6ds33::AccelerometerScale::G04)
-            .unwrap();
-        Self { lsm }
+            .map_err(|_| AccelError::Init)?;
+        Ok(Self { lsm })
     }
 }
 
+pub struct Read;
+
+#[derive(Clone, Copy)]
+pub struct AccelValues {
+    pub x: i16,
+    pub y: i16,
+    pub z: i16,
+}
+
 impl Actor for Accelerometer {
+    type Message<'m> = Read;
+    type Response = Option<AccelValues>;
     type OnMountFuture<'m, M>
     where
         Self: 'm,
         M: 'm,
     = impl Future<Output = ()> + 'm;
-    fn on_mount<'m, M>(&'m mut self, _: Address<Self>, _: &'m mut M) -> Self::OnMountFuture<'m, M>
+    fn on_mount<'m, M>(
+        &'m mut self,
+        _: Address<Self>,
+        inbox: &'m mut M,
+    ) -> Self::OnMountFuture<'m, M>
     where
         M: Inbox<Self> + 'm,
         Self: 'm,
     {
         async move {
             loop {
-                if let Ok((x, y, z)) = self.lsm.read_accelerometer() {
-                    info!("Result: x: {}, y: {}, z: {}", x, y, z);
+                if let Some(mut m) = inbox.next().await {
+                    let response = if let Ok((x, y, z)) = self.lsm.read_accelerometer() {
+                        info!("Accel: x: {}, y: {}, z: {}", x, y, z);
+                        let x = (x * i16::MAX as f32) as i16;
+                        let y = (y * i16::MAX as f32) as i16;
+                        let z = (z * i16::MAX as f32) as i16;
+                        Some(AccelValues { x, y, z })
+                    } else {
+                        None
+                    };
+                    m.set_response(response);
                 }
             }
         }

firmware/src/gatt.rs

@@ -1,5 +1,8 @@
-use crate::analog::{AnalogSensors, Read as AnalogRead};
 use crate::counter::{Counter, CounterMessage};
+use crate::{
+    accel::{Accelerometer, Read as AccelRead},
+    analog::{AnalogSensors, Read as AnalogRead},
+};
 use core::future::Future;
 use drogue_device::{Actor, Address, Inbox};
 use nrf_softdevice::ble::Connection;
@@ -84,6 +87,7 @@ pub struct BurrBoardMonitor {
     analog: Address<AnalogSensors>,
     button_a: Address<Counter>,
     button_b: Address<Counter>,
+    accel: Option<Address<Accelerometer>>,
     connections: Vec<Connection, 2>,
     notifications: Notifications,
 }
@@ -103,6 +107,7 @@ impl BurrBoardMonitor {
     pub fn new(
         service: &'static BurrBoardService,
         analog: Address<AnalogSensors>,
+        accel: Option<Address<Accelerometer>>,
         button_a: Address<Counter>,
         button_b: Address<Counter>,
     ) -> Self {
@@ -111,6 +116,7 @@ impl BurrBoardMonitor {
             connections: Vec::new(),
             ticker: Ticker::every(Duration::from_secs(10)),
             analog,
+            accel,
             button_a,
             button_b,
             notifications: Notifications {
@@ -212,6 +218,11 @@ impl Actor for BurrBoardMonitor {
                         }
                     }
                     Either::Right((_, _)) => {
+                        let accel = if let Some(accel) = self.accel {
+                            accel.request(AccelRead).unwrap().await
+                        } else {
+                            None
+                        };
                         let analog = self.analog.request(AnalogRead).unwrap().await;
                         let button_a_presses = self
                             .button_a
@@ -233,6 +244,12 @@ impl Actor for BurrBoardMonitor {
                         self.service.button_a_set(button_a_presses);
                         self.service.button_b_set(button_b_presses);
 
+                        if let Some(accel) = accel {
+                            self.service.accel_x_set(accel.x);
+                            self.service.accel_y_set(accel.y);
+                            self.service.accel_z_set(accel.z);
+                        }
+
                         for c in self.connections.iter() {
                             if self.notifications.temperature {
                                 self.service.temperature_notify(&c, temperature).unwrap();
@@ -253,6 +270,18 @@ impl Actor for BurrBoardMonitor {
                             if self.notifications.button_b {
                                 self.service.button_b_notify(&c, button_b_presses).unwrap();
                             }
+
+                            if let Some(accel) = accel {
+                                if self.notifications.accel_x {
+                                    self.service.accel_x_notify(&c, accel.x);
+                                }
+                                if self.notifications.accel_y {
+                                    self.service.accel_y_notify(&c, accel.y);
+                                }
+                                if self.notifications.accel_z {
+                                    self.service.accel_z_notify(&c, accel.z);
+                                }
+                            }
                         }
                     }
                 }

firmware/src/main.rs

@@ -136,8 +136,10 @@ async fn main(s: embassy::executor::Spawner, p: Peripherals) {
     Timer::after(Duration::from_millis(500)).await;
 
     // Actor for accelerometer
-    // static ACCEL: ActorContext<Accelerometer> = ActorContext::new();
-    //let _accel = ACCEL.mount(s, Accelerometer::new(p.TWISPI0, p.P0_12, p.P0_11));
+    static ACCEL: ActorContext<Accelerometer> = ActorContext::new();
+    let accel: Option<Address<Accelerometer>> = Accelerometer::new(p.TWISPI0, p.P0_12, p.P0_11)
+        .map(|a| ACCEL.mount(s, a))
+        .ok();
 
     // Actor for all analog sensors
     static ANALOG: ActorContext<AnalogSensors> = ActorContext::new();
@@ -205,7 +207,7 @@ async fn main(s: embassy::executor::Spawner, p: Peripherals) {
         static MONITOR: ActorContext<BurrBoardMonitor> = ActorContext::new();
         let monitor = MONITOR.mount(
             s,
-            BurrBoardMonitor::new(&server.board, analog, counter_a, counter_b),
+            BurrBoardMonitor::new(&server.board, analog, accel, counter_a, counter_b),
         );
         s.spawn(bluetooth_task(
             sd,