From 48a7363608b4cb0bbbcf0a3be8bbe458c7c0e0c1 Mon Sep 17 00:00:00 2001 From: James Stoyell Date: Mon, 21 Mar 2016 19:44:48 +0000 Subject: [PATCH] APM_OBC: Added params for AUVSI student competition --- libraries/APM_OBC/APM_OBC.cpp | 73 +++++++++++++++++++++++++---------- libraries/APM_OBC/APM_OBC.h | 4 ++ 2 files changed, 57 insertions(+), 20 deletions(-) diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp index 4d3d105065..9666960fde 100644 --- a/libraries/APM_OBC/APM_OBC.cpp +++ b/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,21 +153,26 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof return; } // we always check for fence breach - if (geofence_breached || check_altlimit()) { - if (!_terminate) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Fence TERMINATE"); - _terminate.set(1); + 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 - if (_state != STATE_PREFLIGHT && - (mode == OBC_MANUAL || mode == OBC_FBW) && - _rc_fail_time != 0 && - (AP_HAL::millis() - last_valid_rc_ms) > (unsigned)_rc_fail_time.get()) { - if (!_terminate) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate"); - _terminate.set(1); + // 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 || !_rc_term_manual_only) && + _rc_fail_time != 0 && + (AP_HAL::millis() - last_valid_rc_ms) > (unsigned)_rc_fail_time.get()) { + if (!_terminate) { + GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate"); + _terminate.set(1); + } } } @@ -204,10 +233,12 @@ 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 - if (!_terminate) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Dual loss TERMINATE"); - _terminate.set(1); + // 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; @@ -225,10 +256,12 @@ 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 - if (!_terminate) { - GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Dual loss TERMINATE"); - _terminate.set(1); + // 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"); diff --git a/libraries/APM_OBC/APM_OBC.h b/libraries/APM_OBC/APM_OBC.h index ad2f3b919d..05ae3fb970 100644 --- a/libraries/APM_OBC/APM_OBC.h +++ b/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;