From 6c2f18cc2f70d2b1319d897bc629c97155f89b3e Mon Sep 17 00:00:00 2001
From: Ebin <stargazer18@gmail.com>
Date: Wed, 20 Jun 2018 16:41:58 +0530
Subject: [PATCH] Rover: manual mode support for balancebot

---
 APMrover2/BalanceBot.cpp | 15 ++++++++-------
 APMrover2/Parameters.cpp |  9 +++++++++
 APMrover2/Parameters.h   |  3 +++
 3 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/APMrover2/BalanceBot.cpp b/APMrover2/BalanceBot.cpp
index 237b89235a..6a3a40fde4 100644
--- a/APMrover2/BalanceBot.cpp
+++ b/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
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index cb39f3f572..433d532d5b 100644
--- a/APMrover2/Parameters.cpp
+++ b/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
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 8bdce61800..9ec838ed88 100644
--- a/APMrover2/Parameters.h
+++ b/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[];