Rover: Added BalanceBot as a rover frame

APMrover2/AP_MotorsUGV.cpp

@@ -241,6 +241,10 @@ void AP_MotorsUGV::output(bool armed, float ground_speed, float dt)
     // slew limit throttle
     slew_limit_throttle(dt);
 
+    if (rover.is_BalanceBot()) {
+        rover.balance_pitch(_throttle, armed);
+    }
+
     // output for regular steering/throttle style frames
     output_regular(armed, ground_speed, _steering, _throttle);
 

APMrover2/BalanceBot.cpp

@@ -0,0 +1,21 @@
+#include<stdio.h>
+#include "Rover.h"
+
+
+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;
+}
+
+// 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
+{
+    return ((enum frame_class)g2.frame_class.get() == FRAME_BALANCEBOT);
+}

APMrover2/GCS_Mavlink.cpp

@@ -211,6 +211,21 @@ void Rover::send_pid_tuning(mavlink_channel_t chan)
             return;
         }
     }
+
+    // pitch to throttle pid
+    if (g.gcs_pid_mask & 4) {
+        pid_info = &g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info();
+        mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
+                                    pid_info->desired,
+                                    ahrs.pitch,
+                                    0,
+                                    pid_info->P,
+                                    pid_info->I,
+                                    pid_info->D);
+        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
+            return;
+        }
+    }
 }
 
 void Rover::send_fence_status(mavlink_channel_t chan)

APMrover2/Parameters.cpp

@@ -522,7 +522,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Param: FRAME_CLASS
     // @DisplayName: Frame Class
     // @Description: Frame Class
-    // @Values: 0:Undefined,1:Rover,2:Boat
+    // @Values: 0:Undefined,1:Rover,2:Boat,3:BalanceBot
     // @User: Standard
     AP_GROUPINFO("FRAME_CLASS", 16, ParametersG2, frame_class, 1),
 

APMrover2/Rover.h

@@ -558,6 +558,10 @@ public:
     void mavlink_delay_cb();
     void failsafe_check();
 
+    // BalanceBot.cpp
+    void balance_pitch(float &, bool);
+    bool is_BalanceBot() const;
+
     void update_soft_armed();
     // Motor test
     void motor_test_output();

APMrover2/defines.h

@@ -132,7 +132,8 @@ enum pilot_steer_type_t {
 enum frame_class {
     FRAME_UNDEFINED = 0,
     FRAME_ROVER = 1,
-    FRAME_BOAT = 2
+    FRAME_BOAT = 2,
+    FRAME_BALANCEBOT = 3,
 };
 
 #define AUX_SWITCH_PWM_TRIGGER_HIGH 1800   // pwm value above which the ch7 or ch8 option will be invoked