diff --git a/src/stacked.rs b/src/stacked.rs index 5028770..e1e462e 100644 --- a/src/stacked.rs +++ b/src/stacked.rs @@ -110,7 +110,18 @@ impl DroneController for StackedController { let angular_rate_setpoint = if self.input.mode != ModeInput::Acro { let rotation_setpoint = if self.input.mode != ModeInput::Rotation { let lin_accel_setpoint = if self.input.mode != ModeInput::Acceleration { - panic!("Not Implemented") + let lin_vel_setpoint: Velocity = if self.input.mode != ModeInput::Velocity { + println!("LAYER NOT IMPLENETED. TAKING 0 AS INPUT;"); + Velocity(na::Vector3::zeros()) + } else { + Velocity(na::vector![ + self.input.velocity.x, + self.input.velocity.y, + self.input.velocity.z + ]) + }; + self.linvel_rt + .update(lin_vel_setpoint, &self.drone_state, frame_dt) } else { Acceleration(na::vector![ self.input.acceleration.x, diff --git a/src/stacked/mixer.rs b/src/stacked/mixer.rs index 1755241..c708ce4 100644 --- a/src/stacked/mixer.rs +++ b/src/stacked/mixer.rs @@ -7,7 +7,7 @@ pub enum MotorMixingMode { } impl Default for MotorMixingMode { fn default() -> Self { - Self::ThrottleAuthorityReasonable { min_scale: 0.1 } + Self::ThrottleAuthorityReasonable { min_scale: 0.05 } } }