From 98050d51bff833a0f5a21ad11e52a8a1790fcd61 Mon Sep 17 00:00:00 2001 From: Franchioping Date: Wed, 10 Dec 2025 13:11:51 +0000 Subject: [PATCH] change around axis to align with engine --- src/drone.rs | 26 ++++++++++- src/drone/controller.rs | 4 +- src/drone/fpvcontroller.rs | 2 +- src/drone/pidcontroller.rs | 88 +++++++++++++++++++++++++++++++------- src/engine.rs | 2 +- src/main.rs | 17 ++++---- src/rendering.rs | 2 +- 7 files changed, 111 insertions(+), 30 deletions(-) diff --git a/src/drone.rs b/src/drone.rs index e40f022..c40b513 100644 --- a/src/drone.rs +++ b/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>; 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::::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); } } diff --git a/src/drone/controller.rs b/src/drone/controller.rs index 7a7bfda..0fd3405 100644 --- a/src/drone/controller.rs +++ b/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) {} 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]; } diff --git a/src/drone/fpvcontroller.rs b/src/drone/fpvcontroller.rs index 0f93cb8..a993350 100644 --- a/src/drone/fpvcontroller.rs +++ b/src/drone/fpvcontroller.rs @@ -33,7 +33,7 @@ impl DroneController for FPVController { fn set_position(&mut self, _position: rp::Isometry) {} 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; diff --git a/src/drone/pidcontroller.rs b/src/drone/pidcontroller.rs index 559517d..3a6f18c 100644 --- a/src/drone/pidcontroller.rs +++ b/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 { 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 { + 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; } /* diff --git a/src/engine.rs b/src/engine.rs index 744d73a..d20cfb0 100644 --- a/src/engine.rs +++ b/src/engine.rs @@ -122,7 +122,7 @@ impl Default for World { fn default() -> Self { let gravity: na::Vector3 = 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(); diff --git a/src/main.rs b/src/main.rs index 2606db5..ecb0d29 100644 --- a/src/main.rs +++ b/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; } } diff --git a/src/rendering.rs b/src/rendering.rs index 66ddb60..2e4d6ae 100644 --- a/src/rendering.rs +++ b/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(); }