diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index f5803533ae..05742a8872 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -72,6 +72,7 @@ uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function| uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command|configuration|Empty|Empty|Empty|output function| uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm +uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs system to run pre-arm checks. uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index a53449ade0..c49127b37b 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -1,12 +1,8 @@ uint64 timestamp # time since system start (microseconds) -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 ARMING_STATE_MAX = 3 # FailureDetector status uint16 FAILURE_NONE = 0 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index 0d36707314..34e242e933 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -2,8 +2,10 @@ uint64 timestamp # time since system start (microseconds) +bool preflight_check_passing +bool prearm_check_passing + bool calibration_enabled -bool system_sensors_initialized bool system_hotplug_timeout # true if the hotplug sensor search is over bool auto_mission_available bool angular_velocity_valid @@ -18,6 +20,7 @@ bool power_input_valid # set if input power is valid bool battery_healthy # set if battery is available and not low bool escs_error # set to true if one or more ESCs reporting esc_status are offline bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure +bool overload # maximum cpuload exceeded or free memory dangerously low bool position_reliant_on_gps bool position_reliant_on_optical_flow diff --git a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h index 74b6571f92..cb9b952b37 100644 --- a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h +++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.h @@ -36,7 +36,7 @@ #include #include -#include +#include enum arm_auth_methods { ARM_AUTH_METHOD_ARM_REQ = 0, diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp deleted file mode 100644 index 15400161d4..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ /dev/null @@ -1,238 +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 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; - 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 == _arm_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, !isArmed(), - 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][_arm_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 (_arm_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 (_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; - } - } - - if (!hil_enabled && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (_arm_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) { - - // Sensors need to be initialized for STANDBY state, except for HIL - if (!status_flags.system_sensors_initialized) { - feedback_provided = true; - valid_transition = false; - } - } - - // 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 999bcd864a..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp +++ /dev/null @@ -1,96 +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 "../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; - - void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; } - - transition_result_t - arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety, - 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); - - // Getters - uint8_t getArmState() const { return _arm_state; } - - bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); } - bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); } - bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); } - bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); } - - static const char *getArmStateName(uint8_t arming_state); - const char *getArmStateName() const { return getArmStateName(_arm_state); } - -private: - static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state); - - uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT}; - hrt_abstime _last_preflight_check = 0; ///< initialize so it gets checked immediately - - // This array defines the arming state transitions. The rows are the new state, and the columns - // 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 5da49a9a3a..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp +++ /dev/null @@ -1,297 +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 - -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 - system_sensors_initialized; // Current vehicle_status_s.system_sensors_initialized - bool safety_switch_available; // Current safety_s.safety_switch_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 -#define ATT_ARMED true -#define ATT_DISARMED false -#define ATT_SENSORS_INITIALIZED true -#define ATT_SENSORS_NOT_INITIALIZED false -#define ATT_SAFETY_AVAILABLE true -#define ATT_SAFETY_NOT_AVAILABLE true -#define ATT_SAFETY_OFF true -#define 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED - }, - - // Safety switch arming tests - - { - "transition: standby to armed, no safety switch", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, 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 switch off", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, 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_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - vehicle_status_s::ARMING_STATE_STANDBY, - { vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED - }, - - // Sensor tests - - //{ "transition to standby error: init to standby - sensors not initialized", - // { vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, - // vehicle_status_s::ARMING_STATE_STANDBY, - // { vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, - - // Safety switch arming tests - - { - "no transition: init to armed, safety switch on", - { vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, 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 vehicle_status_flags_s status_flags {}; - struct safety_s safety {}; - 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]; - - PreFlightCheck::arm_requirements_t arm_req{}; - - // Setup initial machine state - arm_state_machine.forceArmState(test->current_state.arming_state); - status_flags.system_sensors_initialized = test->system_sensors_initialized; - status.hil_state = test->hil_state; - // The power status of the test unit is not relevant for the unit test - status_flags.circuit_breaker_engaged_power_check = true; - safety.safety_switch_available = test->safety_switch_available; - safety.safety_off = test->safety_off; - - vehicle_control_mode_s control_mode{}; - - // Attempt transition - 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 - 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 24f6be2c93..0000000000 --- a/src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt +++ /dev/null @@ -1,41 +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 PreFlightCheck) - -px4_add_functional_gtest(SRC ArmStateMachineTest.cpp LINKLIBS ArmStateMachine) diff --git a/src/modules/commander/Arming/CMakeLists.txt b/src/modules/commander/Arming/CMakeLists.txt index ab21b1a479..62889ea6c6 100644 --- a/src/modules/commander/Arming/CMakeLists.txt +++ b/src/modules/commander/Arming/CMakeLists.txt @@ -32,6 +32,5 @@ ############################################################################ add_subdirectory(ArmAuthorization) -add_subdirectory(ArmStateMachine) add_subdirectory(HealthFlags) add_subdirectory(PreFlightCheck) diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt index edec715b23..92468509be 100644 --- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt +++ b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt @@ -33,7 +33,6 @@ px4_add_library(PreFlightCheck PreFlightCheck.cpp - checks/preArmCheck.cpp checks/magnetometerCheck.cpp checks/magConsistencyCheck.cpp checks/accelerometerCheck.cpp @@ -46,7 +45,6 @@ px4_add_library(PreFlightCheck checks/powerCheck.cpp checks/airframeCheck.cpp checks/ekf2Check.cpp - checks/failureDetectorCheck.cpp checks/manualControlCheck.cpp checks/modeCheck.cpp checks/cpuResourceCheck.cpp diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index a36d1b7581..7913afdcc7 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -52,10 +52,9 @@ static constexpr unsigned max_mandatory_baro_count = 1; bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, - bool report_failures, const bool prearm, const hrt_abstime &time_since_boot) + bool report_failures) { - report_failures = (report_failures && status_flags.system_hotplug_timeout - && !status_flags.calibration_enabled); + report_failures = (report_failures && !status_flags.calibration_enabled); bool failed = false; @@ -142,7 +141,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed - if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en, + if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, (max_airspeed_check_en == 1), arming_max_airspeed_allowed) && !(bool)optional) { failed = true; @@ -174,7 +173,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu /* ---- SYSTEM POWER ---- */ if (status_flags.power_input_valid && !status_flags.circuit_breaker_engaged_power_check) { - if (!powerCheck(mavlink_log_pub, status, report_failures, prearm)) { + if (!powerCheck(mavlink_log_pub, status, report_failures)) { failed = true; } } @@ -193,22 +192,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu if (estimator_type == 2) { - const bool in_grace_period = time_since_boot < 10_s; - const bool do_report_ekf2_failures = report_failures && (!in_grace_period || prearm); - const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, false, do_report_ekf2_failures) && - ekf2CheckSensorBias(mavlink_log_pub, do_report_ekf2_failures); - - // For the first 10 seconds the ekf2 can be unhealthy, and we just mark it - // as not present. - // After that or if we're forced to report, we'll set the flags as is. - - if (!ekf_healthy && !do_report_ekf2_failures) { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status); - - } else { - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); - } + const bool ekf_healthy = ekf2Check(mavlink_log_pub, status, report_failures) && + ekf2CheckSensorBias(mavlink_log_pub, report_failures); + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); if (control_mode.flag_control_attitude_enabled || control_mode.flag_control_velocity_enabled @@ -218,11 +205,6 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } } - /* ---- Failure Detector ---- */ - if (!failureDetectorCheck(mavlink_log_pub, status, report_failures, prearm)) { - failed = true; - } - failed = failed || !manualControlCheck(mavlink_log_pub, report_failures); failed = failed || !modeCheck(mavlink_log_pub, report_failures, status); failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures); diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index fae89e005a..461c09a00a 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -59,44 +59,10 @@ public: /** * Runs a preflight check on all sensors to see if they are properly calibrated and healthy * - * The function won't fail the test if optional sensors are not found, however, - * it will fail the test if optional sensors are found but not in working condition. - * - * @param mavlink_log_pub - * Mavlink output orb handle reference for feedback when a sensor fails - * @param checkMag - * true if the magneteometer should be checked - * @param checkAcc - * true if the accelerometers should be checked - * @param checkGyro - * true if the gyroscopes should be checked - * @param checkBaro - * true if the barometer should be checked - * @param checkAirspeed - * true if the airspeed sensor should be checked - * @param checkRC - * true if the Remote Controller should be checked - * @param checkGNSS - * true if the GNSS receiver should be checked - * @param checkPower - * true if the system power should be checked **/ static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, - bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot); - - struct arm_requirements_t { - bool arm_authorization = false; - bool esc_check = false; - bool global_position = false; - bool mission = false; - bool geofence = false; - }; - - static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const vehicle_control_mode_s &control_mode, - const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, - bool report_fail = true); + bool reportFailures); private: static bool sensorAvailabilityCheck(const bool report_failure, @@ -119,18 +85,13 @@ private: const bool is_mandatory, bool &report_fail); static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status); static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, - const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed); + const bool report_fail, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed); static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail); - static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, - const bool prearm); - static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, - const bool report_fail); + static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail); + static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail); static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail); - static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, - const bool prearm); - static bool manualControlCheck(orb_advert_t *mavlink_log_pub, const bool report_fail); static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status); static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp index ffaceea471..b47b74ea3d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/airspeedCheck.cpp @@ -43,7 +43,7 @@ using namespace time_literals; bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, - const bool report_fail, const bool prearm, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed) + const bool report_fail, const bool max_airspeed_check_en, const float arming_max_airspeed_allowed) { bool present = true; bool success = true; @@ -83,8 +83,7 @@ bool PreFlightCheck::airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover * might have been removed. */ - if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed - && prearm) { + if (max_airspeed_check_en && fabsf(airspeed_validated.calibrated_airspeed_m_s) > arming_max_airspeed_allowed) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or pitot"); } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index ff8b56acda..69b5ec912f 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -44,8 +44,7 @@ using namespace time_literals; -bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, - const bool report_fail) +bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool report_fail) { bool success = true; // start with a pass and change to a fail if any test fails diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp deleted file mode 100644 index 8f1b5e52e7..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/failureDetectorCheck.cpp +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 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 "../PreFlightCheck.hpp" - -#include - -bool PreFlightCheck::failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, - const bool report_fail, const bool prearm) -{ - // Ignore failure detector check after arming - if (!prearm) { - return true; - } - - if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) { - if (report_fail) { - if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Roll failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Pitch failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Altitude failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Parachute failure detected"); - } - - if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { - mavlink_log_critical(mavlink_log_pub, "Preflight Fail: ESC failure detected"); - } - } - - return false; - } - - return true; -} diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index c5a134de23..1a371d87a7 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -42,16 +42,10 @@ using namespace time_literals; -bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, - const bool prearm) +bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail) { bool success = true; - if (!prearm) { - // Ignore power check after arming. - return true; - } - if (status.hil_state == vehicle_status_s::HIL_STATE_ON) { // Ignore power check in HITL. return true; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp deleted file mode 100644 index e6552084e0..0000000000 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019-2020 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 "../PreFlightCheck.hpp" - -#include -#include -#include -#include - -bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const vehicle_control_mode_s &control_mode, - const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail) -{ - bool prearm_ok = true; - - // rate control mode require valid angular velocity - if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! angular velocity invalid"); } - - prearm_ok = false; - } - - // attitude control mode require valid attitude - if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! attitude invalid"); } - - prearm_ok = false; - } - - // velocity control mode require valid velocity - if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! velocity invalid"); } - - prearm_ok = false; - } - - // position control mode require valid position - if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! position invalid"); } - - prearm_ok = false; - } - - // manual control mode require valid manual control (rc) - if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! manual control lost"); } - - prearm_ok = false; - } - - if (status_flags.flight_terminated) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Flight termination active"); } - - prearm_ok = false; - } - - // USB not connected - if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe"); } - - prearm_ok = false; - } - - // battery and system power status - if (!status_flags.circuit_breaker_engaged_power_check) { - - // Fail transition if power is not good - if (!status_flags.power_input_valid) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Connect power module"); } - - prearm_ok = false; - } - - // main battery level - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_SENSORBATTERY, true, true, - status_flags.battery_healthy, status); - - // Only arm if healthy - if (!status_flags.battery_healthy) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Check battery"); } - } - - prearm_ok = false; - } - } - - // Arm Requirements: mission - if (arm_requirements.mission) { - - if (!status_flags.auto_mission_available) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! No valid mission"); } - } - - prearm_ok = false; - } - - if (!status_flags.global_position_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Missions require a global position"); } - } - - prearm_ok = false; - } - } - - if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) { - - if (!status_flags.global_position_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Global position required"); } - } - - prearm_ok = false; - } - - if (!status_flags.home_position_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Home position invalid"); } - } - - prearm_ok = false; - } - } - - // safety button - if (safety.safety_switch_available && !safety.safety_off) { - // Fail transition if we need safety switch press - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Press safety switch first"); } - } - - prearm_ok = false; - } - - if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Avoidance system not ready"); } - } - - prearm_ok = false; - - } - - if (arm_requirements.esc_check && status_flags.escs_error) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline"); } - - prearm_ok = false; - } - } - - if (arm_requirements.esc_check && status_flags.escs_failure) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); } - - prearm_ok = false; - } - } - - if (status.is_vtol) { - - if (status.in_transition_mode) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is in transition state"); } - - prearm_ok = false; - } - } - - if (!status_flags.circuit_breaker_vtol_fw_arming_check - && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (prearm_ok) { - if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Vehicle is not in multicopter mode"); } - - prearm_ok = false; - } - } - } - - if (arm_requirements.geofence && status.geofence_violated) { - if (report_fail) { - mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle outside geofence"); - } - - prearm_ok = false; - } - - // Arm Requirements: authorization - // check last, and only if everything else has passed - if (arm_requirements.arm_authorization && prearm_ok) { - if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) { - // feedback provided in arm_auth_check - prearm_ok = false; - } - } - - - return prearm_ok; -} diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 538a79f93f..6518a1ad21 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -55,6 +55,7 @@ px4_add_module( Safety.cpp state_machine_helper.cpp worker_thread.cpp + preArmCheck.cpp DEPENDS circuit_breaker failure_detector @@ -62,7 +63,6 @@ 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 4dacd0bdc3..56816f13c9 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -284,28 +284,11 @@ int Commander::custom_command(int argc, char *argv[]) } if (!strcmp(argv[0], "check")) { - uORB::Subscription vehicle_status_sub{ORB_ID(vehicle_status)}; - vehicle_status_s vehicle_status{}; - vehicle_status_sub.copy(&vehicle_status); + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS); - uORB::Subscription vehicle_status_flags_sub{ORB_ID(vehicle_status_flags)}; - vehicle_status_flags_s vehicle_status_flags{}; - vehicle_status_flags_sub.copy(&vehicle_status_flags); - - uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; - vehicle_control_mode_s vehicle_control_mode{}; - vehicle_control_mode_sub.copy(&vehicle_control_mode); - - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, - vehicle_control_mode, - true, true, 30_s); - PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); - - bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode, safety_s{}, - PreFlightCheck::arm_requirements_t{}, vehicle_status); - PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); - - print_health_flags(vehicle_status); + px4_usleep(1000000); + uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; + print_health_flags(vehicle_status_sub.get()); return 0; } @@ -479,7 +462,15 @@ int Commander::custom_command(int argc, char *argv[]) int Commander::print_status() { - PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName()); + // You can index into the array with an arming_state_t in order to get its textual representation + const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = { + "INIT", + "STANDBY", + "ARMED", + //"SHUTDOWN", + }; + + PX4_INFO("arming: %s", arming_state_names[_status.arming_state]); PX4_INFO("navigation: %s", nav_state_names[_status.nav_state]); perf_print_counter(_loop_perf); perf_print_counter(_preflight_check_perf); @@ -491,14 +482,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) return Commander::main(argc, argv); } -bool Commander::shutdown_if_allowed() -{ - 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); -} - static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason) { switch (calling_reason) { @@ -694,79 +677,73 @@ static constexpr const char *main_state_str(uint8_t main_state) } } -transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks) +transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool force) { - // 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 (_armed.armed) { + return TRANSITION_NOT_CHANGED; + } + + // 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)) { - run_preflight_checks = false; + && (_last_disarmed_timestamp != 0) && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) { + + force = true; } - if (run_preflight_checks && !_arm_state_machine.isArmed()) { - if (_vehicle_control_mode.flag_control_manual_enabled) { - if (_vehicle_control_mode.flag_control_climb_rate_enabled && - !_status.rc_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"); - tune_negative(true); - return TRANSITION_DENIED; - } + if (!force) { + // run preflight checks one last time + runPreflight(); - if (!_vehicle_control_mode.flag_control_climb_rate_enabled && - !_status.rc_signal_lost && !_is_throttle_low && _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"); - tune_negative(true); - return TRANSITION_DENIED; - } - - } 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"); + if (!_armed.ready_to_arm) { tune_negative(true); return TRANSITION_DENIED; } - if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL) - && !_status_flags.home_position_valid) { - mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t"); - events::send(events::ID("commander_arm_denied_geofence_rtl"), - {events::Log::Critical, events::LogInternal::Info}, - "Arming denied: Geofence RTL requires valid home"); - tune_negative(true); - return TRANSITION_DENIED; + if (!_vehicle_control_mode.flag_control_manual_enabled) { + // non-manual mode + 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"); + + tune_negative(true); + return TRANSITION_DENIED; + } + } + + // Arm Requirements: authorization + // check last, and only if everything else has passed + if (_arm_requirements.arm_authorization) { + if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) { + // feedback provided in arm_auth_check + tune_negative(true); + return TRANSITION_DENIED; + } } } - 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); + _armed.armed = true; - 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); + _status.armed_time = hrt_absolute_time(); + _status.arming_state = vehicle_status_s::ARMING_STATE_ARMED; + _status.latest_arming_reason = (uint8_t)calling_reason; - _status_changed = true; + 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); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); - } - - return arming_res; + return TRANSITION_CHANGED; } transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced) { + if (!_armed.armed) { + return TRANSITION_NOT_CHANGED; + } + if (!forced) { const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed || is_ground_rover(_status)); const bool mc_manual_thrust_mode = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING @@ -788,27 +765,23 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f } } - 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); + _armed.armed = false; - 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); + _status.armed_time = 0; + _status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED; + _status.latest_disarming_reason = (uint8_t)calling_reason; - if (_param_com_force_safety.get()) { - _safety_handler.enableSafety(); - } + _last_disarmed_timestamp = hrt_absolute_time(); - _status_changed = 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); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); + if (_param_com_force_safety.get()) { + _safety_handler.enableSafety(); } - return arming_res; + return TRANSITION_CHANGED; } Commander::Commander() : @@ -817,15 +790,14 @@ Commander::Commander() : { _vehicle_land_detected.landed = true; + _status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED; + _status.system_id = 1; _status.component_id = 1; _status.system_type = 0; _status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN; - // XXX for now just set sensors as initialized - _status_flags.system_sensors_initialized = true; - // We want to accept RC inputs as default _status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; _status.nav_state_timestamp = hrt_absolute_time(); @@ -851,6 +823,9 @@ Commander::Commander() : _param_rc_map_fltmode = param_find("RC_MAP_FLTMODE"); updateParameters(); + + // run preflight immediately to find all relevant parameters, but don't report + PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode, false); } Commander::~Commander() @@ -1024,23 +999,13 @@ 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 == _status.system_id) - && (cmd.source_component == _status.component_id) - && (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) { - // TODO: replace with a proper allowed transition - _arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE); - } transition_result_t arming_res = TRANSITION_DENIED; arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal; if (arming_action == vehicle_command_s::ARMING_ACTION_ARM) { - arming_res = arm(arm_disarm_reason, cmd.from_external || !forced); + arming_res = arm(arm_disarm_reason, forced); } else if (arming_action == vehicle_command_s::ARMING_ACTION_DISARM) { arming_res = disarm(arm_disarm_reason, forced); @@ -1052,18 +1017,25 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { cmd_result = vehicle_command_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_pub.get().manual_home)) { - set_home_position(); - } } } } break; + case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS: { + // update preflight and force report + if (_armed.armed) { + // return MAV_RESULT_TEMPORARILY_REJECTED in the case the system is armed + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED; + + runPreflight(); + } + } + break; + case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: { if (cmd.param1 > 1.5f) { // Test termination command triggers lockdown but not actual termination. @@ -1349,7 +1321,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) { + } else if ((param1 == 1) && !_armed.armed && (px4_reboot_request(false, 400_ms) == 0)) { // 1: Reboot autopilot answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1359,7 +1331,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(BOARD_HAS_POWER_CONTROL) - } else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { + } else if ((param1 == 2) && !_armed.armed && (px4_shutdown_request(400_ms) == 0)) { // 2: Shutdown autopilot answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1369,7 +1341,7 @@ Commander::handle_command(const vehicle_command_s &cmd) #if defined(CONFIG_BOARDCTL_RESET) - } else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) { + } else if ((param1 == 3) && !_armed.armed && (px4_reboot_request(true, 400_ms) == 0)) { // 3: Reboot autopilot and keep it in the bootloader until upgraded. answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1386,27 +1358,12 @@ 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 (_armed.armed || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { - - /* try to go to INIT/PREFLIGHT arming state */ - 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 - 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT - (cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal)) - ) { - - answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED); - break; - - } - if ((int)(cmd.param1) == 1) { /* gyro calibration */ answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); @@ -1505,7 +1462,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 (_armed.armed || _worker_thread.isBusy()) { // reject if armed or shutting down answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); @@ -1540,9 +1497,8 @@ 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()) { - - // reject if armed or shutting down + if (_armed.armed || _worker_thread.isBusy()) { + // reject if armed or already busy answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED); } else { @@ -1609,7 +1565,6 @@ Commander::handle_command(const vehicle_command_s &cmd) case vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE: case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR: case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR: - case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE: /* ignore commands that are handled by other parts of the system */ break; @@ -1631,7 +1586,7 @@ Commander::handle_command(const vehicle_command_s &cmd) unsigned Commander::handle_command_motor_test(const vehicle_command_s &cmd) { - if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { + if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1679,7 +1634,7 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd) unsigned Commander::handle_command_actuator_test(const vehicle_command_s &cmd) { - if (_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { + if (_armed.armed || (_safety.safety_switch_available && !_safety.safety_off)) { return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED; } @@ -1759,7 +1714,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 (_armed.armed) { disarm(arm_disarm_reason); } else { @@ -1804,7 +1759,7 @@ void Commander::executeActionRequest(const action_request_s &action_request) // if there's never been a mode change force RC switch as initial state if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT - && !_arm_state_machine.isArmed() && (_internal_state.main_state_changes == 0) + && !_armed.armed && (_internal_state.main_state_changes == 0) && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL || action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) { _internal_state.main_state = action_request.mode; @@ -2171,16 +2126,13 @@ Commander::run() arm_auth_init(&_mavlink_log_pub, &_status.system_id); - // run preflight immediately to find all relevant parameters, but don't report - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode, - false, true, hrt_elapsed_time(&_boot_timestamp)); - while (!should_exit()) { perf_begin(_loop_perf); - const actuator_armed_s actuator_armed_prev{_armed}; - const vehicle_status_flags_s vehicle_status_flags_prev{_status_flags}; + const actuator_armed_s armed_prev{_armed}; + const vehicle_status_s status_prev{_status}; + //const vehicle_status_flags_s status_flags_prev{_status_flags}; /* update parameters */ const bool params_updated = _parameter_update_sub.updated(); @@ -2191,7 +2143,7 @@ Commander::run() _parameter_update_sub.copy(&update); /* update parameters */ - if (!_arm_state_machine.isArmed()) { + if (!_armed.armed) { updateParameters(); _status_changed = true; @@ -2209,7 +2161,7 @@ Commander::run() if (_power_button_state_sub.copy(&button_state)) { if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) { - if (shutdown_if_allowed() && (px4_shutdown_request() == 0)) { + if (px4_shutdown_request() == 0) { while (1) { px4_usleep(1); } } } @@ -2234,8 +2186,6 @@ Commander::run() } else { _status_flags.power_input_valid = true; } - - _system_power_usb_connected = system_power.usb_connected; } } @@ -2245,7 +2195,7 @@ Commander::run() _vehicle_land_detected_sub.copy(&_vehicle_land_detected); // Only take actions if armed - if (_arm_state_machine.isArmed()) { + if (_armed.armed) { 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"); @@ -2262,7 +2212,7 @@ Commander::run() if (_param_com_home_en.get() && !_home_pub.get().manual_home) { // 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)) { + if (!_vehicle_land_detected.landed) { if (was_landed) { set_home_position(); @@ -2289,7 +2239,7 @@ Commander::run() _safety.safety_switch_available, _status); // disarm if safety is now on and still armed - if (_arm_state_machine.isArmed() && _safety.safety_switch_available && !_safety.safety_off + if (_armed.armed && _safety.safety_switch_available && !_safety.safety_off && (_status.hil_state == vehicle_status_s::HIL_STATE_OFF)) { disarm(arm_disarm_reason_t::safety_button); } @@ -2385,10 +2335,12 @@ Commander::run() } } + cpuload_check(); + estimator_check(); // Auto disarm when landed or kill switch engaged - if (_arm_state_machine.isArmed()) { + if (_armed.armed) { // Check for auto-disarm on landing or pre-flight if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) { @@ -2450,16 +2402,6 @@ Commander::run() battery_status_check(); } - /* If in INIT state, try to proceed to STANDBY state */ - if (!_status_flags.calibration_enabled && _arm_state_machine.isInit()) { - - _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety, - vehicle_status_s::ARMING_STATE_STANDBY, _armed, - 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 */ if (_mission_result_sub.updated()) { const mission_result_s &mission_result = _mission_result_sub.get(); @@ -2506,7 +2448,7 @@ Commander::run() } // Transition main state to loiter or auto-mission after takeoff is completed. - if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed + if (_armed.armed && !_vehicle_land_detected.landed && (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF || _status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) && (mission_result.timestamp >= _status.nav_state_timestamp) @@ -2530,7 +2472,7 @@ Commander::run() const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0; // Geofence actions - if (_arm_state_machine.isArmed() + if (_armed.armed && (_geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE) && !in_low_battery_failsafe_delay) { @@ -2630,7 +2572,7 @@ Commander::run() } /* Check for mission flight termination */ - if (_arm_state_machine.isArmed() && _mission_result_sub.get().flight_termination && + if (_armed.armed && _mission_result_sub.get().flight_termination && !_status_flags.circuit_breaker_flight_termination_disabled) { @@ -2680,8 +2622,7 @@ Commander::run() const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC; - if (!_arm_state_machine.isArmed() && (is_mavlink || !_mode_switch_mapped) - && (_internal_state.main_state_changes == 0)) { + if (!_armed.armed && (is_mavlink || !_mode_switch_mapped) && (_internal_state.main_state_changes == 0)) { // if there's never been a mode change force position control as initial state _internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL; _internal_state.main_state_changes++; @@ -2715,7 +2656,7 @@ Commander::run() // but only if actually in air. if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) && !in_low_battery_failsafe_delay && !_geofence_warning_action_on - && _arm_state_machine.isArmed() + && _armed.armed && !_status_flags.rc_calibration_in_progress && manual_control_setpoint.valid && manual_control_setpoint.sticks_moving @@ -2751,8 +2692,10 @@ Commander::run() avoidance_check(); + cpuload_check(); + /* check if we are disarmed and there is a better mode to wait in */ - if (!_arm_state_machine.isArmed()) { + if (!_armed.armed) { /* if there is no radio control but GPS lock the user might want to fly using * just a tablet. Since the RC will force its mode switch setting on connecting * we can as well just wait in a hold mode which enables tablet control. @@ -2802,7 +2745,7 @@ Commander::run() auto fd_status_flags = _failure_detector.getStatusFlags(); _status_changed = true; - if (_arm_state_machine.isArmed()) { + if (_armed.armed) { if (fd_status_flags.arm_escs) { // 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs if (hrt_elapsed_time(&_status.armed_time) < 500_ms) { @@ -2929,7 +2872,7 @@ Commander::run() checkWindSpeedThresholds(); } - _status_flags.flight_terminated = _armed.force_failsafe || _armed.manual_lockdown; + _status_flags.flight_terminated = _armed.force_failsafe || _armed.lockdown || _armed.manual_lockdown; /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); @@ -2954,7 +2897,7 @@ Commander::run() // automatically set or update home position if (_param_com_home_en.get() && !_home_pub.get().manual_home) { - if (!_arm_state_machine.isArmed() && _vehicle_land_detected.landed) { + if (!_armed.armed && _vehicle_land_detected.landed) { const bool can_set_home_lpos_first_time = (!_home_pub.get().valid_lpos && _status_flags.local_position_valid); const bool can_set_home_gpos_first_time = ((!_home_pub.get().valid_hpos || !_home_pub.get().valid_alt) && (_status_flags.global_position_valid || _status_flags.gps_position_valid)); @@ -2969,29 +2912,6 @@ Commander::run() } } - // check for arming state change - if (_was_armed != _arm_state_machine.isArmed()) { - _status_changed = true; - - if (_arm_state_machine.isArmed()) { - if (!_vehicle_land_detected.landed) { // check if takeoff already detected upon arming - _have_taken_off_since_arming = true; - } - - } else { // increase the flight uuid upon disarming - 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(); - } - } - - if (!_arm_state_machine.isArmed()) { - /* Reset the flag if disarmed. */ - _have_taken_off_since_arming = false; - _imbalanced_propeller_check_triggered = false; - } /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(_status, @@ -3015,22 +2935,19 @@ Commander::run() _status.nav_state_timestamp = hrt_absolute_time(); } - if (_status.failsafe != _failsafe_old) { - _status_changed = true; - - if (_status.failsafe) { + if (_armed.armed) { + if (!status_prev.failsafe && _status.failsafe) { + // failsafe activated mavlink_log_info(&_mavlink_log_pub, "Failsafe mode activated\t"); events::send(events::ID("commander_failsafe_activated"), events::Log::Info, "Failsafe mode activated"); - } else { + } else if (status_prev.failsafe && !_status.failsafe) { + // failsafe deactivated mavlink_log_info(&_mavlink_log_pub, "Failsafe mode deactivated\t"); events::send(events::ID("commander_failsafe_deactivated"), events::Log::Info, "Failsafe mode deactivated"); } - - _failsafe_old = _status.failsafe; } - // prearm mode switch ((PrearmedMode)_param_com_prearm_mode.get()) { case PrearmedMode::DISABLED: @@ -3063,34 +2980,32 @@ Commander::run() break; } + if (!_status_flags.system_hotplug_timeout) { + _status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); + } // publish states (armed, control_mode, vehicle_status, commander_state, vehicle_status_flags, failure_detector_status) at 2 Hz or immediately when changed if (hrt_elapsed_time(&_status.timestamp) >= 500_ms || _status_changed || nav_state_changed - || !(_armed == actuator_armed_prev)) { + || (status_prev.failsafe != _status.failsafe) + || !(_armed == armed_prev)) { - // Evaluate current prearm status (skip during arm -> disarm transition) - if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) { + // Evaluate current preflight check and prearm status (skip during arm -> disarm transition) + if (!armed_prev.armed && !_armed.armed && !_status_flags.calibration_enabled) { - _status_flags.system_hotplug_timeout = (hrt_elapsed_time(&_boot_timestamp) > HOTPLUG_SENS_TIMEOUT); + bool report = false; - perf_begin(_preflight_check_perf); - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode, - false, true, hrt_elapsed_time(&_boot_timestamp)); - perf_end(_preflight_check_perf); + // force rerun if data link lost changed + if (status_prev.data_link_lost && !_status.data_link_lost && (hrt_elapsed_time(&_boot_timestamp) > 10_s)) { + report = true; + } - // skip arm authorization check until actual arming attempt - PreFlightCheck::arm_requirements_t arm_req = _arm_requirements; - arm_req.arm_authorization = false; - bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req, - _status, false); - - const bool prearm_check_ok = preflight_check_res && prearm_check_res; - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, prearm_check_ok, _status); + if (report || hrt_elapsed_time(&_last_preflight_check) > 300_ms) { + runPreflight(report, false); + _last_preflight_check = hrt_absolute_time(); + } } // publish actuator_armed first (used by output modules) - _armed.armed = _arm_state_machine.isArmed(); - _armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby(); _armed.timestamp = hrt_absolute_time(); _armed_pub.publish(_armed); @@ -3098,7 +3013,6 @@ Commander::run() update_control_mode(); // vehicle_status publish (after prearm/preflight updates above) - _status.arming_state = _arm_state_machine.getArmState(); _status.timestamp = hrt_absolute_time(); _status_pub.publish(_status); @@ -3126,48 +3040,52 @@ Commander::run() _failure_detector_status_pub.publish(fd_status); } - /* play arming and battery warning tunes */ - if (!_arm_tune_played && _arm_state_machine.isArmed() && - (_safety.safety_switch_available || (_safety.safety_switch_available && _safety.safety_off))) { + // check for arming state change + if (!armed_prev.armed && _armed.armed) { + // disarmed -> armed - /* play tune when armed */ - set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); - _arm_tune_played = true; - - } else if (!_status_flags.usb_connected && - (_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); - - } else if ((_status.hil_state != vehicle_status_s::HIL_STATE_ON) && - (_battery_warning == battery_status_s::BATTERY_WARNING_LOW)) { - /* play tune on battery warning */ - set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); - - } else if (_status.failsafe && _arm_state_machine.isArmed()) { - tune_failsafe(true); - - } else { - set_tune(tune_control_s::TUNE_ID_STOP); - } - - /* reset arm_tune_played when disarmed */ - if (!_arm_state_machine.isArmed() || (_safety.safety_switch_available && !_safety.safety_off)) { - - // Notify the user that it is safe to approach the vehicle - if (_arm_tune_played) { - tune_neutral(true); + // check if takeoff already detected upon arming + if (!_vehicle_land_detected.landed) { + _have_taken_off_since_arming = true; } - _arm_tune_played = false; + } else if (armed_prev.armed && !_armed.armed) { + // armed -> disarmed + + // increase the flight uuid upon disarming + const int32_t flight_uuid = _param_flight_uuid.get() + 1; + _param_flight_uuid.set(flight_uuid); + _param_flight_uuid.commit_no_notification(); + + // Reset the flag if disarmed + _have_taken_off_since_arming = false; + _imbalanced_propeller_check_triggered = false; } - /* play sensor failure tunes if we already waited for hotplug sensors to come up and failed */ - if (!_status_flags.system_sensors_initialized && - !vehicle_status_flags_prev.system_hotplug_timeout && _status_flags.system_hotplug_timeout) { + // play arming and battery warning tunes + if (_armed.armed) { + if (!armed_prev.armed) { + // play tune when armed + set_tune(tune_control_s::TUNE_ID_ARMING_WARNING); - set_tune_override(tune_control_s::TUNE_ID_GPS_WARNING); + } else if (_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL) { + + // play tune on battery critical + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST); + + } else if (_battery_warning == battery_status_s::BATTERY_WARNING_LOW) { + // play tune on battery warning + set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW); + + } else if (_status.failsafe) { + tune_failsafe(true); + } + + } else if (!_armed.armed) { + if (armed_prev.armed) { + // Notify the user that it is safe to approach the vehicle + tune_neutral(true); + } } // check if the worker has finished @@ -3196,11 +3114,9 @@ Commander::run() _last_local_position_valid = _status_flags.local_position_valid; _last_global_position_valid = _status_flags.global_position_valid; - _was_armed = _arm_state_machine.isArmed(); - arm_auth_update(now, params_updated); - px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed()); + px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed); perf_end(_loop_perf); @@ -3276,30 +3192,30 @@ 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 = _armed.armed ? 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 (_armed.armed) { led_mode = led_control_s::MODE_ON; set_normal_color = true; - } else if (!_status_flags.system_sensors_initialized && _status_flags.system_hotplug_timeout) { + } else if (!_status_flags.preflight_check_passing && _status_flags.system_hotplug_timeout) { led_mode = led_control_s::MODE_BLINK_FAST; led_color = led_control_s::COLOR_RED; - } else if (_arm_state_machine.isStandby()) { + } else if (!_armed.armed && _armed.ready_to_arm) { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - } else if (!_status_flags.system_sensors_initialized && !_status_flags.system_hotplug_timeout) { + } else if (!_status_flags.preflight_check_passing && !_status_flags.system_hotplug_timeout) { led_mode = led_control_s::MODE_BREATHE; set_normal_color = true; - } else if (_arm_state_machine.isInit()) { + } else if (!_status_flags.system_hotplug_timeout) { // if in init status it should not be in the error state led_mode = led_control_s::MODE_OFF; @@ -3339,7 +3255,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 (_armed.armed) { if (_status.failsafe) { BOARD_ARMED_LED_OFF(); @@ -3355,7 +3271,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 (_armed.ready_to_arm) { BOARD_ARMED_LED_OFF(); // ready to arm, blink at 1Hz @@ -3430,7 +3346,7 @@ Commander::update_control_mode() _vehicle_control_mode = {}; /* set vehicle_control_mode according to set_navigation_state */ - _vehicle_control_mode.flag_armed = _arm_state_machine.isArmed(); + _vehicle_control_mode.flag_armed = _armed.armed; switch (_status.nav_state) { case vehicle_status_s::NAVIGATION_STATE_MANUAL: @@ -3579,7 +3495,7 @@ Commander::print_reject_mode(uint8_t main_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(_armed.armed); _last_print_mode_reject_time = hrt_absolute_time(); } @@ -3710,12 +3626,6 @@ void Commander::data_link_check() mavlink_log_info(&_mavlink_log_pub, "Data link regained\t"); events::send(events::ID("commander_dl_regained"), events::Log::Info, "Data link regained"); } - - if (!_arm_state_machine.isArmed() && !_status_flags.calibration_enabled) { - // make sure to report preflight check failures to a connecting GCS - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode, - true, false, hrt_elapsed_time(&_boot_timestamp)); - } } _datalink_last_heartbeat_gcs = telemetry.timestamp; @@ -3885,7 +3795,7 @@ void Commander::battery_status_check() battery_required_count++; } - if (_arm_state_machine.isArmed()) { + if (_armed.armed) { if ((_last_connected_batteries & (1 << index)) && !battery.connected) { mavlink_log_critical(&_mavlink_log_pub, "Battery %d disconnected. Land now! \t", index + 1); @@ -3931,11 +3841,9 @@ void Commander::battery_status_check() fault_index++) { if (battery.faults & (1 << fault_index)) { mavlink_log_emergency(&_mavlink_log_pub, "Battery %d: %s. %s \t", index + 1, - battery_fault_reason_str(static_cast(fault_index)), - _arm_state_machine.isArmed() ? "Land now!" : ""); + battery_fault_reason_str(static_cast(fault_index)), _armed.armed ? "Land now!" : ""); - events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ? - events::px4::enums::suggested_action_t::land : + events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land : events::px4::enums::suggested_action_t::none; /* EVENT @@ -3969,7 +3877,7 @@ void Commander::battery_status_check() if (_rtl_time_estimate_sub.copy(&rtl_time_estimate) && (hrt_absolute_time() - rtl_time_estimate.timestamp) < 2_s && rtl_time_estimate.valid - && _arm_state_machine.isArmed() + && _armed.armed && !_vehicle_land_detected.ground_contact // not in any landing stage && !_rtl_time_actions_done && PX4_ISFINITE(worst_battery_time_s) @@ -3995,7 +3903,7 @@ void Commander::battery_status_check() bool battery_warning_level_increased_while_armed = false; bool update_internal_battery_state = false; - if (_arm_state_machine.isArmed()) { + if (_armed.armed) { if (worst_warning > _battery_warning) { battery_warning_level_increased_while_armed = true; update_internal_battery_state = true; @@ -4059,7 +3967,7 @@ void Commander::battery_status_check() if (_battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) { #if defined(BOARD_HAS_POWER_CONTROL) - if (shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) { + if (!_armed.armed && (px4_shutdown_request(400_ms) == 0)) { mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down\t"); events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning}, "Dangerously low battery! Shutting system down"); @@ -4080,6 +3988,15 @@ void Commander::battery_status_check() } } +void Commander::cpuload_check() +{ + cpuload_s cpuload; + + if (_cpuload_sub.update(&cpuload)) { + _status_flags.overload = (cpuload.load > (_param_com_cpu_max.get() * 0.01f)) || (cpuload.ram_usage >= 0.99f); + } +} + void Commander::estimator_check() { // Check if quality checking of position accuracy and consistency is to be performed @@ -4168,7 +4085,8 @@ void Commander::estimator_check() if (run_quality_checks && _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - if (!_arm_state_machine.isArmed()) { + if (!_armed.armed) { + // reset _nav_test_failed = false; _nav_test_passed = false; @@ -4232,7 +4150,7 @@ void Commander::estimator_check() bool xy_valid = lpos.xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; - if (!_arm_state_machine.isArmed()) { + if (!_armed.armed) { if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { xy_valid = false; } @@ -4339,6 +4257,7 @@ void Commander::estimator_check() if (condition_gps_position_was_valid && !_status_flags.gps_position_valid) { PX4_DEBUG("GPS no longer valid"); + set_tune(tune_control_s::TUNE_ID_GPS_WARNING); } } @@ -4423,8 +4342,7 @@ void Commander::esc_status_check() if ((esc_status.esc_online_flags & (1 << index)) == 0) { snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1); esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0'; - events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ? - events::px4::enums::suggested_action_t::land : + events::px4::enums::suggested_action_t action = _armed.armed ? events::px4::enums::suggested_action_t::land : events::px4::enums::suggested_action_t::none; // TODO: use esc_status.esc[index].actuator_function as index after SYS_CTRL_ALLOC becomes default events::send(events::ID("commander_esc_offline"), @@ -4432,8 +4350,7 @@ void Commander::esc_status_check() } } - mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, - _arm_state_machine.isArmed() ? "Land now!" : ""); + mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg, _armed.armed ? "Land now!" : ""); _last_esc_online_flags = esc_status.esc_online_flags; _status_flags.escs_error = true; @@ -4467,7 +4384,7 @@ void Commander::esc_status_check() } mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1, - esc_fault_reason_str(fault_reason_index), _arm_state_machine.isArmed() ? user_action : ""); + esc_fault_reason_str(fault_reason_index), _armed.armed ? user_action : ""); events::send (events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning}, @@ -4543,6 +4460,26 @@ void Commander::checkWindSpeedThresholds() } } +void Commander::runPreflight(bool report_precheck, bool report_prearm) +{ + if (!_status_flags.calibration_enabled) { + perf_begin(_preflight_check_perf); + _status_flags.preflight_check_passing = PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, + _vehicle_control_mode, report_precheck); + perf_end(_preflight_check_perf); + + _status_flags.prearm_check_passing = preArmCheck(_status_flags, _vehicle_control_mode, _safety, _arm_requirements, + _status, report_prearm); + + _armed.ready_to_arm = _status_flags.preflight_check_passing && _status_flags.prearm_check_passing; + + } else { + _armed.ready_to_arm = false; + } + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, _armed.ready_to_arm, _status); +} + int Commander::print_usage(const char *reason) { if (reason) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 5397d87c15..d2d0cd6291 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -34,7 +34,6 @@ #pragma once /* Helper classes */ -#include "Arming/ArmStateMachine/ArmStateMachine.hpp" #include "Arming/PreFlightCheck/PreFlightCheck.hpp" #include "failure_detector/FailureDetector.hpp" #include "Safety.hpp" @@ -98,6 +97,8 @@ using systemlib::Hysteresis; using namespace time_literals; +using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t; + class Commander : public ModuleBase, public ModuleParams { public: @@ -129,7 +130,7 @@ public: private: 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); + transition_result_t arm(arm_disarm_reason_t calling_reason, bool force = false); transition_result_t disarm(arm_disarm_reason_t calling_reason, bool forced = false); void battery_status_check(); @@ -146,9 +147,8 @@ private: void data_link_check(); void avoidance_check(); - + void cpuload_check(); void esc_status_check(); - void estimator_check(); bool handle_command(const vehicle_command_s &cmd); @@ -175,14 +175,26 @@ private: void update_control_mode(); - bool shutdown_if_allowed(); - bool stabilization_required(); void send_parachute_command(); void checkWindSpeedThresholds(); + struct arm_requirements_t { + bool arm_authorization = false; + bool esc_check = false; + bool global_position = false; + bool mission = false; + bool geofence = false; + }; + + bool preArmCheck(const vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, + const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status, + bool report_fail = true); + + void runPreflight(bool report_preflight = true, bool report_prearm = true); + void updateParameters(); DEFINE_PARAMETERS( @@ -257,6 +269,7 @@ private: (ParamInt) _param_cbrk_velposerr, (ParamInt) _param_cbrk_vtolarming, + (ParamFloat) _param_com_cpu_max, (ParamInt) _param_com_flt_time_max, (ParamFloat) _param_com_wind_max ) @@ -291,10 +304,8 @@ private: static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms}; 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{}; + arm_requirements_t _arm_requirements{}; hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */ @@ -371,6 +382,7 @@ private: hrt_abstime _boot_timestamp{0}; hrt_abstime _last_disarmed_timestamp{0}; + hrt_abstime _last_preflight_check{0}; hrt_abstime _overload_start{0}; ///< time when CPU overload started hrt_abstime _led_armed_state_toggle{0}; @@ -381,11 +393,7 @@ private: uint8_t _heading_reset_counter{0}; bool _status_changed{true}; - bool _arm_tune_played{false}; - bool _was_armed{false}; - bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag bool _have_taken_off_since_arming{false}; - bool _system_power_usb_connected{false}; geofence_result_s _geofence_result{}; vehicle_land_detected_s _vehicle_land_detected{}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f1bb34c84c..13bdeb0c67 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -967,7 +967,7 @@ PARAM_DEFINE_FLOAT(COM_KILL_DISARM, 5.0f); * @max 100 * @increment 1 */ -PARAM_DEFINE_FLOAT(COM_CPU_MAX, 90.0f); +PARAM_DEFINE_FLOAT(COM_CPU_MAX, 95.0f); /** * Required number of redundant power modules 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 4e76c256c9..01c6fbba4f 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -42,7 +42,6 @@ #include "../state_machine_helper.h" #include -#include "../Arming/ArmStateMachine/ArmStateMachine.hpp" #include "../Arming/PreFlightCheck/PreFlightCheck.hpp" class StateMachineHelperTest : public UnitTest diff --git a/src/modules/commander/preArmCheck.cpp b/src/modules/commander/preArmCheck.cpp new file mode 100644 index 0000000000..b9ceb48aef --- /dev/null +++ b/src/modules/commander/preArmCheck.cpp @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (c) 2019-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 "Commander.hpp" + +#include + +bool Commander::preArmCheck(const vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, + const safety_s &safety, const arm_requirements_t &arm_requirements, const vehicle_status_s &status, + bool report_fail) +{ + bool prearm_ok = true; + + // rate control mode require valid angular velocity + if (control_mode.flag_control_rates_enabled && !status_flags.angular_velocity_valid) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! angular velocity invalid"); } + + prearm_ok = false; + } + + // attitude control mode require valid attitude + if (control_mode.flag_control_attitude_enabled && !status_flags.attitude_valid) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! attitude invalid"); } + + prearm_ok = false; + } + + // velocity control mode require valid velocity + if (control_mode.flag_control_velocity_enabled && !status_flags.local_velocity_valid) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! velocity invalid"); } + + prearm_ok = false; + } + + // position control mode require valid position + if (control_mode.flag_control_position_enabled && !status_flags.local_position_valid) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! position invalid"); } + + prearm_ok = false; + } + + // manual control mode require valid manual control (rc) + if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! manual control lost"); } + + prearm_ok = false; + } + + if (status_flags.flight_terminated) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Flight termination active"); } + + prearm_ok = false; + } + + // USB not connected + if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Flying with USB is not safe"); } + + prearm_ok = false; + } + + // battery and system power status + if (!status_flags.circuit_breaker_engaged_power_check) { + + // Fail transition if power is not good + if (!status_flags.power_input_valid) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Connect power module"); } + + prearm_ok = false; + } + + // Only arm if healthy + if (!status_flags.battery_healthy) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Check battery"); } + } + + prearm_ok = false; + } + } + + // Arm Requirements: mission + if (arm_requirements.mission) { + + if (!status_flags.auto_mission_available) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! No valid mission"); } + } + + prearm_ok = false; + } + + if (!status_flags.global_position_valid) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Missions require a global position"); } + } + + prearm_ok = false; + } + } + + if (arm_requirements.global_position && !status_flags.circuit_breaker_engaged_posfailure_check) { + + if (!status_flags.global_position_valid) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Global position required"); } + } + + prearm_ok = false; + } + + if (!status_flags.home_position_valid) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Home position invalid"); } + } + + prearm_ok = false; + } + } + + // safety button + if (safety.safety_switch_available && !safety.safety_off) { + // Fail transition if we need safety switch press + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Press safety switch first"); } + } + + prearm_ok = false; + } + + if (status_flags.avoidance_system_required && !status_flags.avoidance_system_valid) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Avoidance system not ready"); } + } + + prearm_ok = false; + + } + + + if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_climb_rate_enabled && + !_status.rc_signal_lost && _is_throttle_above_center) { + + if (report_fail) { + 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"); + } + + prearm_ok = false; + } + + if (!control_mode.flag_control_climb_rate_enabled && + !_status.rc_signal_lost && !_is_throttle_low && _status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) { + + if (report_fail) { + 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"); + } + + prearm_ok = false; + } + + } + + if ((_geofence_result.geofence_action == geofence_result_s::GF_ACTION_RTL) + && !status_flags.home_position_valid) { + + if (report_fail) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied: Geofence RTL requires valid home\t"); + events::send(events::ID("commander_arm_denied_geofence_rtl"), {events::Log::Critical, events::LogInternal::Info}, + "Arming denied: Geofence RTL requires valid home"); + } + + prearm_ok = false; + } + + // failure detector + if (status.failure_detector_status != vehicle_status_s::FAILURE_NONE) { + if (report_fail) { + if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Roll failure detected"); + } + + if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Pitch failure detected"); + } + + if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Altitude failure detected"); + } + + if (status.failure_detector_status & vehicle_status_s::FAILURE_EXT) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Parachute failure detected"); + } + + if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied! ESC failure detected"); + } + } + + prearm_ok = false; + } + + if (arm_requirements.esc_check && status_flags.escs_error) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! One or more ESCs are offline"); } + + prearm_ok = false; + } + } + + if (arm_requirements.esc_check && status_flags.escs_failure) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! One or more ESCs have a failure"); } + + prearm_ok = false; + } + } + + if (status.is_vtol) { + + if (status.in_transition_mode) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Vehicle is in transition state"); } + + prearm_ok = false; + } + } + + if (!status_flags.circuit_breaker_vtol_fw_arming_check + && status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { + if (prearm_ok) { + if (report_fail) { mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Vehicle is not in multicopter mode"); } + + prearm_ok = false; + } + } + } + + if (arm_requirements.geofence && status.geofence_violated) { + if (report_fail) { + mavlink_log_critical(&_mavlink_log_pub, "Arming denied, vehicle outside geofence"); + } + + prearm_ok = false; + } + + return prearm_ok; +} diff --git a/src/modules/mavlink/streams/HEARTBEAT.hpp b/src/modules/mavlink/streams/HEARTBEAT.hpp index 4da54af5a9..86fff1f427 100644 --- a/src/modules/mavlink/streams/HEARTBEAT.hpp +++ b/src/modules/mavlink/streams/HEARTBEAT.hpp @@ -112,27 +112,28 @@ 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: - system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE; - break; + if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + if (!vehicle_status.failsafe) { + system_status = MAV_STATE_ACTIVE; - case vehicle_status_s::ARMING_STATE_STANDBY: - system_status = MAV_STATE_STANDBY; - break; + } else { + system_status = MAV_STATE_CRITICAL; + } - case vehicle_status_s::ARMING_STATE_SHUTDOWN: - system_status = MAV_STATE_POWEROFF; - break; + } else { + // disarmed + if (vehicle_status_flags.calibration_enabled) { + system_status = MAV_STATE_CALIBRATING; + + } else { + system_status = MAV_STATE_STANDBY; + } } // system_status overrides if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) { system_status = MAV_STATE_FLIGHT_TERMINATION; - - } else if (vehicle_status_flags.calibration_enabled) { - system_status = MAV_STATE_CALIBRATING; }