84 lines
2.1 KiB
Rust
84 lines
2.1 KiB
Rust
use rapier3d::prelude as rp;
|
|
use std::any::Any;
|
|
|
|
use crate::drone::controller::DroneController;
|
|
pub use crate::drone::controller::JoystickInput;
|
|
use crate::drone::MotorCharacteristics;
|
|
|
|
pub struct FPVController {
|
|
input: JoystickInput,
|
|
rate_yaw: f32,
|
|
rate_pitch: f32,
|
|
rate_roll: f32,
|
|
}
|
|
|
|
impl FPVController {
|
|
pub fn set_input(&mut self, input: JoystickInput) {
|
|
self.input = input;
|
|
}
|
|
}
|
|
|
|
impl Default for FPVController {
|
|
fn default() -> Self {
|
|
Self {
|
|
input: JoystickInput::default(),
|
|
rate_yaw: 1.0,
|
|
rate_pitch: 1.0,
|
|
rate_roll: 1.0,
|
|
}
|
|
}
|
|
}
|
|
|
|
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] {
|
|
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;
|
|
let throttle = self.input.throttle_input;
|
|
|
|
/*
|
|
* 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,
|
|
];
|
|
// 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;
|
|
}
|
|
|
|
/*
|
|
* These should be implemented like this for all the classes that implement DroneController
|
|
* Works as an example implementation
|
|
*/
|
|
fn as_any(&self) -> &dyn Any {
|
|
self
|
|
}
|
|
fn as_mut_any(&mut self) -> &mut dyn Any {
|
|
self
|
|
}
|
|
}
|