Copter: add GCS failsafe

ArduCopter/AP_State.pde

@@ -91,6 +91,12 @@ static void set_failsafe_gps(bool mode)
     ap.failsafe_gps = mode;
 }
 
+// ---------------------------------------------
+static void set_failsafe_gcs(bool mode)
+{
+    ap.failsafe_gcs = mode;
+}
+
 // ---------------------------------------------
 void set_takeoff_complete(bool b)
 {

ArduCopter/ArduCopter.pde

@@ -372,11 +372,13 @@ static union {
         uint8_t failsafe_radio     : 1; // 8    // A status flag for the radio failsafe
         uint8_t failsafe_batt      : 1; // 9    // A status flag for the battery failsafe
         uint8_t failsafe_gps       : 1; // 10   // A status flag for the gps failsafe
-        uint8_t do_flip            : 1; // 11   // Used to enable flip code
+        uint8_t failsafe_gcs       : 1; // 11   // A status flag for the ground station failsafe
-        uint8_t takeoff_complete   : 1; // 12
+        uint8_t rc_override_active : 1; // 12   // true if rc control are overwritten by ground station
-        uint8_t land_complete      : 1; // 13
+        uint8_t do_flip            : 1; // 13   // Used to enable flip code
-        uint8_t compass_status     : 1; // 14
+        uint8_t takeoff_complete   : 1; // 14
-        uint8_t gps_status         : 1; // 15
+        uint8_t land_complete      : 1; // 15
+        uint8_t compass_status     : 1; // 16
+        uint8_t gps_status         : 1; // 17
     };
     uint16_t value;
 } ap;
@@ -750,6 +752,7 @@ static AC_WPNav wp_nav(&inertial_nav, &g.pi_loiter_lat, &g.pi_loiter_lon, &g.pid
 ////////////////////////////////////////////////////////////////////////////////
 // The number of GPS fixes we have had
 static uint8_t gps_fix_count;
+static int16_t pmTest1;
 
 // System Timers
 // --------------
@@ -771,6 +774,8 @@ static uint32_t rtl_loiter_start_time;
 static uint8_t auto_disarming_counter;
 // prevents duplicate GPS messages from entering system
 static uint32_t last_gps_time;
+// the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
+static uint32_t last_heartbeat_ms;
 
 // Used to exit the roll and pitch auto trim function
 static uint8_t auto_trim_counter;
@@ -923,6 +928,7 @@ static void perf_update(void)
     }
     perf_info_reset();
     gps_fix_count = 0;
+    pmTest1 = 0;
 }
 
 void loop()
@@ -1194,6 +1200,9 @@ static void slow_loop()
         slow_loopCounter++;
         superslow_loopCounter++;
 
+        // check if we've lost contact with the ground station
+        failsafe_gcs_check();
+
         // record if the compass is healthy
         set_compass_healthy(compass.healthy);
 

ArduCopter/GCS_Mavlink.pde

@@ -1867,6 +1867,11 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         v[6] = packet.chan7_raw;
         v[7] = packet.chan8_raw;
         hal.rcin->set_overrides(v, 8);
+
+        // record that rc are overwritten so we can trigger a failsafe if we lose contact with groundstation
+        ap.rc_override_active = true;
+        // a RC override message is consiered to be a 'heartbeat' from the ground station for failsafe purposes
+        last_heartbeat_ms = millis();
         break;
     }
 
@@ -1951,15 +1956,16 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
     }
 #endif //  HIL_MODE != HIL_MODE_DISABLED
 
+
+    case MAVLINK_MSG_ID_HEARTBEAT:
+    {
+        // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
+        if(msg->sysid != g.sysid_my_gcs) break;
+        last_heartbeat_ms = millis();
+        pmTest1++;
+        break;
+    }
 /*
- *       case MAVLINK_MSG_ID_HEARTBEAT:
- *               {
- *                       // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
- *                       if(msg->sysid != g.sysid_my_gcs) break;
- *                       rc_override_fs_timer = millis();
- *                       break;
- *               }
- *
  * #if HIL_MODE != HIL_MODE_DISABLED
  *               // This is used both as a sensor and to pass the location
  *               // in HIL_ATTITUDE mode.

ArduCopter/Log.pde

@@ -375,6 +375,7 @@ struct PACKED log_Performance {
     uint16_t num_long_running;
     uint16_t num_loops;
     uint32_t max_time;
+    int16_t  pm_test;
     uint8_t i2c_lockup_count;
 };
 
@@ -389,6 +390,7 @@ static void Log_Write_Performance()
         num_long_running : perf_info_get_num_long_running(),
         num_loops        : perf_info_get_num_loops(),
         max_time         : perf_info_get_max_time(),
+        pm_test          : pmTest1,
         i2c_lockup_count : hal.i2c->lockup_count()
     };
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
@@ -790,7 +792,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_COMPASS_MSG, sizeof(log_Compass),             
       "MAG", "hhhhhhhhh",    "MagX,MagY,MagZ,OfsX,OfsY,OfsZ,MOfsX,MOfsY,MOfsZ" },
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance), 
-      "PM",  "BBBHHIB",       "RenCnt,RenBlw,FixCnt,NLon,NLoop,MaxT,I2CErr" },
+      "PM",  "BBBHHIhB",       "RenCnt,RenBlw,FixCnt,NLon,NLoop,MaxT,PMT,I2CErr" },
     { LOG_CMD_MSG, sizeof(log_Cmd),                 
       "CMD", "BBBBBeLL",     "CTot,CNum,CId,COpt,Prm1,Alt,Lat,Lng" },
     { LOG_ATTITUDE_MSG, sizeof(log_Attitude),       

ArduCopter/Parameters.h

@@ -194,9 +194,10 @@ public:
         k_param_rc_speed = 192,
         k_param_failsafe_battery_enabled,
         k_param_throttle_mid,
-        k_param_failsafe_gps_enabled,  // 195
+        k_param_failsafe_gps_enabled,
         k_param_rc_9,
         k_param_rc_12,
+        k_param_failsafe_gcs,           // 198
 
         //
         // 200: flight modes
@@ -274,6 +275,7 @@ public:
     AP_Int16        pack_capacity;              // Battery pack capacity less reserve
     AP_Int8         failsafe_battery_enabled;   // battery failsafe enabled
     AP_Int8         failsafe_gps_enabled;       // gps failsafe enabled
+    AP_Int8         failsafe_gcs;               // ground station failsafe behavior
 
     AP_Int8         compass_enabled;
     AP_Int8         optflow_enabled;

ArduCopter/Parameters.pde

@@ -106,6 +106,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(failsafe_gps_enabled, "FS_GPS_ENABLE", FS_GPS),
 
+    // @Param: FS_GCS_ENABLE
+    // @DisplayName: Ground Station Failsafe Enable
+    // @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds
+    // @Values: 0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode
+    // @User: Standard
+    GSCALAR(failsafe_gcs, "FS_GCS_ENABLE", FS_GCS_DISABLED),
+
     // @Param: VOLT_DIVIDER
     // @DisplayName: Voltage Divider
     // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin voltage * INPUT_VOLTS/1024 * VOLT_DIVIDER)

ArduCopter/config.h

@@ -396,6 +396,18 @@
  # define FAILSAFE_GPS_TIMEOUT_MS       5000    // gps failsafe triggers after 5 seconds with no GPS
 #endif
 
+// GCS failsafe
+#ifndef FS_GCS
+ # define FS_GCS                        DISABLED
+#endif
+#ifndef FS_GCS_TIMEOUT_MS
+ # define FS_GCS_TIMEOUT_MS             5000    // gcs failsafe triggers after 5 seconds with no GCS heartbeat
+#endif
+// possible values for FS_GCS parameter
+#define FS_GCS_DISABLED                     0
+#define FS_GCS_ENABLED_ALWAYS_RTL           1
+#define FS_GCS_ENABLED_CONTINUE_MISSION     2
+
 //////////////////////////////////////////////////////////////////////////////
 //  MAGNETOMETER
 #ifndef MAGNETOMETER

ArduCopter/defines.h

@@ -435,7 +435,8 @@ enum gcs_severity {
 #define ERROR_SUBSYSTEM_FAILSAFE_RADIO      5
 #define ERROR_SUBSYSTEM_FAILSAFE_BATT       6
 #define ERROR_SUBSYSTEM_FAILSAFE_GPS        7
-#define ERROR_SUBSYSTEM_FAILSAFE_FENCE      8
+#define ERROR_SUBSYSTEM_FAILSAFE_GCS        8
+#define ERROR_SUBSYSTEM_FAILSAFE_FENCE      9
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1

ArduCopter/events.pde

@@ -171,6 +171,84 @@ static void failsafe_gps_off_event(void)
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GPS, ERROR_CODE_FAILSAFE_RESOLVED);
 }
 
+// failsafe_gcs_check - check for ground station failsafe
+static void failsafe_gcs_check()
+{
+    uint32_t last_gcs_update_ms;
+
+    // return immediately if gcs failsafe is disabled, gcs has never been connected or we are not overriding rc controls from the gcs
+    if( g.failsafe_gcs == FS_GCS_DISABLED || last_heartbeat_ms == 0 || !ap.rc_override_active) {
+        return;
+    }
+
+    // calc time since last gps update
+    last_gcs_update_ms = millis() - last_heartbeat_ms;
+
+    // check if all is well
+    if( last_gcs_update_ms < FS_GCS_TIMEOUT_MS) {
+        // check for recovery from gps failsafe
+        if( ap.failsafe_gcs ) {
+            failsafe_gcs_off_event();
+            set_failsafe_gcs(false);
+        }
+        return;
+    }
+
+    // do nothing if gps failsafe already triggered or motors disarmed
+    if( ap.failsafe_gcs || !motors.armed()) {
+        return;
+    }
+
+    // GCS failsafe event has occured
+    // update state, log to dataflash
+    set_failsafe_gcs(true);
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_OCCURRED);
+
+    // This is how to handle a failsafe.
+    // use the throttle failsafe setting to decide what to do
+    switch(control_mode) {
+        case STABILIZE:
+        case ACRO:
+            // if throttle is zero disarm motors
+            if (g.rc_3.control_in == 0) {
+                init_disarm_motors();
+            }else if(ap.home_is_set == true && home_distance > wp_nav.get_waypoint_radius()) {
+                set_mode(RTL);
+            }else{
+                // We have no GPS or are very close to home so we will land
+                set_mode(LAND);
+            }
+            break;
+        case AUTO:
+            // if g.failsafe_gcs is 1 do RTL, 2 means continue with the mission
+            if (g.failsafe_gcs == FS_GCS_ENABLED_ALWAYS_RTL) {
+                if(home_distance > wp_nav.get_waypoint_radius()) {
+                    set_mode(RTL);
+                }else{
+                    // We are very close to home so we will land
+                    set_mode(LAND);
+                }
+            }
+            // if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
+            break;
+        default:
+            if(ap.home_is_set == true && home_distance > wp_nav.get_waypoint_radius()) {
+                set_mode(RTL);
+            }else{
+                // We have no GPS or are very close to home so we will land
+                set_mode(LAND);
+            }
+            break;
+    }
+}
+
+// failsafe_gps_off_event - actions to take when GPS contact is restored
+static void failsafe_gcs_off_event(void)
+{
+    // log recovery of GPS in logs?
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
+}
+
 static void update_events()     // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
 {
     if(event_repeat == 0 || (millis() - event_timer) < event_delay)