diff --git a/msg/VehicleStatus.msg b/msg/VehicleStatus.msg index 8fcb8c2fa9..bf545ea02b 100644 --- a/msg/VehicleStatus.msg +++ b/msg/VehicleStatus.msg @@ -6,13 +6,8 @@ uint64 armed_time # Arming timestamp (microseconds) uint64 takeoff_time # Takeoff timestamp (microseconds) uint8 arming_state -uint8 ARMING_STATE_INIT = 0 -uint8 ARMING_STATE_STANDBY = 1 -uint8 ARMING_STATE_ARMED = 2 -uint8 ARMING_STATE_STANDBY_ERROR = 3 -uint8 ARMING_STATE_SHUTDOWN = 4 -uint8 ARMING_STATE_IN_AIR_RESTORE = 5 -uint8 ARMING_STATE_MAX = 6 +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 uint8 latest_arming_reason uint8 latest_disarming_reason diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp deleted file mode 100644 index af86b8b4ee..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ /dev/null @@ -1,174 +0,0 @@ -/**************************************************************************** - * - * 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]; - -transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status, - const arming_state_t new_arming_state, actuator_armed_s &armed, HealthAndArmingChecks &checks, - const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, 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; - bool feedback_provided = false; - - /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == _arm_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][_arm_state]; - - // Preflight check - if (valid_transition - && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) - && fRunPreArmChecks - && !(status.hil_state == vehicle_status_s::HIL_STATE_ON) - && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) { - - checks.update(); - - if (!checks.canArm(status.nav_state)) { - feedback_provided = true; // Preflight checks report error messages - valid_transition = false; - } - } - - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { - /* enforce lockdown in HIL */ - armed.lockdown = true; - - /* recover from a prearm fail */ - if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { - _arm_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; - } - } - - // Finish up the state transition - if (valid_transition) { - ret = TRANSITION_CHANGED; - - // Record arm/disarm reason - if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition - status.latest_disarming_reason = (uint8_t)calling_reason; - - } else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition - status.latest_arming_reason = (uint8_t)calling_reason; - } - - // Switch state - _arm_state = new_arming_state; - - if (isArmed()) { - 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", - getArmStateName(_arm_state), getArmStateName(new_arming_state)); - events::send( - events::ID("commander_transition_denied"), events::Log::Critical, - "Arming state transition denied: {1} to {2}", - 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 deleted file mode 100644 index e52c634b18..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ /dev/null @@ -1,95 +0,0 @@ -/**************************************************************************** - * - * 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 "../../HealthAndArmingChecks/HealthAndArmingChecks.hpp" -#include -#include -#include -#include -#include - -typedef enum { - TRANSITION_DENIED = -1, - TRANSITION_NOT_CHANGED = 0, - TRANSITION_CHANGED -} transition_result_t; - -using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; - -class ArmStateMachine -{ -public: - ArmStateMachine() = default; - ~ArmStateMachine() = default; - - void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } - - transition_result_t - arming_state_transition(vehicle_status_s &status, const arming_state_t new_arming_state, - actuator_armed_s &armed, HealthAndArmingChecks &checks, const bool fRunPreArmChecks, - orb_advert_t *mavlink_log_pub, 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 getArmStateEvent(uint8_t arming_state); - - uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT}; - - // 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/ArmStateMachineTest.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp deleted file mode 100644 index f141f2ddcf..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ /dev/null @@ -1,273 +0,0 @@ -/**************************************************************************** - * - * 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 -#include "ArmStateMachine.hpp" - -TEST(ArmStateMachineTest, ArmingStateTransitionTest) -{ - ArmStateMachine arm_state_machine; - - // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed - // to simulate machine state prior to testing an arming state transition. This structure is also - // use to represent the expected machine state after the transition has been requested. - typedef struct { - arming_state_t arming_state; // vehicle_status_s.arming_state - bool armed; // actuator_armed_s.armed - } ArmingTransitionVolatileState_t; - - // This structure represents a test case for arming_state_transition. It contains the machine - // state prior to transition, the requested state to transition to and finally the expected - // machine state after transition. - typedef struct { - const char *assertMsg; // Text to show when test case fails - ArmingTransitionVolatileState_t current_state; // Machine state prior to transition - hil_state_t hil_state; // Current vehicle_status_s.hil_state - bool safety_button_available; // Current safety_s.safety_button_available - bool safety_off; // Current safety_s.safety_off - arming_state_t requested_state; // Requested arming state to transition to - ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition - transition_result_t expected_transition_result; // Expected result from arming_state_transition - } ArmingTransitionTest_t; - - // We use these defines so that our test cases are more readable - static constexpr bool ATT_ARMED = true; - static constexpr bool ATT_DISARMED = false; - static constexpr bool ATT_SAFETY_AVAILABLE = true; - static constexpr bool ATT_SAFETY_NOT_AVAILABLE = true; - static constexpr bool ATT_SAFETY_OFF = true; - static constexpr bool ATT_SAFETY_ON = false; - - // These are test cases for arming_state_transition - static const ArmingTransitionTest_t rgArmingTransitionTests[] = { - // TRANSITION_NOT_CHANGED tests - - { - "no transition: identical states", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED - }, - - // TRANSITION_CHANGED tests - - // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s - - { - "transition: init to standby", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: init to standby error", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: init to reboot", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to init", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to standby error", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY_ERROR, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: armed to standby", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby error to reboot", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - { - "transition: in air restore to armed", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED - }, - - { - "transition: in air restore to reboot", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED - }, - - // hil on tests, standby error to standby not normally allowed - - { - "transition: standby error to standby, hil on", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - // Safety button arming tests - - { - "transition: standby to armed, no safety button", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED - }, - - { - "transition: standby to armed, safety button off", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED - }, - - // TRANSITION_DENIED tests - - // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s - - { - "no transition: init to armed", - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: armed to init", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_INIT, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED - }, - - { - "no transition: armed to reboot", - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_SHUTDOWN, - { vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED - }, - - { - "no transition: standby error to armed", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: standby error to standby", - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: reboot to armed", - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED - }, - - { - "no transition: in air restore to standby", - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED - }, - - // Safety button arming tests - - { - "no transition: init to armed, safety button on", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_ARMED, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED - }, - }; - - struct vehicle_status_s status {}; - struct actuator_armed_s armed {}; - - size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); - - for (size_t i = 0; i < cArmingTransitionTests; i++) { - const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i]; - - // Setup initial machine state - arm_state_machine.forceArmState(test->current_state.arming_state); - status.hil_state = test->hil_state; - - HealthAndArmingChecks health_and_arming_checks(nullptr, status); - - // Attempt transition - transition_result_t result = arm_state_machine.arming_state_transition( - status, - test->requested_state, - armed, - health_and_arming_checks, - true /* enable pre-arm checks */, - nullptr /* no mavlink_log_pub */, - arm_disarm_reason_t::unit_test); - - // Validate result of transition - EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg; - EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg; - EXPECT_EQ(arm_state_machine.isArmed(), test->expected_state.armed) << test->assertMsg; - } -} diff --git a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt b/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt deleted file mode 100644 index e9f9c54232..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -############################################################################ -# -# 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 health_and_arming_checks) - -px4_add_functional_gtest(SRC ArmStateMachineTest.cpp - LINKLIBS ArmStateMachine health_and_arming_checks hysteresis sensor_calibration ArmAuthorization mode_util - ) - diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt index 37aa7473d9..067278f60a 100644 --- a/src/modules/commander/Arming/CMakeLists.txt +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -32,4 +32,3 @@ ############################################################################ add_subdirectory(ArmAuthorization) -add_subdirectory(ArmStateMachine) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index fdc5e949b1..69b3ae4bd5 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -66,7 +66,6 @@ px4_add_module( health_and_arming_checks hysteresis ArmAuthorization - ArmStateMachine sensor_calibration world_magnetic_model mode_util diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 4c317f6bda..d4b8d539d4 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -473,7 +473,7 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { - PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); + PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed"); PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]); PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]); PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no"); @@ -487,13 +487,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) return Commander::main(argc, argv); } -bool Commander::shutdownIfAllowed() -{ - return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks, - false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown); -} - static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { switch (calling_reason) { @@ -533,20 +526,37 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) { - // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming + if (isArmed()) { + return TRANSITION_NOT_CHANGED; + } + + if (_vehicle_status.calibration_enabled + || _vehicle_status.rc_calibration_in_progress + || _actuator_armed.in_esc_calibration_mode) { + + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: calibrating\t"); + events::send(events::ID("commander_arm_denied_calibrating"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: calibrating"); + tune_negative(true); + return TRANSITION_DENIED; + } + + // allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidental in-air disarming if (calling_reason == arm_disarm_reason_t::rc_switch - && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + && ((_last_disarmed_timestamp != 0) && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) { + run_preflight_checks = false; } - if (run_preflight_checks && !_arm_state_machine.isArmed()) { + if (run_preflight_checks) { if (_vehicle_control_mode.flag_control_manual_enabled) { + if (_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t"); - events::send(events::ID("commander_arm_denied_throttle_center"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: throttle above center"); + events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: throttle above center"); tune_negative(true); return TRANSITION_DENIED; } @@ -554,10 +564,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ if (!_vehicle_control_mode.flag_control_climb_rate_enabled && !_failsafe_flags.manual_control_signal_lost && !_is_throttle_low && _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t"); - events::send(events::ID("commander_arm_denied_throttle_high"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: high throttle"); + events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: high throttle"); tune_negative(true); return TRANSITION_DENIED; } @@ -565,35 +575,45 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } else if (calling_reason == arm_disarm_reason_t::rc_stick || calling_reason == arm_disarm_reason_t::rc_switch || calling_reason == arm_disarm_reason_t::rc_button) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t"); - events::send(events::ID("commander_arm_denied_not_manual"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: switch to manual mode first"); + events::send(events::ID("commander_arm_denied_not_manual"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: switch to manual mode first"); + tune_negative(true); + return TRANSITION_DENIED; + } + + _health_and_arming_checks.update(); + + if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) { tune_negative(true); return TRANSITION_DENIED; } } - transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks, - &_mavlink_log_pub, calling_reason); + _vehicle_status.armed_time = hrt_absolute_time(); + _vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; + _vehicle_status.latest_arming_reason = (uint8_t)calling_reason; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); - events::send(events::ID("commander_armed_by"), events::Log::Info, - "Armed by {1}", calling_reason); + mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); + events::send(events::ID("commander_armed_by"), events::Log::Info, + "Armed by {1}", calling_reason); - _status_changed = true; - - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); + if (_param_com_home_en.get()) { + _home_position.setHomePosition(); } - return arming_res; + _status_changed = true; + + return TRANSITION_CHANGED; } transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced) { + if (!isArmed()) { + return TRANSITION_NOT_CHANGED; + } + if (!forced) { const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed || is_ground_vehicle(_vehicle_status)); @@ -606,36 +626,43 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) { if (calling_reason != arm_disarm_reason_t::rc_stick) { - mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t"); - events::send(events::ID("commander_disarming_denied_not_landed"), + mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t"); + events::send(events::ID("commander_disarm_denied_not_landed"), {events::Log::Critical, events::LogInternal::Info}, - "Disarming denied, not landed"); + "Disarming denied: not landed"); } return TRANSITION_DENIED; } } - transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false, - &_mavlink_log_pub, calling_reason); + _vehicle_status.armed_time = 0; + _vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED; + _vehicle_status.latest_disarming_reason = (uint8_t)calling_reason; + _vehicle_status.takeoff_time = 0; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason)); - events::send(events::ID("commander_disarmed_by"), events::Log::Info, - "Disarmed by {1}", calling_reason); + _have_taken_off_since_arming = false; - if (_param_com_force_safety.get()) { - _safety.activateSafety(); - } + _last_disarmed_timestamp = hrt_absolute_time(); - _status_changed = true; + _user_mode_intention.onDisarm(); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); + mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason)); + events::send(events::ID("commander_disarmed_by"), events::Log::Info, + "Disarmed by {1}", calling_reason); + + if (_param_com_force_safety.get()) { + _safety.activateSafety(); } - return arming_res; + // update flight uuid + const int32_t flight_uuid = _param_flight_uuid.get() + 1; + _param_flight_uuid.set(flight_uuid); + _param_flight_uuid.commit_no_notification(); + + _status_changed = true; + + return TRANSITION_CHANGED; } Commander::Commander() : @@ -643,19 +670,15 @@ Commander::Commander() : { _vehicle_land_detected.landed = true; + _vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED; _vehicle_status.system_id = 1; _vehicle_status.component_id = 1; - _vehicle_status.system_type = 0; _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; - _vehicle_status.nav_state = _user_mode_intention.get(); _vehicle_status.nav_state_user_intention = _user_mode_intention.get(); _vehicle_status.nav_state_timestamp = hrt_absolute_time(); - - /* mark all signals lost as long as they haven't been found */ _vehicle_status.gcs_connection_lost = true; - _vehicle_status.power_input_valid = true; // default for vtol is rotary wing @@ -858,16 +881,6 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { // Arm is forced (checks skipped) when param2 is set to a magic number. const bool forced = (static_cast(lroundf(cmd.param2)) == 21196); - const bool cmd_from_io = (static_cast(roundf(cmd.param3)) == 1234); - - // Flick to in-air restore first if this comes from an onboard system and from IO - if (!forced && cmd_from_io - && (cmd.source_system == _vehicle_status.system_id) - && (cmd.source_component == _vehicle_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; arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external : @@ -886,47 +899,26 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; - - /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ - if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED) - && (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) - && (_param_com_home_en.get())) { - _home_position.setHomePosition(); - } } } } break; case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { - if (cmd.param1 > 1.5f) { - // Test termination command triggers lockdown but not actual termination. - if (!_lockdown_triggered) { - _actuator_armed.lockdown = true; - _lockdown_triggered = true; - PX4_WARN("forcing lockdown (motors off)"); - } + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; - } else if (cmd.param1 > 0.5f) { + if (cmd.param1 > 0.5f) { // Trigger real termination. - if (!_flight_termination_triggered) { - _actuator_armed.force_failsafe = true; - _flight_termination_triggered = true; - PX4_WARN("forcing failsafe (termination)"); - send_parachute_command(); + if (!isArmed()) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; + + } else if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION)) { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - } else { - _actuator_armed.force_failsafe = false; - _actuator_armed.lockdown = false; - - _lockdown_triggered = false; - _flight_termination_triggered = false; - - PX4_WARN("disabling failsafe and lockdown"); } - - cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; } break; @@ -1130,7 +1122,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) { + } else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) { // 1: Reboot autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1140,7 +1132,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(BOARD_HAS_POWER_CONTROL) - } else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) { + } else if ((param1 == 2) && !isArmed() && (px4_shutdown_request(400_ms) == 0)) { // 2: Shutdown autopilot answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1150,7 +1142,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) { + } else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1167,25 +1159,13 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { + if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks, - false /* fRunPreArmChecks */, &_mavlink_log_pub, - (cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal)) - ) { - - answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED); - break; - - } - if ((int)(cmd.param1) == 1) { /* gyro calibration */ answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1292,7 +1272,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 (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { + if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1327,7 +1307,7 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) { + if (isArmed() || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1424,7 +1404,7 @@ Commander::handle_command(const vehicle_command_s &cmd) unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd) { - if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { + if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) { return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1504,7 +1484,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break; case action_request_s::ACTION_TOGGLE_ARMING: - if (_arm_state_machine.isArmed()) { + if (isArmed()) { disarm(arm_disarm_reason); } else { @@ -1540,7 +1520,6 @@ void Commander::executeActionRequest(const action_request_s &action_request) _status_changed = true; _actuator_armed.manual_lockdown = true; - send_parachute_command(); } break; @@ -1674,20 +1653,12 @@ void Commander::run() vtolStatusUpdate(); - _home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed); + _home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed); handleAutoDisarm(); battery_status_check(); - /* If in INIT state, try to proceed to STANDBY state */ - if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) { - - _arm_state_machine.arming_state_transition(_vehicle_status, - vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, - true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby); - } - checkForMissionUpdate(); manualControlCheck(); @@ -1754,40 +1725,30 @@ void Commander::run() } } - // check for arming state changes - if (_was_armed != _arm_state_machine.isArmed()) { - _status_changed = true; - } - - if (!_was_armed && _arm_state_machine.isArmed() && !_vehicle_land_detected.landed) { - _have_taken_off_since_arming = true; - } - - if (_was_armed && !_arm_state_machine.isArmed()) { - const int32_t flight_uuid = _param_flight_uuid.get() + 1; - _param_flight_uuid.set(flight_uuid); - _param_flight_uuid.commit_no_notification(); - - _last_disarmed_timestamp = hrt_absolute_time(); - - _user_mode_intention.onDisarm(); - _vehicle_status.takeoff_time = 0; - } - - if (!_arm_state_machine.isArmed()) { - /* Reset the flag if disarmed. */ - _have_taken_off_since_arming = false; - } - + // update actuator_armed + _actuator_armed.armed = isArmed(); _actuator_armed.prearmed = getPrearmState(); + _actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed(); + _actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION) + || (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON)); + // _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL + _actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION); + // _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION + + // if force_failsafe or manual_lockdown activated send parachute command + if ((!actuator_armed_prev.force_failsafe && _actuator_armed.force_failsafe) + || (!actuator_armed_prev.manual_lockdown && _actuator_armed.manual_lockdown) + ) { + if (isArmed()) { + send_parachute_command(); + } + } // publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed || !(_actuator_armed == actuator_armed_prev)) { // publish actuator_armed first (used by output modules) - _actuator_armed.armed = _arm_state_machine.isArmed(); - _actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby(); _actuator_armed.timestamp = hrt_absolute_time(); _actuator_armed_pub.publish(_actuator_armed); @@ -1795,7 +1756,6 @@ void Commander::run() updateControlMode(); // vehicle_status publish (after prearm/preflight updates above) - _vehicle_status.arming_state = _arm_state_machine.getArmState(); _vehicle_status.timestamp = hrt_absolute_time(); _vehicle_status_pub.publish(_vehicle_status); @@ -1822,11 +1782,9 @@ void Commander::run() _status_changed = false; - _was_armed = _arm_state_machine.isArmed(); - arm_auth_update(hrt_absolute_time(), params_updated); - px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed()); + px4_indicate_external_reset_lockout(LockoutComponent::Commander, isArmed()); perf_end(_loop_perf); @@ -1877,7 +1835,7 @@ void Commander::checkForMissionUpdate() } } - if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed + if (isArmed() && !_vehicle_land_detected.landed && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) && mission_result.finished) { @@ -1901,6 +1859,10 @@ void Commander::checkForMissionUpdate() bool Commander::getPrearmState() const { + if (_vehicle_status.calibration_enabled) { + return false; + } + switch ((PrearmedMode)_param_com_prearm_mode.get()) { case PrearmedMode::DISABLED: /* skip prearmed state */ @@ -1936,7 +1898,7 @@ void Commander::handlePowerButtonState() if (_power_button_state_sub.copy(&button_state)) { if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { - if (shutdownIfAllowed() && (px4_shutdown_request() == 0)) { + if (!isArmed() && (px4_shutdown_request() == 0)) { while (1) { px4_usleep(1); } } } @@ -1973,7 +1935,7 @@ void Commander::landDetectorUpdate() _vehicle_land_detected_sub.copy(&_vehicle_land_detected); // Only take actions if armed - if (_arm_state_machine.isArmed()) { + if (isArmed()) { if (!was_landed && _vehicle_land_detected.landed) { mavlink_log_info(&_mavlink_log_pub, "Landing detected\t"); events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected"); @@ -1987,9 +1949,8 @@ void Commander::landDetectorUpdate() // automatically set or update home position if (_param_com_home_en.get()) { - // set the home position when taking off, but only if we were previously disarmed - // and at least 500 ms from commander start spent to avoid setting home on in-air restart - if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) { + // set the home position when taking off + if (!_vehicle_land_detected.landed) { if (was_landed) { _home_position.setHomePosition(); @@ -2060,7 +2021,7 @@ void Commander::vtolStatusUpdate() void Commander::updateTunes() { // play arming and battery warning tunes - if (!_arm_tune_played && _arm_state_machine.isArmed()) { + if (!_arm_tune_played && isArmed()) { /* play tune when armed */ set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); @@ -2069,6 +2030,7 @@ void Commander::updateTunes() } else if (!_vehicle_status.usb_connected && (_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) && (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) { + /* play tune on battery critical */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); @@ -2077,7 +2039,7 @@ void Commander::updateTunes() /* play tune on battery warning */ set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); - } else if (_vehicle_status.failsafe && _arm_state_machine.isArmed()) { + } else if (_vehicle_status.failsafe && isArmed()) { tune_failsafe(true); } else { @@ -2085,7 +2047,7 @@ void Commander::updateTunes() } /* reset arm_tune_played when disarmed */ - if (!_arm_state_machine.isArmed()) { + if (!isArmed()) { // Notify the user that it is safe to approach the vehicle if (_arm_tune_played) { @@ -2119,7 +2081,7 @@ void Commander::checkWorkerThread() void Commander::handleAutoDisarm() { // Auto disarm when landed or kill switch engaged - if (_arm_state_machine.isArmed()) { + if (isArmed()) { // Check for auto-disarm on landing or pre-flight if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { @@ -2178,7 +2140,7 @@ bool Commander::handleModeIntentionAndFailsafe() const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction(); FailsafeBase::State state{}; - state.armed = _arm_state_machine.isArmed(); + state.armed = isArmed(); state.vtol_in_transition_mode = _vehicle_status.in_transition_mode; state.mission_finished = _mission_result_sub.get().finished; state.user_intended_mode = _user_mode_intention.get(); @@ -2210,13 +2172,6 @@ bool Commander::handleModeIntentionAndFailsafe() case FailsafeBase::Action::Terminate: _vehicle_status.nav_state = _vehicle_status.NAVIGATION_STATE_TERMINATION; - _actuator_armed.force_failsafe = true; - - if (!_flight_termination_triggered) { - _flight_termination_triggered = true; - send_parachute_command(); - } - break; default: @@ -2293,14 +2248,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 = _arm_state_machine.isArmed() ? 1_ms : 250_ms; + uint64_t overload_warn_delay = 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 (_arm_state_machine.isArmed()) { + } else if (isArmed()) { led_mode = led_control_s::MODE_ON; set_normal_color = true; @@ -2308,18 +2263,9 @@ 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 (_arm_state_machine.isStandby()) { + } else { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - - } 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; - - } else { - // STANDBY_ERROR and other states - led_mode = led_control_s::MODE_BLINK_NORMAL; - led_color = led_control_s::COLOR_RED; } if (set_normal_color) { @@ -2352,7 +2298,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) #if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS) - if (_arm_state_machine.isArmed()) { + if (isArmed()) { if (_vehicle_status.failsafe) { BOARD_ARMED_LED_OFF(); @@ -2368,7 +2314,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) BOARD_ARMED_LED_ON(); } - } else if (_arm_state_machine.isStandby()) { + } else if (_vehicle_status.pre_flight_checks_pass) { BOARD_ARMED_LED_OFF(); // ready to arm, blink at 1Hz @@ -2404,7 +2350,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning) void Commander::updateControlMode() { _vehicle_control_mode = {}; - mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state, + mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state, _vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode); _vehicle_control_mode.timestamp = hrt_absolute_time(); _vehicle_control_mode_pub.publish(_vehicle_control_mode); @@ -2426,7 +2372,7 @@ void Commander::printRejectMode(uint8_t nav_state) /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ - tune_negative(_arm_state_machine.isArmed()); + tune_negative(isArmed()); _last_print_mode_reject_time = hrt_absolute_time(); } @@ -2698,7 +2644,7 @@ void Commander::battery_status_check() if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) - if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) { + if (!isArmed() && (px4_shutdown_request(60_s) == 0)) { mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t"); events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning}, "Dangerously low battery! Shutting system down"); @@ -2731,7 +2677,7 @@ void Commander::manualControlCheck() _is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f); _is_throttle_low = (manual_control_setpoint.throttle < -0.8f); - if (_arm_state_machine.isArmed()) { + if (isArmed()) { // Abort autonomous mode and switch to position mode if sticks are moved significantly // but only if actually in air. if (manual_control_setpoint.sticks_moving @@ -2794,10 +2740,9 @@ void Commander::send_parachute_command() vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE; vcmd.param1 = static_cast(vehicle_command_s::PARACHUTE_ACTION_RELEASE); - uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; - vcmd.source_system = vehicle_status_sub.get().system_id; - vcmd.target_system = vehicle_status_sub.get().system_id; - vcmd.source_component = vehicle_status_sub.get().component_id; + vcmd.source_system = _vehicle_status.system_id; + vcmd.target_system = _vehicle_status.system_id; + vcmd.source_component = _vehicle_status.component_id; vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE uORB::Publication vcmd_pub{ORB_ID(vehicle_command)}; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 3678b9ea9d..e6f79a1aca 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2017-2022 PX4 Development Team. All rights reserved. + * Copyright (c) 2017-2023 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 @@ -34,7 +34,6 @@ #pragma once /* Helper classes */ -#include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "failure_detector/FailureDetector.hpp" #include "failsafe/failsafe.h" #include "Safety.hpp" @@ -87,6 +86,14 @@ using math::constrain; using systemlib::Hysteresis; +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED +} transition_result_t; + +using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; + using namespace time_literals; class Commander : public ModuleBase, public ModuleParams @@ -116,6 +123,8 @@ public: void enable_hil(); private: + bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); } + void answer_command(const vehicle_command_s &cmd, uint8_t result); transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true); @@ -153,8 +162,6 @@ private: void updateControlMode(); - bool shutdownIfAllowed(); - void send_parachute_command(); void checkForMissionUpdate(); @@ -196,11 +203,9 @@ private: /* Decouple update interval and hysteresis counters, all depends on intervals */ static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; - static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms}; vehicle_status_s _vehicle_status{}; - ArmStateMachine _arm_state_machine{}; Failsafe _failsafe_instance{this}; FailsafeBase &_failsafe{_failsafe_instance}; FailureDetector _failure_detector{this}; @@ -242,9 +247,6 @@ private: bool _failsafe_user_override_request{false}; ///< override request due to stick movements - bool _flight_termination_triggered{false}; - bool _lockdown_triggered{false}; - bool _open_drone_id_system_lost{true}; bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; @@ -257,7 +259,6 @@ private: bool _is_throttle_low{false}; bool _arm_tune_played{false}; - bool _was_armed{false}; bool _have_taken_off_since_arming{false}; bool _status_changed{true}; diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 18665491f0..c344876360 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -107,28 +107,23 @@ private: // uint8_t system_status (MAV_STATE) - System status flag. uint8_t system_status = MAV_STATE_UNINIT; - switch (vehicle_status.arming_state) { - case vehicle_status_s::ARMING_STATE_ARMED: + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE; - break; - case vehicle_status_s::ARMING_STATE_STANDBY: + } else if (vehicle_status.calibration_enabled || vehicle_status.rc_calibration_in_progress + || actuator_armed.in_esc_calibration_mode) { + system_status = MAV_STATE_CALIBRATING; + + } else if (vehicle_status.pre_flight_checks_pass) { system_status = MAV_STATE_STANDBY; - break; - - case vehicle_status_s::ARMING_STATE_SHUTDOWN: - system_status = MAV_STATE_POWEROFF; - break; } // system_status overrides if (actuator_armed.force_failsafe || (actuator_armed.lockdown && vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF) || actuator_armed.manual_lockdown || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { - system_status = MAV_STATE_FLIGHT_TERMINATION; - } else if (vehicle_status.calibration_enabled) { - system_status = MAV_STATE_CALIBRATING; + system_status = MAV_STATE_FLIGHT_TERMINATION; }