Rover: manual mode support for balancebot

APMrover2/BalanceBot.cpp

@@ -1,15 +1,16 @@
 #include<stdio.h>
 #include "Rover.h"
 
-
+// Function to set a desired pitch angle according to throttle
 void Rover::balance_pitch(float &throttle, bool armed)
 {
-    float balance_throttle = 0;
-    float throttle_new = 0;
-    float demanded_pitch = 0;
-    balance_throttle = g2.attitude_control.get_throttle_out_from_pitch(demanded_pitch, armed)*100;
-    throttle_new = constrain_float(throttle + balance_throttle, -100, 100);
-    throttle = throttle_new;
+    // calculate desired pitch angle
+    float demanded_pitch = radians(-(throttle/100) * 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;
+
+    throttle = constrain_float(balance_throttle, -100, 100);
 }
 
 // returns true if vehicle is a balance bot

APMrover2/Parameters.cpp

@@ -547,6 +547,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("PIVOT_TURN_RATE", 20, ParametersG2, pivot_turn_rate, 90),
 
+    // @Param: BAL_PITCH_MAX
+    // @DisplayName: BalanceBot Maximum Pitch
+    // @Description: Pitch angle in degrees at 100% throttle
+    // @Units: deg
+    // @Range: 0 5
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -341,6 +341,9 @@ public:
 
     // pivot turn rate
     AP_Int16 pivot_turn_rate;
+
+    // pitch angle at 100% throttle
+    AP_Float bal_pitch_max;
 };
 
 extern const AP_Param::Info var_info[];