fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ArmStateMachine: move arm state into the class

Browse Source
This commit is contained in:
Matthias Grob 2022-05-17 13:49:06 +02:00
parent 47532ca07b
commit 37c485ce89
4 changed files with 90 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
View File

@ -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::px4::enums::arming_state_t, events::px4::enums::arming_state_t>(
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;
}

18
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
View File

@ -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);
private:
static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state);
// 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 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

4
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
View File

@ -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;
}

40
src/modules/commander/Commander.cpp
View File

@ -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<int>(lroundf(cmd.param2)) == 21196);
const bool cmd_from_io = (static_cast<int>(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;