Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_AdvancedFailsafe: use GCS_SEND_TEXT rather than gcs().send_text 

Co-authored-by: muramura <ma2maru@gmail.com>
This commit is contained in:
Peter Barker 2024-08-07 13:17:19 +10:00 committed by Andrew Tridgell
parent 5db0eecb99
commit edf42f799c
1 changed files with 14 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp
View File

@ -195,7 +195,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
const AC_Fence *ap_fence = AP::fence(); const AC_Fence *ap_fence = AP::fence();
if ((ap_fence != nullptr && ap_fence->get_breaches() != 0) || check_altlimit()) { if ((ap_fence != nullptr && ap_fence->get_breaches() != 0) || check_altlimit()) {
if (!_terminate) { if (!_terminate) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to fence breach"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to fence breach");
_terminate.set_and_notify(1); _terminate.set_and_notify(1);
} }
} }
@ -213,7 +213,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
(mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) && (mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 && _rc_fail_time_seconds > 0 &&
(AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) { (AP_HAL::millis() - last_valid_rc_ms) > (_rc_fail_time_seconds * 1000.0f)) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to RC failure"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to RC failure");
_terminate.set_and_notify(1); _terminate.set_and_notify(1);
} }
@ -241,7 +241,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
// we startup in preflight mode. This mode ends when // we startup in preflight mode. This mode ends when
// we first enter auto. // we first enter auto.
if (mode == AFS_AUTO) { if (mode == AFS_AUTO) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO");
_state = STATE_AUTO; _state = STATE_AUTO;
} }
break; break;
@ -249,7 +249,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
case STATE_AUTO: case STATE_AUTO:
// this is the normal mode. // this is the normal mode.
if (!gcs_link_ok) { if (!gcs_link_ok) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: DATA_LINK_LOSS"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: DATA_LINK_LOSS");
_state = STATE_DATA_LINK_LOSS; _state = STATE_DATA_LINK_LOSS;
if (_wp_comms_hold) { if (_wp_comms_hold) {
_saved_wp = mission.get_current_nav_cmd().index; _saved_wp = mission.get_current_nav_cmd().index;
@ -266,7 +266,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
break; break;
} }
if (!gps_lock_ok) { if (!gps_lock_ok) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: GPS_LOSS"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: GPS_LOSS");
_state = STATE_GPS_LOSS; _state = STATE_GPS_LOSS;
if (_wp_gps_loss) { if (_wp_gps_loss) {
_saved_wp = mission.get_current_nav_cmd().index; _saved_wp = mission.get_current_nav_cmd().index;
@ -287,13 +287,13 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
// leads to termination if AFS_DUAL_LOSS is 1 // leads to termination if AFS_DUAL_LOSS is 1
if(_enable_dual_loss) { if(_enable_dual_loss) {
if (!_terminate) { if (!_terminate) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss");
_terminate.set_and_notify(1); _terminate.set_and_notify(1);
} }
} }
} else if (gcs_link_ok) { } else if (gcs_link_ok) {
_state = STATE_AUTO; _state = STATE_AUTO;
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GCS now OK"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GCS now OK");
if (option_is_set(Option::CONTINUE_AFTER_RECOVERED)) { if (option_is_set(Option::CONTINUE_AFTER_RECOVERED)) {
break; break;
@ -314,11 +314,11 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
// losing GCS link when GPS lock lost // losing GCS link when GPS lock lost
// leads to termination if AFS_DUAL_LOSS is 1 // leads to termination if AFS_DUAL_LOSS is 1
if (!_terminate && _enable_dual_loss) { if (!_terminate && _enable_dual_loss) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to dual loss");
_terminate.set_and_notify(1); _terminate.set_and_notify(1);
} }
} else if (gps_lock_ok) { } else if (gps_lock_ok) {
gcs().send_text(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GPS now OK"); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "AFS State: AFS_AUTO, GPS now OK");
_state = STATE_AUTO; _state = STATE_AUTO;
// we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS // we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
if (_saved_wp != 0 && if (_saved_wp != 0 &&
@ -428,7 +428,7 @@ bool AP_AdvancedFailsafe::should_crash_vehicle(void)
// returns true if AFS is in the desired termination state // returns true if AFS is in the desired termination state
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {
if (!_enable) { if (!_enable) {
gcs().send_text(MAV_SEVERITY_INFO, "AFS not enabled, can't terminate the vehicle"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "AFS not enabled, can't terminate the vehicle");
return false; return false;
} }
@ -439,13 +439,13 @@ bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reaso
if(should_terminate == is_terminating) { if(should_terminate == is_terminating) {
if (is_terminating) { if (is_terminating) {
gcs().send_text(MAV_SEVERITY_INFO, "Terminating due to %s", reason); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Terminating due to %s", reason);
} else { } else {
gcs().send_text(MAV_SEVERITY_INFO, "Aborting termination due to %s", reason); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Aborting termination due to %s", reason);
} }
return true; return true;
} else if (should_terminate && _terminate_action != TERMINATE_ACTION_TERMINATE) { } else if (should_terminate && _terminate_action != TERMINATE_ACTION_TERMINATE) {
gcs().send_text(MAV_SEVERITY_INFO, "Unable to terminate, termination is not configured"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unable to terminate, termination is not configured");
} }
return false; return false;
} }
@ -482,7 +482,7 @@ void AP_AdvancedFailsafe::max_range_update(void)
if (distance_km > _max_range_km) { if (distance_km > _max_range_km) {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - _term_range_notice_ms > 5000) { if (now - _term_range_notice_ms > 5000) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Terminating due to range %.1fkm", distance_km); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Terminating due to range %.1fkm", distance_km);
_term_range_notice_ms = now; _term_range_notice_ms = now;
} }
_terminate.set_and_notify(1); _terminate.set_and_notify(1);