From e806d0c3851af78b56a699d459b83e289904d6b7 Mon Sep 17 00:00:00 2001
From: Ebin <stargazer18@gmail.com>
Date: Thu, 13 Dec 2018 13:19:08 +0530
Subject: [PATCH] Rover: add param to offset balancing point for balance bots

---
 APMrover2/Parameters.cpp  | 9 +++++++++
 APMrover2/Parameters.h    | 3 +++
 APMrover2/balance_bot.cpp | 2 +-
 3 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 153354ca1c..9583896e6b 100644
--- a/APMrover2/Parameters.cpp
+++ b/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
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 5e39363c87..9e6881cc97 100644
--- a/APMrover2/Parameters.h
+++ b/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[];
diff --git a/APMrover2/balance_bot.cpp b/APMrover2/balance_bot.cpp
index 7cd9fbdb06..cf35149794 100644
--- a/APMrover2/balance_bot.cpp
+++ b/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;