APM_OBC: Added params for AUVSI student competition

libraries/APM_OBC/APM_OBC.cpp

@@ -117,6 +117,30 @@ const AP_Param::GroupInfo APM_OBC::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("MAX_COM_LOSS", 14, APM_OBC, _max_comms_loss, 0),
 
+    // @Param: GEOFENCE
+    // @DisplayName: Enable geofence Advanced Failsafe
+    // @Description: This enables the geofence part of the AFS. Will only be in effect if AFS_ENABLE is also 1
+    // @User: Advanced
+    AP_GROUPINFO("GEOFENCE",     15, APM_OBC, _enable_geofence_fs, 1),
+
+    // @Param: RC
+    // @DisplayName: Enable RC Advanced Failsafe
+    // @Description: This enables the RC part of the AFS. Will only be in effect if AFS_ENABLE is also 1
+    // @User: Advanced
+    AP_GROUPINFO("RC",           16, APM_OBC, _enable_RC_fs, 1),
+
+    // @Param: RC_MAN_ONLY
+    // @DisplayName: Enable RC Termination only in manual control modes
+    // @Description: If this parameter is set to 1, then an RC loss will only cause the plane to terminate in manual control modes. If it is 0, then the plane will terminate in any flight mode.
+    // @User: Advanced
+    AP_GROUPINFO("RC_MAN_ONLY",    17, APM_OBC, _rc_term_manual_only, 1),
+
+    // @Param: DUAL_LOSS
+    // @DisplayName: Enable dual loss terminate due to failure of both GCS and GPS simultaneously
+    // @Description: This enables the dual loss termination part of the AFS system. If this parameter is 1 and both GPS and the ground control station fail simultaneously, this will be considered a "dual loss" and cause termination.
+    // @User: Advanced
+    AP_GROUPINFO("DUAL_LOSS",      18, APM_OBC, _enable_dual_loss, 1),
+
     AP_GROUPEND
 };
 
@@ -129,16 +153,20 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
         return;
     }
     // we always check for fence breach
+    if(_enable_geofence_fs) {
         if (geofence_breached || check_altlimit()) {
             if (!_terminate) {
                 GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Fence TERMINATE");
                 _terminate.set(1);
             }
         }
+    }
 
-    // check for RC failure in manual mode
+    // check if RC termination is enabled
+    if(_enable_RC_fs) {
+        // check for RC failure in manual mode or RC failure when AFS_RC_MANUAL is 0
         if (_state != STATE_PREFLIGHT && 
-        (mode == OBC_MANUAL || mode == OBC_FBW) && 
+            (mode == OBC_MANUAL || mode == OBC_FBW || !_rc_term_manual_only) && 
             _rc_fail_time != 0 && 
             (AP_HAL::millis() - last_valid_rc_ms) > (unsigned)_rc_fail_time.get()) {
             if (!_terminate) {
@@ -146,6 +174,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
                 _terminate.set(1);
             }
         }
+    }
     
     // tell the failsafe board if we are in manual control
     // mode. This tells it to pass through controls from the
@@ -204,11 +233,13 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
     case STATE_DATA_LINK_LOSS:
         if (!gps_lock_ok) {
             // losing GPS lock when data link is lost
-            // leads to termination
+            // leads to termination if AFS_DUAL_LOSS is 1
+            if(_enable_dual_loss) {
                 if (!_terminate) {
                     GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Dual loss TERMINATE");
                     _terminate.set(1);
                 }
+            }
         } else if (gcs_link_ok) {
             _state = STATE_AUTO;
             GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GCS OK");
@@ -225,11 +256,13 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
     case STATE_GPS_LOSS:
         if (!gcs_link_ok) {
             // losing GCS link when GPS lock lost
-            // leads to termination
+            // leads to termination if AFS_DUAL_LOSS is 1
+            if(_enable_dual_loss) {
                 if (!_terminate) {
                     GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Dual loss TERMINATE");
                     _terminate.set(1);
                 }
+            }
         } else if (gps_lock_ok) {
             GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS OK");
             _state = STATE_AUTO;

libraries/APM_OBC/APM_OBC.h

@@ -102,6 +102,10 @@ private:
     AP_Int16 _rc_fail_time;
     AP_Int8  _max_gps_loss;
     AP_Int8  _max_comms_loss;
+    AP_Int8  _enable_geofence_fs;
+    AP_Int8  _enable_RC_fs;
+    AP_Int8  _rc_term_manual_only;
+    AP_Int8  _enable_dual_loss;
 
     bool _heartbeat_pin_value;