Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

Commander: remove system_sensors_initialized

Browse Source 
and system_hotplug_timeout. They're not in use for over 2 years.
Instead control LED with preflight checks.
This commit is contained in:
Matthias Grob
2022-06-03 12:37:08 +02:00
committed by Daniel Agar
parent 0922f003f5
commit c59809b14a
7 changed files with 31 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/vehicle_status_flags.msg
+1 -2
View File
@@ -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
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
+2 -39
View File
@@ -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;
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
-1
View File
@@ -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
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
+23 -35
View File
@@ -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;
src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+1 -2
View File
@@ -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;
src/modules/commander/Commander.cpp
+4 -21
View File
@@ -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;
src/modules/commander/Commander.hpp
-2
View File
@@ -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{};