diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 0d36707314..70b9adf46e 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -3,8 +3,7 @@ uint64 timestamp # time since system start (microseconds) bool calibration_enabled -bool system_sensors_initialized -bool system_hotplug_timeout # true if the hotplug sensor search is over +bool pre_flight_checks_pass # true if all checks necessary to arm pass bool auto_mission_available bool angular_velocity_valid bool attitude_valid diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index 70c06166bd..5d6bf330e1 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -60,43 +60,18 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s ret = TRANSITION_NOT_CHANGED; } else { - - /* - * Get sensing state if necessary - */ bool preflight_check_ret = true; /* only perform the pre-arm check if we have to */ if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !hil_enabled) { - preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, - true, true, time_since_boot); - - if (preflight_check_ret) { - status_flags.system_sensors_initialized = true; - } + preflight_check_ret = + PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, true, true, time_since_boot); feedback_provided = true; } - /* re-run the pre-flight check as long as sensors are failing */ - if (!status_flags.system_sensors_initialized - && fRunPreArmChecks - && ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY)) - && !hil_enabled) { - - if ((_last_preflight_check == 0) || (hrt_elapsed_time(&_last_preflight_check) > 1000 * 1000)) { - - status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status, - status_flags, control_mode, false, !isArmed(), - time_since_boot); - - _last_preflight_check = hrt_absolute_time(); - } - } - // Check that we have a valid state transition bool valid_transition = arming_transitions[new_arming_state][_arm_state]; @@ -129,7 +104,6 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if (hil_enabled) { /* enforce lockdown in HIL */ armed.lockdown = true; - status_flags.system_sensors_initialized = true; /* recover from a prearm fail */ if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { @@ -142,17 +116,6 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s } } - if (!hil_enabled && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (_arm_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { - - // Sensors need to be initialized for STANDBY state, except for HIL - if (!status_flags.system_sensors_initialized) { - feedback_provided = true; - valid_transition = false; - } - } - // Finish up the state transition if (valid_transition) { ret = TRANSITION_CHANGED; diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 217367f9d1..127120a3a6 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -75,7 +75,6 @@ private: static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state); uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT}; - hrt_abstime _last_preflight_check = 0; ///< initialize so it gets checked immediately // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index 9511d1276e..2c082d8546 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -53,8 +53,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) const char *assertMsg; // Text to show when test case fails ArmingTransitionVolatileState_t current_state; // Machine state prior to transition hil_state_t hil_state; // Current vehicle_status_s.hil_state - bool - system_sensors_initialized; // Current vehicle_status_s.system_sensors_initialized bool safety_button_available; // Current safety_s.safety_button_available bool safety_off; // Current safety_s.safety_off arming_state_t requested_state; // Requested arming state to transition to @@ -65,8 +63,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // We use these defines so that our test cases are more readable #define ATT_ARMED true #define ATT_DISARMED false -#define ATT_SENSORS_INITIALIZED true -#define ATT_SENSORS_NOT_INITIALIZED false #define ATT_SAFETY_AVAILABLE true #define ATT_SAFETY_NOT_AVAILABLE true #define ATT_SAFETY_OFF true @@ -78,7 +74,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "no transition: identical states", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_INIT, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED }, @@ -89,70 +85,70 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "transition: init to standby", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: init to standby error", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: init to reboot", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby to init", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_INIT, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby to standby error", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY_ERROR, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: armed to standby", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: standby error to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, { "transition: in air restore to armed", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED }, { "transition: in air restore to reboot", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED }, @@ -161,7 +157,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "transition: standby error to standby, hil on", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED }, @@ -170,14 +166,14 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "transition: standby to armed, no safety button", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED }, { "transition: standby to armed, safety button off", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED }, @@ -188,65 +184,58 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) { "no transition: init to armed", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: armed to init", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_INIT, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED }, { "no transition: armed to reboot", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_SHUTDOWN, { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED }, { "no transition: standby error to armed", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: standby error to standby", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: reboot to armed", - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED }, { "no transition: in air restore to standby", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_STANDBY, { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED }, - // Sensor tests - - //{ "transition to standby error: init to standby - sensors not initialized", - // { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - // vehicle_status_s::ARMING_STATE_STANDBY, - // { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - - // safety button arming tests + // Safety button arming tests { "no transition: init to armed, safety button on", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, vehicle_status_s::ARMING_STATE_ARMED, { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED }, @@ -265,7 +254,6 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // Setup initial machine state arm_state_machine.forceArmState(test->current_state.arming_state); - status_flags.system_sensors_initialized = test->system_sensors_initialized; status.hil_state = test->hil_state; // The power status of the test unit is not relevant for the unit test status_flags.circuit_breaker_engaged_power_check = true; diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index a36d1b7581..eaa063392d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -54,8 +54,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, bool report_failures, const bool prearm, const hrt_abstime &time_since_boot) { - report_failures = (report_failures && status_flags.system_hotplug_timeout - && !status_flags.calibration_enabled); + report_failures = (report_failures && !status_flags.calibration_enabled); bool failed = false; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index d51ea0aeb2..77a346bd7f 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -829,9 +829,6 @@ Commander::Commander() : _vehicle_status.system_type = 0; _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; - // XXX for now just set sensors as initialized - _vehicle_status_flags.system_sensors_initialized = true; - // We want to accept RC inputs as default _vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; _vehicle_status.nav_state_timestamp = hrt_absolute_time(); @@ -2201,7 +2198,6 @@ Commander::run() perf_begin(_loop_perf); const actuator_armed_s actuator_armed_prev{_actuator_armed}; - const vehicle_status_flags_s vehicle_status_flags_prev{_vehicle_status_flags}; /* update parameters */ const bool params_updated = _parameter_update_sub.updated(); @@ -3007,9 +3003,6 @@ Commander::run() // Evaluate current prearm status (skip during arm -> disarm transition) if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) { - - _vehicle_status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); - perf_begin(_preflight_check_perf); bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode, @@ -3023,8 +3016,9 @@ Commander::run() _safety.isButtonAvailable(), _safety.isSafetyOff(), arm_req, _vehicle_status, false); - const bool prearm_check_ok = preflight_check_res && prearm_check_res; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, prearm_check_ok, _vehicle_status); + _vehicle_status_flags.pre_flight_checks_pass = preflight_check_res && prearm_check_res; + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, + _vehicle_status_flags.pre_flight_checks_pass, _vehicle_status); } // publish actuator_armed first (used by output modules) @@ -3101,13 +3095,6 @@ Commander::run() _arm_tune_played = false; } - /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ - if (!_vehicle_status_flags.system_sensors_initialized && - !vehicle_status_flags_prev.system_hotplug_timeout && _vehicle_status_flags.system_hotplug_timeout) { - - set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING); - } - // check if the worker has finished if (_worker_thread.hasResult()) { int ret = _worker_thread.getResultAndReset(); @@ -3229,7 +3216,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_ON; set_normal_color = true; - } else if (!_vehicle_status_flags.system_sensors_initialized && _vehicle_status_flags.system_hotplug_timeout) { + } else if (!_vehicle_status_flags.pre_flight_checks_pass) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; @@ -3237,10 +3224,6 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - } else if (!_vehicle_status_flags.system_sensors_initialized && !_vehicle_status_flags.system_hotplug_timeout) { - led_mode = led_control_s::MODE_BREATHE; - set_normal_color = true; - } else if (_arm_state_machine.isInit()) { // if in init status it should not be in the error state led_mode = led_control_s::MODE_OFF; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 4278c0aa01..2bd5a9b5be 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -292,8 +292,6 @@ private: /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; - - static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s}; /**< wait for hotplug sensors to come online for upto 8 seconds */ static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; ArmStateMachine _arm_state_machine{};