Rover: add param to offset balancing point for balance bots

APMrover2/Parameters.cpp

@@ -694,6 +694,15 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(gripper, "GRIP_", 39, ParametersG2, AP_Gripper),
 #endif
 
+    // @Param: BAL_PITCH_TRIM
+    // @DisplayName: Balance Bot pitch trim angle
+    // @Description: Balance Bot pitch trim for balancing. This offsets the tilt of the center of mass.
+    // @Units: deg
+    // @Range: -2 2
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("BAL_PITCH_TRIM", 40, ParametersG2, bal_pitch_trim, 0),
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -396,6 +396,9 @@ public:
 
     // mission behave
     AP_Int8 mis_done_behave;
+
+    // balance both pitch trim
+    AP_Float bal_pitch_trim;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/balance_bot.cpp

@@ -5,7 +5,7 @@
 void Rover::balancebot_pitch_control(float &throttle)
 {
     // calculate desired pitch angle
-    const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max);
+    const float demanded_pitch = radians(-throttle * 0.01f * g2.bal_pitch_max) + radians(g2.bal_pitch_trim);
 
     // calculate speed from wheel encoders
     float veh_speed_pct = 0.0f;