diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index b6e0db6287..32c7e308f0 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -51,15 +51,8 @@ bool Rover::in_stationary_loiter() /***************************************** Set the flight control servos based on the current calculated values *****************************************/ -void Rover::set_servos(void) { - // Apply slew rate limit on non Manual modes - if (control_mode == &mode_manual || control_mode == &mode_learning) { - if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { - g2.motors.set_throttle(0.0f); - g2.motors.set_steering(0.0f); - } - } - +void Rover::set_servos(void) +{ // send output signals to motors if (motor_test) { g2.motors.slew_limit_throttle(false); diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index 9fcedd7d30..8e2d18db4d 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -3,9 +3,15 @@ void ModeManual::update() { - // copy RC scaled inputs to outputs - g2.motors.set_throttle(channel_throttle->get_control_in()); - g2.motors.set_steering(channel_steer->get_control_in()); + // check for radio failsafe + if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) { + g2.motors.set_throttle(0.0f); + g2.motors.set_steering(0.0f); + } else { + // copy RC scaled inputs to outputs + g2.motors.set_throttle(channel_throttle->get_control_in()); + g2.motors.set_steering(channel_steer->get_control_in()); + } // mark us as in_reverse when using a negative throttle to stop AHRS getting off rover.set_reverse(is_negative(g2.motors.get_throttle()));