diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index 2baaffdff9..d7dfccc45e 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -38,51 +38,6 @@ constexpr bool ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]; -events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(uint8_t arming_state) -{ - switch (arming_state) { - case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init; - - case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby; - - case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore; - } - - static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore, - "enum def mismatch"); - - return events::px4::enums::arming_state_t::init; -} - -const char *ArmStateMachine::getArmingStateName(uint8_t arming_state) -{ - switch (arming_state) { - - case vehicle_status_s::ARMING_STATE_INIT: return "Init"; - - case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby"; - - case vehicle_status_s::ARMING_STATE_ARMED: return "Armed"; - - case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error"; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown"; - - case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore"; - - default: return "Unknown"; - } - - static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, - "enum def mismatch"); -} - transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks, @@ -96,13 +51,12 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s "ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1"); transition_result_t ret = TRANSITION_DENIED; - arming_state_t current_arming_state = status.arming_state; bool feedback_provided = false; const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON); /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_arming_state) { + if (new_arming_state == _arm_state) { ret = TRANSITION_NOT_CHANGED; } else { @@ -136,7 +90,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s 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, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, + status_flags, control_mode, false, !isArmed(), time_since_boot); _last_preflight_check = hrt_absolute_time(); @@ -144,7 +98,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s } // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status.arming_state]; + bool valid_transition = arming_transitions[new_arming_state][_arm_state]; if (valid_transition) { // We have a good transition. Now perform any secondary validation. @@ -152,7 +106,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s // Do not perform pre-arm checks if coming from in air restore // Allow if vehicle_status_s::HIL_STATE_ON - if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) { + if (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) { bool prearm_check_ret = true; @@ -177,8 +131,8 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s status_flags.system_sensors_initialized = true; /* recover from a prearm fail */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY; + if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { + _arm_state = vehicle_status_s::ARMING_STATE_STANDBY; } // HIL can always go to standby @@ -189,7 +143,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if (!hil_enabled && (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { + (_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) { @@ -205,7 +159,7 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) || (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY); ret = TRANSITION_CHANGED; - status.arming_state = new_arming_state; + _arm_state = new_arming_state; if (was_armed && !armed.armed) { // disarm transition status.latest_disarming_reason = (uint8_t)calling_reason; @@ -228,13 +182,58 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s if (!feedback_provided) { // FIXME: this catch-all does not provide helpful information to the user mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t", - getArmingStateName(status.arming_state), getArmingStateName(new_arming_state)); + getArmStateName(_arm_state), getArmStateName(new_arming_state)); events::send( events::ID("commander_transition_denied"), events::Log::Critical, "Arming state transition denied: {1} to {2}", - eventArmingState(status.arming_state), eventArmingState(new_arming_state)); + getArmStateEvent(_arm_state), getArmStateEvent(new_arming_state)); } } return ret; } + +const char *ArmStateMachine::getArmStateName(uint8_t arming_state) +{ + switch (arming_state) { + + case vehicle_status_s::ARMING_STATE_INIT: return "Init"; + + case vehicle_status_s::ARMING_STATE_STANDBY: return "Standby"; + + case vehicle_status_s::ARMING_STATE_ARMED: return "Armed"; + + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return "Standby error"; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: return "Shutdown"; + + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return "In-air restore"; + + default: return "Unknown"; + } + + static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, + "enum def mismatch"); +} + +events::px4::enums::arming_state_t ArmStateMachine::getArmStateEvent(uint8_t arming_state) +{ + switch (arming_state) { + case vehicle_status_s::ARMING_STATE_INIT: return events::px4::enums::arming_state_t::init; + + case vehicle_status_s::ARMING_STATE_STANDBY: return events::px4::enums::arming_state_t::standby; + + case vehicle_status_s::ARMING_STATE_ARMED: return events::px4::enums::arming_state_t::armed; + + case vehicle_status_s::ARMING_STATE_STANDBY_ERROR: return events::px4::enums::arming_state_t::standby_error; + + case vehicle_status_s::ARMING_STATE_SHUTDOWN: return events::px4::enums::arming_state_t::shutdown; + + case vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE: return events::px4::enums::arming_state_t::inair_restore; + } + + static_assert(vehicle_status_s::ARMING_STATE_MAX - 1 == (int)events::px4::enums::arming_state_t::inair_restore, + "enum def mismatch"); + + return events::px4::enums::arming_state_t::init; +} diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp index 43e59f25d0..999bcd864a 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -52,7 +52,7 @@ public: ArmStateMachine() = default; ~ArmStateMachine() = default; - static const char *getArmingStateName(uint8_t arming_state); + void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } transition_result_t arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, @@ -61,9 +61,21 @@ public: vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements, const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason); + // Getters + uint8_t getArmState() const { return _arm_state; } + + bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); } + bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); } + bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); } + bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); } + + static const char *getArmStateName(uint8_t arming_state); + const char *getArmStateName() const { return getArmStateName(_arm_state); } + private: - static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state); + 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 diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp index a1251efe21..2527fec810 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp @@ -268,7 +268,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) PreFlightCheck::arm_requirements_t arm_req{}; // Setup initial machine state - status.arming_state = test->current_state.arming_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 @@ -297,7 +297,7 @@ TEST(ArmStateMachineTest, ArmingStateTransitionTest) // Validate result of transition EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg; - EXPECT_EQ(status.arming_state, test->expected_state.arming_state) << test->assertMsg; + EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg; EXPECT_EQ(armed.armed, test->expected_state.armed) << test->assertMsg; EXPECT_EQ(armed.ready_to_arm, test->expected_state.ready_to_arm) << test->assertMsg; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f63ceff0f4..507eca31ca 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -479,7 +479,7 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { - PX4_INFO("arming: %s", _arm_state_machine.getArmingStateName(_status.arming_state)); + PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]); perf_print_counter(_loop_perf); perf_print_counter(_preflight_check_perf); @@ -829,7 +829,6 @@ Commander::Commander() : // We want to accept RC inputs as default _status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; _status.nav_state_timestamp = hrt_absolute_time(); - _status.arming_state = vehicle_status_s::ARMING_STATE_INIT; /* mark all signals lost as long as they haven't been found */ _status.rc_signal_lost = true; @@ -1027,12 +1026,13 @@ Commander::handle_command(const vehicle_command_s &cmd) const bool forced = (static_cast(lroundf(cmd.param2)) == 21196); const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); - if (!forced) { - // Flick to in-air restore first if this comes from an onboard system and from IO - if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id - && cmd_from_io && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) { - _status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE; - } + // Flick to in-air restore first if this comes from an onboard system and from IO + if (!forced && cmd_from_io + && (cmd.source_system == _status.system_id) + && (cmd.source_component == _status.component_id) + && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) { + // TODO: replace with a proper allowed transition + _arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE); } transition_result_t arming_res = TRANSITION_DENIED; @@ -1386,8 +1386,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN || _worker_thread.isBusy()) { + if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1506,9 +1505,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: { // Magnetometer quick calibration using world magnetic model and known heading - if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || (_status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN) - || _worker_thread.isBusy()) { + if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1543,9 +1540,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { - if ((_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) - || _status.arming_state == vehicle_status_s::ARMING_STATE_SHUTDOWN - || _worker_thread.isBusy()) { + if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -2455,7 +2450,7 @@ Commander::run() } /* If in INIT state, try to proceed to STANDBY state */ - if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) { _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, @@ -3099,6 +3094,7 @@ Commander::run() update_control_mode(); // vehicle_status publish (after prearm/preflight updates above) + _status.arming_state = _arm_state_machine.getArmState(); _status.timestamp = hrt_absolute_time(); _status_pub.publish(_status); @@ -3276,14 +3272,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) uint8_t led_color = led_control_s::COLOR_WHITE; bool set_normal_color = false; - uint64_t overload_warn_delay = (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) ? 1_ms : 250_ms; + uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms; // set mode if (overload && (time_now_us >= _overload_start + overload_warn_delay)) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_PURPLE; - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + } else if (_arm_state_machine.isArmed()) { led_mode = led_control_s::MODE_ON; set_normal_color = true; @@ -3291,7 +3287,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; - } else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + } else if (_arm_state_machine.isStandby()) { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; @@ -3299,7 +3295,7 @@ 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 (_status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + } 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; @@ -4166,7 +4162,7 @@ void Commander::estimator_check() if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) { + if (!_arm_state_machine.isArmed()) { _nav_test_failed = false; _nav_test_passed = false;