Use SBG sensor status (#47)

* wip * Add GPS status and IMU flags getter. * feat: add option to sensor fields * feat: implement Arbitrary for messages in std * Resolve conflict * Fix proptest std test feature * Seperate Data and increase testing thread size * Update workflow * Fix workflow * add macro for common derives in the messages crate * Update libraries/sbg-rs/src/data_conversion.rs Co-authored-by: Jonathan Papineau <[email protected]> * Update libraries/sbg-rs/src/data_conversion.rs Co-authored-by: Jonathan Papineau <[email protected]> * Update libraries/sbg-rs/src/data_conversion.rs Co-authored-by: Jonathan Papineau <[email protected]> * Update libraries/sbg-rs/src/data_conversion.rs Co-authored-by: Jonathan Papineau <[email protected]> --------- Co-authored-by: Noah Sprenger <[email protected]> Co-authored-by: Noah Sprenger <[email protected]>
uorocketry · Mar 19, 2024 · ebef0e4 · ebef0e4
1 parent 8defbad
commit ebef0e4
Show file tree

Hide file tree

Showing 25 changed files with 844 additions and 395 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -46,6 +46,22 @@ jobs:
 #        with:
 #          command: make
 #          args: test-device --no-run
+  test:
+    runs-on: ubuntu-latest
+    env:
+      RUST_MIN_STACK: 8388608
+    steps:
+      - uses: actions/checkout@v2
+      - name: Install packages
+        run: sudo apt update && sudo apt install -y cmake
+      - uses: actions-rs/toolchain@v1
+        with:
+          toolchain: stable
+      - uses: actions-rs/cargo@v1
+        name: "Install cargo-make"
+        with:
+          command: install
+          args: --force cargo-make
       - uses: actions-rs/cargo@v1
         name: "Run Host Tests"
         with:

diff --git a/Cargo.lock b/Cargo.lock
diff --git a/Cargo.toml b/Cargo.toml
@@ -1,4 +1,5 @@
 [workspace]
+resolver = "2"
 
 members = [
     "boards/*",

diff --git a/Makefile.toml b/Makefile.toml
@@ -22,7 +22,8 @@ dependencies = [
 
 [tasks.test-messages]
 command = "cargo"
-args = ["test", "-p", "messages", "--target", "${CARGO_MAKE_RUST_TARGET_TRIPLE}"]
+args = ["test", "-p", "messages", "--target", "${CARGO_MAKE_RUST_TARGET_TRIPLE}", "--features", "std", "--no-default-features"]
+env = {RUST_MIN_STACK = "8388608"}
 
 # -----------------------
 # Embedded Testing

diff --git a/boards/beacon/src/data_manager.rs b/boards/beacon/src/data_manager.rs
@@ -7,13 +7,16 @@ pub struct DataManager {
     pub air: Option<Message>,
     pub ekf_nav_1: Option<Message>,
     pub ekf_nav_2: Option<Message>,
+    pub ekf_nav_acc: Option<Message>,
     pub ekf_quat: Option<Message>,
     pub imu_1: Option<Message>,
     pub imu_2: Option<Message>,
     pub utc_time: Option<Message>,
     pub gps_vel: Option<Message>,
+    pub gps_vel_acc: Option<Message>,
     pub gps_pos_1: Option<Message>,
     pub gps_pos_2: Option<Message>,
+    pub gps_pos_acc: Option<Message>,
     pub state: Option<StateData>,
     pub logging_rate: Option<RadioRate>,
 }
@@ -24,13 +27,16 @@ impl DataManager {
             air: None,
             ekf_nav_1: None,
             ekf_nav_2: None,
+            ekf_nav_acc: None,
             ekf_quat: None,
             imu_1: None,
             imu_2: None,
             utc_time: None,
             gps_vel: None,
+            gps_vel_acc: None,
             gps_pos_1: None,
             gps_pos_2: None,
+            gps_pos_acc: None,
             state: None,
             logging_rate: Some(RadioRate::Slow), // start slow.
         }
@@ -47,18 +53,21 @@ impl DataManager {
     }
 
     /// Do not clone instead take to reduce CPU load.
-    pub fn take_sensors(&mut self) -> [Option<Message>; 10] {
+    pub fn take_sensors(&mut self) -> [Option<Message>; 13] {
         [
             self.air.take(),
             self.ekf_nav_1.take(),
             self.ekf_nav_2.take(),
+            self.ekf_nav_acc.take(),
             self.ekf_quat.take(),
             self.imu_1.take(),
             self.imu_2.take(),
             self.utc_time.take(),
             self.gps_vel.take(),
+            self.gps_vel_acc.take(),
             self.gps_pos_1.take(),
             self.gps_pos_2.take(),
+            self.gps_pos_acc.take(),
         ]
     }
 
@@ -68,6 +77,12 @@ impl DataManager {
     pub fn handle_data(&mut self, data: Message) {
         match data.data {
             messages::Data::Sensor(ref sensor) => match sensor.data {
+                messages::sensor::SensorData::EkfNavAcc(_) => {
+                    self.ekf_nav_acc = Some(data);
+                }
+                messages::sensor::SensorData::GpsPosAcc(_) => {
+                    self.gps_pos_acc = Some(data);
+                }
                 messages::sensor::SensorData::Air(_) => {
                     self.air = Some(data);
                 }
@@ -83,6 +98,9 @@ impl DataManager {
                 messages::sensor::SensorData::GpsVel(_) => {
                     self.gps_vel = Some(data);
                 }
+                messages::sensor::SensorData::GpsVelAcc(_) => {
+                    self.gps_vel_acc = Some(data);
+                }
                 messages::sensor::SensorData::Imu1(_) => {
                     self.imu_1 = Some(data);
                 }

diff --git a/boards/camera/src/data_manager.rs b/boards/camera/src/data_manager.rs
@@ -61,7 +61,10 @@ impl DataManager {
     }
     pub fn is_launched(&self) -> bool {
         match self.air.as_ref() {
-            Some(air) => air.altitude > HEIGHT_MIN,
+            Some(air) => match air.altitude {
+                Some(altitude) => altitude > HEIGHT_MIN,
+                None => false,
+            }
             None => false,
         }
     }
@@ -100,7 +103,10 @@ impl DataManager {
     }
     pub fn is_below_main(&self) -> bool {
         match self.air.as_ref() {
-            Some(air) => air.altitude < MAIN_HEIGHT,
+            Some(air) => match air.altitude {
+                Some(altitude) => altitude < MAIN_HEIGHT,
+                None => false
+            }
             None => false,
         }
     }

diff --git a/boards/communication/src/data_manager.rs b/boards/communication/src/data_manager.rs
@@ -7,13 +7,16 @@ pub struct DataManager {
     pub air: Option<Message>,
     pub ekf_nav_1: Option<Message>,
     pub ekf_nav_2: Option<Message>,
+    pub ekf_nav_acc: Option<Message>,
     pub ekf_quat: Option<Message>,
     pub imu_1: Option<Message>,
     pub imu_2: Option<Message>,
     pub utc_time: Option<Message>,
     pub gps_vel: Option<Message>,
+    pub gps_vel_acc: Option<Message>,
     pub gps_pos_1: Option<Message>,
     pub gps_pos_2: Option<Message>,
+    pub gps_pos_acc: Option<Message>,
     pub state: Option<StateData>,
     pub logging_rate: Option<RadioRate>,
 }
@@ -24,13 +27,16 @@ impl DataManager {
             air: None,
             ekf_nav_1: None,
             ekf_nav_2: None,
+            ekf_nav_acc: None,
             ekf_quat: None,
             imu_1: None,
             imu_2: None,
             utc_time: None,
             gps_vel: None,
+            gps_vel_acc: None,
             gps_pos_1: None,
             gps_pos_2: None,
+            gps_pos_acc: None,
             state: None,
             logging_rate: Some(RadioRate::Slow), // start slow.
         }
@@ -47,18 +53,21 @@ impl DataManager {
     }
 
     /// Do not clone instead take to reduce CPU load.
-    pub fn take_sensors(&mut self) -> [Option<Message>; 10] {
+    pub fn take_sensors(&mut self) -> [Option<Message>; 13] {
         [
             self.air.take(),
             self.ekf_nav_1.take(),
             self.ekf_nav_2.take(),
+            self.ekf_nav_acc.take(),
             self.ekf_quat.take(),
             self.imu_1.take(),
             self.imu_2.take(),
             self.utc_time.take(),
             self.gps_vel.take(),
+            self.gps_vel_acc.take(),
             self.gps_pos_1.take(),
             self.gps_pos_2.take(),
+            self.gps_pos_acc.take(),
         ]
     }
 
@@ -68,6 +77,12 @@ impl DataManager {
     pub fn handle_data(&mut self, data: Message) {
         match data.data {
             messages::Data::Sensor(ref sensor) => match sensor.data {
+                messages::sensor::SensorData::EkfNavAcc(_) => {
+                    self.ekf_nav_acc = Some(data);
+                }
+                messages::sensor::SensorData::GpsPosAcc(_) => {
+                    self.gps_pos_acc = Some(data);
+                }
                 messages::sensor::SensorData::Air(_) => {
                     self.air = Some(data);
                 }
@@ -83,6 +98,9 @@ impl DataManager {
                 messages::sensor::SensorData::GpsVel(_) => {
                     self.gps_vel = Some(data);
                 }
+                messages::sensor::SensorData::GpsVelAcc(_) => {
+                    self.gps_vel_acc = Some(data);
+                }
                 messages::sensor::SensorData::Imu1(_) => {
                     self.imu_1 = Some(data);
                 }

diff --git a/boards/power/src/data_manager.rs b/boards/power/src/data_manager.rs
@@ -7,7 +7,7 @@ impl DataManager {
     pub fn new() -> Self {
         Self {}
     }
-    pub fn handle_data(&mut self, data: Message) {
+    pub fn handle_data(&mut self, _data: Message) {
         // to do
     }
 }

diff --git a/boards/recovery/src/data_manager.rs b/boards/recovery/src/data_manager.rs
@@ -75,7 +75,10 @@ impl DataManager {
     }
     pub fn is_launched(&self) -> bool {
         match self.air.as_ref() {
-            Some(air) => air.altitude > HEIGHT_MIN,
+            Some(air) => match air.altitude {
+                Some(altitude) => altitude > HEIGHT_MIN,
+                None => false,
+            }
             None => false,
         }
     }
@@ -114,7 +117,10 @@ impl DataManager {
     }
     pub fn is_below_main(&self) -> bool {
         match self.air.as_ref() {
-            Some(air) => air.altitude < MAIN_HEIGHT,
+            Some(air) => match air.altitude {
+                Some(altitude) => altitude < MAIN_HEIGHT,
+                None => false,
+            }
             None => false,
         }
     }
@@ -125,14 +131,23 @@ impl DataManager {
         match data.data {
             messages::Data::Sensor(sensor) => match sensor.data {
                 messages::sensor::SensorData::Air(air_data) => {
-                    let tup_data = (air_data.altitude, air_data.time_stamp);
-                    self.air = Some(air_data);
-                    if let Some(recent) = self.historical_barometer_altitude.recent() {
-                        if recent.1 != tup_data.1 {
+                    /*
+                        NOTE!!!
+                        There should be added a counter to check how many times 
+                        the alt is dropped, if the number is high switch to 
+                        the on board barometer. 
+                     */
+
+                    if let Some(alt) = air_data.altitude {
+                        let tup_data: (f32, u32) = (alt, air_data.time_stamp);
+                        self.air = Some(air_data);
+                        if let Some(recent) = self.historical_barometer_altitude.recent() {
+                            if recent.1 != tup_data.1 {
+                                self.historical_barometer_altitude.write(tup_data);
+                            }
+                        } else {
                             self.historical_barometer_altitude.write(tup_data);
                         }
-                    } else {
-                        self.historical_barometer_altitude.write(tup_data);
                     }
                 }
                 messages::sensor::SensorData::EkfNav1(nav1_data) => {

diff --git a/boards/sensor/src/data_manager.rs b/boards/sensor/src/data_manager.rs
@@ -1,19 +1,19 @@
 use crate::app::sleep_system;
 use common_arm::{spawn, HydraError};
 use messages::sensor::{
-    Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime,
+    Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, SensorData, UtcTime, EkfNavAcc, GpsPosAcc, GpsVelAcc,
 };
 use messages::Message;
 
 #[derive(Clone)]
 pub struct DataManager {
     pub air: Option<Air>,
-    pub ekf_nav: Option<(EkfNav1, EkfNav2)>,
+    pub ekf_nav: Option<(EkfNav1, EkfNav2, EkfNavAcc)>,
     pub ekf_quat: Option<EkfQuat>,
     pub imu: Option<(Imu1, Imu2)>,
     pub utc_time: Option<UtcTime>,
-    pub gps_vel: Option<GpsVel>,
-    pub gps_pos: Option<(GpsPos1, GpsPos2)>,
+    pub gps_vel: Option<(GpsVel, GpsVelAcc)>,
+    pub gps_pos: Option<(GpsPos1, GpsPos2, GpsPosAcc)>,
 }
 
 impl DataManager {
@@ -29,18 +29,21 @@ impl DataManager {
         }
     }
 
-    pub fn clone_sensors(&self) -> [Option<SensorData>; 10] {
+    pub fn clone_sensors(&self) -> [Option<SensorData>; 13] {
         [
             self.air.clone().map(|x| x.into()),
             self.ekf_nav.clone().map(|x| x.0.into()),
             self.ekf_nav.clone().map(|x| x.1.into()),
+            self.ekf_nav.clone().map(|x| x.2.into()),
             self.ekf_quat.clone().map(|x| x.into()),
             self.imu.clone().map(|x| x.0.into()),
             self.imu.clone().map(|x| x.1.into()),
             self.utc_time.clone().map(|x| x.into()),
-            self.gps_vel.clone().map(|x| x.into()),
+            self.gps_vel.clone().map(|x| x.0.into()),
+            self.gps_vel.clone().map(|x| x.1.into()),
             self.gps_pos.clone().map(|x| x.0.into()),
             self.gps_pos.clone().map(|x| x.1.into()),
+            self.gps_pos.clone().map(|x| x.2.into()),
         ]
     }
 

diff --git a/libraries/messages/Cargo.toml b/libraries/messages/Cargo.toml
@@ -12,7 +12,11 @@ defmt = "0.3.2"
 fugit = "0.3.6"
 heapless = "0.7.16"
 ts-rs = { version = "6.2.1", optional = true }
-mavlink = { git = "https://github.com/uorocketry/rust-mavlink", default-features = false}
+mavlink = { git = "https://github.com/uorocketry/rust-mavlink", default-features = false }
+bitflags = { version = "2.3.1", features = ["serde"] }
+proptest = { version = "1.2.0", optional = true }
+proptest-derive = { version = "0.3.0", optional = true }
+messages-proc-macros-lib = { path = "messages-proc-macros-lib" }
 
 [dev-dependencies]
 proptest = "1.2.0"
@@ -21,5 +25,5 @@ postcard = { version = "1.0.4", features = ["alloc"] }
 
 [features]
 default = ["mavlink/embedded", "mavlink/uorocketry"]
-std = ["mavlink/default"]
+std = ["mavlink/default", "dep:proptest", "dep:proptest-derive"]
 ts = ["std", "dep:ts-rs"]