Rover: Crash checks for Balance Bot

APMrover2/Parameters.cpp

@@ -556,6 +556,16 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("BAL_PITCH_MAX", 21, ParametersG2, bal_pitch_max, 5),
 
+    // @Param: BAL_PITCH_CRASH
+    // @DisplayName: BalanceBot Crash Pitch
+    // @Description: Pitch angle for crash check
+    // @Units: deg
+    // @Range: 0 60
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("BAL_PITCH_CRASH", 22, ParametersG2, bal_pitch_crash, 30),
+
+
     AP_GROUPEND
 };
 

APMrover2/Parameters.h

@@ -344,6 +344,9 @@ public:
 
     // pitch angle at 100% throttle
     AP_Float bal_pitch_max;
+
+    // balance bot pitch for crash check
+    AP_Float bal_pitch_crash;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/crash_check.cpp

@@ -12,6 +12,17 @@ void Rover::crash_check()
 {
   static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
 
+  if (is_balancebot() && arming.is_armed()) {
+      // Crashed if pitch > bal_pitch_crash
+      if (fabsf(ahrs.pitch) > radians(g2.bal_pitch_crash)) {
+          // log an error in the dataflash
+          Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
+          // send message to gcs
+          gcs().send_text(MAV_SEVERITY_EMERGENCY, "Balance Bot crashed");
+          disarm_motors();
+      }
+  }
+
   // return immediately if disarmed, crash checking is disabled or vehicle is Hold, Manual or Acro mode
   if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (!control_mode->is_autopilot_mode())) {
     crash_counter = 0;