Rover: added crash check

APMrover2/APMrover2.cpp

@@ -76,6 +76,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(dataflash_periodic,     50,    300),
     SCHED_TASK(button_update,          5,     100),
     SCHED_TASK(stats_update,           1,     100),
+    SCHED_TASK(crash_check,           10,    1000),
 };
 
 /*

APMrover2/Log.cpp

@@ -365,6 +365,25 @@ void Rover::Log_Write_RC(void)
     }
 }
 
+struct PACKED log_Error {
+  LOG_PACKET_HEADER;
+  uint64_t time_us;
+  uint8_t sub_system;
+  uint8_t error_code;
+};
+
+// Write an error packet
+void Rover::Log_Write_Error(uint8_t sub_system, uint8_t error_code)
+{
+  struct log_Error pkt = {
+      LOG_PACKET_HEADER_INIT(LOG_ERROR_MSG),
+      time_us       : AP_HAL::micros64(),
+      sub_system    : sub_system,
+      error_code    : error_code,
+  };
+  DataFlash.WriteBlock(&pkt, sizeof(pkt));
+}
+
 void Rover::Log_Write_Baro(void)
 {
     DataFlash.Log_Write_Baro(barometer);
@@ -436,6 +455,8 @@ const LogStructure Rover::log_structure[] = {
       "STER", "Qff",   "TimeUS,Demanded,Achieved" },
     { LOG_GUIDEDTARGET_MSG, sizeof(log_GuidedTarget),
       "GUID",  "QBffffff",    "TimeUS,Type,pX,pY,pZ,vX,vY,vZ" },
+    { LOG_ERROR_MSG, sizeof(log_Error),
+      "ERR",   "QBB",         "TimeUS,Subsys,ECode" },
 };
 
 void Rover::log_init(void)

APMrover2/Parameters.cpp

@@ -316,6 +316,13 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
 	GSCALAR(fs_gcs_enabled, "FS_GCS_ENABLE",   0),
 
+    // @Param: FS_CRASH_CHECK
+    // @DisplayName: Crash check action
+    // @Description: What to do on a crash event. When enabled the rover will go to hold if a crash is detected.
+    // @Values: 0:Disabled,1:HOLD,2:HoldAndDisarm
+    // @User: Standard
+    GSCALAR(fs_crash_check, "FS_CRASH_CHECK",    FS_CRASH_HOLD),
+
 	// @Param: RNGFND_TRIGGR_CM
 	// @DisplayName: Rangefinder trigger distance
 	// @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle

APMrover2/Parameters.h

@@ -136,6 +136,7 @@ public:
         k_param_fs_throttle_enabled,
         k_param_fs_throttle_value,
         k_param_fs_gcs_enabled,
+        k_param_fs_crash_check,
 
         // obstacle control
         k_param_sonar_enabled = 190, // deprecated, can be removed
@@ -271,6 +272,7 @@ public:
     AP_Int8     fs_throttle_enabled;
     AP_Int16    fs_throttle_value;
 	AP_Int8	    fs_gcs_enabled;
+    AP_Int8     fs_crash_check;
 
     // obstacle control
     AP_Int16    sonar_trigger_cm;

APMrover2/Rover.h

@@ -389,7 +389,6 @@ private:
     // Guided
     GuidedMode guided_mode;  // stores which GUIDED mode the vehicle is in
 
-
 private:
     // private member functions
     void ahrs_update();
@@ -439,6 +438,7 @@ private:
     void Log_Write_Current();
     void Log_Write_Attitude();
     void Log_Write_RC(void);
+    void Log_Write_Error(uint8_t sub_system, uint8_t error_code);
     void Log_Write_Baro(void);
     void Log_Write_Home_And_Origin();
     void Log_Write_Vehicle_Startup_Messages();
@@ -547,6 +547,7 @@ private:
     bool in_stationary_loiter(void);
     void set_loiter_active(const AP_Mission::Mission_Command& cmd);
     void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
+    void crash_check();
 
 public:
     bool print_log_menu(void);

APMrover2/crash_check.cpp

@@ -0,0 +1,44 @@
+#include "Rover.h"
+
+// Code to detect a crash or block
+#define CRASH_CHECK_TRIGGER_SEC     2       // 2 seconds blocked indicates a crash
+#define CRASH_CHECK_THROTTLE_MIN    5.0f    // vehicle must have a throttle greater that 5% to be considered crashed
+#define CRASH_CHECK_VEL_MIN         0.08f   // vehicle must have a velocity under 0.08 m/s to be considered crashed
+
+// crash_check - disarms motors if a crash or block has been detected
+// crashes are detected by the vehicle being static (no speed) for more than CRASH_CHECK_TRIGGER_SEC and motor are running
+// called at 10Hz
+void Rover::crash_check()
+{
+  static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
+
+  // return immediately if disarmed, or crash checking disabled or in HOLD mode
+  if (!arming.is_armed() || g.fs_crash_check == FS_CRASH_DISABLE || (control_mode != GUIDED && control_mode != AUTO)) {
+    crash_counter = 0;
+    return;
+  }
+
+  // TODO : Check if min vel can be calculated
+  // min_vel = ( CRASH_CHECK_THROTTLE_MIN * g.speed_cruise) / g.throttle_cruise;
+
+  if ((ahrs.groundspeed() >= CRASH_CHECK_VEL_MIN)|| ((100 * fabsf(channel_throttle->norm_output())) < CRASH_CHECK_THROTTLE_MIN)) {
+    crash_counter = 0;
+    return;
+  }
+
+  // we may be crashing
+  crash_counter++;
+
+  // check if crashing for 2 seconds
+  if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * 10)) {
+    // 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, "Crash: Going to HOLD");
+    // change mode to hold and disarm
+    set_mode(HOLD);
+    if (g.fs_crash_check == FS_CRASH_HOLD_AND_DISARM) {
+      disarm_motors();
+    }
+  }
+}

APMrover2/defines.h

@@ -60,6 +60,7 @@ enum GuidedMode {
 #define LOG_ARM_DISARM_MSG      0x08
 #define LOG_STEERING_MSG        0x0D
 #define LOG_GUIDEDTARGET_MSG    0x0E
+#define LOG_ERROR_MSG           0x13
 
 #define TYPE_AIRSTART_MSG		0x00
 #define TYPE_GROUNDSTART_MSG	0x01
@@ -90,3 +91,15 @@ enum GuidedMode {
 #define MAVLINK_SET_POS_TYPE_MASK_FORCE           (1<<9)
 #define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE      (1<<10)
 #define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE (1<<11)
+
+// Error message sub systems and error codes
+#define ERROR_SUBSYSTEM_CRASH_CHECK         12
+// subsystem specific error codes -- crash checker
+#define ERROR_CODE_CRASH_CHECK_CRASH        1
+
+enum fs_crash_action {
+  FS_CRASH_DISABLE = 0,
+  FS_CRASH_HOLD = 1,
+  FS_CRASH_HOLD_AND_DISARM = 2
+};
+