From ba8664193acae83a2de5533080658ae65c9bd9aa Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Thu, 6 Jul 2017 14:50:17 +0900 Subject: [PATCH] Rover: fix failsafe throttle --- APMrover2/Steering.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 271913f513..4303f63363 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -243,10 +243,14 @@ void Rover::calc_nav_steer() { *****************************************/ void Rover::set_servos(void) { // Apply slew rate limit on non Manual modes - if (control_mode != MANUAL && control_mode != LEARNING) { - g2.motors.slew_limit_throttle(true); - } else { + if (control_mode == MANUAL || control_mode == LEARNING) { g2.motors.slew_limit_throttle(false); + if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { + g2.motors.set_throttle(0.0f); + g2.motors.set_steering(0.0f); + } + } else { + g2.motors.slew_limit_throttle(true); } // send output signals to motors