Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 16:48:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

APM_OBC: Added params for AUVSI student competition

Browse Source
This commit is contained in:
James Stoyell 2016-03-21 19:44:48 +00:00 committed by Tom Pittenger
parent af6d8e3c36
commit 48a7363608
2 changed files with 57 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
libraries/APM_OBC/APM_OBC.cpp
View File

@ -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");

4
libraries/APM_OBC/APM_OBC.h
View File

@ -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;