Cansat
/
RustPhysicsMQ
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

This commit is contained in:
Franchioping 2025-12-11 18:08:07 +00:00
parent 98050d51bf
commit b36cbe9aed
5 changed files with 64 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
src/drone.rs
View File

@ -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);
}
}

8
src/drone/controller.rs
View File

@ -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_time(&mut self, time: f32);
fn set_motor_characteristics(&self, motor_characteristics: &MotorCharacteristics);
fn set_rotation(&mut self, _rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {}
fn set_angular_velocity(&mut self, _angvel: nalgebra::Vector3<f32>) {}
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] {

3
src/drone/fpvcontroller.rs
View File

@ -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;

59
src/drone/pidcontroller.rs
View File

@ -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();
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<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 {
fn set_position(&mut self, position: rp::Isometry<f32>) {
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 = 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 - 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()

31
src/main.rs
View File

@ -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;