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

AC_PrecLand: use GCS_SEND_TEXT where possible

This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2024-02-15 16:26:16 +01:00 committed by Andrew Tridgell
parent 39e7e4bed1
commit 573436e81b
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -403,7 +403,7 @@ bool AC_PrecLand::target_acquired()
if ((AP_HAL::millis()-_last_update_ms) > LANDING_TARGET_TIMEOUT_MS) { if ((AP_HAL::millis()-_last_update_ms) > LANDING_TARGET_TIMEOUT_MS) {
if (_target_acquired) { if (_target_acquired) {
// just lost the landing target, inform the user. This message will only be sent once every time target is lost // just lost the landing target, inform the user. This message will only be sent once every time target is lost
gcs().send_text(MAV_SEVERITY_CRITICAL, "PrecLand: Target Lost"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PrecLand: Target Lost");
} }
// not had a sensor update since a long time // not had a sensor update since a long time
// probably lost the target // probably lost the target
@ -515,7 +515,7 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
// Update if a new Line-Of-Sight measurement is available // Update if a new Line-Of-Sight measurement is available
if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) {
if (!_estimator_initialized) { if (!_estimator_initialized) {
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Target Found"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Target Found");
_estimator_initialized = true; _estimator_initialized = true;
} }
_target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x; _target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x;
@ -548,7 +548,7 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f);
if (!_estimator_initialized) { if (!_estimator_initialized) {
// Inform the user landing target has been found // Inform the user landing target has been found
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Target Found"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Target Found");
// start init of EKF. We will let the filter consume the data for a while before it available for consumption // start init of EKF. We will let the filter consume the data for a while before it available for consumption
// reset filter state // reset filter state
if (_inertial_data_delayed->inertialNavVelocityValid) { if (_inertial_data_delayed->inertialNavVelocityValid) {
@ -604,11 +604,11 @@ void AC_PrecLand::check_ekf_init_timeout()
if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) { if (AP_HAL::millis()-_last_update_ms > EKF_INIT_SENSOR_MIN_UPDATE_MS) {
// we have lost the target, not enough readings to initialize the EKF // we have lost the target, not enough readings to initialize the EKF
_estimator_initialized = false; _estimator_initialized = false;
gcs().send_text(MAV_SEVERITY_CRITICAL, "PrecLand: Init Failed"); GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "PrecLand: Init Failed");
} else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) { } else if (AP_HAL::millis()-_estimator_init_ms > EKF_INIT_TIME_MS) {
// the target has been visible for a while, EKF should now have initialized to a good value // the target has been visible for a while, EKF should now have initialized to a good value
_target_acquired = true; _target_acquired = true;
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Init Complete"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Init Complete");
} }
} }
} }

2
libraries/AC_PrecLand/AC_PrecLand_Companion.cpp
View File

@ -47,7 +47,7 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u
//we do not support this frame //we do not support this frame
if (!_wrong_frame_msg_sent) { if (!_wrong_frame_msg_sent) {
_wrong_frame_msg_sent = true; _wrong_frame_msg_sent = true;
gcs().send_text(MAV_SEVERITY_INFO,"Plnd: Frame not supported "); GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Plnd: Frame not supported ");
} }
return; return;
} }

6
libraries/AC_PrecLand/AC_PrecLand_StateMachine.cpp
View File

@ -186,7 +186,7 @@ AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::retry_landing(Vector3
_retry_state = RetryLanding::IN_PROGRESS; _retry_state = RetryLanding::IN_PROGRESS;
// inform the user what we are doing // inform the user what we are doing
if (_retry_count <= _precland->get_max_retry_allowed()) { if (_retry_count <= _precland->get_max_retry_allowed()) {
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Retrying"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Retrying");
} }
retry_pos_m = go_to_pos; retry_pos_m = go_to_pos;
return Status::RETRYING; return Status::RETRYING;
@ -219,7 +219,7 @@ AC_PrecLand_StateMachine::Status AC_PrecLand_StateMachine::retry_landing(Vector3
if (fabsf(pos.z - retry_pos_m.z) < MAX_POS_ERROR_M) { if (fabsf(pos.z - retry_pos_m.z) < MAX_POS_ERROR_M) {
// we have descended to the original height where we started the climb from // we have descended to the original height where we started the climb from
_retry_state = RetryLanding::COMPLETE; _retry_state = RetryLanding::COMPLETE;
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Retry Completed"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Retry Completed");
} }
return Status::RETRYING; return Status::RETRYING;
} }
@ -249,7 +249,7 @@ AC_PrecLand_StateMachine::FailSafeAction AC_PrecLand_StateMachine::get_failsafe_
// start the timer // start the timer
failsafe_start_ms = AP_HAL::millis(); failsafe_start_ms = AP_HAL::millis();
failsafe_initialized = true; failsafe_initialized = true;
gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Failsafe Measures"); GCS_SEND_TEXT(MAV_SEVERITY_INFO, "PrecLand: Failsafe Measures");
} }
// Depending on the strictness we will either land vertically, wait for some time and then land vertically, not land at all // Depending on the strictness we will either land vertically, wait for some time and then land vertically, not land at all