From b36cbe9aedb0f80ce59cd2e570b3a7edb1f8e904 Mon Sep 17 00:00:00 2001 From: Franchioping Date: Thu, 11 Dec 2025 18:08:07 +0000 Subject: [PATCH] flight controller fixed, fixed torque being applied according to world rotation and not body rotation --- src/drone.rs | 31 +++++++++++--------- src/drone/controller.rs | 8 +++--- src/drone/fpvcontroller.rs | 3 -- src/drone/pidcontroller.rs | 59 +++++++++++++++----------------------- src/main.rs | 31 +++++++++++++------- 5 files changed, 64 insertions(+), 68 deletions(-) diff --git a/src/drone.rs b/src/drone.rs index c40b513..35b155f 100644 --- a/src/drone.rs +++ b/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, 3.0, 5.0], - rp::point![-5.0, 3.0, -5.0], - rp::point![5.0, 3.0, -5.0], + rp::point![5.0, 0.0, 5.0], + rp::point![-5.0, 0.0, 5.0], + rp::point![-5.0, 0.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.y = calculate_drag(velocity.y, up_area, DRAG_CONSTANT); - drag.z = calculate_drag(velocity.z, 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.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); } } diff --git a/src/drone/controller.rs b/src/drone/controller.rs index 0fd3405..d73ae30 100644 --- a/src/drone/controller.rs +++ b/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); - fn set_time(&mut self, time: f32); - fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics); + fn set_rotation(&mut self, _rotation: nalgebra::Unit>) {} + fn set_angular_velocity(&mut self, _angvel: nalgebra::Vector3) {} + 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) {} fn set_time(&mut self, _time: f32) {} fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {} fn get_motor_throttles(&mut self) -> [f32; 4] { diff --git a/src/drone/fpvcontroller.rs b/src/drone/fpvcontroller.rs index a993350..eb22294 100644 --- a/src/drone/fpvcontroller.rs +++ b/src/drone/fpvcontroller.rs @@ -30,9 +30,6 @@ impl Default for FPVController { } 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(&mut self) -> [f32; 4] { let yaw = self.rate_yaw * self.input.yaw_input; let roll = self.rate_roll * self.input.roll_input; diff --git a/src/drone/pidcontroller.rs b/src/drone/pidcontroller.rs index 3a6f18c..52e7570 100644 --- a/src/drone/pidcontroller.rs +++ b/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>, current_rotation: na::Unit>, + angular_velocity: na::Vector3, } -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 { - let dt: f32 = self.state.dt(); - - 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; + return self.state.angular_velocity; } pub fn get_desired_angular_velocity(&self) -> na::Vector3 { - 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 { - fn set_position(&mut self, position: rp::Isometry) { +impl DroneController for PController { + fn set_rotation(&mut self, rotation: nalgebra::Unit>) { 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) { + 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 - pitch - roll - yaw, - throttle + pitch - roll + yaw, - throttle + pitch + roll - yaw, + throttle + yaw - pitch + roll, + throttle - yaw - pitch - roll, + throttle + yaw + pitch - roll, + 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() diff --git a/src/main.rs b/src/main.rs index ecb0d29..a2d182b 100644 --- a/src/main.rs +++ b/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, - yaw_input: 0.0, - roll_input: 0.1, + throttle_input: 0.75, + yaw_input: 0.4, + 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!( - "Translation: {:}", - world.bodies.get(drone.rb_handle).unwrap().translation() - ); + // println!( + // "Translation: {:}", + // world.bodies.get(drone.rb_handle).unwrap().translation() + // ); println!( "Angular Velocity: {:}", world.bodies.get(drone.rb_handle).unwrap().angvel() ); println!( - "Velocity: {:}", - world.bodies.get(drone.rb_handle).unwrap().linvel() + "Rotation: {:?}", + 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;