RustPhysicsMQ/src/drone/pidcontroller.rs

use nalgebra::{self as na, Vector3};
use rapier3d::prelude as rp;
use std::{any::Any, f32, io::PipeReader};

use crate::drone::controller::DroneController;
pub use crate::drone::controller::JoystickInput;
use crate::drone::MotorCharacteristics;

#[derive(Default)]
pub struct PControllerState {
    last_time: f32,
    current_time: f32,
    last_rotation: na::Unit<na::Quaternion<f32>>,
    current_rotation: na::Unit<na::Quaternion<f32>>,
    angular_velocity: na::Vector3<f32>,
}

impl PControllerState {
    pub fn dt(&self) -> f32 {
        return self.current_time - self.last_time;
    }
}

pub struct PController {
    input: JoystickInput,
    pub target_rate: f32,
    pub multiply_mode: bool,
    state: PControllerState,
}

impl Default for PController {
    fn default() -> Self {
        Self {
            input: JoystickInput::default(),
            target_rate: f32::consts::PI,
            state: Default::default(),
            multiply_mode: false,
        }
    }
}

impl PController {
    pub fn set_input(&mut self, input: JoystickInput) {
        self.input = input;
    }
    pub fn get_angular_velocity(&self) -> na::Vector3<f32> {
        return self.state.angular_velocity;
    }

    pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
        let coords = na::Vector3::new(
            self.input.roll_input,
            self.input.yaw_input,
            self.input.pitch_input,
        ) * self.target_rate;
        return coords;
    }
}

impl DroneController for PController {
    fn set_rotation(&mut self, rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {
        self.state.last_rotation = self.state.current_rotation;
        self.state.current_rotation = rotation;
    }
    fn set_angular_velocity(&mut self, angvel: nalgebra::Vector3<f32>) {
        self.state.angular_velocity = angvel;
    }
    fn set_time(&mut self, time: f32) {
        self.state.last_time = self.state.current_time;
        self.state.current_time = time;
    }
    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
    fn get_motor_throttles(&mut self) -> [f32; 4] {
        let rot = self.state.current_rotation;
        let current = rot.inverse().transform_vector(&self.get_angular_velocity());
        let target = self.get_desired_angular_velocity();

        let diff = target - current;
        let throttle = self.input.throttle_input;

        let roll = diff.z * 0.01;
        let pitch = diff.x * 0.01;
        let yaw = diff.y;

        let mut motors: [f32; 4] = match self.multiply_mode {
            true => {
                let mut t = [
                    1.0 + yaw - pitch + roll,
                    1.0 - yaw - pitch - roll,
                    1.0 + yaw + pitch - roll,
                    1.0 - yaw + pitch + roll,
                ];
                for m in t.iter_mut() {
                    *m *= self.input.throttle_input;
                }
                t
            }
            false => {
                let mut t = [
                    throttle + yaw - pitch + roll,
                    throttle - yaw - pitch - roll,
                    throttle + yaw + pitch - roll,
                    throttle - yaw + pitch + roll,
                ];
                t
            }
        };

        let max = motors
            .iter()
            .copied()
            .max_by(|a, b| a.total_cmp(b))
            .unwrap_or(0.0);
        if max > 1.0 {
            for v in motors.iter_mut() {
                *v /= max;
            }
        }

        return motors;
    }

    fn as_any(&self) -> &dyn Any {
        self
    }
    fn as_mut_any(&mut self) -> &mut dyn Any {
        self
    }
}