diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 10545784..91b9aa8f 100644 --- 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 index 25177363..9023e0c6 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -149,6 +149,9 @@ name = "bitflags" version = "2.4.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed570934406eb16438a4e976b1b4500774099c13b8cb96eec99f620f05090ddf" +dependencies = [ + "serde", +] [[package]] name = "byteorder" @@ -768,6 +771,7 @@ checksum = "523dc4f511e55ab87b694dc30d0f820d60906ef06413f93d4d7a1385599cc149" name = "messages" version = "0.1.0" dependencies = [ + "bitflags 2.4.2", "defmt", "derive_more", "fugit", @@ -1242,6 +1246,7 @@ name = "sbg-rs" version = "0.1.0" dependencies = [ "atsamd-hal", + "bitflags 2.4.2", "cmake", "common-arm", "cortex-m", @@ -1251,6 +1256,7 @@ dependencies = [ "heapless", "messages", "nb 1.1.0", + "serde", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index 84813068..4984755c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,4 +1,5 @@ [workspace] +resolver = "2" members = [ "boards/*", diff --git a/Makefile.toml b/Makefile.toml index d91c48e3..0d9144ee 100644 --- 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 index 86f320be..6d600691 100644 --- a/boards/beacon/src/data_manager.rs +++ b/boards/beacon/src/data_manager.rs @@ -7,13 +7,16 @@ pub struct DataManager { pub air: Option, pub ekf_nav_1: Option, pub ekf_nav_2: Option, + pub ekf_nav_acc: Option, pub ekf_quat: Option, pub imu_1: Option, pub imu_2: Option, pub utc_time: Option, pub gps_vel: Option, + pub gps_vel_acc: Option, pub gps_pos_1: Option, pub gps_pos_2: Option, + pub gps_pos_acc: Option, pub state: Option, pub logging_rate: Option, } @@ -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; 10] { + pub fn take_sensors(&mut self) -> [Option; 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 index 513bf9ea..5e62aca9 100644 --- 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 index 86f320be..6d600691 100644 --- a/boards/communication/src/data_manager.rs +++ b/boards/communication/src/data_manager.rs @@ -7,13 +7,16 @@ pub struct DataManager { pub air: Option, pub ekf_nav_1: Option, pub ekf_nav_2: Option, + pub ekf_nav_acc: Option, pub ekf_quat: Option, pub imu_1: Option, pub imu_2: Option, pub utc_time: Option, pub gps_vel: Option, + pub gps_vel_acc: Option, pub gps_pos_1: Option, pub gps_pos_2: Option, + pub gps_pos_acc: Option, pub state: Option, pub logging_rate: Option, } @@ -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; 10] { + pub fn take_sensors(&mut self) -> [Option; 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 index 50fbc152..0b20b216 100644 --- 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 index 293ca0c7..605d6285 100644 --- 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 index f3e7d695..4c895db0 100644 --- 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, - pub ekf_nav: Option<(EkfNav1, EkfNav2)>, + pub ekf_nav: Option<(EkfNav1, EkfNav2, EkfNavAcc)>, pub ekf_quat: Option, pub imu: Option<(Imu1, Imu2)>, pub utc_time: Option, - pub gps_vel: Option, - 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; 10] { + pub fn clone_sensors(&self) -> [Option; 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 index fdffe472..50777345 100644 --- 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"] diff --git a/libraries/messages/messages-proc-macros-lib/Cargo.toml b/libraries/messages/messages-proc-macros-lib/Cargo.toml new file mode 100644 index 00000000..1e032687 --- /dev/null +++ b/libraries/messages/messages-proc-macros-lib/Cargo.toml @@ -0,0 +1,13 @@ +[package] +name = "messages-proc-macros-lib" +version = "0.1.0" +edition = "2021" + +[lib] +name = "messages_proc_macros_lib" +path = "src/lib.rs" +proc-macro = true + +[dependencies] +quote = "1.0.33" +serde = { version = "1.0", features = ["derive"] } \ No newline at end of file diff --git a/libraries/messages/messages-proc-macros-lib/src/lib.rs b/libraries/messages/messages-proc-macros-lib/src/lib.rs new file mode 100644 index 00000000..93861bed --- /dev/null +++ b/libraries/messages/messages-proc-macros-lib/src/lib.rs @@ -0,0 +1,23 @@ +use proc_macro::TokenStream; +use quote::quote; + +/// Simple macro to easily add derives that are common for the messages crates. +#[proc_macro_attribute] +pub fn common_derives(args: TokenStream, input: TokenStream) -> TokenStream { + let mut output = TokenStream::from(quote! { + #[derive(serde::Serialize, serde::Deserialize, Clone, Debug)] + #[cfg_attr(feature = "ts", derive(ts_rs::TS))] + #[cfg_attr(feature = "ts", ts(export))] + #[cfg_attr(test, derive(proptest_derive::Arbitrary))] + }); + + // Allow to omit the defmt::Format derive. Useful if this should be manually implemented. + if !args.to_string().contains("NoFormat") { + output.extend(TokenStream::from(quote! { + #[derive(defmt::Format)] + })); + } + + output.extend(input); + output +} diff --git a/libraries/messages/src/command.rs b/libraries/messages/src/command.rs index 2d86046f..6ad0b804 100644 --- a/libraries/messages/src/command.rs +++ b/libraries/messages/src/command.rs @@ -1,26 +1,14 @@ use crate::sender::Sender; -use defmt::Format; use derive_more::From; -use serde::{Deserialize, Serialize}; +use messages_proc_macros_lib::common_derives; -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Command { pub data: CommandData, } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub enum CommandData { DeployDrogue(DeployDrogue), DeployMain(DeployMain), @@ -28,43 +16,33 @@ pub enum CommandData { RadioRateChange(RadioRateChange), } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub struct DeployDrogue { pub val: bool, } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub struct DeployMain { pub val: bool, // Auth? } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub struct PowerDown { pub board: Sender, // This isn't proper naming !! This is the board to be powered down. Changes name of sender.rs to board.rs. } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub struct RadioRateChange { pub rate: RadioRate, } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub enum RadioRate { Fast, Slow, diff --git a/libraries/messages/src/health.rs b/libraries/messages/src/health.rs index 0c852eca..3ff228ef 100644 --- a/libraries/messages/src/health.rs +++ b/libraries/messages/src/health.rs @@ -1,17 +1,8 @@ -use defmt::Format; use derive_more::From; -use serde::{Deserialize, Serialize}; +use messages_proc_macros_lib::common_derives; -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub struct Health { pub data: HealthData, pub status: HealthState, @@ -23,29 +14,23 @@ pub struct Health { /// Error: something is wrong, and it's critical /// We need some way to quantify this concept of good and bad health. /// Could be current range or voltage range. If failover happens on the regulators that is warning. -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub enum HealthState { Nominal, Warning, Error, } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub enum HealthData { // RegulatorStatus(RegulatorStatus), HealthStatus(HealthStatus), } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub struct HealthStatus { pub v5: Option, pub v3_3: Option, diff --git a/libraries/messages/src/lib.rs b/libraries/messages/src/lib.rs index 7cfa6052..c89fb673 100644 --- a/libraries/messages/src/lib.rs +++ b/libraries/messages/src/lib.rs @@ -10,24 +10,18 @@ use crate::health::Health; use crate::sender::Sender; use crate::sensor::Sensor; use crate::state::State; -use defmt::Format; use derive_more::From; use fugit::Instant; /// This is to help control versions. pub use mavlink; -use serde::{Deserialize, Serialize}; - -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; +use messages_proc_macros_lib::common_derives; pub mod command; pub mod health; mod logging; pub mod sender; pub mod sensor; +pub mod sensor_status; pub mod state; pub const MAX_SIZE: usize = 64; @@ -36,10 +30,7 @@ pub use logging::{ErrorContext, Event, Log, LogLevel}; /// Topmost message. Encloses all the other possible messages, and is the only thing that should /// be sent over the wire. -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] +#[common_derives] pub struct Message { /// Time in milliseconds since epoch. Note that the epoch here can be arbitrary and is not the /// Unix epoch. @@ -52,10 +43,8 @@ pub struct Message { pub data: Data, } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] +#[common_derives] +#[derive(From)] #[serde(rename_all = "lowercase")] pub enum Data { State(State), diff --git a/libraries/messages/src/logging/log.rs b/libraries/messages/src/logging/log.rs index 9a3202a9..0fdfca46 100644 --- a/libraries/messages/src/logging/log.rs +++ b/libraries/messages/src/logging/log.rs @@ -1,34 +1,21 @@ use crate::logging::Event; -use defmt::Format; -use serde::{Deserialize, Serialize}; +use messages_proc_macros_lib::common_derives; -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] +#[common_derives] pub struct Log { pub level: LogLevel, pub event: Event, } -impl Log { - pub fn new(level: LogLevel, event: Event) -> Self { - Log { level, event } - } -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] +#[common_derives] pub enum LogLevel { Info, Warning, Error, } + +impl Log { + pub fn new(level: LogLevel, event: Event) -> Self { + Log { level, event } + } +} diff --git a/libraries/messages/src/logging/mod.rs b/libraries/messages/src/logging/mod.rs index d05cee3b..3106567d 100644 --- a/libraries/messages/src/logging/mod.rs +++ b/libraries/messages/src/logging/mod.rs @@ -2,23 +2,16 @@ mod log; mod macros; use core::fmt::Formatter; +use derive_more::From; use macros::{display_context, display_event}; -use serde::{Deserialize, Serialize}; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[cfg(test)] -use proptest_derive::Arbitrary; +use messages_proc_macros_lib::common_derives; pub use log::{Log, LogLevel}; /// Custom events for Hydra. These are used to send logging information to the ground-station in /// a space efficient way. -#[derive(Serialize, Deserialize, Clone, Debug)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] +#[common_derives(NoFormat)] +#[derive(Copy, From)] pub enum Event { Initialized(), MainDeploy(), @@ -33,10 +26,8 @@ display_event!( /// This is optionally used to add extra context to any errors. This information can then be sent /// to the ground station to have a more informative error message. -#[derive(Serialize, Deserialize, Clone, Debug, Copy)] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -#[cfg_attr(test, derive(Arbitrary))] +#[common_derives(NoFormat)] +#[derive(Copy, From)] pub enum ErrorContext { GroundStation, UnkownCanMessage, diff --git a/libraries/messages/src/sender.rs b/libraries/messages/src/sender.rs index 4318c4ad..2bc90999 100644 --- a/libraries/messages/src/sender.rs +++ b/libraries/messages/src/sender.rs @@ -1,17 +1,8 @@ -use defmt::Format; -use serde::{Deserialize, Serialize}; - -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; +use messages_proc_macros_lib::common_derives; // I don't agree with the naming, We can use these as Ids to sent commands to that specific board. -#[derive(Serialize, Deserialize, Clone, Debug, Format, Copy)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(Copy)] pub enum Sender { GroundStation, SensorBoard, diff --git a/libraries/messages/src/sensor.rs b/libraries/messages/src/sensor.rs index 666978b0..580bb1c3 100644 --- a/libraries/messages/src/sensor.rs +++ b/libraries/messages/src/sensor.rs @@ -1,112 +1,54 @@ -use defmt::Format; +use crate::sensor_status::{ + AirStatus, EkfStatus, GpsPositionStatus, GpsVelStatus, ImuStatus, UtcTimeStatus, +}; use derive_more::From; -use serde::{Deserialize, Serialize}; +use messages_proc_macros_lib::common_derives; -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Sensor { /// Used to differentiate between multiple components on the same sender. Unused right now. - pub component_id: u8, + // pub component_id: u8, pub data: SensorData, } -#[derive(Serialize, Deserialize, Clone, Debug, From, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +#[derive(From)] pub enum SensorData { UtcTime(UtcTime), Air(Air), EkfQuat(EkfQuat), EkfNav1(EkfNav1), EkfNav2(EkfNav2), + EkfNavAcc(EkfNavAcc), Imu1(Imu1), Imu2(Imu2), GpsVel(GpsVel), + GpsVelAcc(GpsVelAcc), GpsPos1(GpsPos1), GpsPos2(GpsPos2), -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct GpsPos1 { - #[doc = "< Time in us since the sensor power up."] - pub time_stamp: u32, - #[doc = "< GPS position status, type and bitmask."] - pub status: u32, - #[doc = "< GPS time of week in ms."] - pub time_of_week: u32, - #[doc = "< Latitude in degrees, positive north."] - pub latitude: f64, - #[doc = "< Longitude in degrees, positive east."] - pub longitude: f64, - #[doc = "< Altitude above Mean Sea Level in meters."] - pub altitude: f64, - #[doc = "< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation)."] - pub undulation: f32, -} - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] -pub struct GpsPos2 { - #[doc = "< 1 sigma latitude accuracy in meters."] - pub latitude_accuracy: f32, - #[doc = "< 1 sigma longitude accuracy in meters."] - pub longitude_accuracy: f32, - #[doc = "< 1 sigma altitude accuracy in meters."] - pub altitude_accuracy: f32, - #[doc = "< Number of space vehicles used to compute the solution (since version 1.4)."] - pub num_sv_used: u8, - #[doc = "< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4)."] - pub base_station_id: u16, - #[doc = "< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4)."] - pub differential_age: u16, + GpsPosAcc(GpsPosAcc), } /* Replace with new health monitor */ -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Regulator { pub status: bool, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Voltage { pub voltage: f32, pub rolling_avg: f32, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Current { pub current: f32, pub rolling_avg: f32, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Temperature { pub temperature: f32, pub rolling_avg: f32, @@ -114,150 +56,172 @@ pub struct Temperature { /* Replace with new health monitor */ -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +pub struct GpsPos1 { + #[doc = "< Latitude in degrees, positive north."] + pub latitude: Option, + #[doc = "< Longitude in degrees, positive east."] + pub longitude: Option, +} + +#[common_derives] +pub struct GpsPos2 { + #[doc = "< GPS time of week in ms."] + pub time_of_week: Option, + #[doc = "< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation)."] + pub undulation: Option, + #[doc = "< Altitude above Mean Sea Level in meters."] + pub altitude: Option, +} + +#[common_derives] +pub struct GpsPosAcc { + #[doc = "< Time in us since the sensor power up."] + pub time_stamp: u32, + #[doc = "< GPS position status, type and bitmask."] + pub status: GpsPositionStatus, + #[doc = "< 1 sigma latitude accuracy in meters."] + pub latitude_accuracy: Option, + #[doc = "< 1 sigma longitude accuracy in meters."] + pub longitude_accuracy: Option, + #[doc = "< 1 sigma altitude accuracy in meters."] + pub altitude_accuracy: Option, + #[doc = "< Number of space vehicles used to compute the solution (since version 1.4)."] + pub num_sv_used: Option, + #[doc = "< Base station id for differential corrections (0-4095). Set to 0xFFFF if differential corrections are not used (since version 1.4)."] + pub base_station_id: Option, + #[doc = "< Differential correction age in 0.01 seconds. Set to 0XFFFF if differential corrections are not used (since version 1.4)."] + pub differential_age: Option, +} + +#[common_derives] pub struct UtcTime { #[doc = "< Time in us since the sensor power up."] pub time_stamp: u32, #[doc = "< UTC time and clock status information"] - pub status: u16, + pub status: UtcTimeStatus, #[doc = "< Year for example: 2013."] - pub year: u16, + pub year: Option, #[doc = "< Month in year [1 .. 12]."] - pub month: i8, + pub month: Option, #[doc = "< Day in month [1 .. 31]."] - pub day: i8, + pub day: Option, #[doc = "< Hour in day [0 .. 23]."] - pub hour: i8, + pub hour: Option, #[doc = "< Minute in hour [0 .. 59]."] - pub minute: i8, + pub minute: Option, #[doc = "< Second in minute [0 .. 60]. (60 is used only when a leap second is added)"] - pub second: i8, + pub second: Option, #[doc = "< Nanosecond of current second in ns."] - pub nano_second: i32, + pub nano_second: Option, #[doc = "< GPS time of week in ms."] - pub gps_time_of_week: u32, + pub gps_time_of_week: Option, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Air { - #[doc = "< Time in us since the sensor power up OR measurement delay in us."] + #[doc = "< Time in us since the sensor power up."] pub time_stamp: u32, #[doc = "< Airdata sensor status bitmask."] - pub status: u16, + pub status: AirStatus, #[doc = "< Raw absolute pressure measured by the barometer sensor in Pascals."] - pub pressure_abs: f32, + pub pressure_abs: Option, #[doc = "< Altitude computed from barometric altimeter in meters and positive upward."] - pub altitude: f32, + pub altitude: Option, #[doc = "< Raw differential pressure measured by the pitot tube in Pascal."] - pub pressure_diff: f32, + pub pressure_diff: Option, #[doc = "< True airspeed measured by a pitot tube in m.s^-1 and positive forward."] - pub true_airspeed: f32, + pub true_airspeed: Option, #[doc = "< Outside air temperature in °C that could be used to compute true airspeed from differential pressure."] - pub air_temperature: f32, + pub air_temperature: Option, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct EkfQuat { #[doc = "< Time in us since the sensor power up."] pub time_stamp: u32, #[doc = "< Orientation quaternion stored in W, X, Y, Z form."] - pub quaternion: [f32; 4usize], + pub quaternion: Option<[f32; 4usize]>, #[doc = "< Roll, Pitch and Yaw angles 1 sigma standard deviation in rad."] - pub euler_std_dev: [f32; 3usize], + pub euler_std_dev: Option<[f32; 3usize]>, #[doc = "< EKF solution status bitmask and enum."] - pub status: u32, + pub status: EkfStatus, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] +pub struct EkfNavAcc { + #[doc = "< EKF solution status bitmask and enum."] + pub status: EkfStatus, + #[doc = "< North, East, Down velocity 1 sigma standard deviation in m.s^-1."] + pub velocity_std_dev: Option<[f32; 3usize]>, + #[doc = "< Latitude, longitude and altitude 1 sigma standard deviation in meters."] + pub position_std_dev: Option<[f32; 3usize]>, +} + +#[common_derives] pub struct EkfNav1 { #[doc = "< Time in us since the sensor power up."] pub time_stamp: u32, #[doc = "< North, East, Down velocity in m.s^-1."] - pub velocity: [f32; 3usize], - #[doc = "< North, East, Down velocity 1 sigma standard deviation in m.s^-1."] - pub velocity_std_dev: [f32; 3usize], + pub velocity: Option<[f32; 3usize]>, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct EkfNav2 { #[doc = "< Latitude, Longitude in degrees positive North and East.\nAltitude above Mean Sea Level in meters."] - pub position: [f64; 3usize], + pub position: Option<[f64; 3usize]>, #[doc = "< Altitude difference between the geoid and the Ellipsoid in meters (Height above Ellipsoid = altitude + undulation)."] - pub undulation: f32, - #[doc = "< Latitude, longitude and altitude 1 sigma standard deviation in meters."] - pub position_std_dev: [f32; 3usize], - #[doc = "< EKF solution status bitmask and enum."] - pub status: u32, + pub undulation: Option, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Imu1 { #[doc = "< Time in us since the sensor power up."] pub time_stamp: u32, #[doc = "< IMU status bitmask."] - pub status: u16, + pub status: ImuStatus, #[doc = "< X, Y, Z accelerometers in m.s^-2."] - pub accelerometers: [f32; 3usize], + pub accelerometers: Option<[f32; 3usize]>, #[doc = "< X, Y, Z gyroscopes in rad.s^-1."] - pub gyroscopes: [f32; 3usize], + pub gyroscopes: Option<[f32; 3usize]>, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct Imu2 { #[doc = "< Internal temperature in °C."] - pub temperature: f32, + pub temperature: Option, #[doc = "< X, Y, Z delta velocity in m.s^-2."] - pub delta_velocity: [f32; 3usize], + pub delta_velocity: Option<[f32; 3usize]>, #[doc = "< X, Y, Z delta angle in rad.s^-1."] - pub delta_angle: [f32; 3usize], + pub delta_angle: Option<[f32; 3usize]>, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct GpsVel { + #[doc = "< GPS time of week in ms."] + pub time_of_week: Option, #[doc = "< Time in us since the sensor power up."] pub time_stamp: u32, #[doc = "< GPS velocity status, type and bitmask."] - pub status: u32, - #[doc = "< GPS time of week in ms."] - pub time_of_week: u32, + pub status: GpsVelStatus, #[doc = "< GPS North, East, Down velocity in m.s^-1."] - pub velocity: [f32; 3usize], - #[doc = "< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1."] - pub velocity_acc: [f32; 3usize], + pub velocity: Option<[f32; 3usize]>, #[doc = "< Track ground course in degrees."] - pub course: f32, + pub course: Option, +} + +#[common_derives] +pub struct GpsVelAcc { #[doc = "< Course accuracy in degrees."] - pub course_acc: f32, + pub course_acc: Option, + #[doc = "< GPS North, East, Down velocity 1 sigma accuracy in m.s^-1."] + pub velocity_acc: Option<[f32; 3usize]>, } impl Sensor { pub fn new(data: impl Into) -> Self { Sensor { - component_id: 0, + // component_id: 0, data: data.into(), } } diff --git a/libraries/messages/src/sensor_status.rs b/libraries/messages/src/sensor_status.rs new file mode 100644 index 00000000..f9ab9a32 --- /dev/null +++ b/libraries/messages/src/sensor_status.rs @@ -0,0 +1,399 @@ +use bitflags::bitflags; +use messages_proc_macros_lib::common_derives; +use serde::{Deserialize, Serialize}; + +// ---------- +// EKF Status +// ---------- + +#[derive(Debug, Clone, Copy, PartialEq)] +pub enum EkfSolutionMode { + #[doc = "The Kalman filter is not initialized and the returned data are all invalid."] + Uninitialized, + #[doc = "The Kalman filter only rely on a vertical reference to compute roll and pitch angles. Heading and navigation data drift freely."] + VerticalGyro, + #[doc = "A heading reference is available, the Kalman filter provides full orientation but navigation data drift freely."] + Ahrs, + #[doc = "The Kalman filter computes orientation and velocity. Position is freely integrated from velocity estimation."] + NavVelocity, + #[doc = "Nominal mode, the Kalman filter computes all parameters (attitude, velocity, position). Absolute position is provided."] + NavPosition, +} + +bitflags! { + #[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Hash)] + pub struct EkfFlags: u32 { + const AttitudeValid = (0x00000001 << 4); + const HeadingValid = (0x00000001 << 5); + const VelocityValid = (0x00000001 << 6); + const PositionValid = (0x00000001 << 7); + const VertRefUsed = (0x00000001 << 8); + const MagRefUsed = (0x00000001 << 9); + const Gps1VelUsed = (0x00000001 << 10); + const Gps1PosUsed = (0x00000001 << 11); + const Gps1HdtUsed = (0x00000001 << 13); + const Gps2VelUsed = (0x00000001 << 14); + const Gps2PosUsed = (0x00000001 << 15); + const Gps2HdtUsed = (0x00000001 << 17); + const OdoUsed = (0x00000001 << 18); + const DvlBtUsed = (0x00000001 << 19); + const DvlWtUsed = (0x00000001 << 20); + const UserPosUsed = (0x00000001 << 21); + const UserVelUsed = (0x00000001 << 22); + const UserHeadingUsed = (0x00000001 << 23); + const UsblUsed = (0x00000001 << 24); + const AirDataUsed = (0x00000001 << 25); + const ZuptUsed = (0x00000001 << 26); + const AlignValid = (0x00000001 << 27); + const DepthUsed = (0x00000001 << 28); + } +} + +#[common_derives] +#[derive(Copy)] +pub struct EkfStatus { + status: u32, +} + +impl EkfStatus { + pub fn new(val: u32) -> Self { + Self { status: val } + } + + pub fn get_solution_mode(&self) -> Option { + match self.status & 0x0000000F { + 0 => Some(EkfSolutionMode::Uninitialized), + 1 => Some(EkfSolutionMode::VerticalGyro), + 2 => Some(EkfSolutionMode::Ahrs), + 3 => Some(EkfSolutionMode::NavVelocity), + 4 => Some(EkfSolutionMode::NavPosition), + _ => None, + } + } + + pub fn get_flags(&self) -> Option { + EkfFlags::from_bits(self.status & !(0x0000000F)) // check this line + } +} + +// ---------- +// UTC Time Status +// ---------- + +pub enum ClockStatus { + #[doc = "< An error has occurred on the clock estimation."] + Error, + #[doc = "< The clock is only based on the internal crystal."] + FreeRunning, + #[doc = "< A PPS has been detected and the clock is converging to it."] + Steering, + #[doc = "< The clock has converged to the PPS and is within 500ns."] + Valid, +} + +pub enum UtcStatus { + #[doc = "< The UTC time is not known, we are just propagating the UTC time internally."] + Invalid, + #[doc = "< We have received valid UTC time information but we don't have the leap seconds information."] + NoLeapSec, + #[doc = "< We have received valid UTC time data with valid leap seconds."] + Valid, +} + +#[common_derives] +#[derive(Copy)] +pub struct UtcTimeStatus { + status: u16, +} + +impl UtcTimeStatus { + pub fn new(status: u16) -> Self { + Self { status } + } + + pub fn get_clock_status(&self) -> Option { + match (self.status >> 1) & 0x000F { + 0 => Some(ClockStatus::Error), + 1 => Some(ClockStatus::FreeRunning), + 2 => Some(ClockStatus::Steering), + 3 => Some(ClockStatus::Valid), + _ => None, + } + } + + pub fn get_utc_status(&self) -> Option { + match (self.status >> 6) & 0x000F { + 0 => Some(UtcStatus::Invalid), + 1 => Some(UtcStatus::NoLeapSec), + 2 => Some(UtcStatus::Valid), + _ => None, + } + } +} + +// ---------- +// Air Status +// ---------- + +bitflags! { + #[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Hash)] + pub struct AirFlags: u16 { + const TimeIsDelay = (0x0001 << 0); + const PressureAbsValid = (0x0001 << 1); + const AltitudeValid = (0x0001 << 2); + const PressureDiffValid = (0x0001 << 3); + const AirpseedValid = (0x0001 << 4); + const TemperatureValid = (0x0001 << 5); + } +} + +#[common_derives] +#[derive(Copy)] +pub struct AirStatus { + status: u16, +} + +impl AirStatus { + pub fn new(status: u16) -> Self { + Self { status } + } + + pub fn get_flags(&self) -> Option { + AirFlags::from_bits(self.status) + } +} + +// ---------- +// IMU Status +// ---------- + +bitflags! { + #[derive(Serialize, Deserialize, Debug, PartialEq, Eq, Hash)] + pub struct ImuFlags: u16 { + const ComOk = (0x00000001 << 0); + const StatusTestPass = (0x00000001 << 1); + const AccelXTestPass = (0x00000001 << 2); + const AccelYTestPass = (0x00000001 << 3); + const AccelZTestPass = (0x00000001 << 4); + const GyroXTestPass = (0x00000001 << 5); + const GyroYTestPass = (0x00000001 << 6); + const GyroZTestPass = (0x00000001 << 7); + const AccelsInRange = (0x00000001 << 8); + const GyrosInRange = (0x00000001 << 9); + } +} + +#[common_derives] +#[derive(Copy)] +pub struct ImuStatus { + status: u16, +} + +impl ImuStatus { + pub fn new(status: u16) -> Self { + Self { status } + } + + pub fn get_flags(&self) -> Option { + ImuFlags::from_bits(self.status) + } +} + +// ---------- +// GPS Status +// ---------- + +pub enum GpsPositionStatusE { + #[doc = "< A valid solution has been computed."] + SolComputed, + #[doc = "< Not enough valid SV to compute a solution."] + InsufficientObs, + #[doc = "< An internal error has occurred."] + InternalError, + #[doc = "< The height limit has been exceeded."] + HeightLimit, +} + +pub enum GpsPositionType { + #[doc = "< No valid position solution available."] + NoSolution, + #[doc = "< An unknown solution type has been computed."] + UnknownType, + #[doc = "< Single point solution position."] + Single, + #[doc = "< Standard Pseudorange Differential Solution (DGPS)."] + PseudoRangeDiff, + #[doc = "< SBAS satellite used for differential corrections."] + Sbas, + #[doc = "< Omnistar VBS Position (L1 sub-meter)."] + OmniStar, + #[doc = "< Floating RTK ambiguity solution (20 cms RTK)."] + RtkFloat, + #[doc = "< Integer RTK ambiguity solution (2 cms RTK)."] + RtkInt, + #[doc = "< Precise Point Positioning with float ambiguities."] + PppFloat, + #[doc = "< Precise Point Positioning with fixed ambiguities."] + PppInt, + #[doc = "< Fixed location solution position."] + Fixed, +} + +pub enum GpsSatelliteUsed { + L1, + L2, + L5, + GloL1, + GloL2, + GloL3, + GalE1, + GalE5A, + GalE5B, + GalE5Alt, + GalE6, + BdsB1, + BdsB2, + BdsB3, + QzssL1, + QzssL2, + QzssL5, +} + +#[common_derives] +#[derive(Copy)] +pub struct GpsPositionStatus { + status: u32, +} + +impl GpsPositionStatus { + pub fn new(status: u32) -> Self { + Self { status } + } + + pub fn get_satellites_used(&self) -> Option<[GpsSatelliteUsed; 17]> { + unimplemented!("Need to implement this"); + } + + pub fn get_status(&self) -> Option { + match self.status & 0x0000003F { + 0 => Some(GpsPositionStatusE::SolComputed), + 1 => Some(GpsPositionStatusE::InsufficientObs), + 2 => Some(GpsPositionStatusE::InternalError), + 3 => Some(GpsPositionStatusE::HeightLimit), + _ => None, + } + } + + pub fn get_type(&self) -> Option { + match (self.status >> 6) & 0x0000003F { + 0 => Some(GpsPositionType::NoSolution), + 1 => Some(GpsPositionType::UnknownType), + 2 => Some(GpsPositionType::Single), + 3 => Some(GpsPositionType::PseudoRangeDiff), + 4 => Some(GpsPositionType::Sbas), + 5 => Some(GpsPositionType::OmniStar), + 6 => Some(GpsPositionType::RtkFloat), + 7 => Some(GpsPositionType::RtkInt), + 8 => Some(GpsPositionType::PppFloat), + 9 => Some(GpsPositionType::PppInt), + 10 => Some(GpsPositionType::Fixed), + _ => None, + } + } +} + +// ---------- +// GPS Vel Status +// ---------- + +pub enum GpsVelStatusE { + #[doc = "< A valid solution has been computed."] + SolComputed, + #[doc = "< Not enough valid SV to compute a solution."] + InsufficientObs, + #[doc = "< An internal error has occurred."] + InternalError, + #[doc = "< Velocity limit exceeded."] + VelLimit, +} + +pub enum GpsVelType { + #[doc = "< No valid velocity solution available."] + NoSolution, + #[doc = "< An unknown solution type has been computed."] + UnknownType, + #[doc = "< A Doppler velocity has been computed."] + Doppler, + #[doc = "< A differential velocity has been computed between two positions."] + Differential, +} + +#[common_derives] +#[derive(Copy)] +pub struct GpsVelStatus { + status: u32, +} + +impl GpsVelStatus { + pub fn new(status: u32) -> Self { + Self { status } + } + + pub fn get_status(&self) -> Option { + match self.status & 0x0000003F { + 0 => Some(GpsVelStatusE::SolComputed), + 1 => Some(GpsVelStatusE::InsufficientObs), + 2 => Some(GpsVelStatusE::InternalError), + 3 => Some(GpsVelStatusE::VelLimit), + _ => None, + } + } + + pub fn get_type(&self) -> Option { + match (self.status >> 6) & 0x0000003F { + 0 => Some(GpsVelType::NoSolution), + 1 => Some(GpsVelType::UnknownType), + 2 => Some(GpsVelType::Doppler), + 3 => Some(GpsVelType::Differential), + _ => None, + } + } +} + +// ---------- +// Tests +// ---------- + +#[cfg(test)] +mod tests { + use crate::sensor_status::{EkfFlags, EkfSolutionMode, EkfStatus}; + + #[test] + fn ekf_status_valid_enum_success() { + let status = EkfStatus::new(0b011); + + assert_eq!( + status.get_solution_mode(), + Some(EkfSolutionMode::NavVelocity) + ) + } + + #[test] + fn ekf_status_invalid_enum_expect_none() { + let status = EkfStatus::new(0b111); + + assert_eq!(status.get_solution_mode(), None) + } + + #[test] + fn ekf_status_mixed_enum_flags_success() { + let status = EkfStatus::new(0b00110010010); + + let expected_flags = + EkfFlags::AttitudeValid | EkfFlags::PositionValid | EkfFlags::VertRefUsed; + + assert_eq!(status.get_solution_mode(), Some(EkfSolutionMode::Ahrs)); + + assert_eq!(status.get_flags(), Some(expected_flags)); + } +} diff --git a/libraries/messages/src/state.rs b/libraries/messages/src/state.rs index 7d8ea01b..3c44a04f 100644 --- a/libraries/messages/src/state.rs +++ b/libraries/messages/src/state.rs @@ -1,24 +1,11 @@ -use defmt::Format; -use serde::{Deserialize, Serialize}; +use messages_proc_macros_lib::common_derives; -#[cfg(test)] -use proptest_derive::Arbitrary; - -#[cfg(feature = "ts")] -use ts_rs::TS; - -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub struct State { pub data: StateData, } -#[derive(Serialize, Deserialize, Clone, Debug, Format)] -#[cfg_attr(test, derive(Arbitrary))] -#[cfg_attr(feature = "ts", derive(TS))] -#[cfg_attr(feature = "ts", ts(export))] +#[common_derives] pub enum StateData { Initializing, WaitForTakeoff, diff --git a/libraries/sbg-rs/Cargo.toml b/libraries/sbg-rs/Cargo.toml index 8e3e60a1..cb2bd91e 100644 --- a/libraries/sbg-rs/Cargo.toml +++ b/libraries/sbg-rs/Cargo.toml @@ -1,20 +1,23 @@ -[package] -name = "sbg-rs" -version = "0.1.0" -edition = "2021" - -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - -[dependencies] -embedded-hal = "0.2.7" -nb = "1.0.0" -defmt = "0.3.2" -atsamd-hal = { workspace = true } -cortex-m = { workspace = true } -cortex-m-rt = "^0.7.0" -messages = { path = "../../libraries/messages" } -common-arm = { path = "../../libraries/common-arm" } -heapless = "0.7.16" - -[build-dependencies] -cmake = "0.1" \ No newline at end of file +[package] +name = "sbg-rs" +version = "0.1.0" +edition = "2021" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +embedded-hal = "0.2.7" +nb = "1.0.0" +defmt = "0.3.2" +atsamd-hal = { workspace = true } +cortex-m = { workspace = true } +serde = { workspace = true } + +cortex-m-rt = "^0.7.0" +messages = { path = "../../libraries/messages" } +common-arm = { path = "../../libraries/common-arm" } +heapless = "0.7.16" +bitflags = { version = "2.4.2", features = ["serde"] } + +[build-dependencies] +cmake = "0.1" diff --git a/libraries/sbg-rs/src/data_conversion.rs b/libraries/sbg-rs/src/data_conversion.rs index a9361231..2ff7a675 100644 --- a/libraries/sbg-rs/src/data_conversion.rs +++ b/libraries/sbg-rs/src/data_conversion.rs @@ -1,123 +1,174 @@ use crate::bindings::{ - SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsPos, SbgLogGpsVel, SbgLogImuData, - SbgLogUtcData, + SbgLogAirData, SbgLogEkfNavData, SbgLogEkfQuatData, SbgLogGpsVel, SbgLogImuData, SbgLogUtcData, SbgLogGpsPos, }; -use messages::sensor::{ - Air, EkfNav1, EkfNav2, EkfQuat, GpsPos1, GpsPos2, GpsVel, Imu1, Imu2, UtcTime, +use bitflags::Flags; +use messages::sensor::{Air, EkfNav1, EkfNav2, EkfNavAcc, EkfQuat, GpsVel, Imu1, Imu2, UtcTime, GpsPos1, GpsPos2, GpsPosAcc, GpsVelAcc}; +use messages::sensor_status::{ + AirFlags, AirStatus, EkfFlags, EkfStatus, GpsVelStatus, GpsVelStatusE, ImuFlags, ImuStatus, + UtcStatus, UtcTimeStatus, GpsPositionStatus, GpsPositionStatusE }; -impl From for (GpsPos1, GpsPos2) { +/// Simple helper function to work with the flags structure and set the fields as needed. +#[inline] +fn check(flags: &Option, test: F, value: T) -> Option +where + F: Flags, +{ + match flags { + Some(x) if x.contains(test) => Some(value), + _ => None, + } +} + +impl From for (GpsPos1, GpsPos2, GpsPosAcc) { fn from(value: SbgLogGpsPos) -> Self { + let status = GpsPositionStatus::new(value.status); + + let valid = matches!(status.get_status(), Some(GpsPositionStatusE::SolComputed)); + ( GpsPos1 { - time_stamp: value.timeStamp, - status: value.status, - time_of_week: value.timeOfWeek, - latitude: value.latitude, - longitude: value.longitude, - altitude: value.altitude, - undulation: value.undulation, + latitude: if valid {Some(value.latitude)} else {None}, + longitude: if valid {Some(value.longitude)} else {None}, }, GpsPos2 { - latitude_accuracy: value.latitudeAccuracy, - longitude_accuracy: value.longitudeAccuracy, - altitude_accuracy: value.altitudeAccuracy, - num_sv_used: value.numSvUsed, - base_station_id: value.baseStationId, - differential_age: value.differentialAge, + altitude: if valid {Some(value.altitude)} else {None}, + undulation: if valid {Some(value.undulation)} else {None}, + time_of_week: if valid {Some(value.timeOfWeek)} else {None}, }, + GpsPosAcc { + status, + time_stamp: value.timeStamp, + latitude_accuracy: if valid {Some(value.latitudeAccuracy)} else {None}, + longitude_accuracy: if valid {Some(value.longitudeAccuracy)} else {None}, + altitude_accuracy: if valid {Some(value.altitudeAccuracy)} else {None}, + num_sv_used: if valid {Some(value.numSvUsed)} else {None}, + base_station_id: if valid {Some(value.baseStationId)} else {None}, + differential_age: if valid {Some(value.differentialAge)} else {None}, + } ) } } impl From for UtcTime { fn from(value: SbgLogUtcData) -> Self { + let status = UtcTimeStatus::new(value.status); + let valid = matches!(status.get_utc_status(), Some(UtcStatus::Valid | UtcStatus::NoLeapSec)); + Self { - time_stamp: value.timeStamp, - status: value.status, - year: value.year, - month: value.month, - day: value.day, - hour: value.hour, - minute: value.minute, - second: value.second, - nano_second: value.nanoSecond, - gps_time_of_week: value.gpsTimeOfWeek, + time_stamp: value.timeStamp, // not convinced this is matched valid to the Utc Status bitmask. + status, + year: if valid { Some(value.year) } else { None }, + month: if valid { Some(value.month) } else { None }, + day: if valid { Some(value.day) } else { None }, + hour: if valid { Some(value.hour) } else { None }, + minute: if valid { Some(value.minute) } else { None }, + second: if valid { Some(value.second) } else { None }, + nano_second: if valid { Some(value.nanoSecond) } else { None }, + gps_time_of_week: if valid { + Some(value.gpsTimeOfWeek) + } else { + None + }, } } } impl From for Air { fn from(value: SbgLogAirData) -> Self { + let status = AirStatus::new(value.status); + let flags = status.get_flags(); + Self { - time_stamp: value.timeStamp, - status: value.status, - pressure_abs: value.pressureAbs, - altitude: value.altitude, - pressure_diff: value.pressureDiff, - true_airspeed: value.trueAirspeed, - air_temperature: value.airTemperature, + time_stamp: value.timeStamp, // TODO: check if valid. + status, + pressure_abs: check(&flags, AirFlags::PressureAbsValid, value.pressureAbs), + altitude: check(&flags, AirFlags::AltitudeValid, value.altitude), + pressure_diff: check(&flags, AirFlags::PressureDiffValid, value.pressureDiff), + true_airspeed: check(&flags, AirFlags::AirpseedValid, value.trueAirspeed), + air_temperature: check(&flags, AirFlags::TemperatureValid, value.airTemperature), } } } impl From for EkfQuat { fn from(value: SbgLogEkfQuatData) -> Self { + let status = EkfStatus::new(value.status); + let flags = status.get_flags(); + Self { time_stamp: value.timeStamp, - quaternion: value.quaternion, - euler_std_dev: value.eulerStdDev, - status: value.status, + quaternion: check(&flags, EkfFlags::HeadingValid, value.quaternion), + euler_std_dev: check(&flags, EkfFlags::HeadingValid, value.eulerStdDev), + status, } } } -impl From for (EkfNav1, EkfNav2) { +impl From for (EkfNav1, EkfNav2, EkfNavAcc) { fn from(value: SbgLogEkfNavData) -> Self { + let status = EkfStatus::new(value.status); + let flags = status.get_flags(); + ( EkfNav1 { time_stamp: value.timeStamp, - velocity: value.velocity, - velocity_std_dev: value.velocityStdDev, + velocity: check(&flags, EkfFlags::VelocityValid, value.velocity), + }, EkfNav2 { - position: value.position, - undulation: value.undulation, - position_std_dev: value.positionStdDev, - status: value.status, + undulation: check(&flags, EkfFlags::AttitudeValid, value.undulation), + position: check(&flags, EkfFlags::PositionValid, value.position), }, + EkfNavAcc { + velocity_std_dev: check(&flags, EkfFlags::VelocityValid, value.velocityStdDev), + position_std_dev: check(&flags, EkfFlags::PositionValid, value.positionStdDev), + status, + } ) } } impl From for (Imu1, Imu2) { fn from(value: SbgLogImuData) -> Self { + let status = ImuStatus::new(value.status); + let flags = status.get_flags(); + ( Imu1 { time_stamp: value.timeStamp, - status: value.status, - accelerometers: value.accelerometers, - gyroscopes: value.gyroscopes, + gyroscopes: check(&flags, ImuFlags::GyrosInRange, value.gyroscopes), + status, + accelerometers: check(&flags, ImuFlags::AccelsInRange, value.accelerometers), }, Imu2 { - temperature: value.temperature, - delta_velocity: value.deltaVelocity, - delta_angle: value.deltaAngle, + temperature: Some(value.temperature), // we cannot check since no flag exists. Keep in option for uniformity. + delta_velocity: check(&flags, ImuFlags::AccelsInRange, value.deltaVelocity), + delta_angle: check(&flags, ImuFlags::GyrosInRange, value.deltaAngle), }, ) } } -impl From for GpsVel { +impl From for (GpsVel, GpsVelAcc) { fn from(value: SbgLogGpsVel) -> Self { - Self { - time_stamp: value.timeStamp, - status: value.status, - time_of_week: value.timeOfWeek, - velocity: value.velocity, - velocity_acc: value.velocityAcc, - course: value.course, - course_acc: value.courseAcc, - } + let status = GpsVelStatus::new(value.status); + + let valid = matches!(status.get_status(), Some(GpsVelStatusE::SolComputed)); + + ( + GpsVel { + time_stamp: value.timeStamp, + time_of_week: if valid {Some(value.timeOfWeek) } else {None}, + status, + velocity: if valid { Some(value.velocity) } else { None }, + course: if valid { Some(value.course) } else { None }, + }, + GpsVelAcc { + velocity_acc: if valid { Some(value.velocityAcc) } else { None }, + course_acc: if valid { Some(value.courseAcc) } else { None }, + } + + ) } } diff --git a/libraries/sbg-rs/src/sbg.rs b/libraries/sbg-rs/src/sbg.rs index 9c0094a6..45922492 100644 --- a/libraries/sbg-rs/src/sbg.rs +++ b/libraries/sbg-rs/src/sbg.rs @@ -56,10 +56,10 @@ pub enum CallbackData { UtcTime(UtcTime), Air(Air), EkfQuat(EkfQuat), - EkfNav((EkfNav1, EkfNav2)), + EkfNav((EkfNav1, EkfNav2, EkfNavAcc)), Imu((Imu1, Imu2)), - GpsVel(GpsVel), - GpsPos((GpsPos1, GpsPos2)), + GpsVel((GpsVel, GpsVelAcc)), + GpsPos((GpsPos1, GpsPos2, GpsPosAcc)), } struct UARTSBGInterface {