fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 09:43:34 +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-20 13:51:51 +02:00
parent 47532ca07b
commit 37c485ce89
4 changed files with 90 additions and 83 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
+55 -56
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;
}
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
+15 -3
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
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
+2 -2
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;
}