diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d416358eeb..e89626300e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2195,9 +2195,9 @@ int commander_thread_main(int argc, char *argv[]) /* report a regain */ if (telemetry_last_dl_loss[i] > 0) { mavlink_and_console_log_info(mavlink_fd, "data link #%i regained", i); - } else if (telemetry_last_dl_loss[i] == 0){ - /* do not report a new data link in order to not spam the user */ - status.data_link_found_new = true; + } else if (telemetry_last_dl_loss[i] == 0) { + /* new link */ + status.condition_system_prearm_error_reported = false; status_changed = true; } @@ -2208,10 +2208,6 @@ int commander_thread_main(int argc, char *argv[]) /* telemetry was healthy also in last iteration * we don't have to check a timeout */ have_link = true; - if(status.data_link_found_new) { - status.data_link_found_new = false; - status_changed = true; - } } } else { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 295ea3dcd2..c2f7126e85 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -123,6 +123,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, true /* pre-arm */ ); } /* re-run the pre-flight check as long as sensors are failing */ @@ -130,6 +131,7 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); status->condition_system_sensors_initialized = !prearm_ret; } @@ -272,10 +274,6 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition) { status->condition_system_prearm_error_reported = false; } - if(status->data_link_found_new == true) - { - status->condition_system_prearm_error_reported = false; - } /* end of atomic state update */ #ifdef __PX4_NUTTX @@ -743,7 +741,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en return status->nav_state != nav_state_old; } -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report) { /* */ @@ -761,7 +759,14 @@ int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, if (!status->cb_usb && status->usb_connected && prearm) { preflight_ok = false; - if(reportFailures) mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + if (reportFailures) { + mavlink_and_console_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + } + + /* report once, then set the flag */ + if (mavlink_fd >= 0 && reportFailures && !preflight_ok) { + status->condition_system_prearm_error_reported = true; } return !preflight_ok; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 19b9394e3d..7c209cf07f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); -int preflight_check(const struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); +int preflight_check(struct vehicle_status_s *status, const int mavlink_fd, bool prearm, bool force_report=false); #endif /* STATE_MACHINE_HELPER_H_ */