ArmStateMachine: replace state name array with method

This commit is contained in:
Matthias Grob 2022-05-16 18:25:26 +02:00
parent c5bbf4553b
commit 47532ca07b
5 changed files with 27 additions and 15 deletions

View File

@ -1,5 +1,3 @@
# If you change the order, add or remove arming_state_t states make sure to update the arrays
# in state_machine_helper.cpp as well.
uint64 timestamp # time since system start (microseconds)
uint8 ARMING_STATE_INIT = 0

View File

@ -60,6 +60,29 @@ events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(uint8_t arm
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,
@ -205,7 +228,7 @@ 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",
arming_state_names[status.arming_state], arming_state_names[new_arming_state]);
getArmingStateName(status.arming_state), getArmingStateName(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}",

View File

@ -52,6 +52,8 @@ public:
ArmStateMachine() = default;
~ArmStateMachine() = default;
static const char *getArmingStateName(uint8_t arming_state);
transition_result_t
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,
@ -59,16 +61,6 @@ 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);
// You can index into the array with an arming_state_t in order to get its textual representation
const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"INIT",
"STANDBY",
"ARMED",
"STANDBY_ERROR",
"SHUTDOWN",
"IN_AIR_RESTORE",
};
private:
static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state);

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.arming_state_names[_status.arming_state]);
PX4_INFO("arming: %s", _arm_state_machine.getArmingStateName(_status.arming_state));
PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]);
perf_print_counter(_loop_perf);
perf_print_counter(_preflight_check_perf);

View File

@ -100,7 +100,6 @@ enum class position_nav_loss_actions_t {
LAND_TERMINATE = 1, // Land/Terminate. Assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to TERMINATION.
};
extern const char *const arming_state_names[];
extern const char *const nav_state_names[];
enum RCLossExceptionBits {