From 074080c8167c4d280d28d9b375f7d89750352887 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 31 Mar 2022 12:00:42 +0200 Subject: [PATCH] Commander: separate out arm state machine to class Pure refactoring and just the first step to avoid conflicts on the way. --- .../ArmStateMachine/ArmStateMachine.cpp | 217 ++++++++++++++++++ .../ArmStateMachine/ArmStateMachine.hpp | 82 +++++++ .../Arming/ArmStateMachine/CMakeLists.txt | 39 ++++ src/modules/commander/Arming/CMakeLists.txt | 3 +- src/modules/commander/CMakeLists.txt | 1 + src/modules/commander/Commander.cpp | 17 +- src/modules/commander/Commander.hpp | 2 + .../state_machine_helper_test.cpp | 22 +- .../commander/state_machine_helper.cpp | 196 ---------------- src/modules/commander/state_machine_helper.h | 12 - 10 files changed, 367 insertions(+), 224 deletions(-) create mode 100644 src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp create mode 100644 src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp create mode 100644 src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp new file mode 100644 index 0000000000..593101fb57 --- /dev/null +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ArmStateMachine.hpp" + +#include + +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; +} + +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, + orb_advert_t *mavlink_log_pub, 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) +{ + // Double check that our static arrays are still valid + static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); + static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1, + "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) { + 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; + } + + 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, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, + 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][status.arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + + // 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) { + + bool prearm_check_ret = true; + + if (fRunPreArmChecks && preflight_check_ret) { + // only bother running prearm if preflight was successful + prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements, + status); + } + + if (!preflight_check_ret || !prearm_check_ret) { + // the prearm and preflight checks already print the rejection reason + feedback_provided = true; + valid_transition = false; + } + } + } + } + + if (hil_enabled) { + /* enforce lockdown in HIL */ + armed.lockdown = true; + 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; + } + + // HIL can always go to standby + if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { + valid_transition = true; + } + } + + if (!hil_enabled && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status.arming_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) { + bool was_armed = armed.armed; + armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); + 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; + + if (was_armed && !armed.armed) { // disarm transition + status.latest_disarming_reason = (uint8_t)calling_reason; + + } else if (!was_armed && armed.armed) { // arm transition + status.latest_arming_reason = (uint8_t)calling_reason; + } + + if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + status.armed_time = hrt_absolute_time(); + + } else { + status.armed_time = 0; + } + } + } + + if (ret == TRANSITION_DENIED) { + /* print to MAVLink and console if we didn't provide any feedback yet */ + 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]); + 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)); + } + } + + return ret; +} diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp new file mode 100644 index 0000000000..d6add016f6 --- /dev/null +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "../PreFlightCheck/PreFlightCheck.hpp" +#include +#include +#include +#include +#include +#include +#include + +#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t + +using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; + +class ArmStateMachine +{ +public: + ArmStateMachine() = default; + ~ArmStateMachine() = default; + + 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, + actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, + 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); + + 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 + // will be true for a valid transition or false for a invalid transition. In some cases even + // though the transition is marked as true additional checks must be made. See arming_state_transition + // code for those checks. + static constexpr bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] + = { + // INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE + { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false }, + { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true }, + { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false }, + { /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true }, + { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI + }; +}; diff --git a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt new file mode 100644 index 0000000000..438093be2b --- /dev/null +++ b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2022 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + + +px4_add_library(ArmStateMachine + ArmStateMachine.cpp +) +target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_link_libraries(ArmStateMachine PUBLIC PreFlightCheck) diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt index 1d638ad3dd..ab21b1a479 100644 --- a/src/modules/commander/Arming/CMakeLists.txt +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -31,6 +31,7 @@ # ############################################################################ -add_subdirectory(PreFlightCheck) add_subdirectory(ArmAuthorization) +add_subdirectory(ArmStateMachine) add_subdirectory(HealthFlags) +add_subdirectory(PreFlightCheck) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index c6fcb4e5bb..538a79f93f 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -62,6 +62,7 @@ px4_add_module( hysteresis PreFlightCheck ArmAuthorization + ArmStateMachine HealthFlags sensor_calibration world_magnetic_model diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 61632f6ba0..e486fa0953 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -479,7 +479,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) bool Commander::shutdown_if_allowed() { - return TRANSITION_DENIED != arming_state_transition(_status, _vehicle_control_mode, _safety, + return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, _armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown); @@ -732,7 +732,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } } - transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety, + transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks, &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), calling_reason); @@ -774,7 +774,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f } } - transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety, + transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, false, &_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp), calling_reason); @@ -1368,7 +1368,7 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(_status, _vehicle_control_mode, safety_s{}, + if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, _armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT @@ -2375,10 +2375,11 @@ 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) { - arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, - true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, - _arm_requirements, hrt_elapsed_time(&_boot_timestamp), - arm_disarm_reason_t::transition_to_standby); + _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, + vehicle_status_s::ARMING_STATE_STANDBY, _armed, + true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, + _arm_requirements, hrt_elapsed_time(&_boot_timestamp), + arm_disarm_reason_t::transition_to_standby); } /* start mission result check */ diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 2382b982be..a71ac6ba70 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -34,6 +34,7 @@ #pragma once /* Helper classes */ +#include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" #include "Safety.hpp" @@ -294,6 +295,7 @@ private: 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{}; PreFlightCheck::arm_requirements_t _arm_requirements{}; hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */ diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 74e931b6f6..1d44b4adee 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -42,6 +42,7 @@ #include "../state_machine_helper.h" #include +#include "../Arming/ArmStateMachine/ArmStateMachine.hpp" #include "../Arming/PreFlightCheck/PreFlightCheck.hpp" class StateMachineHelperTest : public UnitTest @@ -50,6 +51,8 @@ public: StateMachineHelperTest() = default; ~StateMachineHelperTest() override = default; + ArmStateMachine _arm_state_machine{}; + bool run_tests() override; private: @@ -303,13 +306,18 @@ bool StateMachineHelperTest::armingStateTransitionTest() vehicle_control_mode_s control_mode{}; // Attempt transition - transition_result_t result = arming_state_transition(status, control_mode, safety, test->requested_state, armed, - true /* enable pre-arm checks */, - nullptr /* no mavlink_log_pub */, - status_flags, - arm_req, - 2e6, /* 2 seconds after boot, everything should be checked */ - arm_disarm_reason_t::unit_test); + transition_result_t result = _arm_state_machine.arming_state_transition( + status, + control_mode, + safety, + test->requested_state, + armed, + true /* enable pre-arm checks */, + nullptr /* no mavlink_log_pub */, + status_flags, + arm_req, + 2e6, /* 2 seconds after boot, everything should be checked */ + arm_disarm_reason_t::unit_test); // Validate result of transition ut_compare(test->assertMsg, test->expected_transition_result, result); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6ed5d53761..105c018a4f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -61,22 +61,6 @@ static constexpr const char reason_no_datalink[] = "no datalink"; static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink"; static constexpr const char reason_no_gps[] = "no GPS"; -// 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 -// will be true for a valid transition or false for a invalid transition. In some cases even -// though the transition is marked as true additional checks must be made. See arming_state_transition -// code for those checks. -static constexpr const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] -= { - // INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE - { /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false }, - { /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true }, - { /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false }, - { /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true }, - { /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI -}; - // 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", @@ -87,28 +71,6 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { "IN_AIR_RESTORE", }; -static inline events::px4::enums::arming_state_t 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; -} - // You can index into the array with an navigation_state_t in order to get its textual representation const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "MANUAL", @@ -135,8 +97,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = { "ORBIT" }; -static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately - void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed, const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act, const float ll_delay); @@ -159,162 +119,6 @@ void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsaf const offboard_loss_actions_t offboard_loss_act, const offboard_loss_rc_actions_t offboard_loss_rc_act); -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, actuator_armed_s &armed, const bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, 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) -{ - // Double check that our static arrays are still valid - static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0"); - static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1, - "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) { - 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; - } - - 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, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, - 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][status.arming_state]; - - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - - // 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) { - - bool prearm_check_ret = true; - - if (fRunPreArmChecks && preflight_check_ret) { - // only bother running prearm if preflight was successful - prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements, - status); - } - - if (!preflight_check_ret || !prearm_check_ret) { - // the prearm and preflight checks already print the rejection reason - feedback_provided = true; - valid_transition = false; - } - } - } - } - - if (hil_enabled) { - /* enforce lockdown in HIL */ - armed.lockdown = true; - 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; - } - - // HIL can always go to standby - if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { - valid_transition = true; - } - } - - if (!hil_enabled && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (status.arming_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) { - bool was_armed = armed.armed; - armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED); - 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; - - if (was_armed && !armed.armed) { // disarm transition - status.latest_disarming_reason = (uint8_t)calling_reason; - - } else if (!was_armed && armed.armed) { // arm transition - status.latest_arming_reason = (uint8_t)calling_reason; - } - - if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { - status.armed_time = hrt_absolute_time(); - - } else { - status.armed_time = 0; - } - } - } - - if (ret == TRANSITION_DENIED) { - /* print to MAVLink and console if we didn't provide any feedback yet */ - 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]); - 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)); - } - } - - return ret; -} - transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, const vehicle_status_flags_s &status_flags, commander_state_s &internal_state) diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 45806db1d6..b0cf652e44 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -44,13 +44,10 @@ #include -#include "Arming/PreFlightCheck/PreFlightCheck.hpp" - #include #include #include #include -#include #include #include #include @@ -106,21 +103,12 @@ enum class position_nav_loss_actions_t { extern const char *const arming_state_names[]; extern const char *const nav_state_names[]; -using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; - enum RCLossExceptionBits { RCL_EXCEPT_MISSION = (1 << 0), RCL_EXCEPT_HOLD = (1 << 1), RCL_EXCEPT_OFFBOARD = (1 << 2) }; -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, - actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, - 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); - transition_result_t main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state, const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);