dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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 2023-07-28 17:12:01 -04:00 committed by GitHub
parent 84b6b472b4
commit 88e7452492
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 162 additions and 814 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/VehicleStatus.msg
View File

@ -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

174
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp
View File

@ -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;
}

95
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp
View File

@ -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
};
};

273
src/modules/commander/Arming/ArmStateMachine/ArmStateMachineTest.cpp
View File

@ -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;
}
}

44
src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt
View File

@ -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
)

1
src/modules/commander/Arming/CMakeLists.txt
View File

@ -32,4 +32,3 @@
############################################################################
add_subdirectory(ArmAuthorization)
add_subdirectory(ArmStateMachine)

1
src/modules/commander/CMakeLists.txt
View File

@ -66,7 +66,6 @@ px4_add_module(
health_and_arming_checks
hysteresis
ArmAuthorization
ArmStateMachine
sensor_calibration
world_magnetic_model
mode_util

339
src/modules/commander/Commander.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -473,7 +473,7 @@ int Commander::custom_command(int argc, char *argv[])
int Commander::print_status()
{
PX4_INFO("Arm state: %s", _arm_state_machine.getArmStateName());
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
@ -487,13 +487,6 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
return Commander::main(argc, argv);
}
bool Commander::shutdownIfAllowed()
{
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_SHUTDOWN, _actuator_armed, _health_and_arming_checks,
false /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::shutdown);
}
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
{
switch (calling_reason) {
@ -533,20 +526,37 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
{
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
if (isArmed()) {
return TRANSITION_NOT_CHANGED;
}
if (_vehicle_status.calibration_enabled
|| _vehicle_status.rc_calibration_in_progress
|| _actuator_armed.in_esc_calibration_mode) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: calibrating\t");
events::send(events::ID("commander_arm_denied_calibrating"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: calibrating");
tune_negative(true);
return TRANSITION_DENIED;
}
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidental in-air disarming
if (calling_reason == arm_disarm_reason_t::rc_switch
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
&& ((_last_disarmed_timestamp != 0) && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s))) {
run_preflight_checks = false;
}
if (run_preflight_checks && !_arm_state_machine.isArmed()) {
if (run_preflight_checks) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
events::send(events::ID("commander_arm_denied_throttle_center"),
{events::Log::Critical, events::LogInternal::Info},
"Arming denied: throttle above center");
events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: throttle above center");
tune_negative(true);
return TRANSITION_DENIED;
}
@ -554,10 +564,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
&& _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
events::send(events::ID("commander_arm_denied_throttle_high"),
{events::Log::Critical, events::LogInternal::Info},
"Arming denied: high throttle");
events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: high throttle");
tune_negative(true);
return TRANSITION_DENIED;
}
@ -565,35 +575,45 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
} else if (calling_reason == arm_disarm_reason_t::rc_stick
|| calling_reason == arm_disarm_reason_t::rc_switch
|| calling_reason == arm_disarm_reason_t::rc_button) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
events::send(events::ID("commander_arm_denied_not_manual"),
{events::Log::Critical, events::LogInternal::Info},
"Arming denied: switch to manual mode first");
events::send(events::ID("commander_arm_denied_not_manual"), {events::Log::Critical, events::LogInternal::Info},
"Arming denied: switch to manual mode first");
tune_negative(true);
return TRANSITION_DENIED;
}
_health_and_arming_checks.update();
if (!_health_and_arming_checks.canArm(_vehicle_status.nav_state)) {
tune_negative(true);
return TRANSITION_DENIED;
}
}
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks,
&_mavlink_log_pub, calling_reason);
_vehicle_status.armed_time = hrt_absolute_time();
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_ARMED;
_vehicle_status.latest_arming_reason = (uint8_t)calling_reason;
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
"Armed by {1}", calling_reason);
mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason));
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
"Armed by {1}", calling_reason);
_status_changed = true;
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
if (_param_com_home_en.get()) {
_home_position.setHomePosition();
}
return arming_res;
_status_changed = true;
return TRANSITION_CHANGED;
}
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
{
if (!isArmed()) {
return TRANSITION_NOT_CHANGED;
}
if (!forced) {
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
|| is_ground_vehicle(_vehicle_status));
@ -606,36 +626,43 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
if (calling_reason != arm_disarm_reason_t::rc_stick) {
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
events::send(events::ID("commander_disarming_denied_not_landed"),
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t");
events::send(events::ID("commander_disarm_denied_not_landed"),
{events::Log::Critical, events::LogInternal::Info},
"Disarming denied, not landed");
"Disarming denied: not landed");
}
return TRANSITION_DENIED;
}
}
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false,
&_mavlink_log_pub, calling_reason);
_vehicle_status.armed_time = 0;
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
_vehicle_status.latest_disarming_reason = (uint8_t)calling_reason;
_vehicle_status.takeoff_time = 0;
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
"Disarmed by {1}", calling_reason);
_have_taken_off_since_arming = false;
if (_param_com_force_safety.get()) {
_safety.activateSafety();
}
_last_disarmed_timestamp = hrt_absolute_time();
_status_changed = true;
_user_mode_intention.onDisarm();
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
"Disarmed by {1}", calling_reason);
if (_param_com_force_safety.get()) {
_safety.activateSafety();
}
return arming_res;
// update flight uuid
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
_param_flight_uuid.set(flight_uuid);
_param_flight_uuid.commit_no_notification();
_status_changed = true;
return TRANSITION_CHANGED;
}
Commander::Commander() :
@ -643,19 +670,15 @@ Commander::Commander() :
{
_vehicle_land_detected.landed = true;
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
_vehicle_status.system_id = 1;
_vehicle_status.component_id = 1;
_vehicle_status.system_type = 0;
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
_vehicle_status.nav_state = _user_mode_intention.get();
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
/* mark all signals lost as long as they haven't been found */
_vehicle_status.gcs_connection_lost = true;
_vehicle_status.power_input_valid = true;
// default for vtol is rotary wing
@ -858,16 +881,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
// Arm is forced (checks skipped) when param2 is set to a magic number.
const bool forced = (static_cast<int>(lroundf(cmd.param2)) == 21196);
const bool cmd_from_io = (static_cast<int>(roundf(cmd.param3)) == 1234);
// Flick to in-air restore first if this comes from an onboard system and from IO
if (!forced && cmd_from_io
&& (cmd.source_system == _vehicle_status.system_id)
&& (cmd.source_component == _vehicle_status.component_id)
&& (arming_action == vehicle_command_s::ARMING_ACTION_ARM)) {
// TODO: replace with a proper allowed transition
_arm_state_machine.forceArmState(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE);
}
transition_result_t arming_res = TRANSITION_DENIED;
arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external :
@ -886,47 +899,26 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED)
&& (hrt_absolute_time() > (_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))
&& (_param_com_home_en.get())) {
_home_position.setHomePosition();
}
}
}
}
break;
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
if (cmd.param1 > 1.5f) {
// Test termination command triggers lockdown but not actual termination.
if (!_lockdown_triggered) {
_actuator_armed.lockdown = true;
_lockdown_triggered = true;
PX4_WARN("forcing lockdown (motors off)");
}
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
} else if (cmd.param1 > 0.5f) {
if (cmd.param1 > 0.5f) {
// Trigger real termination.
if (!_flight_termination_triggered) {
_actuator_armed.force_failsafe = true;
_flight_termination_triggered = true;
PX4_WARN("forcing failsafe (termination)");
send_parachute_command();
if (!isArmed()) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
} else if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION)) {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
_actuator_armed.force_failsafe = false;
_actuator_armed.lockdown = false;
_lockdown_triggered = false;
_flight_termination_triggered = false;
PX4_WARN("disabling failsafe and lockdown");
}
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
}
break;
@ -1130,7 +1122,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 1) && shutdownIfAllowed() && (px4_reboot_request(false, 400_ms) == 0)) {
} else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) {
// 1: Reboot autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1140,7 +1132,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
#if defined(BOARD_HAS_POWER_CONTROL)
} else if ((param1 == 2) && shutdownIfAllowed() && (px4_shutdown_request(400_ms) == 0)) {
} else if ((param1 == 2) && !isArmed() && (px4_shutdown_request(400_ms) == 0)) {
// 2: Shutdown autopilot
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1150,7 +1142,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
#if defined(CONFIG_BOARDCTL_RESET)
} else if ((param1 == 3) && shutdownIfAllowed() && (px4_reboot_request(true, 400_ms) == 0)) {
} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) {
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1167,25 +1159,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
if (isArmed() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
} else {
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_INIT, _actuator_armed, _health_and_arming_checks,
false /* fRunPreArmChecks */, &_mavlink_log_pub,
(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
) {
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
break;
}
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
@ -1292,7 +1272,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
// Magnetometer quick calibration using world magnetic model and known heading
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
if (isArmed() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@ -1327,7 +1307,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
if (isArmed() || _worker_thread.isBusy()) {
// reject if armed or shutting down
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
@ -1424,7 +1404,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
{
if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
}
@ -1504,7 +1484,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
case action_request_s::ACTION_ARM: arm(arm_disarm_reason); break;
case action_request_s::ACTION_TOGGLE_ARMING:
if (_arm_state_machine.isArmed()) {
if (isArmed()) {
disarm(arm_disarm_reason);
} else {
@ -1540,7 +1520,6 @@ void Commander::executeActionRequest(const action_request_s &action_request)
_status_changed = true;
_actuator_armed.manual_lockdown = true;
send_parachute_command();
}
break;
@ -1674,20 +1653,12 @@ void Commander::run()
vtolStatusUpdate();
_home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed);
_home_position.update(_param_com_home_en.get(), !isArmed() && _vehicle_land_detected.landed);
handleAutoDisarm();
battery_status_check();
/* If in INIT state, try to proceed to STANDBY state */
if (!_vehicle_status.calibration_enabled && _arm_state_machine.isInit()) {
_arm_state_machine.arming_state_transition(_vehicle_status,
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks,
true /* fRunPreArmChecks */, &_mavlink_log_pub, arm_disarm_reason_t::transition_to_standby);
}
checkForMissionUpdate();
manualControlCheck();
@ -1754,40 +1725,30 @@ void Commander::run()
}
}
// check for arming state changes
if (_was_armed != _arm_state_machine.isArmed()) {
_status_changed = true;
}
if (!_was_armed && _arm_state_machine.isArmed() && !_vehicle_land_detected.landed) {
_have_taken_off_since_arming = true;
}
if (_was_armed && !_arm_state_machine.isArmed()) {
const int32_t flight_uuid = _param_flight_uuid.get() + 1;
_param_flight_uuid.set(flight_uuid);
_param_flight_uuid.commit_no_notification();
_last_disarmed_timestamp = hrt_absolute_time();
_user_mode_intention.onDisarm();
_vehicle_status.takeoff_time = 0;
}
if (!_arm_state_machine.isArmed()) {
/* Reset the flag if disarmed. */
_have_taken_off_since_arming = false;
}
// update actuator_armed
_actuator_armed.armed = isArmed();
_actuator_armed.prearmed = getPrearmState();
_actuator_armed.ready_to_arm = _vehicle_status.pre_flight_checks_pass || isArmed();
_actuator_armed.lockdown = ((_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION)
|| (_vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON));
// _actuator_armed.manual_lockdown // action_request_s::ACTION_KILL
_actuator_armed.force_failsafe = (_vehicle_status.nav_state == _vehicle_status.NAVIGATION_STATE_TERMINATION);
// _actuator_armed.in_esc_calibration_mode // VEHICLE_CMD_PREFLIGHT_CALIBRATION
// if force_failsafe or manual_lockdown activated send parachute command
if ((!actuator_armed_prev.force_failsafe && _actuator_armed.force_failsafe)
|| (!actuator_armed_prev.manual_lockdown && _actuator_armed.manual_lockdown)
) {
if (isArmed()) {
send_parachute_command();
}
}
// publish states (armed, control_mode, vehicle_status, failure_detector_status) at 2 Hz or immediately when changed
if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed
|| !(_actuator_armed == actuator_armed_prev)) {
// publish actuator_armed first (used by output modules)
_actuator_armed.armed = _arm_state_machine.isArmed();
_actuator_armed.ready_to_arm = _arm_state_machine.isArmed() || _arm_state_machine.isStandby();
_actuator_armed.timestamp = hrt_absolute_time();
_actuator_armed_pub.publish(_actuator_armed);
@ -1795,7 +1756,6 @@ void Commander::run()
updateControlMode();
// vehicle_status publish (after prearm/preflight updates above)
_vehicle_status.arming_state = _arm_state_machine.getArmState();
_vehicle_status.timestamp = hrt_absolute_time();
_vehicle_status_pub.publish(_vehicle_status);
@ -1822,11 +1782,9 @@ void Commander::run()
_status_changed = false;
_was_armed = _arm_state_machine.isArmed();
arm_auth_update(hrt_absolute_time(), params_updated);
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _arm_state_machine.isArmed());
px4_indicate_external_reset_lockout(LockoutComponent::Commander, isArmed());
perf_end(_loop_perf);
@ -1877,7 +1835,7 @@ void Commander::checkForMissionUpdate()
}
}
if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
if (isArmed() && !_vehicle_land_detected.landed
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
&& mission_result.finished) {
@ -1901,6 +1859,10 @@ void Commander::checkForMissionUpdate()
bool Commander::getPrearmState() const
{
if (_vehicle_status.calibration_enabled) {
return false;
}
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
case PrearmedMode::DISABLED:
/* skip prearmed state */
@ -1936,7 +1898,7 @@ void Commander::handlePowerButtonState()
if (_power_button_state_sub.copy(&button_state)) {
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
if (shutdownIfAllowed() && (px4_shutdown_request() == 0)) {
if (!isArmed() && (px4_shutdown_request() == 0)) {
while (1) { px4_usleep(1); }
}
}
@ -1973,7 +1935,7 @@ void Commander::landDetectorUpdate()
_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
// Only take actions if armed
if (_arm_state_machine.isArmed()) {
if (isArmed()) {
if (!was_landed && _vehicle_land_detected.landed) {
mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
@ -1987,9 +1949,8 @@ void Commander::landDetectorUpdate()
// automatically set or update home position
if (_param_com_home_en.get()) {
// set the home position when taking off, but only if we were previously disarmed
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
// set the home position when taking off
if (!_vehicle_land_detected.landed) {
if (was_landed) {
_home_position.setHomePosition();
@ -2060,7 +2021,7 @@ void Commander::vtolStatusUpdate()
void Commander::updateTunes()
{
// play arming and battery warning tunes
if (!_arm_tune_played && _arm_state_machine.isArmed()) {
if (!_arm_tune_played && isArmed()) {
/* play tune when armed */
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
@ -2069,6 +2030,7 @@ void Commander::updateTunes()
} else if (!_vehicle_status.usb_connected &&
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
/* play tune on battery critical */
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
@ -2077,7 +2039,7 @@ void Commander::updateTunes()
/* play tune on battery warning */
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
} else if (_vehicle_status.failsafe && _arm_state_machine.isArmed()) {
} else if (_vehicle_status.failsafe && isArmed()) {
tune_failsafe(true);
} else {
@ -2085,7 +2047,7 @@ void Commander::updateTunes()
}
/* reset arm_tune_played when disarmed */
if (!_arm_state_machine.isArmed()) {
if (!isArmed()) {
// Notify the user that it is safe to approach the vehicle
if (_arm_tune_played) {
@ -2119,7 +2081,7 @@ void Commander::checkWorkerThread()
void Commander::handleAutoDisarm()
{
// Auto disarm when landed or kill switch engaged
if (_arm_state_machine.isArmed()) {
if (isArmed()) {
// Check for auto-disarm on landing or pre-flight
if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
@ -2178,7 +2140,7 @@ bool Commander::handleModeIntentionAndFailsafe()
const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
FailsafeBase::State state{};
state.armed = _arm_state_machine.isArmed();
state.armed = isArmed();
state.vtol_in_transition_mode = _vehicle_status.in_transition_mode;
state.mission_finished = _mission_result_sub.get().finished;
state.user_intended_mode = _user_mode_intention.get();
@ -2210,13 +2172,6 @@ bool Commander::handleModeIntentionAndFailsafe()
case FailsafeBase::Action::Terminate:
_vehicle_status.nav_state = _vehicle_status.NAVIGATION_STATE_TERMINATION;
_actuator_armed.force_failsafe = true;
if (!_flight_termination_triggered) {
_flight_termination_triggered = true;
send_parachute_command();
}
break;
default:
@ -2293,14 +2248,14 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
uint8_t led_color = led_control_s::COLOR_WHITE;
bool set_normal_color = false;
uint64_t overload_warn_delay = _arm_state_machine.isArmed() ? 1_ms : 250_ms;
uint64_t overload_warn_delay = isArmed() ? 1_ms : 250_ms;
// set mode
if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
led_mode = led_control_s::MODE_BLINK_FAST;
led_color = led_control_s::COLOR_PURPLE;
} else if (_arm_state_machine.isArmed()) {
} else if (isArmed()) {
led_mode = led_control_s::MODE_ON;
set_normal_color = true;
@ -2308,18 +2263,9 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
led_mode = led_control_s::MODE_BLINK_FAST;
led_color = led_control_s::COLOR_RED;
} else if (_arm_state_machine.isStandby()) {
} else {
led_mode = led_control_s::MODE_BREATHE;
set_normal_color = true;
} else if (_arm_state_machine.isInit()) {
// if in init status it should not be in the error state
led_mode = led_control_s::MODE_OFF;
} else {
// STANDBY_ERROR and other states
led_mode = led_control_s::MODE_BLINK_NORMAL;
led_color = led_control_s::COLOR_RED;
}
if (set_normal_color) {
@ -2352,7 +2298,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
#if !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
if (_arm_state_machine.isArmed()) {
if (isArmed()) {
if (_vehicle_status.failsafe) {
BOARD_ARMED_LED_OFF();
@ -2368,7 +2314,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
BOARD_ARMED_LED_ON();
}
} else if (_arm_state_machine.isStandby()) {
} else if (_vehicle_status.pre_flight_checks_pass) {
BOARD_ARMED_LED_OFF();
// ready to arm, blink at 1Hz
@ -2404,7 +2350,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
void Commander::updateControlMode()
{
_vehicle_control_mode = {};
mode_util::getVehicleControlMode(_arm_state_machine.isArmed(), _vehicle_status.nav_state,
mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state,
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
_vehicle_control_mode.timestamp = hrt_absolute_time();
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
@ -2426,7 +2372,7 @@ void Commander::printRejectMode(uint8_t nav_state)
/* only buzz if armed, because else we're driving people nuts indoors
they really need to look at the leds as well. */
tune_negative(_arm_state_machine.isArmed());
tune_negative(isArmed());
_last_print_mode_reject_time = hrt_absolute_time();
}
@ -2698,7 +2644,7 @@ void Commander::battery_status_check()
if (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
#if defined(BOARD_HAS_POWER_CONTROL)
if (shutdownIfAllowed() && (px4_shutdown_request(60_s) == 0)) {
if (!isArmed() && (px4_shutdown_request(60_s) == 0)) {
mavlink_log_critical(&_mavlink_log_pub, "Dangerously low battery! Shutting system down in 60 seconds\t");
events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
"Dangerously low battery! Shutting system down");
@ -2731,7 +2677,7 @@ void Commander::manualControlCheck()
_is_throttle_above_center = (manual_control_setpoint.throttle > 0.2f);
_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
if (_arm_state_machine.isArmed()) {
if (isArmed()) {
// Abort autonomous mode and switch to position mode if sticks are moved significantly
// but only if actually in air.
if (manual_control_setpoint.sticks_moving
@ -2794,10 +2740,9 @@ void Commander::send_parachute_command()
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
uORB::SubscriptionData<vehicle_status_s> vehicle_status_sub{ORB_ID(vehicle_status)};
vcmd.source_system = vehicle_status_sub.get().system_id;
vcmd.target_system = vehicle_status_sub.get().system_id;
vcmd.source_component = vehicle_status_sub.get().component_id;
vcmd.source_system = _vehicle_status.system_id;
vcmd.target_system = _vehicle_status.system_id;
vcmd.source_component = _vehicle_status.component_id;
vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};

21
src/modules/commander/Commander.hpp
View File

@ -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};

19
src/modules/mavlink/streams/HEARTBEAT.hpp
View File

@ -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;
}