From 85c3286c6eee14aeac680be926e923485a61eaa8 Mon Sep 17 00:00:00 2001 From: Ebin Date: Thu, 13 Dec 2018 13:24:02 +0530 Subject: [PATCH] Rover: balance bots continue balancing when stopped --- APMrover2/mode.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index 3a92032d96..c129acf001 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -309,7 +309,7 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_ float throttle_out; // call speed or stop controller - if (is_zero(target_speed)) { + if (is_zero(target_speed) && !rover.is_balancebot()) { bool stopped; throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped); } else { @@ -333,11 +333,14 @@ bool Mode::stop_vehicle() { // call throttle controller and convert output to -100 to +100 range bool stopped = false; - float throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped); + float throttle_out; - // if vehicle is balance bot, calculate actual throttle required for balancing + // if vehicle is balance bot, calculate throttle required for balancing if (rover.is_balancebot()) { + throttle_out = 100.0f * attitude_control.get_throttle_out_speed(0, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt); rover.balancebot_pitch_control(throttle_out); + } else { + throttle_out = 100.0f * attitude_control.get_throttle_out_stop(g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt, stopped); } // relax mainsail if present