diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 250a92cfb4..c9d5259f33 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1930,9 +1930,9 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { arming_state_changed = true; + } else { + print_reject_arm("NOT ARMING: Configuration error"); } - } else { - print_reject_arm("NOT ARMING: Configuration error"); } stick_on_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 2f3da424c6..c9c09ac300 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -93,6 +93,8 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; +static bool sensor_feedback_provided = false; + transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings @@ -241,10 +243,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && (!status->condition_system_sensors_initialized)) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); + if (!sensor_feedback_provided) { + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); + sensor_feedback_provided = true; + } feedback_provided = true; valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition @@ -255,6 +259,11 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s status->arming_state = new_arming_state; } + /* reset feedback state */ + if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + sensor_feedback_provided = false; + } + /* end of atomic state update */ #ifdef __PX4_NUTTX irqrestore(flags);