Removed all static config references. Make commander integrate yaw in…

… angle mode. Map all rc commands through commander. Add expo curves to analog channels.

.cargo/config.toml

@@ -9,7 +9,6 @@ rustflags = [
   "-C", "link-arg=--nmagic",
   "-C", "link-arg=-Tlink.x",
   "-C", "link-arg=-Tdefmt.x",
-  "-Z", "trap-unreachable=no",
   "-C", "inline-threshold=5",
   "-C", "no-vectorize-loops",
 ]

src/config/mod.rs

@@ -33,15 +33,15 @@ pub const RP2040_DEV_CONFIG: Configuration = Configuration {
     header: CFG_HEADER,
     imu_cfg: [Some(ImuConfig {
             acc_cal: Some(Calibration {
-                scale: Vector3::new(0.99470156, 0.99557877, 0.987214),
-                offset: Vector3::new(0.09380102, -0.24849701, -0.250834)
+                scale: Vector3::new(0.9941048, 0.9951916, 0.987686),
+                offset: Vector3::new(-0.08339453, -0.21677876, 0.27320194)
             }),
         gyr_cal: Some(Calibration {
             scale: Vector3::new(1.0, 1.0, 1.0),
-            offset: Vector3::new(0.013425202, 0.02018836, -0.0020406505)
+            offset: Vector3::new(-0.01429255, 0.020241564, 0.0021124864)
         }),
         imu_ext: Some(Extrinsics {
-            rotation: rotation_matrices::Rotation::RotY180,
+            rotation: rotation_matrices::Rotation::RotX180,
             translation: VEC_ZEROS,
         }),
         imu_type: Some(ImuTypeConf::Icm20948(icm20948_async::Icm20948Config {
@@ -135,8 +135,8 @@ pub const RP2040_DEV_CONFIG: Configuration = Configuration {
     },
     mixing_matrix: MotorMixing::QuadX,
     radio_timeout: Duration::from_millis(200),
-    motor_gov_timeout: Duration::from_millis(100),
-    tx_map: crate::transmitter::tx_12_profiles::TX12_DEFAULT_MAP,
+    dshot_timeout: Duration::from_millis(100),
+    tx_map: crate::transmitter::tx_12_profiles::TX12_8CH_DEFAULT_MAP,
     footer: CFG_FOOTER,
 };
 
@@ -145,7 +145,7 @@ 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 = 252622182913624215;
+pub const CFG_HEADER: u64 = 252622182913621215;
 pub const CFG_FOOTER: u64 = 145389224228254269;
 
 #[derive(Debug, Clone, Copy)]
@@ -161,7 +161,7 @@ pub struct Configuration {
     pub mav_freq: MavStreamableFrequencies,
     pub mixing_matrix: MotorMixing,
     pub radio_timeout: Duration,
-    pub motor_gov_timeout: Duration,
+    pub dshot_timeout: Duration,
     pub tx_map: TransmitterMap,
     pub footer: u64,
 }

src/constants.rs

@@ -1,3 +1,11 @@
+/// Double of PI: PI*2
+pub const PI_2: f32 = core::f32::consts::PI*2.0;
+
+/// Half of PI: PI/2
+pub const PI_D2: f32 = core::f32::consts::PI/2.0;
+
+/// Quarter of PI: PI/4
+pub const PI_D4: f32 = core::f32::consts::PI/4.0;
 
 /// The value of 1/sqrt(2)
 pub const INV_SQRT_2: f32 = 0.7071067811865475;

src/lib.rs

@@ -25,6 +25,7 @@ pub mod t_led_blinker;
 
 pub mod t_commander;
 pub mod t_arm_checker;
+pub mod t_vehicle_state;
 pub mod t_attitude_estimator;
 pub mod t_attitude_controller;
 pub mod t_motor_mixing;

src/main.rs

@@ -76,14 +76,14 @@ async fn main(spawner: embassy_executor::Spawner) {
 
     // Load the configuration from flash and initialize the static reference
     spawner.must_spawn(holsatus_flight::t_config_master::config_master(flash));
-    let config = holsatus_flight::messaging::STATIC_CONFIG_REF.spin_get().await;
 
     // Create quad-motor PIO runner
     let driver_dshot_pio = {
         use holsatus_flight::drivers::rp2040::dshot_pio::DshotPio;
         use embassy_rp::{bind_interrupts, peripherals::PIO0, pio::*};
         bind_interrupts!(struct Pio0Irqs {PIO0_IRQ_0 => InterruptHandler<PIO0>;});
-        DshotPio::<4, _>::new(p.PIO0, Pio0Irqs, p.PIN_10, p.PIN_11, p.PIN_12, p.PIN_13, config.dshot_speed.clk_div(),
+        let speed = holsatus_flight::messaging::CFG_DSHOT_SPEED.spin_get().await;
+        DshotPio::<4, _>::new(p.PIO0, Pio0Irqs, p.PIN_10, p.PIN_11, p.PIN_12, p.PIN_13, speed.clk_div(),
         )
     };
 
@@ -133,7 +133,6 @@ async fn main(spawner: embassy_executor::Spawner) {
 
     // Initilize the core0 main task (sensors, control loop, remote control, etc.)
     spawner.must_spawn(holsatus_flight::main_core0::main_core0(
-        config,
         i2c_async,
         uart_rx_sbus,
         driver_dshot_pio,

src/main_core0.rs

@@ -10,7 +10,6 @@ use embassy_rp::{
     uart::{self, UartRx},
 };
 
-use crate::config::Configuration;
 use embassy_sync::{
     blocking_mutex::raw::ThreadModeRawMutex,
     mutex::Mutex,
@@ -23,7 +22,6 @@ pub type I2cAsyncType = I2cDevice<'static, ThreadModeRawMutex, I2cInnerAsyncType
 
 #[embassy_executor::task]
 pub async fn main_core0(
-    config: &'static Configuration,
     i2c_async: I2c<'static, I2C0, i2c::Async>,
     uart_rx_sbus: UartRx<'static, UART1, uart::Async>,
     driver_dshot_pio: DshotPio<'static, 4, PIO0>,
@@ -37,7 +35,6 @@ pub async fn main_core0(
     // Start SBUS reader task
     spawner.must_spawn(crate::t_sbus_reader::sbus_reader(
         uart_rx_sbus,
-        config.radio_timeout,
     ));
 
     // Setup IMU governor task, responsible for selecting the active IMU
@@ -51,7 +48,6 @@ pub async fn main_core0(
         I2cDevice::new(i2c_async_mutex),
         0,
         0,
-        config
     ));
 
     // Start the estimator, control loop and motor mixing
@@ -64,8 +60,6 @@ pub async fn main_core0(
     // Start the motor governor task
     spawner.must_spawn(crate::t_motor_governor::motor_governor(
         driver_dshot_pio,
-        config.motor_dir,
-        config.motor_gov_timeout,
     ));
 
     // Start the task which manages the arming blocker flag
@@ -75,8 +69,8 @@ pub async fn main_core0(
     spawner.must_spawn(crate::t_status_printer::status_printer());
 
     // Star calibration routine tasks
-    spawner.must_spawn(crate::t_gyr_calibration::gyr_calibration(config));
-    spawner.must_spawn(crate::t_acc_calibration::acc_calibration(config));
+    spawner.must_spawn(crate::t_gyr_calibration::gyr_calibration());
+    spawner.must_spawn(crate::t_acc_calibration::acc_calibration());
 
 
     // Start the transmitter governor task

src/messaging.rs

@@ -1,10 +1,10 @@
 use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, channel::Channel};
-use embassy_time::Instant;
+use embassy_time::{Duration, Instant};
 use nalgebra::{Quaternion, Unit, Vector3, Vector4};
 use sbus::SBusPacket;
 
 use crate::{
-    common::types::{AttitudeReference, MotorState, VehicleState}, config::{Calibration, Configuration}, sensors::imu::types::ImuData6Dof, t_flight_detector::LandedState, t_motor_governor::ArmBlocker, t_sbus_reader::RxError, transmitter::{ControlRequest, EventRequest, TransmitterMap}
+    airframe::MotorMixing, common::types::{AttitudeReference, MotorState, VehicleState}, config::{AttitudePids, DshotSpeed, ImuConfig, MagConfig}, sensors::imu::types::ImuData6Dof, t_flight_detector::LandedState, t_motor_governor::ArmBlocker, t_sbus_reader::RxError, transmitter::{ControlRequest, EventRequest, TransmitterMap}
 };
 
 macro_rules! watch {
@@ -24,27 +24,22 @@ macro_rules! watch {
 }
 
 // Shorthand for the watch and channel types
-type CsWatch<T, const N: usize> = embassy_sync::watch::Watch<ThreadModeRawMutex, T, N>;
+type SWatch<T, const N: usize> = embassy_sync::watch::Watch<ThreadModeRawMutex, T, N>;
 
-/// Queue of discrete system requests, such as sensor calibration, configuration changes, etc.
-pub static REQUEST_QUEUE: Channel<ThreadModeRawMutex, EventRequest, 20> = Channel::new();
-
-// TODO - Make the vehicle state governor task.
-watch!(VEHICLE_STATE, VehicleState, 3, "Channel containing the vehicle state, selected by the vehicle state governor task.");
 
 // Best effort IMU and MAG data, selected by the respective governors
 watch!(IMU_DATA, ImuData6Dof, 3, "Best effort IMU data, selected by the IMU governor task.");
 watch!(MAG_DATA, Vector3<f32>, 3, "Best effort MAG data, selected by the MAG governor task.");
 
 /// Array of watch channels for the `N_IMU` individual IMU sensors. These should generally not be used, as the
 /// IMU governor task is responsible for selecting the active sensor, transmitted through the `IMU_DATA` channel.
-pub static IMU_SENSOR: [CsWatch<(ImuData6Dof, Instant), 3>; crate::N_IMU] = [IMU_SENSOR_REPEAT; crate::N_IMU];
-const IMU_SENSOR_REPEAT: CsWatch<(ImuData6Dof, Instant), 3> = CsWatch::new();
+pub static IMU_SENSOR: [SWatch<(ImuData6Dof, Instant), 3>; crate::N_IMU] = [IMU_SENSOR_REPEAT; crate::N_IMU];
+const IMU_SENSOR_REPEAT: SWatch<(ImuData6Dof, Instant), 3> = SWatch::new();
 
 /// Array of watch channels for the `N_MAG` individual MAG sensors. These should generally not be used, as the
 /// MAG governor task is responsible for selecting the active sensor, transmitted through the `MAG_DATA` channel.
-pub static MAG_SENSOR: [CsWatch<(Vector3<f32>, Instant), 3>; crate::N_MAG] = [MAG_SENSOR_REPEAT; crate::N_MAG];
-const MAG_SENSOR_REPEAT: CsWatch<(Vector3<f32>, Instant), 3> = CsWatch::new();
+pub static MAG_SENSOR: [SWatch<(Vector3<f32>, Instant), 3>; crate::N_MAG] = [MAG_SENSOR_REPEAT; crate::N_MAG];
+const MAG_SENSOR_REPEAT: SWatch<(Vector3<f32>, Instant), 3> = SWatch::new();
 
 // Indicates the ID of the active IMU and MAG sensor
 watch!(IMU_ACTIVE_ID, Option<u8>, 2, "The ID of the active IMU sensor, selected by the IMU governor task.");
@@ -55,52 +50,52 @@ watch!(GYR_CALIBRATING, bool, 3, "This channel is set by the gyroscope calibrati
 watch!(ACC_CALIBRATING, bool, 3, "This channel is set by the accelerometer calibration task when the calibration process is active.");
 watch!(MAG_CALIBRATING, bool, 3, "This channel is set by the magnetometer calibration task when the calibration process is active.");
 
-watch!(RC_SAVE_CONFIG, bool, 2, "Command to commit any changes to the configuration to flash");
-
-// Command to start the calibration process for the individual sensors
-watch!(RC_START_GYR_CALIB, bool, 2, "Command to start calibrating the gyroscopes");
-watch!(RC_START_ACC_CALIB, bool, 2, "Command to start calibrating the accelerometers");
-watch!(RC_START_MAG_CALIB, bool, 2, "Command to start calibrating the magnetometers");
-
 // Flag indicating whether the individual sensors are calibrated
 watch!(GYR_CALIBRATED, bool, 2, "Signals whether the gyroscopes are calibrated");
 watch!(ACC_CALIBRATED, bool, 2, "Signals whether the accelerometers are calibrated");
 watch!(MAG_CALIBRATED, bool, 2, "Signals whether the magnetometers are calibrated");
 
-// Transmits the calibration data for the gyroscopes
-watch!(GYR_CALIBRATIONS, [Option<Calibration>; crate::N_IMU], 3, "Transmits the calibration data for the gyroscopes");
-watch!(ACC_CALIBRATIONS, [Option<Calibration>; crate::N_IMU], 3, "Transmits the calibration data for the accelerometers");
-watch!(MAG_CALIBRATIONS, [Option<Calibration>; crate::N_MAG], 3, "Transmits the calibration data for the magnetometers");
-
-watch!(STATIC_CONFIG_REF, &'static Configuration, 3, "Channel containing reference to statically allocated configuration data");
-
+// Command to start the calibration process for the individual sensors
+watch!(CMD_START_GYR_CALIB, bool, 2, "Command to start calibrating the gyroscopes");
+watch!(CMD_START_ACC_CALIB, bool, 2, "Command to start calibrating the accelerometers");
+watch!(CMD_START_MAG_CALIB, bool, 2, "Command to start calibrating the magnetometers");
+
+// Various commands to the system, set by the commander task
+watch!(CMD_SAVE_CONFIG, bool, 2, "Command to commit any changes to the configuration to flash");
+watch!(CMD_ARM_VEHICLE, bool, 2, "Command to arm or disarm the vehicle, sent by rc_mapper");
+watch!(CMD_EN_INTEGRAL, bool, 2, "Channel which can enable or disable integral terms of the attitude controller");
+watch!(CMD_ATTITUDE_SETPOINT, AttitudeReference, 2, "The target throttle for the motor mixing");
+watch!(CMD_THROTTLE_SETPOINT, f32, 2, "The target throttle for the motor mixing");
+
+// Channels used for remote control
 watch!(SBUS_DATA, Result<SBusPacket, RxError>, 3, "Data from the SBUS receiver task");
-
 watch!(REQUEST_CONTROLS, ControlRequest, 2, "The target attitude (angle or rate) for the attitude controller");
+/// Queue of discrete system requests, such as sensor calibration, configuration changes, etc.
+pub static REQUEST_QUEUE: Channel<ThreadModeRawMutex, EventRequest, 20> = Channel::new();
 
-watch!(ATTITUDE_SETPOINT, AttitudeReference, 2, "The target throttle for the motor mixing");
-watch!(THROTTLE_SETPOINT, f32, 2, "The target throttle for the motor mixing");
-
-watch!(RC_ARMING_CMD, bool, 2, "Command to arm or disarm the vehicle, sent by rc_mapper");
-
-watch!(EN_INTEGRAL, bool, 2, "Channel which can enable or disable integral terms of the attitude controller");
-
-watch!(LOOP_HEALTH, f32, 2, "Loop health transmitted by the attitude controller, is 1.0 when the loop is healthy");
-
-watch!(MOTOR_SPEEDS, [u16; 4], 2, "Motor speeds, as decided by the attitude controller, transmitted to the motor governor");
-
+// Channel for various vehicle states
+watch!(VEHICLE_STATE, VehicleState, 3, "Channel containing the vehicle state, selected by the vehicle state governor task.");
+watch!(ARM_BLOCKER, ArmBlocker, 2, "Arming blocker flag, set by the arming checker task, describes whether it is safe to arm the vehicle");
 watch!(LANDED_STATE, LandedState, 2, "Landed state, set by the flight detector, describes whether the vehicle is landed, airborne or in transition");
 
-watch!(MOTOR_STATE, MotorState, 2, "Motor state, set by the motor governor, describes the arming state and current speed of the motors");
-
 watch!(ATTITUDE_QUAT, Unit<Quaternion<f32>>, 2, "The current vehicle attitude represented as a quaternion");
-
 watch!(ATTITUDE_EULER, Vector3<f32>, 4, "The current vehicle attitude represented as euler angles");
-
 watch!(CONTROLLER_OUTPUT, Vector3<f32>, 2, "The output of the attitude controllers for each axis");
-
 watch!(MOTORS_MIXED, Vector4<u16>, 2, "The mixed controller output and thrust target, using a mixing matrix");
+watch!(LOOP_HEALTH, f32, 2, "Loop health transmitted by the attitude controller, is 1.0 when the loop is healthy");
+watch!(MOTOR_SPEEDS, [u16; 4], 2, "Motor speeds, as decided by the attitude controller, transmitted to the motor governor");
+watch!(MOTOR_STATE, MotorState, 2, "Motor state, set by the motor governor, describes the arming state and current speed of the motors");
 
+// Signals which represent configurations within the system. These are initialized by the configuration governor task,
+// but are subsequently supposed to only by other tasks, such as calibration tasks.
+watch!(CFG_IMU_CONFIG, [Option<ImuConfig>; crate::N_IMU], 3, "Transmits the configurations for the individual IMU sensors");
+watch!(CFG_MAG_CONFIG, [Option<MagConfig>; crate::N_IMU], 3, "Transmits the configurations for the individual MAG sensors");
+watch!(CFG_MIXING_MATRIX, MotorMixing, 2, "The motor mixing matrix, used to convert controller outputs to motor speeds");
+watch!(CFG_LOOP_FREQUENCY, u16, 2, "The desired frequency of the attitude control loop in Hz.");
+watch!(CFG_MAG_FREQUENCY, u16, 2, "The desired sampling rate of the magnetometer in Hz.");
+watch!(CFG_ATTITUDE_PIDS, AttitudePids, 2, "Configurations for the attitude controllers");
+watch!(CFG_REVERSE_MOTOR, [bool;4], 2, "Sets the spin direction of each motor");
+watch!(CFG_DSHOT_SPEED, DshotSpeed, 2, "Speed setting for the DSHOT protocol");
+watch!(CFG_DSHOT_TIMEOUT, Duration, 2, "Sets the timeout of the motor governor");
+watch!(CFG_RADIO_TIMEOUT, Duration, 2, "Sets the timeout of the RC radio receiver");
 watch!(CFG_TRANSMITTER_MAP, TransmitterMap, 2, "The transmitter map, containing the mapping of the transmitter channels to vehicle commands");
-
-watch!(ARM_BLOCKER, ArmBlocker, 10, "Arming blocker flag, set by the arming governor task, decides whether it is safe to arm the vehicle");

src/t_acc_calibration.rs

@@ -1,36 +1,27 @@
 use embassy_time::{with_timeout, Duration, Ticker};
 use nalgebra::{ComplexField, SMatrix, Vector3};
 
-use crate::{config::{Calibration, Configuration}, constants::G_GRAVITY, messaging as msg, sensors::imu::types::ImuData6Dof};
+use crate::{config::Calibration, constants::G_GRAVITY, messaging as msg, sensors::imu::types::ImuData6Dof};
 
 /// Routine to calibrate gyroscopes, by calculating their bias.
 #[embassy_executor::task]
-pub async fn acc_calibration(
-    config: &'static Configuration
-) -> ! {
+pub async fn acc_calibration() -> ! {
 
     // Input channels
     let mut rcv_imu_sensor_array: [_; crate::N_IMU] = core::array::from_fn(|i| msg::IMU_SENSOR[i].receiver().unwrap());
-    let mut rcv_start_acc_calib = msg::RC_START_ACC_CALIB.receiver().unwrap();
+    let mut rcv_start_acc_calib = msg::CMD_START_ACC_CALIB.receiver().unwrap();
 
     // Output channels
-    let snd_acc_calib = msg::ACC_CALIBRATIONS.sender();
-    let snd_acc_calibrating = msg::ACC_CALIBRATING.sender();
-
-    // Publish existing calibration data
-    snd_acc_calib.send(core::array::from_fn(|i|{
-        match config.imu_cfg[i].as_ref() {
-            Some(cfg) => cfg.acc_cal,
-            None => None,
-        }
-    }));
+    let snd_acc_calibing = msg::ACC_CALIBRATING.sender();
+    let snd_imu_config = msg::CFG_IMU_CONFIG.sender();
 
     'infinite: loop {
 
         // Wait for the start signal
-        snd_acc_calibrating.send(false);
+        snd_acc_calibing.send(false);
         if !rcv_start_acc_calib.changed().await { continue 'infinite };
-        snd_acc_calibrating.send(true);
+        snd_acc_calibing.send(true);
+
         defmt::info!("[ACC CALIB]: Starting calibration on {} accelerometers", crate::N_IMU);
 
         // Array of calibrator instances
@@ -89,20 +80,22 @@ pub async fn acc_calibration(
         }
 
         // Get existing calibration data or create new
-        let mut calibrations = snd_acc_calib.try_get().unwrap_or([None; crate::N_IMU]);
+        let mut imu_config = msg::CFG_IMU_CONFIG.spin_get().await;
 
         // Publish result of each sensor if calibration was successful
-        for (id, (prev_cal, cal)) in calibrations.iter_mut().zip(sensor_calibrator.iter()).enumerate() {
-            match cal.calib {
-                Some(calib) => {
+        for (id, (opt_config, calibrator)) in imu_config.iter_mut().zip(sensor_calibrator.iter()).enumerate() {
+            match (calibrator.calib, opt_config) {
+                (Some(calib), Some(config)) => {
                     defmt::info!("[ACC CALIB]: Sensor {} calibration is valid.", id);
-                    *prev_cal = Some(calib);
+                    config.acc_cal = Some(calib);
                 },
-                None => defmt::warn!("[ACC CALIB]: Sensor {} calibration is invalid", id),
+                (None, Some(_)) => defmt::warn!("[ACC CALIB]: Sensor {} calibration is invalid", id),
+                (Some(_), None) => defmt::warn!("[ACC CALIB]: Sensor {} calibration is valid, but no configuration for the sensor exists", id),
+                (None, None) => {/* This is expected for non-existant sensors */},
             }
         }
 
-        snd_acc_calib.send(calibrations);
+        snd_imu_config.send(imu_config);
 
         // Ensure any recent request to calibrate is marked as seen
         let _ = rcv_start_acc_calib.try_get();