|
|
|
@ -131,7 +131,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
@@ -131,7 +131,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
|
|
|
|
|
// we always check for fence breach
|
|
|
|
|
if (geofence_breached || check_altlimit()) { |
|
|
|
|
if (!_terminate) { |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Fence TERMINATE"); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Fence TERMINATE"); |
|
|
|
|
_terminate.set(1); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -142,7 +142,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
@@ -142,7 +142,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
|
|
|
|
|
_rc_fail_time != 0 &&
|
|
|
|
|
(hal.scheduler->millis() - last_valid_rc_ms) > (unsigned)_rc_fail_time.get()) { |
|
|
|
|
if (!_terminate) { |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "RC failure terminate"); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "RC failure terminate"); |
|
|
|
|
_terminate.set(1); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -164,7 +164,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
@@ -164,7 +164,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
|
|
|
|
|
// we startup in preflight mode. This mode ends when
|
|
|
|
|
// we first enter auto.
|
|
|
|
|
if (mode == OBC_AUTO) { |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Starting AFS_AUTO"); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Starting AFS_AUTO"); |
|
|
|
|
_state = STATE_AUTO; |
|
|
|
|
} |
|
|
|
|
break; |
|
|
|
@ -172,7 +172,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
@@ -172,7 +172,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
|
|
|
|
|
case STATE_AUTO: |
|
|
|
|
// this is the normal mode.
|
|
|
|
|
if (!gcs_link_ok) { |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "State DATA_LINK_LOSS"); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "State DATA_LINK_LOSS"); |
|
|
|
|
_state = STATE_DATA_LINK_LOSS; |
|
|
|
|
if (_wp_comms_hold) { |
|
|
|
|
_saved_wp = mission.get_current_nav_cmd().index; |
|
|
|
@ -206,12 +206,12 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
@@ -206,12 +206,12 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
|
|
|
|
|
// losing GPS lock when data link is lost
|
|
|
|
|
// leads to termination
|
|
|
|
|
if (!_terminate) { |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Dual loss 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_CRITICAL, "GCS OK"); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GCS OK"); |
|
|
|
|
// we only return to the mission if we have not exceeded AFS_MAX_COM_LOSS
|
|
|
|
|
if (_saved_wp != 0 &&
|
|
|
|
|
(_max_comms_loss <= 0 ||
|
|
|
|
@ -231,7 +231,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
@@ -231,7 +231,7 @@ APM_OBC::check(APM_OBC::control_mode mode, uint32_t last_heartbeat_ms, bool geof
|
|
|
|
|
_terminate.set(1); |
|
|
|
|
} |
|
|
|
|
} else if (gps_lock_ok) { |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "GPS OK"); |
|
|
|
|
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "GPS OK"); |
|
|
|
|
_state = STATE_AUTO; |
|
|
|
|
// we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
|
|
|
|
|
if (_saved_wp != 0 && |
|
|
|
|