mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 20:03:54 +08:00
commander: collapse ArmStateMachine and simplify
- simplify vehicle_status.arming_state down to just armed and disarmed
- ARMING_STATE_INIT doesn't matter
- ARMING_STATE_STANDBY is effectively pre_flight_checks_pass
- ARMING_STATE_STANDBY_ERROR not needed
- ARMING_STATE_SHUTDOWN effectively not used (all the poweroff/shutdown calls loop forever in place)
- ARMING_STATE_IN_AIR_RESTORE doesn't exist anymore
- collapse ArmStateMachine into commander
- all requests already go through Commander::arm() and Commander::dismarm()
- other minor changes
- VEHICLE_CMD_DO_FLIGHTTERMINATION undocumented (unused?) test command (param1 > 1.5f) removed
- switching to NAVIGATION_STATE_TERMINATION triggers parachute command centrally (only if armed)
---------
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
GitHub
@@ -6,13 +6,8 @@ uint64 armed_time # Arming timestamp (microseconds)
|
||||
uint64 takeoff_time # Takeoff timestamp (microseconds)
|
||||
|
||||
uint8 arming_state
|
||||
uint8 ARMING_STATE_INIT = 0
|
||||
uint8 ARMING_STATE_STANDBY = 1
|
||||
uint8 ARMING_STATE_ARMED = 2
|
||||
uint8 ARMING_STATE_STANDBY_ERROR = 3
|
||||
uint8 ARMING_STATE_SHUTDOWN = 4
|
||||
uint8 ARMING_STATE_IN_AIR_RESTORE = 5
|
||||
uint8 ARMING_STATE_MAX = 6
|
||||
uint8 ARMING_STATE_DISARMED = 1
|
||||
uint8 ARMING_STATE_ARMED = 2
|
||||
|
||||
uint8 latest_arming_reason
|
||||
uint8 latest_disarming_reason
|
||||
|
||||
@@ -1,174 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "ArmStateMachine.hpp"
|
||||
|
||||
#include <systemlib/mavlink_log.h>
|
||||
|
||||
constexpr bool
|
||||
ArmStateMachine::arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX];
|
||||
|
||||
transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
|
||||
const arming_state_t new_arming_state, actuator_armed_s &armed, HealthAndArmingChecks &checks,
|
||||
const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason)
|
||||
{
|
||||
// Double check that our static arrays are still valid
|
||||
static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
|
||||
static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1,
|
||||
"ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
|
||||
|
||||
transition_result_t ret = TRANSITION_DENIED;
|
||||
bool feedback_provided = false;
|
||||
|
||||
/* only check transition if the new state is actually different from the current one */
|
||||
if (new_arming_state == _arm_state) {
|
||||
ret = TRANSITION_NOT_CHANGED;
|
||||
|
||||
} else {
|
||||
// Check that we have a valid state transition
|
||||
bool valid_transition = arming_transitions[new_arming_state][_arm_state];
|
||||
|
||||
// Preflight check
|
||||
if (valid_transition
|
||||
&& (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|
||||
&& fRunPreArmChecks
|
||||
&& !(status.hil_state == vehicle_status_s::HIL_STATE_ON)
|
||||
&& (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) {
|
||||
|
||||
checks.update();
|
||||
|
||||
if (!checks.canArm(status.nav_state)) {
|
||||
feedback_provided = true; // Preflight checks report error messages
|
||||
valid_transition = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
/* enforce lockdown in HIL */
|
||||
armed.lockdown = true;
|
||||
|
||||
/* recover from a prearm fail */
|
||||
if (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
|
||||
_arm_state = vehicle_status_s::ARMING_STATE_STANDBY;
|
||||
}
|
||||
|
||||
// HIL can always go to standby
|
||||
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
|
||||
valid_transition = true;
|
||||
}
|
||||
}
|
||||
|
||||
// Finish up the state transition
|
||||
if (valid_transition) {
|
||||
ret = TRANSITION_CHANGED;
|
||||
|
||||
// Record arm/disarm reason
|
||||
if (isArmed() && (new_arming_state != vehicle_status_s::ARMING_STATE_ARMED)) { // disarm transition
|
||||
status.latest_disarming_reason = (uint8_t)calling_reason;
|
||||
|
||||
} else if (!isArmed() && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) { // arm transition
|
||||
status.latest_arming_reason = (uint8_t)calling_reason;
|
||||
}
|
||||
|
||||
// Switch state
|
||||
_arm_state = new_arming_state;
|
||||
|
||||
if (isArmed()) {
|
||||
status.armed_time = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
status.armed_time = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (ret == TRANSITION_DENIED) {
|
||||
/* print to MAVLink and console if we didn't provide any feedback yet */
|
||||
if (!feedback_provided) {
|
||||
// FIXME: this catch-all does not provide helpful information to the user
|
||||
mavlink_log_critical(mavlink_log_pub, "Transition denied: %s to %s\t",
|
||||
getArmStateName(_arm_state), getArmStateName(new_arming_state));
|
||||
events::send<events::px4::enums::arming_state_t, events::px4::enums::arming_state_t>(
|
||||
events::ID("commander_transition_denied"), events::Log::Critical,
|
||||
"Arming state transition denied: {1} to {2}",
|
||||
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;
|
||||
}
|
||||
@@ -1,95 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "../../HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <px4_platform_common/events.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
typedef enum {
|
||||
TRANSITION_DENIED = -1,
|
||||
TRANSITION_NOT_CHANGED = 0,
|
||||
TRANSITION_CHANGED
|
||||
} transition_result_t;
|
||||
|
||||
using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
|
||||
|
||||
class ArmStateMachine
|
||||
{
|
||||
public:
|
||||
ArmStateMachine() = default;
|
||||
~ArmStateMachine() = default;
|
||||
|
||||
void forceArmState(uint8_t new_arm_state) { _arm_state = new_arm_state; }
|
||||
|
||||
transition_result_t
|
||||
arming_state_transition(vehicle_status_s &status, const arming_state_t new_arming_state,
|
||||
actuator_armed_s &armed, HealthAndArmingChecks &checks, const bool fRunPreArmChecks,
|
||||
orb_advert_t *mavlink_log_pub, arm_disarm_reason_t calling_reason);
|
||||
|
||||
// Getters
|
||||
uint8_t getArmState() const { return _arm_state; }
|
||||
|
||||
bool isInit() const { return (_arm_state == vehicle_status_s::ARMING_STATE_INIT); }
|
||||
bool isStandby() const { return (_arm_state == vehicle_status_s::ARMING_STATE_STANDBY); }
|
||||
bool isArmed() const { return (_arm_state == vehicle_status_s::ARMING_STATE_ARMED); }
|
||||
bool isShutdown() const { return (_arm_state == vehicle_status_s::ARMING_STATE_SHUTDOWN); }
|
||||
|
||||
static const char *getArmStateName(uint8_t arming_state);
|
||||
const char *getArmStateName() const { return getArmStateName(_arm_state); }
|
||||
|
||||
private:
|
||||
static inline events::px4::enums::arming_state_t getArmStateEvent(uint8_t arming_state);
|
||||
|
||||
uint8_t _arm_state{vehicle_status_s::ARMING_STATE_INIT};
|
||||
|
||||
// This array defines the arming state transitions. The rows are the new state, and the columns
|
||||
// are the current state. Using new state and current state you can index into the array which
|
||||
// will be true for a valid transition or false for a invalid transition. In some cases even
|
||||
// though the transition is marked as true additional checks must be made. See arming_state_transition
|
||||
// code for those checks.
|
||||
static constexpr bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
|
||||
= {
|
||||
// INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE
|
||||
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false },
|
||||
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false },
|
||||
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true },
|
||||
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false },
|
||||
{ /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true },
|
||||
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI
|
||||
};
|
||||
};
|
||||
@@ -1,273 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include "ArmStateMachine.hpp"
|
||||
|
||||
TEST(ArmStateMachineTest, ArmingStateTransitionTest)
|
||||
{
|
||||
ArmStateMachine arm_state_machine;
|
||||
|
||||
// These are the critical values from vehicle_status_s and actuator_armed_s which must be primed
|
||||
// to simulate machine state prior to testing an arming state transition. This structure is also
|
||||
// use to represent the expected machine state after the transition has been requested.
|
||||
typedef struct {
|
||||
arming_state_t arming_state; // vehicle_status_s.arming_state
|
||||
bool armed; // actuator_armed_s.armed
|
||||
} ArmingTransitionVolatileState_t;
|
||||
|
||||
// This structure represents a test case for arming_state_transition. It contains the machine
|
||||
// state prior to transition, the requested state to transition to and finally the expected
|
||||
// machine state after transition.
|
||||
typedef struct {
|
||||
const char *assertMsg; // Text to show when test case fails
|
||||
ArmingTransitionVolatileState_t current_state; // Machine state prior to transition
|
||||
hil_state_t hil_state; // Current vehicle_status_s.hil_state
|
||||
bool safety_button_available; // Current safety_s.safety_button_available
|
||||
bool safety_off; // Current safety_s.safety_off
|
||||
arming_state_t requested_state; // Requested arming state to transition to
|
||||
ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition
|
||||
transition_result_t expected_transition_result; // Expected result from arming_state_transition
|
||||
} ArmingTransitionTest_t;
|
||||
|
||||
// We use these defines so that our test cases are more readable
|
||||
static constexpr bool ATT_ARMED = true;
|
||||
static constexpr bool ATT_DISARMED = false;
|
||||
static constexpr bool ATT_SAFETY_AVAILABLE = true;
|
||||
static constexpr bool ATT_SAFETY_NOT_AVAILABLE = true;
|
||||
static constexpr bool ATT_SAFETY_OFF = true;
|
||||
static constexpr bool ATT_SAFETY_ON = false;
|
||||
|
||||
// These are test cases for arming_state_transition
|
||||
static const ArmingTransitionTest_t rgArmingTransitionTests[] = {
|
||||
// TRANSITION_NOT_CHANGED tests
|
||||
|
||||
{
|
||||
"no transition: identical states",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_NOT_CHANGED
|
||||
},
|
||||
|
||||
// TRANSITION_CHANGED tests
|
||||
|
||||
// Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s
|
||||
|
||||
{
|
||||
"transition: init to standby",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: init to standby error",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: init to reboot",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: standby to init",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: standby to standby error",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY_ERROR,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: standby to reboot",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: armed to standby",
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: standby error to reboot",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: in air restore to armed",
|
||||
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: in air restore to reboot",
|
||||
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
// hil on tests, standby error to standby not normally allowed
|
||||
|
||||
{
|
||||
"transition: standby error to standby, hil on",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
// Safety button arming tests
|
||||
|
||||
{
|
||||
"transition: standby to armed, no safety button",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
{
|
||||
"transition: standby to armed, safety button off",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_ON, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_CHANGED
|
||||
},
|
||||
|
||||
// TRANSITION_DENIED tests
|
||||
|
||||
// Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s
|
||||
|
||||
{
|
||||
"no transition: init to armed",
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_INIT, ATT_DISARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
{
|
||||
"no transition: armed to init",
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_INIT,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
{
|
||||
"no transition: armed to reboot",
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_SHUTDOWN,
|
||||
{ vehicle_status_s::ARMING_STATE_ARMED, ATT_ARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
{
|
||||
"no transition: standby error to armed",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
{
|
||||
"no transition: standby error to standby",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY_ERROR, ATT_DISARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
{
|
||||
"no transition: reboot to armed",
|
||||
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_SHUTDOWN, ATT_DISARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
{
|
||||
"no transition: in air restore to standby",
|
||||
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_STANDBY,
|
||||
{ vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED}, TRANSITION_DENIED
|
||||
},
|
||||
|
||||
// Safety button arming tests
|
||||
|
||||
{
|
||||
"no transition: init to armed, safety button on",
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, vehicle_status_s::HIL_STATE_OFF, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON,
|
||||
vehicle_status_s::ARMING_STATE_ARMED,
|
||||
{ vehicle_status_s::ARMING_STATE_STANDBY, ATT_DISARMED}, TRANSITION_DENIED
|
||||
},
|
||||
};
|
||||
|
||||
struct vehicle_status_s status {};
|
||||
struct actuator_armed_s armed {};
|
||||
|
||||
size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]);
|
||||
|
||||
for (size_t i = 0; i < cArmingTransitionTests; i++) {
|
||||
const ArmingTransitionTest_t *test = &rgArmingTransitionTests[i];
|
||||
|
||||
// Setup initial machine state
|
||||
arm_state_machine.forceArmState(test->current_state.arming_state);
|
||||
status.hil_state = test->hil_state;
|
||||
|
||||
HealthAndArmingChecks health_and_arming_checks(nullptr, status);
|
||||
|
||||
// Attempt transition
|
||||
transition_result_t result = arm_state_machine.arming_state_transition(
|
||||
status,
|
||||
test->requested_state,
|
||||
armed,
|
||||
health_and_arming_checks,
|
||||
true /* enable pre-arm checks */,
|
||||
nullptr /* no mavlink_log_pub */,
|
||||
arm_disarm_reason_t::unit_test);
|
||||
|
||||
// Validate result of transition
|
||||
EXPECT_EQ(result, test->expected_transition_result) << test->assertMsg;
|
||||
EXPECT_EQ(arm_state_machine.getArmState(), test->expected_state.arming_state) << test->assertMsg;
|
||||
EXPECT_EQ(arm_state_machine.isArmed(), test->expected_state.armed) << test->assertMsg;
|
||||
}
|
||||
}
|
||||
@@ -1,44 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
|
||||
px4_add_library(ArmStateMachine
|
||||
ArmStateMachine.cpp
|
||||
)
|
||||
target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks)
|
||||
|
||||
px4_add_functional_gtest(SRC ArmStateMachineTest.cpp
|
||||
LINKLIBS ArmStateMachine health_and_arming_checks hysteresis sensor_calibration ArmAuthorization mode_util
|
||||
)
|
||||
|
||||
@@ -32,4 +32,3 @@
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(ArmAuthorization)
|
||||
add_subdirectory(ArmStateMachine)
|
||||
|
||||
@@ -66,7 +66,6 @@ px4_add_module(
|
||||
health_and_arming_checks
|
||||
hysteresis
|
||||
ArmAuthorization
|
||||
ArmStateMachine
|
||||
sensor_calibration
|
||||
world_magnetic_model
|
||||
mode_util
|
||||
|
||||
+142
-197
File diff suppressed because it is too large
Load Diff
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2017-2022 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2017-2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -34,7 +34,6 @@
|
||||
#pragma once
|
||||
|
||||
/* Helper classes */
|
||||
#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
|
||||
#include "failure_detector/FailureDetector.hpp"
|
||||
#include "failsafe/failsafe.h"
|
||||
#include "Safety.hpp"
|
||||
@@ -87,6 +86,14 @@
|
||||
using math::constrain;
|
||||
using systemlib::Hysteresis;
|
||||
|
||||
typedef enum {
|
||||
TRANSITION_DENIED = -1,
|
||||
TRANSITION_NOT_CHANGED = 0,
|
||||
TRANSITION_CHANGED
|
||||
} transition_result_t;
|
||||
|
||||
using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class Commander : public ModuleBase<Commander>, public ModuleParams
|
||||
@@ -116,6 +123,8 @@ public:
|
||||
void enable_hil();
|
||||
|
||||
private:
|
||||
bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); }
|
||||
|
||||
void answer_command(const vehicle_command_s &cmd, uint8_t result);
|
||||
|
||||
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
|
||||
@@ -153,8 +162,6 @@ private:
|
||||
|
||||
void updateControlMode();
|
||||
|
||||
bool shutdownIfAllowed();
|
||||
|
||||
void send_parachute_command();
|
||||
|
||||
void checkForMissionUpdate();
|
||||
@@ -196,11 +203,9 @@ private:
|
||||
|
||||
/* Decouple update interval and hysteresis counters, all depends on intervals */
|
||||
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
|
||||
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
|
||||
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
ArmStateMachine _arm_state_machine{};
|
||||
Failsafe _failsafe_instance{this};
|
||||
FailsafeBase &_failsafe{_failsafe_instance};
|
||||
FailureDetector _failure_detector{this};
|
||||
@@ -242,9 +247,6 @@ private:
|
||||
|
||||
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
|
||||
|
||||
bool _flight_termination_triggered{false};
|
||||
bool _lockdown_triggered{false};
|
||||
|
||||
bool _open_drone_id_system_lost{true};
|
||||
bool _avoidance_system_lost{false};
|
||||
bool _onboard_controller_lost{false};
|
||||
@@ -257,7 +259,6 @@ private:
|
||||
bool _is_throttle_low{false};
|
||||
|
||||
bool _arm_tune_played{false};
|
||||
bool _was_armed{false};
|
||||
bool _have_taken_off_since_arming{false};
|
||||
bool _status_changed{true};
|
||||
|
||||
|
||||
@@ -107,28 +107,23 @@ private:
|
||||
// uint8_t system_status (MAV_STATE) - System status flag.
|
||||
uint8_t system_status = MAV_STATE_UNINIT;
|
||||
|
||||
switch (vehicle_status.arming_state) {
|
||||
case vehicle_status_s::ARMING_STATE_ARMED:
|
||||
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::ARMING_STATE_STANDBY:
|
||||
} else if (vehicle_status.calibration_enabled || vehicle_status.rc_calibration_in_progress
|
||||
|| actuator_armed.in_esc_calibration_mode) {
|
||||
system_status = MAV_STATE_CALIBRATING;
|
||||
|
||||
} else if (vehicle_status.pre_flight_checks_pass) {
|
||||
system_status = MAV_STATE_STANDBY;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::ARMING_STATE_SHUTDOWN:
|
||||
system_status = MAV_STATE_POWEROFF;
|
||||
break;
|
||||
}
|
||||
|
||||
// system_status overrides
|
||||
if (actuator_armed.force_failsafe || (actuator_armed.lockdown
|
||||
&& vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF) || actuator_armed.manual_lockdown
|
||||
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
|
||||
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
||||
|
||||
} else if (vehicle_status.calibration_enabled) {
|
||||
system_status = MAV_STATE_CALIBRATING;
|
||||
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user