Rover: balance bot pitch control added in all modes

APMrover2/BalanceBot.cpp

@@ -2,21 +2,22 @@
 #include "Rover.h"
 
 // Function to set a desired pitch angle according to throttle
-void Rover::balance_pitch(float &throttle, bool armed)
+void Rover::balancebot_pitch_control(float &throttle, bool armed)
 {
     // calculate desired pitch angle
-    float demanded_pitch = radians(-(throttle/100) * g2.bal_pitch_max);
+    const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max);
 
     // calculate required throttle using PID controller
-    float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100;
+    const float balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed) * 100.0f;
 
-    throttle = constrain_float(balance_throttle, -100, 100);
+    // constrain throttle between -100 and 100
+    throttle = constrain_float(balance_throttle, -100.0f, 100.0f);
 }
 
 // returns true if vehicle is a balance bot
 // called in AP_MotorsUGV::output()
 // this affects whether the vehicle tries to control its pitch with throttle output
-bool Rover::is_BalanceBot() const
+bool Rover::is_balancebot() const
 {
     return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT);
 }

APMrover2/Rover.h

@@ -559,8 +559,8 @@ public:
     void failsafe_check();
 
     // BalanceBot.cpp
-    void balance_pitch(float &, bool);
-    bool is_BalanceBot() const;
+    void balancebot_pitch_control(float &, bool);
+    bool is_balancebot() const;
 
     void update_soft_armed();
     // Motor test

APMrover2/mode.cpp

@@ -252,6 +252,11 @@ void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_
         throttle_out = 100.0f * attitude_control.get_throttle_out_speed(target_speed, g2.motors.limit.throttle_lower, g2.motors.limit.throttle_upper, g.speed_cruise, g.throttle_cruise * 0.01f, rover.G_Dt);
     }
 
+    // if vehicle is balance bot, calculate actual throttle required for balancing
+    if (rover.is_balancebot()) {
+        rover.balancebot_pitch_control(throttle_out, rover.arming.is_armed());
+    }
+
     // send to motor
     g2.motors.set_throttle(throttle_out);
 }

APMrover2/mode_hold.cpp

@@ -3,7 +3,14 @@
 
 void ModeHold::update()
 {
+    float throttle = 0.0f;
+
+    // if vehicle is balance bot, calculate actual throttle required for balancing
+    if (rover.is_balancebot()) {
+        rover.balancebot_pitch_control(throttle, rover.arming.is_armed());
+    }
+
     // hold position - stop motors and center steering
-    g2.motors.set_throttle(0.0f);
+    g2.motors.set_throttle(throttle);
     g2.motors.set_steering(0.0f);
 }

APMrover2/mode_manual.cpp

@@ -14,8 +14,8 @@ void ModeManual::update()
     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());
+    if (rover.is_balancebot()) {
+        rover.balancebot_pitch_control(desired_throttle, rover.arming.is_armed());
     }
 
     // copy RC scaled inputs to outputs