diff --git a/src/drone.rs b/src/drone.rs index f7eb114..2740d75 100644 --- a/src/drone.rs +++ b/src/drone.rs @@ -183,7 +183,8 @@ impl Drone { let rb = world.bodies.get(self.rb_handle).unwrap(); self.controller.set_time(world.get_time()); // Pitch, Yaw, Roll - self.controller.set_angular_velocity(*rb.angvel()); + self.controller + .set_angular_velocity(rb.rotation().inverse().transform_vector(&rb.angvel())); self.controller.set_rotation(*rb.rotation()); } diff --git a/src/drone/input.rs b/src/drone/input.rs index 2972aef..6e01608 100644 --- a/src/drone/input.rs +++ b/src/drone/input.rs @@ -6,13 +6,9 @@ pub struct JoystickInput { // Value should be between 0 and 1 pub throttle_input: f32, - // Rotation Directions: https://upload.wikimedia.org/wikipedia/commons/c/c1/Yaw_Axis_Corrected.svg - /* - * Values should be between -1 and 1. - */ + pub roll_input: f32, pub yaw_input: f32, pub pitch_input: f32, - pub roll_input: f32, } impl JoystickInput { diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs index 16b43c7..80ff152 100644 --- a/src/drone/stacked.rs +++ b/src/drone/stacked.rs @@ -58,7 +58,7 @@ impl StackedController { Self { mixer: MotorMixer { motor_map: config.motor_map, - min_throttle: 0.0, + min_throttle: 0.1, max_throttle: 1.0, mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 }, }, diff --git a/src/drone/stacked/modules.rs b/src/drone/stacked/modules.rs index 2188627..0477cb1 100644 --- a/src/drone/stacked/modules.rs +++ b/src/drone/stacked/modules.rs @@ -47,11 +47,9 @@ impl ControllerModule { } else { setpoint }; - let rot = state.rotation; - let current = rot.inverse().transform_vector(&state.angular_velocity); // Output of Rate PID = Desired Torque/Correction Force - processor.update(target_velocity, current, dt) + processor.update(target_velocity, state.angular_velocity, dt) } } }