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();
if ((ap_fence != nullptr && ap_fence->get_breaches() != 0) || check_altlimit()) {
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);
}
}
@ -213,7 +213,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
(mode == AFS_MANUAL || mode == AFS_STABILIZED || !_rc_term_manual_only) &&
_rc_fail_time_seconds > 0 &&
(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);
}
@ -241,7 +241,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
// we startup in preflight mode. This mode ends when
// we first enter 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;
}
break;
@ -249,7 +249,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
case STATE_AUTO:
// this is the normal mode.
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;
if (_wp_comms_hold) {
_saved_wp = mission.get_current_nav_cmd().index;
@ -266,7 +266,7 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
break;
}
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;
if (_wp_gps_loss) {
_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
if(_enable_dual_loss) {
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);
}
}
} else if (gcs_link_ok) {
_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)) {
break;
@ -314,11 +314,11 @@ AP_AdvancedFailsafe::check(uint32_t last_valid_rc_ms)
// losing GCS link when GPS lock lost
// leads to termination if AFS_DUAL_LOSS is 1
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);
}
} 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;
// we only return to the mission if we have not exceeded AFS_MAX_GPS_LOSS
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
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) {
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;
}
@ -439,13 +439,13 @@ bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reaso
if(should_terminate == 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 {
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;
} 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;
}
@ -482,7 +482,7 @@ void AP_AdvancedFailsafe::max_range_update(void)
if (distance_km > _max_range_km) {
uint32_t now = AP_HAL::millis();
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;
}
_terminate.set_and_notify(1);