Changes for 12-03-2024, includes initial Holsatus shell demonstration

src/common/mod.rs

@@ -1,5 +1,7 @@
 use bitflags::bitflags;
 
+use self::types::MotorState;
+
 mod mav_bitflag;
 pub mod rotation_matrices;
 pub mod types;
@@ -25,3 +27,98 @@ bitflags! {
         const MISSION           = 0b_10000000000000;
     }
 }
+
+
+// TODO Move to a more appropriate location
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum SimpleRequest {
+
+    /// Unbound request // TODO remove?
+    Unbound,
+
+    /// Arm all motors
+    ArmMotors,
+
+    /// Disarm all motors. Acts as a kill-switch
+    DisarmMotors,
+
+    /// Change stabilization mode to angle (or horizon) mode
+    AngleMode,
+
+    /// Change stabilization mode to rate (or acro / 3D) mode
+    RateMode,
+
+    /// Start gyroscope calibration
+    StartGyrCalib,
+
+    /// Abort any currently active gyroscope calibration
+    AbortGyrCalib,
+
+    /// Start accelerometer calibration
+    StartAccCalib,
+
+    /// Abort any currently active accelerometer calibration
+    AbortAccCalib,
+
+    /// Start magnetometer calibration
+    StartMagCalib,
+
+    /// Abort any currently active magnetometer calibration
+    AbortMagCalib,
+
+    /// Save any currently modified parameters to flash
+    SaveConfig,
+
+    /// RC Failsafe
+    RcFailsafe,
+}
+
+pub enum EventRequest {
+    ArmMotors(bool),
+    SetAngleMode(bool),
+    StartGyrCalib{seconds: u8, timeout: bool, variance: f32},
+    AbortGyrCalib,
+    StartAccCalib{seconds: u8, timeout: bool, variance: f32},
+    AbortAccCalib,
+    SaveConfig,
+}
+
+pub enum EventResponse {
+    ArmMotors(Result<MotorState, ArmMotorsError>),
+    SetAngleMode(Result<(),SetAngleModeError>),
+    StartGyrCalib(Result<[Option<Result<f32, GyrCalibError>>; crate::N_IMU], StartGyrCalibError>),
+    AbortGyrCalib(Result<[Option<Result<f32, GyrCalibError>>; crate::N_IMU], StartAccCalibError>),
+    StartAccCalib(StartAccCalibError),
+    AbortAccCalib(AbortAccCalibError),
+    SaveConfig,
+}
+
+pub struct GyrCalibError {}
+pub struct AccCalibError {}
+
+pub enum ArmMotorsError {
+    Ok(MotorState),
+    NotSafeToArm,
+}
+
+pub enum SetAngleModeError {
+    NotInManualMode,
+}
+
+pub enum StartGyrCalibError {
+    NotSafeToCalibrate,
+    NotCalibrating,
+}
+
+pub enum StartAccCalibError {
+    NotSafeToCalibrate,
+    NotCalibrating,
+}
+
+pub enum AbortAccCalibError {
+    NotCalibrating,
+}
+
+pub enum SaveConfigError {
+    NotSafeToSave,
+}

src/common/types.rs

@@ -1,4 +1,5 @@
 use defmt::Format;
+use embassy_time::Duration;
 use nalgebra::Vector3;
 
 #[derive(Clone, Copy, Debug, Format, PartialEq)]
@@ -72,26 +73,46 @@ pub enum VehicleState {
     Termination,
 }
 
-use mavlink::common::MavState;
-impl Into<MavState> for VehicleState {
-    fn into(self) -> MavState {
-        match self {
-            VehicleState::Uninit => MavState::MAV_STATE_ACTIVE,
-            VehicleState::Boot => MavState::MAV_STATE_BOOT,
-            VehicleState::Calibrating => MavState::MAV_STATE_CALIBRATING,
-            VehicleState::Standby => MavState::MAV_STATE_STANDBY,
-            VehicleState::Active => MavState::MAV_STATE_ACTIVE,
-            VehicleState::Critical => MavState::MAV_STATE_CRITICAL,
-            VehicleState::Emergency => MavState::MAV_STATE_EMERGENCY,
-            VehicleState::Poweroff => MavState::MAV_STATE_POWEROFF,
-            VehicleState::Termination => MavState::MAV_STATE_FLIGHT_TERMINATION,
-        }
-    }
-}
-
 #[derive(Clone, Copy, Debug, PartialEq)]
 pub enum StabilizationMode {
     Angle,
     Rate,
     None,
-}
+}
+
+/// Mavlink message types that can be streamed, i.e. can be sent at
+/// regular intervals upon request from other Mavlink devices.
+pub enum MavStreamable {
+    Heartbeat,
+    SystemTime,
+    Attitude,
+    GpsRawInt,
+    ScaledImu,
+    GlobalPosition,
+    RcChannels,
+}
+
+#[derive(Clone, Copy, Debug)]
+pub struct MavStreamableFrequencies {
+    pub heartbeat: Option<Duration>,
+    pub system_time: Option<Duration>,
+    pub attitude: Option<Duration>,
+    pub gps_raw_int: Option<Duration>,
+    pub scaled_imu: Option<Duration>,
+    pub rc_channels: Option<Duration>,
+}
+
+impl MavStreamable {
+    pub fn from_id(id: u32) -> Option<MavStreamable> {
+        match id {
+            0 => Some(MavStreamable::Heartbeat),
+            2 => Some(MavStreamable::SystemTime),
+            30 => Some(MavStreamable::Attitude),
+            24 => Some(MavStreamable::GpsRawInt),
+            26 => Some(MavStreamable::ScaledImu),
+            33 => Some(MavStreamable::GlobalPosition),
+            34 => Some(MavStreamable::RcChannels),
+            _ => None,
+        }
+    }
+}

src/config/mod.rs

@@ -23,10 +23,10 @@ pub fn load_config(
 pub const DEFAULT_CONFIG: Configuration = RP2040_DEV_CONFIG;
 
 use icm20948_async::*;
-use crate::{airframe::MotorMixing, filters::pid_controller::PidConfig};
+use crate::{airframe::MotorMixing, common::types::MavStreamableFrequencies, filters::pid_controller::PidConfig};
 
 use crate::{
-    common::rotation_matrices, mavlink::supported_msg::MavStreamableFrequencies,
+    common::rotation_matrices,
     transmitter::TransmitterMap,
 };
 pub const RP2040_DEV_CONFIG: Configuration = Configuration {
@@ -145,8 +145,8 @@ pub const FLASH_AMT: usize = 2 * 1024 * 1024;
 pub const CFG_ADDR_OFFSET: u32 = 0x100000;
 
 // Headers are used to check that loaded configuration is not just garbage data
-pub const CFG_HEADER: u64 = 252622182913621215;
-pub const CFG_FOOTER: u64 = 145389224228254269;
+pub const CFG_HEADER: u64 = 252623182913621215;
+pub const CFG_FOOTER: u64 = 145383224228254269;
 
 #[derive(Debug, Clone, Copy)]
 pub struct Configuration {
@@ -176,6 +176,140 @@ pub struct AttitudePids {
     pub yaw_outer: PidConfig<f32>,
 }
 
+pub enum Type {
+    F64(f64),
+    F32(f32),
+    Isize(isize),
+    I64(i64),
+    I32(i32),
+    I16(i16),
+    I8(i8),
+    Usize(usize),
+    U64(u64),
+    U32(u32),
+    U16(u16),
+    U8(u8),
+    Bool(bool),
+    Unknown,
+    None,
+}
+
+pub trait ParamLookup {
+    /// Look up a system parameter by name, returns either a float or an integer if the parameter is found.
+    /// That is, the result will only ever be `(Some(f32), None)`, `(None, Some(i32)` or `(None, None)`.
+    fn get(&self, name: core::str::SplitInclusive<'_, char>) -> Type;
+    fn set(&self, name: core::str::SplitInclusive<'_, char>, val: Type) -> Type;
+}
+
+impl ParamLookup for Configuration {
+    fn get(&self, mut splits: core::str::SplitInclusive<'_, char>) -> Type {
+        match splits.next() {
+            Some("attpid_") => self.pids.get(splits),
+            _ => Type::Unknown,
+        }
+    }
+
+    fn set(&self, mut _splits: core::str::SplitInclusive<'_, char>, _val: Type) -> Type {
+        todo!()
+    }
+}
+
+impl ParamLookup for AttitudePids {
+
+    fn get(&self, mut splits: core::str::SplitInclusive<'_, char>) -> Type {
+
+        enum Axis {
+            Roll,
+            Pitch,
+            Yaw,
+        }
+
+        enum Layer {
+            Inner,
+            Outer,
+        }
+
+        enum Param {
+            Kp,
+            Ki,
+            Kd,
+            OutLimMax,
+            OutLimMin,
+            LpTau,
+        }
+
+        let axis = match splits.next() {
+            Some("roll_") => Axis::Roll,
+            Some("pitch_") => Axis::Pitch,
+            Some("yaw_") => Axis::Yaw,
+            _ => return Type::Unknown,
+        };
+
+        let layer = match splits.next() {
+            Some("inner_") => Layer::Inner,
+            Some("outer_") => Layer::Outer,
+            _ => return Type::Unknown,
+        };
+
+        let param = match splits.next() {
+            Some("kp") => Param::Kp,
+            Some("ki") => Param::Ki,
+            Some("kd") => Param::Kd,
+            Some("outlimmax") => Param::OutLimMax,
+            Some("outlimmin") => Param::OutLimMin,
+            Some("lptau") => Param::LpTau,
+            _ => return Type::Unknown,
+        };
+
+        match (axis, layer, param) {
+            (Axis::Roll, Layer::Inner, Param::Kp) => Type::F32(self.roll_inner.kp),
+            (Axis::Roll, Layer::Inner, Param::Ki) => Type::F32(self.roll_inner.ki),
+            (Axis::Roll, Layer::Inner, Param::Kd) => Type::F32(self.roll_inner.kd),
+            (Axis::Roll, Layer::Inner, Param::OutLimMax) => self.roll_inner.output_limit.map_or(Type::None, |lim| Type::F32(lim.max)),
+            (Axis::Roll, Layer::Inner, Param::OutLimMin) => self.roll_inner.output_limit.map_or(Type::None, |lim| Type::F32(lim.min)),
+            (Axis::Roll, Layer::Inner, Param::LpTau) => self.roll_inner.lp_filter.map_or(Type::None, |tau| Type::F32(tau)),
+            (Axis::Roll, Layer::Outer, Param::Kp) => Type::F32(self.roll_outer.kp),
+            (Axis::Roll, Layer::Outer, Param::Ki) => Type::F32(self.roll_outer.ki),
+            (Axis::Roll, Layer::Outer, Param::Kd) => Type::F32(self.roll_outer.kd),
+            (Axis::Roll, Layer::Outer, Param::OutLimMax) => self.roll_outer.output_limit.map_or(Type::None, |lim| Type::F32(lim.max)),
+            (Axis::Roll, Layer::Outer, Param::OutLimMin) => self.roll_outer.output_limit.map_or(Type::None, |lim| Type::F32(lim.min)),
+            (Axis::Roll, Layer::Outer, Param::LpTau) => self.roll_outer.lp_filter.map_or(Type::None, |tau| Type::F32(tau)),
+            (Axis::Pitch, Layer::Inner, Param::Kp) => Type::F32(self.pitch_inner.kp),
+            (Axis::Pitch, Layer::Inner, Param::Ki) => Type::F32(self.pitch_inner.ki),
+            (Axis::Pitch, Layer::Inner, Param::Kd) => Type::F32(self.pitch_inner.kd),
+            (Axis::Pitch, Layer::Inner, Param::OutLimMax) => self.pitch_inner.output_limit.map_or(Type::None, |lim| Type::F32(lim.max)),
+            (Axis::Pitch, Layer::Inner, Param::OutLimMin) => self.pitch_inner.output_limit.map_or(Type::None, |lim| Type::F32(lim.min)),
+            (Axis::Pitch, Layer::Inner, Param::LpTau) => self.pitch_inner.lp_filter.map_or(Type::None, |tau| Type::F32(tau)),
+            (Axis::Pitch, Layer::Outer, Param::Kp) => Type::F32(self.pitch_outer.kp),
+            (Axis::Pitch, Layer::Outer, Param::Ki) => Type::F32(self.pitch_outer.ki),
+            (Axis::Pitch, Layer::Outer, Param::Kd) => Type::F32(self.pitch_outer.kd),
+            (Axis::Pitch, Layer::Outer, Param::OutLimMax) => self.pitch_outer.output_limit.map_or(Type::None, |lim| Type::F32(lim.max)),
+            (Axis::Pitch, Layer::Outer, Param::OutLimMin) => self.pitch_outer.output_limit.map_or(Type::None, |lim| Type::F32(lim.min)),
+            (Axis::Pitch, Layer::Outer, Param::LpTau) => self.pitch_outer.lp_filter.map_or(Type::None, |tau| Type::F32(tau)),
+            (Axis::Yaw, Layer::Inner, Param::Kp) => Type::F32(self.yaw_inner.kp),
+            (Axis::Yaw, Layer::Inner, Param::Ki) => Type::F32(self.yaw_inner.ki),
+            (Axis::Yaw, Layer::Inner, Param::Kd) => Type::F32(self.yaw_inner.kd),
+            (Axis::Yaw, Layer::Inner, Param::OutLimMax) => self.yaw_inner.output_limit.map_or(Type::None, |lim| Type::F32(lim.max)),
+            (Axis::Yaw, Layer::Inner, Param::OutLimMin) => self.yaw_inner.output_limit.map_or(Type::None, |lim| Type::F32(lim.min)),
+            (Axis::Yaw, Layer::Inner, Param::LpTau) => self.yaw_inner.lp_filter.map_or(Type::None, |tau| Type::F32(tau)),
+            (Axis::Yaw, Layer::Outer, Param::Kp) => Type::F32(self.yaw_outer.kp),
+            (Axis::Yaw, Layer::Outer, Param::Ki) => Type::F32(self.yaw_outer.ki),
+            (Axis::Yaw, Layer::Outer, Param::Kd) => Type::F32(self.yaw_outer.kd),
+            (Axis::Yaw, Layer::Outer, Param::OutLimMax) => self.yaw_outer.output_limit.map_or(Type::None, |lim| Type::F32(lim.max)),
+            (Axis::Yaw, Layer::Outer, Param::OutLimMin) => self.yaw_outer.output_limit.map_or(Type::None, |lim| Type::F32(lim.min)),
+            (Axis::Yaw, Layer::Outer, Param::LpTau) => self.yaw_outer.lp_filter.map_or(Type::None, |tau| Type::F32(tau)),
+        }
+    }
+
+    fn set(&self, splits: core::str::SplitInclusive<'_, char>, _val: Type) -> Type {
+        match splits {
+            _ => todo!()
+        }
+    }
+
+}
+
+
 #[derive(Debug, Clone, Copy)]
 pub struct ImuConfig {
     ///Accelerometer calibration

src/lib.rs

@@ -1,21 +1,27 @@
 #![allow(incomplete_features)]
 #![feature(generic_const_exprs)]
 #![feature(let_chains)]
+#![feature(async_closure)]
 #![no_std]
 
 pub mod common;
 pub mod config;
 pub mod functions;
 pub mod geo;
 pub mod health;
-pub mod mavlink;
 pub mod messaging;
 pub mod sensors;
 pub mod transmitter;
 pub mod drivers;
 pub mod filters;
 pub mod airframe;
 
+#[cfg(feature = "mavlink")]
+pub mod mavlink;
+
+#[cfg(feature = "shell")]
+pub mod shell;
+
 pub mod constants;
 
 pub const N_IMU: usize = 2;
@@ -41,9 +47,8 @@ pub mod t_config_master;
 pub mod t_rc_mapper;
 pub mod t_flight_detector;
 
-
-pub mod main_core0;
-pub mod main_core1;
+pub mod main_high_prio;
+pub mod main_low_prio;
 
 pub use ahrs;
 pub use embassy_embedded_hal;