diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 8ac54f7d0a..cc2a8a332c 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -26,6 +26,13 @@ bool Mode::enter() void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &throttle_out) { + // no RC input means no throttle and centered steering + if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) { + steering_out = 0; + throttle_out = 0; + return; + } + // apply RC skid steer mixing switch ((enum pilot_steer_type_t)rover.g.pilot_steer_type.get()) { diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index d49c88dc6b..ecb3fbe753 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -3,17 +3,12 @@ void ModeManual::update() { - // check for radio failsafe - if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) { - g2.motors.set_throttle(0.0f); - g2.motors.set_steering(0.0f); - } else { - float desired_steering, desired_throttle; - get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); - // copy RC scaled inputs to outputs - g2.motors.set_throttle(desired_throttle); - g2.motors.set_steering(desired_steering); - } + float desired_steering, desired_throttle; + get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); + + // copy RC scaled inputs to outputs + g2.motors.set_throttle(desired_throttle); + g2.motors.set_steering(desired_steering); // mark us as in_reverse when using a negative throttle to stop AHRS getting off rover.set_reverse(is_negative(g2.motors.get_throttle()));