diff --git a/APMrover2/AP_MotorsUGV.cpp b/APMrover2/AP_MotorsUGV.cpp index 35cb5befe0..d09bddc9ff 100644 --- a/APMrover2/AP_MotorsUGV.cpp +++ b/APMrover2/AP_MotorsUGV.cpp @@ -241,10 +241,6 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt) // slew limit throttle slew_limit_throttle(dt); - if (rover.is_BalanceBot()) { - rover.balance_pitch(_throttle, armed); - } - // output for regular steering/throttle style frames output_regular(armed, ground_speed, _steering, _throttle); diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp index 15fcd7d932..992c0f2e1c 100644 --- a/APMrover2/mode_manual.cpp +++ b/APMrover2/mode_manual.cpp @@ -13,6 +13,11 @@ void ModeManual::update() get_pilot_desired_steering_and_throttle(desired_steering, desired_throttle); get_pilot_desired_lateral(desired_lateral); + // if vehicle is balance bot, calculate actual throttle required for balancing + if (rover.is_BalanceBot()) { + rover.balance_pitch(desired_throttle, rover.arming.is_armed()); + } + // copy RC scaled inputs to outputs g2.motors.set_throttle(desired_throttle); g2.motors.set_steering(desired_steering, false);