flight controller fixed, fixed torque being applied according to world rotation and not body rotation

src/drone.rs

@@ -8,7 +8,7 @@ pub mod fpvcontroller;
 pub mod pidcontroller;
 use controller::*;
 
-const AREA_DENSITY: f32 = 1.23;
+const AIR_DENSITY: f32 = 1.23;
 const DRAG_CONSTANT: f32 = 1.3;
 const DRAG_MAGIC_NUM: f32 = 0.5;
 
@@ -32,10 +32,10 @@ impl Default for MotorCharacteristics {
              * 2 --- 3
              */
             relative_motor_positions: [
-                rp::point![5.0, 3.0, 5.0],
+                rp::point![5.0, 0.0, 5.0],
-                rp::point![-5.0, 3.0, 5.0],
+                rp::point![-5.0, 0.0, 5.0],
-                rp::point![-5.0, 3.0, -5.0],
+                rp::point![-5.0, 0.0, -5.0],
-                rp::point![5.0, 3.0, -5.0],
+                rp::point![5.0, 0.0, -5.0],
             ],
             max_thrust: 10.0,
             max_torque: 1.0,
@@ -52,7 +52,7 @@ pub struct Drone {
 }
 
 fn calculate_drag(velocity: f32, area: f32, constant: f32) -> f32 {
-    return 0.5 * AREA_DENSITY * (velocity * velocity) * area * constant * DRAG_MAGIC_NUM;
+    return AIR_DENSITY * (velocity.abs() * velocity) * area * constant * DRAG_MAGIC_NUM;
 }
 
 impl Drone {
@@ -111,11 +111,12 @@ impl Drone {
         let side_area = self.width * self.height;
         let up_area = self.width * self.width;
 
-        drag.x = calculate_drag(velocity.x, side_area, DRAG_CONSTANT);
+        drag.x = -calculate_drag(velocity.x, side_area, DRAG_CONSTANT);
-        drag.y = calculate_drag(velocity.y, up_area, DRAG_CONSTANT);
+        drag.y = -calculate_drag(velocity.y, up_area, DRAG_CONSTANT);
-        drag.z = calculate_drag(velocity.z, side_area, DRAG_CONSTANT);
+        drag.z = -calculate_drag(velocity.z, side_area, DRAG_CONSTANT);
+        // println!("Drag: {:}", drag);
 
-        body.add_force(-drag, true);
+        body.add_force(drag, true);
     }
 
     fn apply_throttles(&mut self, world: &mut World) {
@@ -135,7 +136,7 @@ impl Drone {
             .iter()
             .enumerate()
         {
-            let throttle = throttles[i].clamp(0.0, 1.0);
+            let throttle = throttles[i].clamp(-1.0, 1.0);
             let thrust = self.motor_characteristics.max_thrust * throttle;
             let torque = self.motor_characteristics.max_torque * throttle;
 
@@ -158,7 +159,7 @@ impl Drone {
             } else {
                 rp::vector![0.0, -torque, 0.0]
             };
-            drone_rb.add_torque(torque, true);
+            drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true);
             println!("Torque: \n{:?}", torque);
             println!("Thrust: \n{:?}", thrust);
         }
@@ -167,12 +168,14 @@ impl Drone {
     fn update_controller(&mut self, world: &World) {
         let rb = world.bodies.get(self.rb_handle).unwrap();
         self.controller.set_time(world.get_time());
-        self.controller.set_position(*rb.position());
+        // Pitch, Yaw, Roll
+        self.controller.set_angular_velocity(*rb.angvel());
+        self.controller.set_rotation(*rb.rotation());
     }
 
     pub fn process_tick(&mut self, world: &mut World) {
         self.update_controller(world);
         self.apply_throttles(world);
-        // self.apply_drag(world);
+        self.apply_drag(world);
     }
 }

src/drone/controller.rs

@@ -39,9 +39,10 @@ pub trait DroneController {
     /*
      * Methods called by Drone, to transmit information to the controller.
      */
-    fn set_position(&mut self, position: rp::Isometry<f32>);
+    fn set_rotation(&mut self, _rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {}
-    fn set_time(&mut self, time: f32);
+    fn set_angular_velocity(&mut self, _angvel: nalgebra::Vector3<f32>) {}
-    fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics);
+    fn set_time(&mut self, _time: f32) {}
+    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
 
     /*
      * Throttle should be between 0 and 1. Values will be by the Drone class.
@@ -65,7 +66,6 @@ impl DefaultController {
 }
 
 impl DroneController for DefaultController {
-    fn set_position(&mut self, _position: rp::Isometry<f32>) {}
     fn set_time(&mut self, _time: f32) {}
     fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
     fn get_motor_throttles(&mut self) -> [f32; 4] {

src/drone/fpvcontroller.rs

@@ -30,9 +30,6 @@ impl Default for FPVController {
 }
 
 impl DroneController for FPVController {
-    fn set_position(&mut self, _position: rp::Isometry<f32>) {}
-    fn set_time(&mut self, _time: f32) {}
-    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
     fn get_motor_throttles(&mut self) -> [f32; 4] {
         let yaw = self.rate_yaw * self.input.yaw_input;
         let roll = self.rate_roll * self.input.roll_input;

src/drone/pidcontroller.rs

@@ -1,4 +1,3 @@
-use glam::{vec3, EulerRot};
 use nalgebra::{self as na, Vector3};
 use rapier3d::prelude as rp;
 use std::{any::Any, f32, io::PipeReader};
@@ -8,26 +7,27 @@ pub use crate::drone::controller::JoystickInput;
 use crate::drone::MotorCharacteristics;
 
 #[derive(Default)]
-pub struct PIDControllerState {
+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 PIDControllerState {
+impl PControllerState {
     pub fn dt(&self) -> f32 {
         return self.current_time - self.last_time;
     }
 }
 
-pub struct PIDController {
+pub struct PController {
     input: JoystickInput,
     pub target_rate: f32,
-    state: PIDControllerState,
+    state: PControllerState,
 }
 
-impl Default for PIDController {
+impl Default for PController {
     fn default() -> Self {
         Self {
             input: JoystickInput::default(),
@@ -37,32 +37,31 @@ impl Default for PIDController {
     }
 }
 
-impl PIDController {
+impl PController {
     pub fn set_input(&mut self, input: JoystickInput) {
         self.input = input;
     }
     pub fn get_angular_velocity(&self) -> na::Vector3<f32> {
-        let dt: f32 = self.state.dt();
+        return self.state.angular_velocity;
-
-        let angles =
-            (self.state.last_rotation.inverse() * self.state.current_rotation).euler_angles();
-        println!("dt: {}", dt);
-        return na::Vector3::new(angles.0, angles.1, angles.2) / dt;
     }
 
     pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
-        return na::Vector3::new(
+        let coords = na::Vector3::new(
             self.input.pitch_input,
             self.input.yaw_input,
             self.input.roll_input,
         ) * self.target_rate;
+        return coords;
     }
 }
 
-impl DroneController for PIDController {
+impl DroneController for PController {
-    fn set_position(&mut self, position: rp::Isometry<f32>) {
+    fn set_rotation(&mut self, rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {
         self.state.last_rotation = self.state.current_rotation;
-        self.state.current_rotation = position.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;
@@ -70,13 +69,14 @@ impl DroneController for PIDController {
     }
     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 current = self.get_angular_velocity();
+
         let diff = target - current;
         println!("Target: {:}", target);
         println!("Current: {:}", current);
         println!("Diff: {:}", diff);
-
         let throttle = self.input.throttle_input;
 
         let roll = diff.z * 0.01;
@@ -96,25 +96,12 @@ impl DroneController for PIDController {
          * 2 --- 3
          */
         let mut motors = [
-            throttle - pitch + roll + yaw,
+            throttle + yaw - pitch + roll,
-            throttle - pitch - roll - yaw,
+            throttle - yaw - pitch - roll,
-            throttle + pitch - roll + yaw,
+            throttle + yaw + pitch - roll,
-            throttle + pitch + roll - yaw,
+            throttle - yaw + pitch + roll,
         ];
 
-        // let mut motors = [
-        //     throttle + yaw + pitch,
-        //     throttle - yaw + pitch,
-        //     throttle + yaw - pitch,
-        //     throttle - yaw - pitch,
-        // ];
-        // let mut motors = [
-        //     throttle - pitch + roll + yaw,
-        //     throttle - pitch - roll - yaw,
-        //     throttle + pitch - roll + yaw,
-        //     throttle + pitch + roll - yaw,
-        // ];
-
         // motors.max
         let max = motors
             .iter()

src/main.rs

@@ -45,11 +45,11 @@ async fn main() {
         None,
     );
 
-    let mut controller = drone::pidcontroller::PIDController::default();
+    let mut controller = drone::pidcontroller::PController::default();
     controller.set_input(JoystickInput {
-        throttle_input: 0.35,
+        throttle_input: 0.75,
-        yaw_input: 0.0,
+        yaw_input: 0.4,
-        roll_input: 0.1,
+        roll_input: 0.7,
         pitch_input: 0.0,
     });
     let mut drone = drone::Drone::new(
@@ -87,18 +87,27 @@ async fn main() {
         world.step();
         let _ = clearscreen::clear();
         drone.process_tick(&mut world);
-        println!(
+        // println!(
-            "Translation: {:}",
+        //     "Translation: {:}",
-            world.bodies.get(drone.rb_handle).unwrap().translation()
+        //     world.bodies.get(drone.rb_handle).unwrap().translation()
-        );
+        // );
         println!(
             "Angular Velocity: {:}",
             world.bodies.get(drone.rb_handle).unwrap().angvel()
         );
         println!(
-            "Velocity: {:}",
+            "Rotation: {:?}",
-            world.bodies.get(drone.rb_handle).unwrap().linvel()
+            world
+                .bodies
+                .get(drone.rb_handle)
+                .unwrap()
+                .rotation()
+                .euler_angles()
         );
+        // println!(
+        //     "Velocity: {:}",
+        //     world.bodies.get(drone.rb_handle).unwrap().linvel()
+        // );
 
         println!(
             "Motor Throttles: {:?}",
@@ -109,7 +118,7 @@ async fn main() {
         renderer.draw(&mut world);
 
         if world.tick % (60 / 30) == 0 {
-            world.clear_ofb();
+            // world.clear_ofb();
             renderer.update_light(&world);
         }
         mq::next_frame().await;