change around axis to align with engine

src/drone.rs

@@ -1,4 +1,4 @@
-use nalgebra as na;
+use nalgebra::{self as na, Vector3};
 use rapier3d::prelude as rp;
 
 use crate::engine::{ColliderExtraData, World};
@@ -8,6 +8,10 @@ pub mod fpvcontroller;
 pub mod pidcontroller;
 use controller::*;
 
+const AREA_DENSITY: f32 = 1.23;
+const DRAG_CONSTANT: f32 = 1.3;
+const DRAG_MAGIC_NUM: f32 = 0.5;
+
 pub struct MotorCharacteristics {
     pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
     pub max_thrust: f32,
@@ -47,6 +51,10 @@ pub struct Drone {
     height: f32,
 }
 
+fn calculate_drag(velocity: f32, area: f32, constant: f32) -> f32 {
+    return 0.5 * AREA_DENSITY * (velocity * velocity) * area * constant * DRAG_MAGIC_NUM;
+}
+
 impl Drone {
     pub fn new(
         world: &mut World,
@@ -95,6 +103,21 @@ impl Drone {
         };
     }
 
+    fn apply_drag(&mut self, world: &mut World) {
+        let body = world.bodies.get_mut(self.rb_handle).unwrap();
+        let velocity = body.linvel();
+        let mut drag = Vector3::<f32>::zeros();
+
+        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.y = calculate_drag(velocity.y, up_area, DRAG_CONSTANT);
+        drag.z = calculate_drag(velocity.z, side_area, DRAG_CONSTANT);
+
+        body.add_force(-drag, true);
+    }
+
     fn apply_throttles(&mut self, world: &mut World) {
         let throttles = self.controller.get_motor_throttles();
 
@@ -150,5 +173,6 @@ impl Drone {
     pub fn process_tick(&mut self, world: &mut World) {
         self.update_controller(world);
         self.apply_throttles(world);
+        // self.apply_drag(world);
     }
 }

src/drone/controller.rs

@@ -46,7 +46,7 @@ pub trait DroneController {
     /*
      * Throttle should be between 0 and 1. Values will be by the Drone class.
      */
-    fn get_motor_throttles(&self) -> [f32; 4];
+    fn get_motor_throttles(&mut self) -> [f32; 4];
 }
 
 /*
@@ -68,7 +68,7 @@ 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(&self) -> [f32; 4] {
+    fn get_motor_throttles(&mut self) -> [f32; 4] {
         return [self.motor_throttle; 4];
     }
 

src/drone/fpvcontroller.rs

@@ -33,7 +33,7 @@ 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(&self) -> [f32; 4] {
+    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;
         let pitch = self.rate_pitch * self.input.pitch_input;

src/drone/pidcontroller.rs

@@ -1,7 +1,7 @@
 use glam::{vec3, EulerRot};
 use nalgebra::{self as na, Vector3};
 use rapier3d::prelude as rp;
-use std::{any::Any, f32};
+use std::{any::Any, f32, io::PipeReader};
 
 use crate::drone::controller::DroneController;
 pub use crate::drone::controller::JoystickInput;
@@ -17,7 +17,7 @@ pub struct PIDControllerState {
 
 impl PIDControllerState {
     pub fn dt(&self) -> f32 {
-        return self.last_time - self.current_time;
+        return self.current_time - self.last_time;
     }
 }
 
@@ -41,15 +41,21 @@ impl PIDController {
     pub fn set_input(&mut self, input: JoystickInput) {
         self.input = input;
     }
-    pub fn get_angular_velocity(&self) {
+    pub fn get_angular_velocity(&self) -> na::Vector3<f32> {
         let dt: f32 = self.state.dt();
+
         let angles =
-            (self.state.current_rotation.inverse() * self.state.last_rotation).euler_angles();
-        // roll,pitch.yaw
-        let vel_roll = angles.0 / dt;
-        let vel_pitch = angles.1 / dt;
-        let vel_yaw = angles.2 / dt;
-        println!("{},{},{}", vel_roll, vel_pitch, vel_yaw);
+            (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(
+            self.input.pitch_input,
+            self.input.yaw_input,
+            self.input.roll_input,
+        ) * self.target_rate;
     }
 }
 
@@ -63,14 +69,64 @@ impl DroneController for PIDController {
         self.state.current_time = time;
     }
     fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
-    fn get_motor_throttles(&self) -> [f32; 4] {
-        self.get_angular_velocity();
-        return [
-            self.input.throttle_input,
-            self.input.throttle_input,
-            self.input.throttle_input,
-            self.input.throttle_input,
+    fn get_motor_throttles(&mut self) -> [f32; 4] {
+        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;
+
+        let pitch = diff.x * 0.01;
+
+        let yaw = diff.y;
+
+        /*
+         * Motor position indices
+         *    ^ - Front
+         *    |
+         *    |
+         * 1 --- 0
+         * |     |
+         * |     |
+         * 2 --- 3
+         */
+        let mut motors = [
+            throttle - pitch + roll + yaw,
+            throttle - pitch - roll - yaw,
+            throttle + pitch - roll + yaw,
+            throttle + pitch + roll - yaw,
         ];
+
+        // 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()
+            .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;
     }
 
     /*

src/engine.rs

@@ -122,7 +122,7 @@ impl Default for World {
     fn default() -> Self {
         let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
         let mut integration_parameters = rp::IntegrationParameters::default();
-        integration_parameters.set_inv_dt(800.0);
+        integration_parameters.set_inv_dt(60.0);
         let physics_pipeline = rp::PhysicsPipeline::new();
         let island_manager = rp::IslandManager::new();
         let broad_phase = rp::DefaultBroadPhase::new();

src/main.rs

@@ -18,7 +18,7 @@ fn window_conf() -> mq::Conf {
         window_resizable: true,
         // fullscreen: true,
         platform: mq::miniquad::conf::Platform {
-            linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
+            // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
             ..Default::default()
         },
         ..Default::default()
@@ -39,6 +39,7 @@ async fn main() {
     // Ground collider
     world.register_free_collider(
         ColliderBuilder::cuboid(20.0, 1.0, 20.0)
+            .translation(vector![0.0, -2.0, 0.0])
             .restitution(0.5)
             .build(),
         None,
@@ -46,17 +47,17 @@ async fn main() {
 
     let mut controller = drone::pidcontroller::PIDController::default();
     controller.set_input(JoystickInput {
-        throttle_input: 0.4,
-        yaw_input: 0.05,
-        roll_input: 0.0,
+        throttle_input: 0.35,
+        yaw_input: 0.0,
+        roll_input: 0.1,
         pitch_input: 0.0,
     });
     let mut drone = drone::Drone::new(
         &mut world,
         Box::new(controller),
         drone::MotorCharacteristics {
-            max_thrust: 2.3,
-            max_torque: 0.01,
+            max_thrust: 2.6,
+            max_torque: 0.1,
             ..Default::default()
         },
         0.350,
@@ -107,11 +108,11 @@ async fn main() {
         // Rendering
         renderer.draw(&mut world);
 
-        if world.tick % (800 / 30) == 0 {
+        if world.tick % (60 / 30) == 0 {
             world.clear_ofb();
             renderer.update_light(&world);
-            mq::next_frame().await;
         }
+        mq::next_frame().await;
     }
 }
 

src/rendering.rs

@@ -269,7 +269,7 @@ impl Renderer {
 
         // mq::draw_sphere(self.light.position, 0.25, None, mq::WHITE);
 
-        // mq::draw_grid(20, 1., mq::BLACK, mq::GRAY);
+        mq::draw_grid(20, 1.5, mq::BLACK, mq::GRAY);
         mq::gl_use_default_material();
         mq::set_default_camera();
     }