diff --git a/.cargo/config.toml b/.cargo/config.toml index 33498df..2ed59bf 100644 --- a/.cargo/config.toml +++ b/.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", ] diff --git a/src/config/mod.rs b/src/config/mod.rs index 294dd56..1d792aa 100644 --- a/src/config/mod.rs +++ b/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, } diff --git a/src/constants.rs b/src/constants.rs index 2930b5a..98ee3c9 100644 --- a/src/constants.rs +++ b/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; diff --git a/src/lib.rs b/src/lib.rs index a5f3b35..8014c44 100644 --- a/src/lib.rs +++ b/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; diff --git a/src/main.rs b/src/main.rs index 2080d68..547e7bf 100644 --- a/src/main.rs +++ b/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;}); - 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, diff --git a/src/main_core0.rs b/src/main_core0.rs index 9447cf4..f4b7633 100644 --- a/src/main_core0.rs +++ b/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 diff --git a/src/messaging.rs b/src/messaging.rs index 3a06e5b..840110b 100644 --- a/src/messaging.rs +++ b/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,13 +24,8 @@ macro_rules! watch { } // Shorthand for the watch and channel types -type CsWatch = embassy_sync::watch::Watch; +type SWatch = embassy_sync::watch::Watch; -/// Queue of discrete system requests, such as sensor calibration, configuration changes, etc. -pub static REQUEST_QUEUE: Channel = 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."); @@ -38,13 +33,13 @@ watch!(MAG_DATA, Vector3, 3, "Best effort MAG data, selected by the MAG gov /// 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, Instant), 3>; crate::N_MAG] = [MAG_SENSOR_REPEAT; crate::N_MAG]; -const MAG_SENSOR_REPEAT: CsWatch<(Vector3, Instant), 3> = CsWatch::new(); +pub static MAG_SENSOR: [SWatch<(Vector3, Instant), 3>; crate::N_MAG] = [MAG_SENSOR_REPEAT; crate::N_MAG]; +const MAG_SENSOR_REPEAT: SWatch<(Vector3, Instant), 3> = SWatch::new(); // Indicates the ID of the active IMU and MAG sensor watch!(IMU_ACTIVE_ID, Option, 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; crate::N_IMU], 3, "Transmits the calibration data for the gyroscopes"); -watch!(ACC_CALIBRATIONS, [Option; crate::N_IMU], 3, "Transmits the calibration data for the accelerometers"); -watch!(MAG_CALIBRATIONS, [Option; 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, 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 = 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>, 2, "The current vehicle attitude represented as a quaternion"); - watch!(ATTITUDE_EULER, Vector3, 4, "The current vehicle attitude represented as euler angles"); - watch!(CONTROLLER_OUTPUT, Vector3, 2, "The output of the attitude controllers for each axis"); - watch!(MOTORS_MIXED, Vector4, 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; crate::N_IMU], 3, "Transmits the configurations for the individual IMU sensors"); +watch!(CFG_MAG_CONFIG, [Option; 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"); diff --git a/src/t_acc_calibration.rs b/src/t_acc_calibration.rs index 654bfd1..d5aa19f 100644 --- a/src/t_acc_calibration.rs +++ b/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(); diff --git a/src/t_arm_checker.rs b/src/t_arm_checker.rs index e44c09b..32fcfb6 100644 --- a/src/t_arm_checker.rs +++ b/src/t_arm_checker.rs @@ -1,5 +1,6 @@ use embassy_futures::select::{select, Either}; use embassy_time::{Duration, Instant, Ticker}; +use nalgebra::Vector3; use crate::{messaging as msg, t_motor_governor::ArmBlocker}; @@ -9,16 +10,16 @@ const INV_SQRT_2: f32 = 0.7071067811865475; pub async fn arm_checker() -> ! { // Input channels let mut rcv_attitude_euler = msg::ATTITUDE_EULER.receiver().unwrap(); - let mut rcv_attitude_setpoint = msg::ATTITUDE_SETPOINT.receiver().unwrap(); - let mut rcv_throttle_setpoint = msg::THROTTLE_SETPOINT.receiver().unwrap(); + let mut rcv_throttle_setpoint = msg::CMD_THROTTLE_SETPOINT.receiver().unwrap(); let mut rcv_sbus_data = msg::SBUS_DATA.receiver().unwrap(); - let mut rcv_arming_cmd = msg::RC_ARMING_CMD.receiver().unwrap(); + let mut rcv_arming_cmd = msg::CMD_ARM_VEHICLE.receiver().unwrap(); let mut rcv_loop_health = msg::LOOP_HEALTH.receiver().unwrap(); let mut rcv_imu_active_id = msg::IMU_ACTIVE_ID.receiver().unwrap(); let mut rcv_gyr_calibrating = msg::GYR_CALIBRATING.receiver().unwrap(); let mut rcv_gyr_calibrated = msg::GYR_CALIBRATED.receiver().unwrap(); let mut rcv_acc_calibrating = msg::ACC_CALIBRATING.receiver().unwrap(); let mut rcv_acc_calibrated = msg::ACC_CALIBRATED.receiver().unwrap(); + let mut rcv_request_controls = msg::REQUEST_CONTROLS.receiver().unwrap(); // Output channels let snd_arm_blocker = msg::ARM_BLOCKER.sender(); @@ -47,7 +48,7 @@ pub async fn arm_checker() -> ! { // Check if the drone is in a high attitude ~ atan(sqrt(tan(pitch)^2 + tan(roll)^2)) if let Some(attitude) = rcv_attitude_euler.try_changed() { - let high_attitude = nalgebra::Vector2::new(attitude.x, attitude.y).norm() > INV_SQRT_2; + let high_attitude = attitude.xy().norm() > INV_SQRT_2; local_arm_blocker_flag.set(ArmBlocker::HIGH_ATTITUDE, high_attitude); } @@ -57,10 +58,9 @@ pub async fn arm_checker() -> ! { } // Check if the attitude commands are too high - use crate::common::types::AttitudeReference::{Angle, Rate}; - if let Some(Angle(setpoint) | Rate(setpoint)) = rcv_attitude_setpoint.try_changed() { - let high_setpoint = setpoint.norm() > 0.1; - local_arm_blocker_flag.set(ArmBlocker::HIGH_ATTITUDE_CMD, high_setpoint); + if let Some(controls) = rcv_request_controls.try_changed() { + let controls_vec = Vector3::new(controls.roll, controls.pitch, controls.yaw); + local_arm_blocker_flag.set(ArmBlocker::HIGH_ATTITUDE_CMD, controls_vec.norm() > 0.1); } // Check that transmitter is not in failsafe diff --git a/src/t_attitude_controller.rs b/src/t_attitude_controller.rs index c1c5580..98a7b1c 100644 --- a/src/t_attitude_controller.rs +++ b/src/t_attitude_controller.rs @@ -8,26 +8,26 @@ use crate::messaging as msg; pub async fn attitude_controller() -> ! { // Input messages let mut rcv_imu_data = msg::IMU_DATA.receiver().unwrap(); - let mut rcv_attitude_setpoint = msg::ATTITUDE_SETPOINT.receiver().unwrap(); + let mut rcv_attitude_setpoint = msg::CMD_ATTITUDE_SETPOINT.receiver().unwrap(); let mut rcv_attitude_euler = msg::ATTITUDE_EULER.receiver().unwrap(); - let mut rcv_en_integral = msg::EN_INTEGRAL.receiver().unwrap(); + let mut rcv_en_integral = msg::CMD_EN_INTEGRAL.receiver().unwrap(); // Output messages let snd_controller_output = msg::CONTROLLER_OUTPUT.sender(); // Get static reference to config - let config = msg::STATIC_CONFIG_REF.spin_get().await; + let pids = msg::CFG_ATTITUDE_PIDS.spin_get().await; // Period between IMU samples as seconds - let imu_period = 1.0 / config.imu_freq as f32; + let imu_period = 1.0 / msg::CFG_LOOP_FREQUENCY.spin_get().await as f32; // Setup PID controllers for attitude (rate and angle) control - let mut pid_roll_outer: _ = Pid::with_config(config.pids.roll_outer, imu_period); - let mut pid_roll_inner: _ = Pid::with_config(config.pids.roll_inner, imu_period); - let mut pid_pitch_outer: _ = Pid::with_config(config.pids.pitch_outer, imu_period); - let mut pid_pitch_inner: _ = Pid::with_config(config.pids.pitch_inner, imu_period); - let mut pid_yaw_outer: _ = Pid::with_config(config.pids.yaw_outer, imu_period); - let mut pid_yaw_inner: _ = Pid::with_config(config.pids.yaw_inner, imu_period); + let mut pid_roll_outer: _ = Pid::with_config(pids.roll_outer, imu_period); + let mut pid_roll_inner: _ = Pid::with_config(pids.roll_inner, imu_period); + let mut pid_pitch_outer: _ = Pid::with_config(pids.pitch_outer, imu_period); + let mut pid_pitch_inner: _ = Pid::with_config(pids.pitch_inner, imu_period); + let mut pid_yaw_outer: _ = Pid::with_config(pids.yaw_outer, imu_period); + let mut pid_yaw_inner: _ = Pid::with_config(pids.yaw_inner, imu_period); // Start with integral controllers disabled pid_roll_outer.enable_integral(false); diff --git a/src/t_attitude_estimator.rs b/src/t_attitude_estimator.rs index ed7d6d5..ab2c6c0 100644 --- a/src/t_attitude_estimator.rs +++ b/src/t_attitude_estimator.rs @@ -15,11 +15,8 @@ pub async fn attitude_estimator() -> ! { let snd_attitude_quat = msg::ATTITUDE_QUAT.sender(); let snd_attitude_euler = msg::ATTITUDE_EULER.sender(); - // Gat static reference to config - let config = msg::STATIC_CONFIG_REF.receiver().unwrap().changed().await; - // Period between IMU samples as seconds - let imu_period = 1.0 / config.imu_freq as f32; + let imu_period = 1.0 / msg::CFG_LOOP_FREQUENCY.spin_get().await as f32; // Setup AHRS filter for attitude estimation let mut ahrs = Madgwick::new(imu_period, 0.01); diff --git a/src/t_blackbox_logging.rs b/src/t_blackbox_logging.rs new file mode 100644 index 0000000..ad2077d --- /dev/null +++ b/src/t_blackbox_logging.rs @@ -0,0 +1,22 @@ +// TODO - investigate how to use the sequential-storage crate +// to log data to flash in a circular buffer in memory. + +// Suggestion for logging contents +// Should also contain system state, such as arming status, and any error flags. +struct LogEntry { + time_ms: u32, + acc: Vector3, + gyr: Vector3, + mag: Vector3, + euler: Vector3, + setpoint: Vector4, + ctrl_output: Vector3, + motor_speeds: Vector4, +} + +// Suggestion for logging header +// Header exists to ensure we can separate logs from different runs, +// And only overwrite the oldest logs when the buffer is full. +struct LogHeader { + id: u32, +} \ No newline at end of file diff --git a/src/t_commander.rs b/src/t_commander.rs index bec574d..174d544 100644 --- a/src/t_commander.rs +++ b/src/t_commander.rs @@ -1,7 +1,11 @@ +use core::f32::consts::PI; + use embassy_futures::select; +use embassy_time::{Duration, Instant, Ticker}; use nalgebra::Vector3; use crate::common::types::AttitudeReference; +use crate::constants::PI_D2; use crate::transmitter::EventRequest; use crate::messaging as msg; @@ -10,19 +14,24 @@ const TASK_ID : &str = "[COMMANDER]"; #[embassy_executor::task] pub async fn commander() { + defmt::info!("{}: Starting commander task", TASK_ID); + // Input messages let rcv_request_queue = msg::REQUEST_QUEUE.receiver(); let mut rcv_request_controls = msg::REQUEST_CONTROLS.receiver().unwrap(); let mut rcv_motor_state = msg::MOTOR_STATE.receiver().unwrap(); + let mut rcv_landed_state = msg::LANDED_STATE.receiver().unwrap(); + let mut rcv_attitude_euler = msg::ATTITUDE_EULER.receiver().unwrap(); // Output messages - let snd_attitude_setpoint = msg::ATTITUDE_SETPOINT.sender(); - let snd_throttle_setpoint = msg::THROTTLE_SETPOINT.sender(); - let snd_arming_command = msg::RC_ARMING_CMD.sender(); - let snd_start_gyr_calib = msg::RC_START_GYR_CALIB.sender(); - let snd_start_acc_calib = msg::RC_START_ACC_CALIB.sender(); - let snd_start_mag_calib = msg::RC_START_MAG_CALIB.sender(); - let snd_save_config = msg::RC_SAVE_CONFIG.sender(); + let snd_attitude_setpoint = msg::CMD_ATTITUDE_SETPOINT.sender(); + let snd_throttle_setpoint = msg::CMD_THROTTLE_SETPOINT.sender(); + let snd_arming_command = msg::CMD_ARM_VEHICLE.sender(); + let snd_start_gyr_calib = msg::CMD_START_GYR_CALIB.sender(); + let snd_start_acc_calib = msg::CMD_START_ACC_CALIB.sender(); + let snd_start_mag_calib = msg::CMD_START_MAG_CALIB.sender(); + let snd_save_config = msg::CMD_SAVE_CONFIG.sender(); + let snd_en_integral = msg::CMD_EN_INTEGRAL.sender(); enum StabMode { Angle, @@ -31,16 +40,29 @@ pub async fn commander() { let mut stab_mode = StabMode::Angle; - defmt::info!("{}: Starting commander task", TASK_ID); + let mut ticker = Ticker::every(Duration::from_hz(20)); + + let mut prev_request_controls_time = Instant::now(); + let mut yaw_integrated = rcv_attitude_euler.get().await.z; + '_infinite: loop { - match select::select( + // Futures are polled in the order they are defined (importance) + match select::select3( + + // Handle discrete events first, if any rcv_request_queue.receive(), + + // Handle continuous events from controls rcv_request_controls.changed(), + + // Periodic checks (runs at 20Hz) + ticker.next() + ).await { // Discrete event from queue - select::Either::First(event) => { + select::Either3::First(event) => { match event { EventRequest::Unbound => defmt::error!("{}: Unbound commands should not end up in the request queue!", TASK_ID), @@ -84,18 +106,63 @@ pub async fn commander() { }, // Continuous event from controls - select::Either::Second(control) => { + select::Either3::Second(control) => { - let control_vec = Vector3::new(control.roll, control.pitch, control.yaw); + let attitude_setpoint: AttitudeReference; + match stab_mode { - let attitude_setpoint = match stab_mode { - StabMode::Angle => AttitudeReference::Angle(control_vec), - StabMode::Rate => AttitudeReference::Rate(control_vec), - }; + // When in angle mode, we integrate the yaw control to get the desired angle + StabMode::Angle => { + + // If the vehicle is armed, and control command is not stale, integrate the yaw control + let time_now = Instant::now(); + if prev_request_controls_time.elapsed() < Duration::from_millis(100) + && rcv_motor_state.get().await.is_armed() { + yaw_integrated += control.yaw * (time_now.duration_since(prev_request_controls_time).as_micros() as f32 / 1e6); + yaw_integrated = crate::functions::wrap(yaw_integrated, -PI, PI); + } else { + yaw_integrated = rcv_attitude_euler.get().await.z; + + } + + prev_request_controls_time = time_now; + + // Clamp the roll and pitch, in case of extreme commands + let control_roll = control.roll.clamp(-PI_D2, PI_D2); + let control_pitch = control.pitch.clamp(-PI_D2, PI_D2); + + attitude_setpoint = AttitudeReference::Angle(Vector3::new(control_roll, control_pitch, yaw_integrated)); + }, + + // When in rate mode, we directly use the controls as the desired rate + StabMode::Rate => attitude_setpoint = { + let rate_vector = Vector3::new(control.roll, control.pitch, control.yaw); + + // NOTE We double the rate_vector to get closer to a typical rate. + // We might want to give this option to the user, but this is probably a good default. + AttitudeReference::Rate(rate_vector * 2.0) + }, + } snd_attitude_setpoint.send(attitude_setpoint); snd_throttle_setpoint.send(control.thrust); }, + + // Periodic checks go here, general business logic + select::Either3::Third(_) => { + + // Enable or disable attitude control integral term if not airborne + if let Some(landed_state) = rcv_landed_state.try_changed() { + use crate::t_flight_detector::LandedState; + match landed_state { + LandedState::Undefined => snd_en_integral.send_if_different(false), + LandedState::OnGround => snd_en_integral.send_if_different(false), + LandedState::InAir => snd_en_integral.send_if_different(true), + LandedState::Landing => snd_en_integral.send_if_different(true), + LandedState::Takeoff => snd_en_integral.send_if_different(true), + } + } + } } } } diff --git a/src/t_config_master.rs b/src/t_config_master.rs index 71b69fd..a2c37b4 100644 --- a/src/t_config_master.rs +++ b/src/t_config_master.rs @@ -7,21 +7,17 @@ static TASK_ID: &str = "[CONFIG_MASTER]"; use crate::messaging as msg; -// We need to get rid of this! -static CONFIG: static_cell::StaticCell = static_cell::StaticCell::new(); - #[embassy_executor::task] pub async fn config_master( mut flash: Flash<'static, FLASH, Async, { crate::config::FLASH_AMT }>, ) { // Input channels - let mut rcv_save_config = msg::RC_SAVE_CONFIG.receiver().unwrap(); - let mut rcv_gyr_calibrations = msg::GYR_CALIBRATIONS.receiver().unwrap(); - let mut rcv_acc_calibrations = msg::ACC_CALIBRATIONS.receiver().unwrap(); - - // Output channels - let snd_config_ref = msg::STATIC_CONFIG_REF.sender(); + let mut rcv_save_config = msg::CMD_SAVE_CONFIG.receiver().unwrap(); + let mut rcv_imu_config = msg::CFG_IMU_CONFIG.receiver().unwrap(); + let mut rcv_mag_config = msg::CFG_MAG_CONFIG.receiver().unwrap(); + let mut rcv_attitude_pids = msg::CFG_ATTITUDE_PIDS.receiver().unwrap(); + let mut rcv_transmitter_map = msg::CFG_TRANSMITTER_MAP.receiver().unwrap(); // Try to load from memory, or use default let mut config = match load_config(&mut flash) { @@ -35,46 +31,71 @@ pub async fn config_master( } }; - // NOTE - Overwrites the loaded transmitter map with the default one - config.tx_map = crate::transmitter::tx_12_profiles::TX12_DEFAULT_MAP; - crate::messaging::CFG_TRANSMITTER_MAP.sender().send(config.tx_map); + // NOTE - Overwrites the loaded transmitter map with the default one + config.tx_map = crate::transmitter::tx_12_profiles::TX12_8CH_DEFAULT_MAP; + + // Distribute initial transmitter map + msg::CFG_TRANSMITTER_MAP.sender().send(config.tx_map); + + // Distribute initial imu configurations + msg::CFG_IMU_CONFIG.sender().send(config.imu_cfg); + + // Distribute initial magnetometer configurations + msg::CFG_MAG_CONFIG.sender().send(config.mag_cfg); + + // Distribute initial motor mixing matrix + msg::CFG_MIXING_MATRIX.sender().send(config.mixing_matrix); + + // Distribute initial PID values + msg::CFG_ATTITUDE_PIDS.sender().send(config.pids); + + // Distribute initial imu (attitude control loop) frequency + msg::CFG_LOOP_FREQUENCY.sender().send(config.imu_freq); + + // Distribute initial magnetometer frequency + msg::CFG_MAG_FREQUENCY.sender().send(config.mag_freq); + + // Distribute initial DSHOT speed + msg::CFG_DSHOT_SPEED.sender().send(config.dshot_speed); + + // Distribute initial DSHOT timeout + msg::CFG_DSHOT_TIMEOUT.sender().send(config.dshot_timeout); - // Initialize static and distribute reference - // TODO - This is not how it should be done. The config governor should split up - // the config and distribute the parts to the tasks that need them. - let config_ref = CONFIG.init(config); - snd_config_ref.send(config_ref); + // Distribute initial DSHOT timeout + msg::CFG_RADIO_TIMEOUT.sender().send(config.radio_timeout); + + // Distribute initial DSHOT timeout + msg::CFG_REVERSE_MOTOR.sender().send(config.motor_dir); defmt::info!("{}: Using the config: {}", TASK_ID,defmt::Debug2Format(&config)); loop { - - // Wait for a signal to save the config + + // Wait for command to save the config rcv_save_config.changed_and(|x|x == &true).await; - defmt::info!("{}: Saving the config to flash", TASK_ID); + // Fetch updated imu configuration values + if let Some(new_imu_config) = rcv_imu_config.try_changed() { + config.imu_cfg = new_imu_config; + } - // Update gyroscope calibration values - if let Some(gyr_calibrations) = rcv_gyr_calibrations.try_changed() { - for (i, gyr_cal) in gyr_calibrations.iter().enumerate() { - if let Some(imu_cfg) = config.imu_cfg[i].as_mut() { - imu_cfg.gyr_cal = *gyr_cal; - } - } + // Fetch updated magnetometer configuration values + if let Some(new_mag_config) = rcv_mag_config.try_changed() { + config.mag_cfg = new_mag_config; } - // Update accelerometer calibration values - if let Some(acc_calibrations) = rcv_acc_calibrations.try_changed() { - for (i, acc_cal) in acc_calibrations.iter().enumerate() { - if let Some(imu_cfg) = config.imu_cfg[i].as_mut() { - imu_cfg.acc_cal = *acc_cal; - } - } + // Fetch updated attitude PID values + if let Some(mew_attitude_pids) = rcv_attitude_pids.try_changed() { + config.pids = mew_attitude_pids; } - defmt::info!("{}: Saving the config: {}", TASK_ID,defmt::Debug2Format(&config)); + // Fetch updated transmitter map + if let Some(new_transmitter_map) = rcv_transmitter_map.try_changed() { + config.tx_map = new_transmitter_map; + } - // Write inactive config to flash storage if something changed + // Save the config to flash // TODO Consider sequential storage key-value type + defmt::info!("{}: Saving the config: {}", TASK_ID,defmt::Debug2Format(&config)); unsafe { crate::config::storage::write(&mut flash, &config, crate::config::CFG_ADDR_OFFSET) }; } } @@ -89,4 +110,23 @@ pub fn load_config( } else { Err(()) } +} + +#[derive(Debug, Copy, Clone)] +pub enum ChangeConfig { + GyrCalib([Option; crate::N_IMU]), + AccCalib([Option; crate::N_IMU]), + MagCalib([Option; crate::N_IMU]), + TxMap(crate::transmitter::TransmitterMap), + ChangePid(ChangePids), +} + +#[derive(Debug, Copy, Clone)] +pub enum ChangePids { + RollInner(crate::filters::pid_controller::PidConfig), + RollOuter(crate::filters::pid_controller::PidConfig), + PitchInner(crate::filters::pid_controller::PidConfig), + PitchOuter(crate::filters::pid_controller::PidConfig), + YawInner(crate::filters::pid_controller::PidConfig), + YawOuter(crate::filters::pid_controller::PidConfig), } \ No newline at end of file diff --git a/src/t_gyr_calibration.rs b/src/t_gyr_calibration.rs index ce83a36..6cd375d 100644 --- a/src/t_gyr_calibration.rs +++ b/src/t_gyr_calibration.rs @@ -1,29 +1,19 @@ use embassy_time::{with_timeout, Duration, Ticker}; use nalgebra::{SMatrix, Vector3}; -use crate::{config::{Calibration, Configuration}, messaging as msg, sensors::imu::types::ImuData6Dof}; +use crate::{config::Calibration, messaging as msg, sensors::imu::types::ImuData6Dof}; /// Routine to calibrate gyroscopes, by calculating their bias. #[embassy_executor::task] -pub async fn gyr_calibration( - config: &'static Configuration -) -> ! { +pub async fn gyr_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_gyr_calib = msg::RC_START_GYR_CALIB.receiver().unwrap(); + let mut rcv_start_gyr_calib = msg::CMD_START_GYR_CALIB.receiver().unwrap(); // Output channels - let snd_gyr_calib = msg::GYR_CALIBRATIONS.sender(); + let snd_imu_config = msg::CFG_IMU_CONFIG.sender(); let snd_gyr_calibrating = msg::GYR_CALIBRATING.sender(); - - // Publish existing calibration data - snd_gyr_calib.send(core::array::from_fn(|i|{ - match config.imu_cfg[i].as_ref() { - Some(cfg) => cfg.gyr_cal, - None => None, - } - })); 'infinite: loop { @@ -34,12 +24,12 @@ pub async fn gyr_calibration( defmt::info!("[GYR CALIB]: Starting bias calibration on {} gyroscopes", crate::N_IMU); // Array of calibrator instances - let mut sensor_calibrator = [GyrCalibrator::<400>::new(); crate::N_IMU]; + let mut sensor_calibrator = [GyrCalibrator::<200>::new(); crate::N_IMU]; let mut timeout_count = 0; let mut done_count = 0; - let mut ticker = Ticker::every(Duration::from_hz(200)); + let mut ticker = Ticker::every(Duration::from_hz(50)); // Calibration loop 'calibration: loop { @@ -88,31 +78,36 @@ pub async fn gyr_calibration( } // Get existing calibration data or create new - let mut calibrations = snd_gyr_calib.try_get().unwrap_or([None; crate::N_IMU]); + let mut calibrations = snd_imu_config.try_get().unwrap_or([None; crate::N_IMU]); // 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.acc_variance() { - Some(var) if var < 0.002 => { - defmt::info!("[GYR CALIB]: Sensor {} calibration is valid: Var({})", id, var); + for (id, (opt_config, calibrator)) in calibrations.iter_mut().zip(sensor_calibrator.iter()).enumerate() { + + match (calibrator.acc_variance(), opt_config) { + // If calibration was successful, update the calibration data + (Some(var), Some(config)) if var < 0.002 => { + defmt::info!("[GYR CALIB]: Sensor {} calibration is valid: Var({})", id, var); + // Set calibration offset/bias for sensor - if let Some(inner) = prev_cal.as_mut() { - inner.offset = cal.get_bias(); + if let Some(gyr_cal) = config.gyr_cal.as_mut() { + gyr_cal.offset = calibrator.get_bias(); } else { - *prev_cal = Some(Calibration{ + config.gyr_cal = Some(Calibration{ scale: Vector3::new(1.0, 1.0, 1.0), - offset: cal.get_bias(), + offset: calibrator.get_bias(), }); } }, - Some(var) => defmt::error!("[GYR CALIB]: Sensor {} had too high variance: Var({:?})", id, var), - None if cal.has_timeout() => defmt::error!("[GYR CALIB]: Sensor {} timed out.", id), + + // If calibration was not successful, log the reason + (Some(var), Some(_)) => defmt::error!("[GYR CALIB]: Sensor {} had too high variance: Var({:?})", id, var), + (_, Some(_)) if calibrator.has_timeout() => defmt::error!("[GYR CALIB]: Sensor {} timed out.", id), _ => defmt::error!("[GYR CALIB]: Sensor {} could not be calibrated.", id), } } - snd_gyr_calib.send(calibrations); + snd_imu_config.send(calibrations); // Ensure any recent request to calibrate is marked as seen let _ = rcv_start_gyr_calib.try_get(); diff --git a/src/t_icm20948_driver.rs b/src/t_icm20948_driver.rs index dc3a5bc..c8c4ca4 100644 --- a/src/t_icm20948_driver.rs +++ b/src/t_icm20948_driver.rs @@ -1,4 +1,4 @@ -use crate::config::{Configuration, ImuTypeConf}; +use crate::config::ImuTypeConf; use embassy_futures::select::{select, Either}; use embassy_time::{with_timeout, Delay, Duration, Instant, Ticker}; @@ -12,9 +12,12 @@ pub async fn icm_20948_driver( i2c: crate::main_core0::I2cAsyncType, imu_id: u8, mag_id: u8, - config: &'static Configuration, ) -> ! { + // Ensure that the IMU and MAG IDs are within bounds + defmt::assert!((imu_id as usize) < crate::N_IMU, "[ICM20948 DRIVER]: IMU ID {} out of bounds (max is {})", imu_id, crate::N_IMU - 1); + defmt::assert!((mag_id as usize) < crate::N_MAG, "[ICM20948 DRIVER]: MAG ID {} out of bounds (max is {})", mag_id, crate::N_MAG - 1); + defmt::trace!("[ICM20948 DRIVER]: Driver task started"); // Input messages @@ -25,7 +28,11 @@ pub async fn icm_20948_driver( let snd_imu_sensor = msg::IMU_SENSOR[imu_id as usize].sender(); let snd_mag_sensor = msg::MAG_SENSOR[mag_id as usize].sender(); - let Some(ImuTypeConf::Icm20948(icm_config)) = config.imu_cfg[0].unwrap().imu_type else { + // Get the IMU and MAG configurations + let imu_config = msg::CFG_IMU_CONFIG.spin_get().await[imu_id as usize].unwrap(); + let mag_config = msg::CFG_MAG_CONFIG.spin_get().await[mag_id as usize].unwrap(); + + let Some(ImuTypeConf::Icm20948(icm_config)) = imu_config.imu_type else { panic!("[ICM20948 DRIVER]: Incorrect config provided") }; @@ -49,9 +56,6 @@ pub async fn icm_20948_driver( let mut imu_is_active: bool = false; let mut mag_is_active: bool = false; - let imu_config = config.imu_cfg[imu_id as usize].unwrap(); - let mag_config = config.mag_cfg[mag_id as usize].unwrap(); - 'infinite: loop { // Wait on any of the two sensors to become active match select( @@ -75,8 +79,10 @@ pub async fn icm_20948_driver( defmt::info!("[ICM20948 DRIVER]: IMU active: {} - MAG active: {}", imu_is_active, mag_is_active); // Create time keeping objects // TODO Get the frequency from the governor - let mut imu_ticker = Ticker::every(Duration::from_hz(config.imu_freq as u64)); - let mag_duration = Duration::from_hz(config.mag_freq as u64); + let loop_frequency = msg::CFG_LOOP_FREQUENCY.spin_get().await; + let mag_frequency = msg::CFG_MAG_FREQUENCY.spin_get().await; + let mut imu_ticker = Ticker::every(Duration::from_hz(loop_frequency as u64)); + let mag_duration = Duration::from_hz(mag_frequency as u64); let mut mag_timer = Instant::now(); while imu_is_active || mag_is_active { @@ -94,13 +100,6 @@ pub async fn icm_20948_driver( // Read and apply calibration to both IMU and MAG data if imu_is_active && mag_is_active && Instant::now() > mag_timer { if let Ok(mut imu_data) = imu.read_9dof().await { - if let Some(imu_ext) = imu_config.imu_ext { - imu_ext.apply(&mut imu_data.gyr); - imu_ext.apply(&mut imu_data.acc); - } - if let Some(mag_ext) = mag_config.mag_ext { - mag_ext.apply(&mut imu_data.mag); - } snd_imu_sensor.send((imu_data.into(), Instant::now())); snd_mag_sensor.send((imu_data.mag, Instant::now())); mag_timer += mag_duration; @@ -109,19 +108,12 @@ pub async fn icm_20948_driver( // Read and apply calibration to IMU data only } else if imu_is_active { if let Ok(mut imu_data) = imu.read_6dof().await { - if let Some(imu_ext) = imu_config.imu_ext { - imu_ext.apply(&mut imu_data.gyr); - imu_ext.apply(&mut imu_data.acc); - } snd_imu_sensor.send((imu_data.into(), Instant::now())); } // Read and apply calibration to MAG data only } else if mag_is_active && Instant::now() > mag_timer { if let Ok(mut mag_data) = imu.read_mag().await { - if let Some(mag_ext) = mag_config.mag_ext { - mag_ext.apply(&mut mag_data); - } snd_mag_sensor.send((mag_data, Instant::now())); mag_timer += mag_duration; } diff --git a/src/t_imu_governor.rs b/src/t_imu_governor.rs index beb2fa6..459f3c3 100644 --- a/src/t_imu_governor.rs +++ b/src/t_imu_governor.rs @@ -9,8 +9,7 @@ use crate::{messaging as msg, sensors::{SensorCondition, SensorRedundancy}}; pub async fn imu_governor() -> ! { // Input messages let mut rcv_imu_sensor_array: [_; crate::N_IMU] = core::array::from_fn(|i| msg::IMU_SENSOR[i].receiver().unwrap()); - let mut rcv_gyr_calib = msg::GYR_CALIBRATIONS.receiver().unwrap(); - let mut rcv_acc_calib = msg::ACC_CALIBRATIONS.receiver().unwrap(); + let mut rcv_imu_config = msg::CFG_IMU_CONFIG.receiver().unwrap(); // Output messages let snd_imu_data = msg::IMU_DATA.sender(); @@ -23,20 +22,13 @@ pub async fn imu_governor() -> ! { snd_imu_active_id.send(imu_redundancy.active_id()); - // Receive initial calibration data - let mut gyr_calib = rcv_gyr_calib.changed().await; - let mut acc_calib = [None; crate::N_IMU]; // rcv_acc_calib.changed().await; + let mut imu_config = rcv_imu_config.changed().await; 'infinite: loop { // Check if there is new gyroscope calibration data - if let Some(new_gyr_calib) = rcv_gyr_calib.try_get() { - gyr_calib = new_gyr_calib - } - - // Check if there is new accelerometer calibration data - if let Some(new_acc_calib) = rcv_acc_calib.try_get() { - acc_calib = new_acc_calib + if let Some(new_imu_config) = rcv_imu_config.try_get() { + imu_config = new_imu_config } // Wait for data from any IMU sensor by polling each sensor channel @@ -68,32 +60,44 @@ pub async fn imu_governor() -> ! { // Check if sensor is degraded and lower state if it is active if imu_redundancy.is_degraded(id) { - warn!("[IMU REDUNDANCY]: IMU sensor {} is degraded", id); if imu_redundancy.is_active(id) { - imu_redundancy.lower_state() + warn!("[IMU REDUNDANCY]: IMU sensor {} is degraded, attempting fallback.", id); + imu_redundancy.lower_state(); } continue 'infinite; } // TODO : Add more checks for other sensor failures - // Transmit state of gyroscope calibration - match gyr_calib.get(id as usize) { - Some(Some(_)) if imu_redundancy.is_active(id) => snd_gyr_calibrated.send(true), - _ => snd_gyr_calibrated.send(false), - } + // If sensor is active, apply calibrations, extrinsics and transmit + if imu_redundancy.is_active(id) { - // Transmit state of accelerometer calibration - match acc_calib.get(id as usize) { - Some(Some(_)) if imu_redundancy.is_active(id) => snd_acc_calibrated.send(true), - _ => snd_acc_calibrated.send(false), - } + if let Some(Some(config)) = imu_config.get(id as usize) { + + // Apply calibration to gyroscope data + if let Some(gyr_cal) = config.gyr_cal { + gyr_cal.apply(&mut data.gyr); + } - // Send data of active sensor - if let Some(Some(gyr_cal)) = gyr_calib.get(id as usize) && let Some(Some(acc_cal)) = acc_calib.get(id as usize) && imu_redundancy.is_active(id) { - gyr_cal.apply(&mut data.gyr); - acc_cal.apply(&mut data.acc); - snd_imu_data.send(data) + // Apply calibration to accelerometer data + if let Some(acc_cal) = config.acc_cal { + acc_cal.apply(&mut data.acc); + } + + // Apply extrinsics to IMU data + if let Some(imu_ext) = config.imu_ext { + imu_ext.apply(&mut data.gyr); + imu_ext.apply(&mut data.acc); + } + + snd_imu_data.send(data) + } + + // Transmit state of calibrations for current sensor + if let Some(Some(config)) = imu_config.get(id as usize) { + snd_gyr_calibrated.send(config.gyr_cal.is_some()); + snd_acc_calibrated.send(config.acc_cal.is_some()); + } } // If active sensor changed, send new active sensor id diff --git a/src/t_motor_governor.rs b/src/t_motor_governor.rs index 91b57cc..d3c55ca 100644 --- a/src/t_motor_governor.rs +++ b/src/t_motor_governor.rs @@ -2,7 +2,7 @@ use crate::common::types::{ArmedState, DisarmReason, MotorState}; use crate::drivers::rp2040::dshot_pio::{DshotPio, DshotPioTrait}; use embassy_futures::select::{select, Either}; use embassy_rp::peripherals::PIO0; -use embassy_time::{with_timeout, Duration, Timer}; +use embassy_time::{with_timeout, Timer}; pub const TASK_ID: &str = "[MOTOR GOVERNOR]"; @@ -63,8 +63,6 @@ use crate::messaging as msg; #[embassy_executor::task] pub async fn motor_governor( mut out_dshot_pio: DshotPio<'static, 4, PIO0>, - reverse_motor: [bool; 4], - timeout: Duration, ) -> ! { // Input messages @@ -96,7 +94,7 @@ pub async fn motor_governor( // Set motor directions for the four motors defmt::info!("{} : Setting motor directions", TASK_ID); for _i in 0..10 { - out_dshot_pio.reverse(reverse_motor); + out_dshot_pio.reverse(msg::CFG_REVERSE_MOTOR.spin_get().await); Timer::after_millis(50).await; } @@ -107,11 +105,13 @@ pub async fn motor_governor( continue 'infinite; } - defmt::info!("{} : Motors armed and active", TASK_ID); + let dshot_timeout = msg::CFG_DSHOT_TIMEOUT.spin_get().await; + + defmt::info!("{} : Motors are armed and active", TASK_ID); 'armed: loop { match with_timeout( - timeout, + dshot_timeout, select( rcv_motor_speed.changed(), rcv_arming_prevention.changed(), diff --git a/src/t_motor_mixing.rs b/src/t_motor_mixing.rs index 3e14e19..5e7e7d5 100644 --- a/src/t_motor_mixing.rs +++ b/src/t_motor_mixing.rs @@ -1,3 +1,5 @@ +use embassy_futures::select::select; + use crate::{functions::map, health::LoopHealth, messaging as msg}; #[embassy_executor::task] @@ -5,38 +7,48 @@ pub async fn motor_mixing() -> ! { // Input messages let mut rcv_controller_output = msg::CONTROLLER_OUTPUT.receiver().unwrap(); - let mut rcv_throttle_setpoint = msg::THROTTLE_SETPOINT.receiver().unwrap(); + let mut rcv_throttle_setpoint = msg::CMD_THROTTLE_SETPOINT.receiver().unwrap(); + let mut rcv_cfg_mixing_matrix = msg::CFG_MIXING_MATRIX.receiver().unwrap(); // Output messages let snd_motor_speeds = msg::MOTOR_SPEEDS.sender(); let snd_loop_health = msg::LOOP_HEALTH.sender(); - // Get static reference to config - let config = msg::STATIC_CONFIG_REF.receiver().unwrap().changed().await; + let mut mixing_matrix = rcv_cfg_mixing_matrix.changed().await; // Initialize loop health monitor - let mut loop_health = LoopHealth::new(config.imu_freq); + let mut loop_health = LoopHealth::new(msg::CFG_LOOP_FREQUENCY.spin_get().await); '_infinite: loop { - // Wait for the next controller output - let controller_output = rcv_controller_output.changed().await; - let throttle_target = rcv_throttle_setpoint.try_get().unwrap_or(0.0); - - // Map raw throttle command [0..1] to usable range [200..1500] - let throttle_mapped = map(throttle_target, 0.0, 1.0, 200., 1500.); - - // Mix thrust and controller output into motor speeds - let motors_mixed = - config.mixing_matrix.mixing_fn(throttle_mapped, controller_output) - .map(|x| (x as u16).clamp(70, 1500)); - - // Publish motor speeds to motor governor task - snd_motor_speeds.send(motors_mixed.into()); - - // Evaluate and send loop health - if loop_health.evaluate() { - snd_loop_health.send(loop_health.get_health()); + match select( + rcv_controller_output.changed(), + rcv_cfg_mixing_matrix.changed(), + ).await { + embassy_futures::select::Either::First(controller_output) => { + let throttle_target = rcv_throttle_setpoint.try_get().unwrap_or(0.0); + + // Map raw throttle command [0..1] to usable range [200..1500] + let throttle_mapped = map(throttle_target, 0.0, 1.0, 200., 1500.); + + // Mix thrust and controller output into motor speeds + let motors_mixed = mixing_matrix.mixing_fn(throttle_mapped, controller_output) + .map(|x| (x as u16).clamp(70, 1500)); + + // Publish motor speeds to motor governor task + snd_motor_speeds.send(motors_mixed.into()); + + // Evaluate and send loop health + if loop_health.evaluate() { + snd_loop_health.send(loop_health.get_health()); + } + } + embassy_futures::select::Either::Second(new_mixing_matrix) => { + // Update the mixing matrix + mixing_matrix = new_mixing_matrix; + } } + + } } diff --git a/src/t_rc_mapper.rs b/src/t_rc_mapper.rs index 7497b58..5600176 100644 --- a/src/t_rc_mapper.rs +++ b/src/t_rc_mapper.rs @@ -2,8 +2,7 @@ use embassy_futures::select::{select, Either}; use crate::messaging as msg; -use crate::transmitter::AnalogCommand::{Pitch, Roll, Thrust, Yaw}; -use crate::transmitter::{analog_map, ChannelType, ControlRequest, EventRequest}; +use crate::transmitter::{ChannelType, ControlRequest, EventRequest}; #[embassy_executor::task] pub async fn rc_mapper() { @@ -15,8 +14,13 @@ pub async fn rc_mapper() { let snd_request_controls = msg::REQUEST_CONTROLS.sender(); let snd_request_queue = msg::REQUEST_QUEUE.sender(); - // Await initial transmitter map + // Await initial transmitter map (or loop until valid map is received) let mut transmitter_map = rcv_cfg_transmitter_map.changed().await; + while let Err(e) = transmitter_map.sanity_check() { + defmt::error!("[RC MAPPER]: Loaded an invalid transmitter from config: {}", e); + transmitter_map = rcv_cfg_transmitter_map.changed().await; + } + let mut prev_sbus_packet: Option = None; let mut control = ControlRequest { roll: 0.0, @@ -31,34 +35,37 @@ pub async fn rc_mapper() { // Received new sbus packet, map it Either::First(Ok(sbus_packet)) => { + use crate::transmitter::AnalogCommand::{Pitch, Roll, Thrust, Yaw}; + // Similar to sbus_packet.channels.iter().zip(transmitter_map.iter()).enumerate().for_each(|(i, (channel, map))| { ... }), // but we avoid using a closure, where we are not able to do an async send into the request queue channel. - for i in 0..sbus_packet.channels.len().min(transmitter_map.len()) { - match &transmitter_map[i] { + for ch in 0..sbus_packet.channels.len().min(transmitter_map.len()) { + match &transmitter_map[ch] { ChannelType::None => {}, - ChannelType::Analog((Roll,cfg)) => control.roll = analog_map(sbus_packet.channels[i], cfg), - ChannelType::Analog((Pitch,cfg)) => control.pitch = analog_map(sbus_packet.channels[i], cfg), - ChannelType::Analog((Yaw,cfg)) => control.yaw = analog_map(sbus_packet.channels[i], cfg), - ChannelType::Analog((Thrust,cfg)) => control.thrust = analog_map(sbus_packet.channels[i], cfg), - ChannelType::Discrete(positions) => { - for &(pos, cmd) in positions { - if pos == sbus_packet.channels[i] { + ChannelType::Analog((Roll, cfg)) => control.roll = cfg.apply(sbus_packet.channels[ch]), + ChannelType::Analog((Pitch, cfg)) => control.pitch = cfg.apply(sbus_packet.channels[ch]), + ChannelType::Analog((Yaw, cfg)) => control.yaw = cfg.apply(sbus_packet.channels[ch]), + ChannelType::Analog((Thrust, cfg)) => control.thrust = cfg.apply(sbus_packet.channels[ch]), + ChannelType::Discrete(bindings) => { + // Check each value-event binding pair for this channel + for &(value, event) in bindings { + if value == sbus_packet.channels[ch] { // If value is same as previous, skip - if let Some(prev) = prev_sbus_packet && prev.channels[i] == sbus_packet.channels[i] { + if let Some(prev) = prev_sbus_packet + && prev.channels[ch] == sbus_packet.channels[ch] { continue; } - - // If mapping is unbound, skip - if cmd == EventRequest::Unbound { + + // Log command if it has changed + defmt::trace!("[RC MAPPER]: From channel {} - {} received discrete command: {:?}", ch, value, defmt::Debug2Format(&event)); + + // If mapping is unbound, skip sending it + if event == EventRequest::Unbound { continue; } - - // Log command if bound - defmt::info!("[RC MAPPER]: From channel {} - {} received discrete command: {:?}", i, pos, defmt::Debug2Format(&cmd)); - // Send command to request queue - snd_request_queue.send(cmd).await; + snd_request_queue.send(event).await; } } }, @@ -70,13 +77,27 @@ pub async fn rc_mapper() { } // Failsafe, notify commander and reset prev_sbus_packet - Either::First(Err(_)) => { - prev_sbus_packet = None; + Either::First(Err(error)) => { + use crate::t_sbus_reader::RxError as E; + match error { + + // We need to not reset prev_sbus_packet on E::SerialRead, as this + // error may occur naturally when the vehicle is on the ground. + E::SerialRead => { defmt::warn!("[RC MAPPER]: Received a serial read error"); }, + E::SerialTimeout => prev_sbus_packet = None, + E::ParseTimeout => prev_sbus_packet = None, + E::Failsafe => prev_sbus_packet = None, + }; snd_request_queue.send(EventRequest::RcFailsafe).await; } // Received new map, save it - Either::Second(new_map) => transmitter_map = new_map, + Either::Second(new_map) => { + match transmitter_map.sanity_check() { + Ok(()) => transmitter_map = new_map, + Err(e) => defmt::warn!("[RC MAPPER]: Received an invalid transmitter map: {}", e), + } + }, } } } \ No newline at end of file diff --git a/src/t_sbus_reader.rs b/src/t_sbus_reader.rs index 3d0f5fc..a588295 100644 --- a/src/t_sbus_reader.rs +++ b/src/t_sbus_reader.rs @@ -1,9 +1,7 @@ -use embassy_rp::{ - peripherals::UART1, - uart::{Async, UartRx}, -}; -use embassy_time::{with_timeout, Duration, Instant}; +use embassy_rp::peripherals::UART1; +use embassy_rp::uart::{Async, UartRx}; +use embassy_time::{with_timeout, Instant}; use sbus::SBusPacketParser; use crate::messaging as msg; @@ -32,10 +30,12 @@ pub enum RxError { /// Utilizes a custom interrupt-based UART driver /// to read entire SBUS packet in one go. #[embassy_executor::task] -pub async fn sbus_reader(mut uart_rx_sbus: UartRx<'static, UART1, Async>, timeout: Duration) { +pub async fn sbus_reader(mut uart_rx_sbus: UartRx<'static, UART1, Async>) { // Output messages let snd_sbus_data = msg::SBUS_DATA.dyn_sender(); + let timeout = msg::CFG_RADIO_TIMEOUT.spin_get().await; + let mut read_buffer = [0u8; 25]; let mut parser = SBusPacketParser::new(); let mut parse_time = Instant::now(); diff --git a/src/t_vehicle_state.rs b/src/t_vehicle_state.rs new file mode 100644 index 0000000..42455bd --- /dev/null +++ b/src/t_vehicle_state.rs @@ -0,0 +1,53 @@ +use embassy_time::{Duration, Ticker}; +use crate::{common::types::VehicleState, messaging as msg}; + +#[embassy_executor::task] +pub async fn arm_checker() -> ! { + // Input channels + let mut rcv_gyr_calibrating = msg::GYR_CALIBRATING.receiver().unwrap(); + let mut rcv_acc_calibrating = msg::ACC_CALIBRATING.receiver().unwrap(); + let mut rcv_landed_state = msg::LANDED_STATE.receiver().unwrap(); + + // Output channels + let snd_vehicle_state = msg::VEHICLE_STATE.sender(); + + // Initialize the vehicle state as being uninitialized + snd_vehicle_state.send(VehicleState::Uninit); + + // REVIEW - This entire task could be housed in the commander instead + + let mut ticker = Ticker::every(Duration::from_hz(10)); + + '_infinite: loop { + + let mut calibrating = false; + let mut airborne = false; + + if let Some(gyr_calibrating) = rcv_gyr_calibrating.try_changed() { + calibrating |= gyr_calibrating; + } + + if let Some(acc_calibrating) = rcv_acc_calibrating.try_changed() { + calibrating |= acc_calibrating; + } + + if let Some(landed_state) = rcv_landed_state.try_changed() { + use crate::t_flight_detector::LandedState as L; + match landed_state { + L::Undefined | L::OnGround => airborne = false, + L::Takeoff | L::Landing | L::InAir => airborne = true, + } + } + + if calibrating { + snd_vehicle_state.send(VehicleState::Calibrating); + } else if airborne { + snd_vehicle_state.send(VehicleState::Active); + } else { + snd_vehicle_state.send(VehicleState::Standby); + } + + ticker.next().await; + + } +} \ No newline at end of file diff --git a/src/transmitter/mod.rs b/src/transmitter/mod.rs index 86e0193..f995249 100644 --- a/src/transmitter/mod.rs +++ b/src/transmitter/mod.rs @@ -1,5 +1,7 @@ use core::ops::{Deref, DerefMut}; +use num_traits::Float; + pub mod tx_12_profiles; #[derive(Debug, Clone, Copy, Default)] @@ -27,6 +29,22 @@ impl DerefMut for TransmitterMap { } } +impl TransmitterMap { + pub fn sanity_check(&self) -> Result<(), (usize, RatesError)> { + for (ch, channel) in self.iter().enumerate() { + match channel { + ChannelType::Analog((_, cfg)) => { + if let Err(e) = cfg.sanity_check_rates() { + return Err((ch, e)); + } + } + _ => {}, // Currently no other checks for other channel types + } + } + Ok(()) + } +} + // TODO Move to a more appropriate location #[derive(Clone, Copy, Debug, PartialEq)] pub enum EventRequest { @@ -92,7 +110,6 @@ pub enum ChannelType { Discrete([(u16, EventRequest); 3]), } -// TODO Support expo curves and other non-linear mappings #[derive(Clone, Copy, Debug, PartialEq)] pub struct AnalogConfig { in_min: u16, @@ -100,50 +117,131 @@ pub struct AnalogConfig { deadband: u16, fullrange: bool, reverse: bool, + rates: Rates, } -/// Maps the raw input value from RC according to the provided configuration. -pub fn analog_map(data: u16, cfg: &AnalogConfig) -> f32 { - - // Select range mapping function - let value = if cfg.fullrange { - analog_map_full(data, cfg) - } else { - analog_map_half(data, cfg) - }; - - // Reverse the value if needed - if cfg.reverse { - -value - } else { - value +impl AnalogConfig { + pub fn apply(&self, data: u16) -> f32 { + // Select range mapping function + let mut value = if self.fullrange { + self.analog_map_full(data) + } else { + self.analog_map_half(data) + }; + + // Reverse the value if needed + if self.reverse { + value = -value + } + + // Apply rates and return + self.rates.apply(value) + } + + /// Full-range, maps the input range to the output range (-1, 1). + /// Typically used for roll, pitch and yaw commands. + fn analog_map_full(&self, data: u16) -> f32 { + + // Center the value around 0 + let mut value = data.saturating_sub(self.in_min) as i32 - (self.in_max - self.in_min) as i32/2; + + // Apply deadband on small values + if value.abs() < self.deadband as i32 { value = 0 } + + // Normalize and clamp the value to the range (-1, 1) + ((2*value) as f32 / (self.in_max - self.in_min) as f32).clamp(-1., 1.) + } + + /// Half-range, maps the input range to the output range (0, 1). + /// Typically used for thrust commands. + pub fn analog_map_half(&self, data: u16) -> f32 { + + // Shift the value down to 0 + let mut value = data.saturating_sub(self.in_min); + + // Apply deadband on small values + if value < self.deadband { value = 0 } + + // Normalize and clamp the value to the range (0, 1) + (value as f32 / (self.in_max - self.in_min) as f32).clamp(0., 1.) + } + + pub fn sanity_check_rates(&self) -> Result<(), RatesError> { + self.rates.sanity_check() } } -/// Full-range, maps the input range to the output range [-1, 1]. -/// Typically used for roll, pitch and yaw commands. -fn analog_map_full(data: u16, cfg: &AnalogConfig) -> f32 { - - // Center the value around 0 - let mut value = data.saturating_sub(cfg.in_min) as i32 - (cfg.in_max - cfg.in_min) as i32/2; +#[derive(Clone, Copy, Debug, PartialEq)] +pub enum Rates { + None, + Standard(StandardRates), + // More to come? +} - // Apply deadband on small values - if value.abs() < cfg.deadband as i32 { value = 0 } +impl Rates { - // Normalize and clamp the value to the range [-1, 1] - ((2*value) as f32 / (cfg.in_max - cfg.in_min) as f32).clamp(-1., 1.) + pub const fn new_standard(rate: f32, expo: f32, slow: f32) -> Self { + Self::Standard(StandardRates::new(rate, expo, slow)) + } + + pub fn apply(&self, input: f32) -> f32 { + match self { + Self::None => input, + Self::Standard(rates) => rates.apply(input), + } + } + + pub fn sanity_check(&self) -> Result<(), RatesError> { + match self { + Self::None => {Ok(())}, + Self::Standard(rates) => rates.sanity_check(), + } + } +} + +#[derive(Clone, Copy, Debug, PartialEq)] +pub struct StandardRates { + rate: f32, + expo: f32, + slow: f32, } -/// Half-range, maps the input range to the output range [0, 1]. -/// Typically used for thrust commands. -pub fn analog_map_half(data: u16, cfg: &AnalogConfig) -> f32 { - - // Shift the value down to 0 - let mut value = data.saturating_sub(cfg.in_min); +impl StandardRates { - // Apply deadband on small values - if value < cfg.deadband { value = 0 } + /// Create a new set of rates. This function will panic if the rates are not within reasonable limits. + pub const fn new(rate: f32, expo: f32, slow: f32) -> Self { + Self { + rate, + expo, + slow, + } + } + + /// Do an assertion on the rates constants to make sure they are within reasonable limits. + pub fn sanity_check(&self) -> Result<(), RatesError>{ + + if self.rate < 0.0 { + return Err(RatesError::NegativeGain); + } else if self.expo < 0.0 { + return Err(RatesError::NegativeGain); + } else if self.slow < 0.0 { + return Err(RatesError::NegativeGain); + } else if self.apply(1.0) > 10.0 { + return Err(RatesError::ExtremeOutput); + } + + Ok(()) + } + + /// Apply the rates to the input command + pub fn apply(&self, input: f32) -> f32 { + ((1. + 0.01 * self.expo * (input * input - 1.0)) * input) + * (self.rate + (input.abs() * self.rate * self.slow * 0.01)) + } +} - // Normalize and clamp the value to the range [0, 1] - (value as f32 / (cfg.in_max - cfg.in_min) as f32).clamp(0., 1.) +#[derive(Clone, Copy, Debug, defmt::Format, PartialEq)] +pub enum RatesError { + NegativeGain, + ExtremeOutput, } \ No newline at end of file diff --git a/src/transmitter/tx_12_profiles.rs b/src/transmitter/tx_12_profiles.rs index 4ece3fd..9f2dd49 100644 --- a/src/transmitter/tx_12_profiles.rs +++ b/src/transmitter/tx_12_profiles.rs @@ -1,8 +1,8 @@ use super::*; -pub const TX12_DEFAULT_MAP: TransmitterMap = TransmitterMap ([ +pub const TX12_8CH_DEFAULT_MAP: TransmitterMap = TransmitterMap ([ - // Channel 1 - Right stick X + // Channel 0 - Right stick X ChannelType::Analog(( AnalogCommand::Roll, AnalogConfig { @@ -11,10 +11,11 @@ pub const TX12_DEFAULT_MAP: TransmitterMap = TransmitterMap ([ deadband: 2, fullrange: true, reverse: false, + rates: Rates::new_standard(0.5, 20., 150.) } )), - // Channel 2 - Right stick Y + // Channel 1 - Right stick Y ChannelType::Analog(( AnalogCommand::Pitch, AnalogConfig { @@ -23,10 +24,11 @@ pub const TX12_DEFAULT_MAP: TransmitterMap = TransmitterMap ([ deadband: 2, fullrange: true, reverse: true, + rates: Rates::new_standard(0.5, 20., 150.) } )), - // Channel 3 - Left stick Y + // Channel 2 - Left stick Y ChannelType::Analog(( AnalogCommand::Thrust, AnalogConfig { @@ -35,10 +37,11 @@ pub const TX12_DEFAULT_MAP: TransmitterMap = TransmitterMap ([ deadband: 2, fullrange: false, reverse: false, + rates: Rates::None } )), - // Channel 4 - Left stick X + // Channel 3 - Left stick X ChannelType::Analog(( AnalogCommand::Yaw, AnalogConfig { @@ -47,31 +50,32 @@ pub const TX12_DEFAULT_MAP: TransmitterMap = TransmitterMap ([ deadband: 2, fullrange: true, reverse: false, + rates: Rates::new_standard(0.5, 20., 150.) } )), - // Channel 5 - Switch E + // Channel 4 - Switch E ChannelType::Discrete([ (172, EventRequest::Unbound), - (992, EventRequest::AbortGyrCalib), - (1810, EventRequest::StartGyrCalib), + (992, EventRequest::AbortAccCalib), + (1810, EventRequest::StartAccCalib), ]), - // Channel 6 - Switch B + // Channel 5 - Switch B ChannelType::Discrete([ (172, EventRequest::AngleMode), (992, EventRequest::Unbound), (1810, EventRequest::RateMode), ]), - // Channel 7 - Switch C + // Channel 6 - Switch C ChannelType::Discrete([ (172, EventRequest::DisarmMotors), (992, EventRequest::Unbound), (1810, EventRequest::ArmMotors), ]), - // Channel 8 - Switch F + // Channel 7 - Switch F ChannelType::Discrete([ (172, EventRequest::Unbound), (992, EventRequest::Unbound),