| 1 | use core::ops::{Add, Sub}; |
| 2 | |
| 3 | use defmt::{info, trace}; |
| 4 | use embassy_sync::{blocking_mutex::raw::ThreadModeRawMutex, channel::Receiver}; |
| 5 | use mpu6050_dmp::{quaternion::Quaternion, yaw_pitch_roll::YawPitchRoll}; |
| 6 | |
| 7 | use crate::mpu::BUFFERED_QUATERNIONS; |
| 8 | |
| 9 | // represents the motor's possible turning direction (clockwise and counter-clockwise) |
| 10 | pub enum MotorTurningState { |
| 11 | CW, |
| 12 | CCW |
| 13 | } |
| 14 | |
| 15 | // holds the motor state, including its angular speed and direction. |
| 16 | pub struct Motor { |
| 17 | angular_rotation_speed: f32, |
| 18 | turning: MotorTurningState, |
| 19 | } |
| 20 | |
| 21 | // represents the body's total motor count |
| 22 | pub const MOTOR_NUM: usize = 4; |
| 23 | |
| 24 | // holds the system state for control, including the motor rotation speeds, the current velocity and altitude, |
| 25 | // and several other data relevant to system control |
| 26 | pub struct Controller { |
| 27 | motors: [Motor; MOTOR_NUM], |
| 28 | altitude: f64, |
| 29 | velocity: f64, |
| 30 | // stores the last reference frame |
| 31 | last_frame: YawPitchRoll, |
| 32 | // stores the target body attitude desired, from a reference frame |
| 33 | attitude: YawPitchRoll |
| 34 | // store the last and next quaternion values for the control calculus |
| 35 | } |
| 36 | |
| 37 | pub struct Frame { |
| 38 | yaw: f32, |
| 39 | pitch: f32, |
| 40 | roll: f32 |
| 41 | } |
| 42 | |
| 43 | impl Add for Frame { |
| 44 | type Output = Self; |
| 45 | |
| 46 | fn add(self, other: Self) -> Self { |
| 47 | Self { |
| 48 | yaw: self.yaw + other.yaw, |
| 49 | pitch: self.pitch + other.pitch, |
| 50 | roll: self.roll + other.roll |
| 51 | } |
| 52 | } |
| 53 | } |
| 54 | |
| 55 | impl Sub for Frame { |
| 56 | type Output = Self; |
| 57 | |
| 58 | fn sub(self, other: Self) -> Self { |
| 59 | Self { |
| 60 | yaw: self.yaw - other.yaw, |
| 61 | pitch: self.pitch - other.pitch, |
| 62 | roll: self.roll - other.roll |
| 63 | } |
| 64 | } |
| 65 | |
| 66 | } |
| 67 | |
| 68 | #[embassy_executor::task] |
| 69 | pub async fn update_control_loop( |
| 70 | quaternion_channel: Receiver<'static, ThreadModeRawMutex, Quaternion, BUFFERED_QUATERNIONS>, |
| 71 | target_frame: Option<Frame>, |
| 72 | ) { |
| 73 | let mut last_yaw: f32 = 0.0; |
| 74 | let mut last_pitch: f32 = 0.0; |
| 75 | let mut last_roll: f32 = 0.0; |
| 76 | |
| 77 | const THRESHOLD: f32 = 3.0; |
| 78 | |
| 79 | loop { |
| 80 | let next_quaternion_value = quaternion_channel.receive().await; |
| 81 | let ypr_format = YawPitchRoll::from(next_quaternion_value); |
| 82 | |
| 83 | let yaw_deg = ypr_format.yaw * 180.0 / core::f32::consts::PI; |
| 84 | let pitch_deg = ypr_format.pitch * 180.0 / core::f32::consts::PI; |
| 85 | let roll_deg = ypr_format.roll * 180.0 / core::f32::consts::PI; |
| 86 | |
| 87 | let y_diff = yaw_deg - last_yaw; |
| 88 | let p_diff = pitch_deg - last_pitch; |
| 89 | let r_diff = roll_deg - last_roll; |
| 90 | |
| 91 | // filter the new value and see if it has differed in more than THRESHOLD than the |
| 92 | // previous. If not, ignore it. |
| 93 | if f32::abs(y_diff) > THRESHOLD |
| 94 | || f32::abs(p_diff) > THRESHOLD |
| 95 | || f32::abs(r_diff) > THRESHOLD |
| 96 | { |
| 97 | info!("YPR differential: y:{}, p:{}, r:{}", y_diff, p_diff, r_diff); |
| 98 | last_yaw = yaw_deg; |
| 99 | last_pitch = pitch_deg; |
| 100 | last_roll = roll_deg; |
| 101 | } |
| 102 | } |
| 103 | } |
| 104 | |
