fix mixer? setpoint values that are too high no longer cause crashes

Cargo.toml

@@ -20,11 +20,6 @@ csv = "1.4.0"
 serde_with = "3"
 
 
-[[bin]]
-name = "test"
-path = "src/main_test.rs"
-
-
 # [profile.dev.package.rapier3d]
 # opt-level = 3
 

src/drone.rs

@@ -4,9 +4,7 @@ use rapier3d::prelude as rp;
 use crate::engine::{ColliderExtraData, World};
 
 pub mod controller;
-pub mod fpvcontroller;
 pub mod input;
-pub mod pidcontroller;
 pub mod stacked;
 use controller::*;
 pub use input::JoystickInput;

src/drone/fpvcontroller.rs

@@ -1,80 +0,0 @@
-#![allow(dead_code)]
-use std::any::Any;
-
-use crate::drone::controller::DroneController;
-
-pub use crate::drone::JoystickInput;
-
-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 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;
-        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
-    }
-}

src/drone/pidcontroller.rs

@@ -1,146 +0,0 @@
-#![allow(dead_code)]
-use nalgebra::{self as na, vector};
-use std::{any::Any, f32};
-
-use crate::drone::JoystickInput;
-use crate::drone::MotorCharacteristics;
-use crate::drone::controller::DroneController;
-
-#[derive(Default)]
-pub struct PIDControllerState {
-    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>,
-    last_error: na::Vector3<f32>,
-    error_sum: na::Vector3<f32>,
-}
-
-impl PIDControllerState {
-    pub fn dt(&self) -> f32 {
-        return self.current_time - self.last_time;
-    }
-}
-
-pub struct PIDController {
-    pub input: JoystickInput,
-    pub target_rate: f32,
-    pub proportional_multiplier: na::Vector3<f32>,
-    pub integral_multiplier: na::Vector3<f32>,
-    pub diferential_multiplier: na::Vector3<f32>,
-    pub multiply_mode: bool,
-    pub state: PIDControllerState,
-}
-
-impl Default for PIDController {
-    fn default() -> Self {
-        Self {
-            input: JoystickInput::default(),
-            target_rate: f32::consts::PI,
-            state: Default::default(),
-            multiply_mode: false,
-            proportional_multiplier: vector![0.13, 3.0, 0.13],
-            integral_multiplier: vector![0.0, 0.0, 0.0],
-            diferential_multiplier: vector![0.001, 0.01, 0.001],
-        }
-    }
-}
-
-impl PIDController {
-    pub fn set_input(&mut self, input: JoystickInput) {
-        self.input = input;
-    }
-    pub fn get_angular_velocity(&self) -> na::Vector3<f32> {
-        return self.state.angular_velocity;
-    }
-
-    pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
-        let coords = na::Vector3::new(
-            self.input.roll_input,
-            self.input.yaw_input,
-            self.input.pitch_input,
-        ) * self.target_rate;
-        return coords;
-    }
-
-    pub fn get_desired_and_error(&mut self) -> [na::Vector3<f32>; 2] {
-        let rot = self.state.current_rotation;
-        let current = rot.inverse().transform_vector(&self.get_angular_velocity());
-        let target = self.get_desired_angular_velocity();
-
-        let error: na::Vector3<f32> = target - current;
-        let error_dif: na::Vector3<f32> = error - self.state.last_error;
-
-        let forces_to_apply: na::Vector3<f32> = vector![
-            error.x * self.proportional_multiplier.x
-                + self.state.error_sum.x * self.integral_multiplier.x
-                + error_dif.x * self.diferential_multiplier.x,
-            error.y * self.proportional_multiplier.y
-                + self.state.error_sum.y * self.integral_multiplier.y
-                + error_dif.y * self.diferential_multiplier.y,
-            error.z * self.proportional_multiplier.z
-                + self.state.error_sum.z * self.integral_multiplier.z
-                + error_dif.z * self.diferential_multiplier.z,
-        ];
-
-        return [forces_to_apply, error];
-    }
-
-    pub fn get_desired_motor_diffs(&mut self) -> na::Vector3<f32> {
-        return self.get_desired_and_error()[0];
-    }
-}
-
-impl DroneController for PIDController {
-    fn set_rotation(&mut self, rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {
-        self.state.last_rotation = self.state.current_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;
-        self.state.current_time = time;
-    }
-    fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
-    fn get_motor_throttles(&mut self) -> [f32; 4] {
-        let [forces_to_apply, error] = self.get_desired_and_error();
-        self.state.error_sum += error * self.state.dt();
-        self.state.last_error = error;
-
-        let throttle = self.input.throttle_input;
-
-        let pitch = forces_to_apply.x;
-        let yaw = forces_to_apply.y;
-        let roll = forces_to_apply.z;
-
-        let mut motors: [f32; 4] = [
-            throttle - pitch + yaw + roll,
-            throttle - pitch - yaw - roll,
-            throttle + pitch + yaw - roll,
-            throttle + pitch - yaw + roll,
-        ];
-
-        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;
-    }
-
-    fn as_any(&self) -> &dyn Any {
-        self
-    }
-    fn as_mut_any(&mut self) -> &mut dyn Any {
-        self
-    }
-}

src/drone/stacked/mixer.rs

@@ -1,14 +1,14 @@
 use nalgebra as na;
 
-#[derive(Default)]
 pub enum MotorMixingMode {
     ThrottleAuthority,
     NoTorqueScalling,
-    ThrottleAuthorityReasonable {
+    ThrottleAuthorityReasonable { min_scale: f32 },
-        min_scale: f32,
+}
-    },
+impl Default for MotorMixingMode {
-    #[default]
+    fn default() -> Self {
-    AttitudeAuthority,
+        Self::ThrottleAuthorityReasonable { min_scale: 0.1 }
+    }
 }
 
 pub struct MotorMixer {
@@ -33,9 +33,6 @@ impl MotorMixer {
                     .mix_throttle_authority_reasonable(throttle, torque, min_scale)
                     .0;
             }
-            AttitudeAuthority => {
-                return self.mix_attitude_authority(throttle, torque);
-            }
         }
     }
 
@@ -57,8 +54,6 @@ impl MotorMixer {
     /// Throttle still has authority, this is, torque is limited by the throttle, but within reason, which makes it "reasonable"
     ///
     /// Avoids the downside of the previous implementation: At 100% Throttle, Torque would always be 0. This lowers the actual throttle.
-    ///
-    /// If min_scale is 1, acts as attitude authority
     pub fn mix_throttle_authority_reasonable(
         &self,
         throttle: f32,
@@ -66,6 +61,9 @@ impl MotorMixer {
         min_scale: f32,
     ) -> ([f32; 4], bool) {
         let mut delta = self.compute_delta(torque);
+        let throttle_range_len = self.max_throttle - self.min_throttle;
+
+        delta.iter_mut().for_each(|x| *x *= 0.25);
 
         let mut scale = 1.0f32;
 
@@ -79,10 +77,14 @@ impl MotorMixer {
 
         scale = scale.clamp(min_scale, 1.0);
 
-        delta.iter_mut().for_each(|x| *x *= scale);
+        let max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0);
+        let min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0);
+        let delta_dif = max_delta - min_delta;
+
+        if delta_dif > throttle_range_len {
+            scale *= throttle_range_len / delta_dif;
+        }
 
-        let max_delta = delta.into_iter().reduce(f32::max).unwrap_or(0.0);
-        let min_delta = delta.into_iter().reduce(f32::min).unwrap_or(0.0);
         let lim_throttle = throttle.clamp(
             self.min_throttle + min_delta.abs(),
             self.max_throttle - max_delta.abs(),
@@ -90,7 +92,7 @@ impl MotorMixer {
 
         let mut motors = [0.0f32; 4];
         for i in 0..4 {
-            motors[i] = lim_throttle + delta[i];
+            motors[i] = lim_throttle + delta[i] * scale;
         }
 
         let saturated = scale < 1.0;

src/main.rs

@@ -18,7 +18,7 @@ use helpers::list_files;
 use std::fs;
 
 const INPUTS_DIR: &str = "inputs/";
-const CONFIGS_DIR: &str = "configurations/";
+const CONFIGS_DIR: &str = "configurations/final/";
 const RESULTS_DIR: &str = "results/";
 
 use std::path::PathBuf;
@@ -95,23 +95,28 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
     sim.run().unwrap();
 }
 
-async fn run_record(output: String) {
+async fn run_record(output: String, config_path: &PathBuf) {
-    println!("Recording inputs to {}", output);
+    println!(
+        "Recording inputs to {}, using config {}",
+        output,
+        config_path.to_str().unwrap_or("<INVALID CONFIG PATH>")
+    );
 
-    let tickrate = 60000.0;
+    let config: SimulationConfig =
-    let mut world = World::new(tickrate);
+        toml::from_str(&fs::read_to_string(config_path).unwrap()).expect("Invalid config file");
+
+    let mut world = World::new(config.tickrate);
 
     let drone = drone::Drone::new(
         &mut world,
-        Box::new(drone::pidcontroller::PIDController {
+        Box::new(drone::stacked::StackedController::new(config.clone())),
-            ..Default::default()
-        }),
         drone::MotorCharacteristics {
-            max_thrust: 2.6,
+            max_thrust: config.max_thrust,
-            max_torque: 0.5,
+            max_torque: config.max_torque,
+            time_constant: config.time_constant,
             ..Default::default()
         },
-        0.350,
+        config.mass,
     );
 
     let mut sim = Simulation::new(
@@ -119,7 +124,7 @@ async fn run_record(output: String) {
         world,
         SimMode::Record(InputRecording::default(), output),
         None,
-        600,
+        config.drone_tick_rate,
     );
 
     sim.run_and_render().await.unwrap();

src/main_test.rs

@@ -1,95 +0,0 @@
-use macroquad::prelude as mq;
-use rapier3d::prelude::*;
-
-mod engine;
-use engine::*;
-
-mod camera;
-mod drone;
-mod rendering;
-
-mod config;
-mod graphics_util;
-
-use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};
-
-fn window_conf() -> mq::Conf {
-    mq::Conf {
-        window_title: "RustDroneSim".to_owned(),
-        window_resizable: true,
-        // fullscreen: true,
-        platform: mq::miniquad::conf::Platform {
-            // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
-            ..Default::default()
-        },
-        ..Default::default()
-    }
-}
-
-#[macroquad::main(window_conf)]
-async fn main() {
-    /* World Setup */
-    let mut world = World::new(600.0);
-
-    world.register_free_collider(
-        ColliderBuilder::cuboid(30.0, 1.0, 30.0)
-            .translation(vector![0.0, -2.0, 0.0])
-            .restitution(0.5)
-            .build(),
-        None,
-    );
-
-    let mut drone_controller = drone::pidcontroller::PIDController::default();
-    drone_controller.set_input(JoystickInput {
-        throttle_input: 0.2,
-        yaw_input: 0.2,
-        roll_input: 0.2,
-        pitch_input: 0.0,
-    });
-    let mut drone_obj = drone::Drone::new(
-        &mut world,
-        Box::new(drone_controller),
-        drone::MotorCharacteristics {
-            max_thrust: 2.6,
-            max_torque: 0.5,
-            ..Default::default()
-        },
-        0.350,
-    );
-
-    /* Renderer Setup */
-    let camera = camera::AttachedCamera::new(
-        drone_obj.rb_handle,
-        vector![1.0, 0.0, 0.0],
-        vector![0.5, 0.0, 0.0],
-    );
-    let mut renderer = Renderer::new(Box::new(camera));
-
-    renderer.light.set_location(
-        vector![70.0, 150.0, -90.0].into(),
-        vector![-0.4, -0.7, 0.6].into(),
-    );
-    renderer.update_light(&world);
-
-    loop {
-        renderer.update_camera(&world);
-
-        if mq::is_key_pressed(mq::KeyCode::Q) {
-            break;
-        }
-
-        // Physics
-        world.step();
-        let _ = clearscreen::clear();
-        drone_obj.process_tick(&mut world);
-
-        // Rendering
-        renderer.draw(&mut world);
-
-        if world.tick % (10) == 0 {
-            // renderer.update_light(&world);
-            // world.clear_ofb();
-            mq::next_frame().await;
-        }
-    }
-}

src/simulation.rs

@@ -159,13 +159,12 @@ impl Simulation {
         }
         self.drone.process_tick(&mut self.world);
 
-        let mut target_angular_vel: na::Vector3<f32> = na::vector![
+        let target_angular_vel: na::Vector3<f32> = na::vector![
             current_input.roll_input,
             current_input.yaw_input,
             current_input.pitch_input,
         ] * 3.14;
 
-        let mut desired_motor_diff: na::Vector3<f32> = na::vector![0.0, 0.0, 0.0];
         let applied_motor_diff: na::Vector3<f32> = na::vector![
             (self.drone.current_throttles[1] + self.drone.current_throttles[2]
                 - self.drone.current_throttles[0]
@@ -178,16 +177,6 @@ impl Simulation {
                 - self.drone.current_throttles[1])
         ] / 2.0;
 
-        if let Some(cont) = self
-            .drone
-            .controller
-            .as_mut_any()
-            .downcast_mut::<crate::drone::pidcontroller::PIDController>()
-        {
-            desired_motor_diff = cont.get_desired_motor_diffs();
-            target_angular_vel = cont.get_desired_angular_velocity();
-        }
-
         if let Some(logger) = &mut self.logger {
             logger
                 .log(&crate::logger::SimLogRow::from_data(
@@ -199,7 +188,7 @@ impl Simulation {
                         .transform_vector(&self.drone.get_angvel(&self.world))
                         .into(),
                     applied_motor_diff,
-                    desired_motor_diff,
+                    na::vector![0.0, 0.0, 0.0],
                 ))
                 .unwrap();
         }