forked from Archive/PX4-Autopilot
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:
parent
84b6b472b4
commit
88e7452492
|
@ -6,13 +6,8 @@ uint64 armed_time # Arming timestamp (microseconds)
|
||||||
uint64 takeoff_time # Takeoff timestamp (microseconds)
|
uint64 takeoff_time # Takeoff timestamp (microseconds)
|
||||||
|
|
||||||
uint8 arming_state
|
uint8 arming_state
|
||||||
uint8 ARMING_STATE_INIT = 0
|
uint8 ARMING_STATE_DISARMED = 1
|
||||||
uint8 ARMING_STATE_STANDBY = 1
|
|
||||||
uint8 ARMING_STATE_ARMED = 2
|
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 latest_arming_reason
|
uint8 latest_arming_reason
|
||||||
uint8 latest_disarming_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(ArmAuthorization)
|
||||||
add_subdirectory(ArmStateMachine)
|
|
||||||
|
|
|
@ -66,7 +66,6 @@ px4_add_module(
|
||||||
health_and_arming_checks
|
health_and_arming_checks
|
||||||
hysteresis
|
hysteresis
|
||||||
ArmAuthorization
|
ArmAuthorization
|
||||||
ArmStateMachine
|
|
||||||
sensor_calibration
|
sensor_calibration
|
||||||
world_magnetic_model
|
world_magnetic_model
|
||||||
mode_util
|
mode_util
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -473,7 +473,7 @@ int Commander::custom_command(int argc, char *argv[])
|
||||||
|
|
||||||
int Commander::print_status()
|
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("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("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
|
||||||
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
|
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);
|
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)
|
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
|
||||||
{
|
{
|
||||||
switch (calling_reason) {
|
switch (calling_reason) {
|
||||||
|
@ -533,19 +526,36 @@ 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)
|
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
|
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;
|
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_manual_enabled) {
|
||||||
|
|
||||||
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
if (_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
||||||
!_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) {
|
!_failsafe_flags.manual_control_signal_lost && _is_throttle_above_center) {
|
||||||
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: throttle above center\t");
|
||||||
events::send(events::ID("commander_arm_denied_throttle_center"),
|
events::send(events::ID("commander_arm_denied_throttle_center"), {events::Log::Critical, events::LogInternal::Info},
|
||||||
{events::Log::Critical, events::LogInternal::Info},
|
|
||||||
"Arming denied: throttle above center");
|
"Arming denied: throttle above center");
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
return TRANSITION_DENIED;
|
return TRANSITION_DENIED;
|
||||||
|
@ -554,9 +564,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
||||||
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
||||||
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
|
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
|
||||||
&& _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
&& _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
|
||||||
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
|
||||||
events::send(events::ID("commander_arm_denied_throttle_high"),
|
events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},
|
||||||
{events::Log::Critical, events::LogInternal::Info},
|
|
||||||
"Arming denied: high throttle");
|
"Arming denied: high throttle");
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
return TRANSITION_DENIED;
|
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
|
} 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_switch
|
||||||
|| calling_reason == arm_disarm_reason_t::rc_button) {
|
|| calling_reason == arm_disarm_reason_t::rc_button) {
|
||||||
|
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
|
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
|
||||||
events::send(events::ID("commander_arm_denied_not_manual"),
|
events::send(events::ID("commander_arm_denied_not_manual"), {events::Log::Critical, events::LogInternal::Info},
|
||||||
{events::Log::Critical, events::LogInternal::Info},
|
|
||||||
"Arming denied: switch to manual mode first");
|
"Arming denied: switch to manual mode first");
|
||||||
tune_negative(true);
|
tune_negative(true);
|
||||||
return TRANSITION_DENIED;
|
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.armed_time = hrt_absolute_time();
|
||||||
vehicle_status_s::ARMING_STATE_ARMED, _actuator_armed, _health_and_arming_checks, run_preflight_checks,
|
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_ARMED;
|
||||||
&_mavlink_log_pub, calling_reason);
|
_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));
|
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,
|
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_armed_by"), events::Log::Info,
|
||||||
"Armed by {1}", calling_reason);
|
"Armed by {1}", calling_reason);
|
||||||
|
|
||||||
_status_changed = true;
|
if (_param_com_home_en.get()) {
|
||||||
|
_home_position.setHomePosition();
|
||||||
} else if (arming_res == TRANSITION_DENIED) {
|
|
||||||
tune_negative(true);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return arming_res;
|
_status_changed = true;
|
||||||
|
|
||||||
|
return TRANSITION_CHANGED;
|
||||||
}
|
}
|
||||||
|
|
||||||
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
|
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool forced)
|
||||||
{
|
{
|
||||||
|
if (!isArmed()) {
|
||||||
|
return TRANSITION_NOT_CHANGED;
|
||||||
|
}
|
||||||
|
|
||||||
if (!forced) {
|
if (!forced) {
|
||||||
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
|
const bool landed = (_vehicle_land_detected.landed || _vehicle_land_detected.maybe_landed
|
||||||
|| is_ground_vehicle(_vehicle_status));
|
|| is_ground_vehicle(_vehicle_status));
|
||||||
|
@ -606,21 +626,27 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
||||||
|
|
||||||
if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
|
if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
|
||||||
if (calling_reason != arm_disarm_reason_t::rc_stick) {
|
if (calling_reason != arm_disarm_reason_t::rc_stick) {
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
|
mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t");
|
||||||
events::send(events::ID("commander_disarming_denied_not_landed"),
|
events::send(events::ID("commander_disarm_denied_not_landed"),
|
||||||
{events::Log::Critical, events::LogInternal::Info},
|
{events::Log::Critical, events::LogInternal::Info},
|
||||||
"Disarming denied, not landed");
|
"Disarming denied: not landed");
|
||||||
}
|
}
|
||||||
|
|
||||||
return TRANSITION_DENIED;
|
return TRANSITION_DENIED;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_vehicle_status,
|
_vehicle_status.armed_time = 0;
|
||||||
vehicle_status_s::ARMING_STATE_STANDBY, _actuator_armed, _health_and_arming_checks, false,
|
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
|
||||||
&_mavlink_log_pub, calling_reason);
|
_vehicle_status.latest_disarming_reason = (uint8_t)calling_reason;
|
||||||
|
_vehicle_status.takeoff_time = 0;
|
||||||
|
|
||||||
|
_have_taken_off_since_arming = false;
|
||||||
|
|
||||||
|
_last_disarmed_timestamp = hrt_absolute_time();
|
||||||
|
|
||||||
|
_user_mode_intention.onDisarm();
|
||||||
|
|
||||||
if (arming_res == TRANSITION_CHANGED) {
|
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s\t", arm_disarm_reason_str(calling_reason));
|
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,
|
events::send<events::px4::enums::arm_disarm_reason_t>(events::ID("commander_disarmed_by"), events::Log::Info,
|
||||||
"Disarmed by {1}", calling_reason);
|
"Disarmed by {1}", calling_reason);
|
||||||
|
@ -629,13 +655,14 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
|
||||||
_safety.activateSafety();
|
_safety.activateSafety();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// 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;
|
_status_changed = true;
|
||||||
|
|
||||||
} else if (arming_res == TRANSITION_DENIED) {
|
return TRANSITION_CHANGED;
|
||||||
tune_negative(true);
|
|
||||||
}
|
|
||||||
|
|
||||||
return arming_res;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Commander::Commander() :
|
Commander::Commander() :
|
||||||
|
@ -643,19 +670,15 @@ Commander::Commander() :
|
||||||
{
|
{
|
||||||
_vehicle_land_detected.landed = true;
|
_vehicle_land_detected.landed = true;
|
||||||
|
|
||||||
|
_vehicle_status.arming_state = vehicle_status_s::ARMING_STATE_DISARMED;
|
||||||
_vehicle_status.system_id = 1;
|
_vehicle_status.system_id = 1;
|
||||||
_vehicle_status.component_id = 1;
|
_vehicle_status.component_id = 1;
|
||||||
|
|
||||||
_vehicle_status.system_type = 0;
|
_vehicle_status.system_type = 0;
|
||||||
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
|
_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNKNOWN;
|
||||||
|
|
||||||
_vehicle_status.nav_state = _user_mode_intention.get();
|
_vehicle_status.nav_state = _user_mode_intention.get();
|
||||||
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
||||||
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
_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.gcs_connection_lost = true;
|
||||||
|
|
||||||
_vehicle_status.power_input_valid = true;
|
_vehicle_status.power_input_valid = true;
|
||||||
|
|
||||||
// default for vtol is rotary wing
|
// default for vtol is rotary wing
|
||||||
|
@ -858,16 +881,6 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
} else {
|
} else {
|
||||||
// Arm is forced (checks skipped) when param2 is set to a magic number.
|
// 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 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;
|
transition_result_t arming_res = TRANSITION_DENIED;
|
||||||
arm_disarm_reason_t arm_disarm_reason = cmd.from_external ? arm_disarm_reason_t::command_external :
|
arm_disarm_reason_t 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 {
|
} else {
|
||||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
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;
|
break;
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
|
case vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION: {
|
||||||
if (cmd.param1 > 1.5f) {
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||||
// 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)");
|
|
||||||
}
|
|
||||||
|
|
||||||
} else if (cmd.param1 > 0.5f) {
|
if (cmd.param1 > 0.5f) {
|
||||||
// Trigger real termination.
|
// Trigger real termination.
|
||||||
if (!_flight_termination_triggered) {
|
if (!isArmed()) {
|
||||||
_actuator_armed.force_failsafe = true;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
|
||||||
_flight_termination_triggered = true;
|
|
||||||
PX4_WARN("forcing failsafe (termination)");
|
} else if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_TERMINATION)) {
|
||||||
send_parachute_command();
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
}
|
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
_actuator_armed.force_failsafe = false;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||||
_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;
|
break;
|
||||||
|
|
||||||
|
@ -1130,7 +1122,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
|
|
||||||
#if defined(CONFIG_BOARDCTL_RESET)
|
#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
|
// 1: Reboot autopilot
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
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)
|
#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
|
// 2: Shutdown autopilot
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
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)
|
#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.
|
// 3: Reboot autopilot and keep it in the bootloader until upgraded.
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
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: {
|
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
|
// reject if armed or shutting down
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||||
|
|
||||||
} else {
|
} 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) {
|
if ((int)(cmd.param1) == 1) {
|
||||||
/* gyro calibration */
|
/* gyro calibration */
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
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: {
|
case vehicle_command_s::VEHICLE_CMD_FIXED_MAG_CAL_YAW: {
|
||||||
// Magnetometer quick calibration using world magnetic model and known heading
|
// 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
|
// reject if armed or shutting down
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
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: {
|
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
|
// reject if armed or shutting down
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
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)
|
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;
|
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_ARM: arm(arm_disarm_reason); break;
|
||||||
|
|
||||||
case action_request_s::ACTION_TOGGLE_ARMING:
|
case action_request_s::ACTION_TOGGLE_ARMING:
|
||||||
if (_arm_state_machine.isArmed()) {
|
if (isArmed()) {
|
||||||
disarm(arm_disarm_reason);
|
disarm(arm_disarm_reason);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1540,7 +1520,6 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
||||||
|
|
||||||
_status_changed = true;
|
_status_changed = true;
|
||||||
_actuator_armed.manual_lockdown = true;
|
_actuator_armed.manual_lockdown = true;
|
||||||
send_parachute_command();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
break;
|
break;
|
||||||
|
@ -1674,20 +1653,12 @@ void Commander::run()
|
||||||
|
|
||||||
vtolStatusUpdate();
|
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();
|
handleAutoDisarm();
|
||||||
|
|
||||||
battery_status_check();
|
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();
|
checkForMissionUpdate();
|
||||||
|
|
||||||
manualControlCheck();
|
manualControlCheck();
|
||||||
|
@ -1754,40 +1725,30 @@ void Commander::run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// check for arming state changes
|
// update actuator_armed
|
||||||
if (_was_armed != _arm_state_machine.isArmed()) {
|
_actuator_armed.armed = 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
_actuator_armed.prearmed = getPrearmState();
|
_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
|
// 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
|
if ((now >= _vehicle_status.timestamp + 500_ms) || _status_changed || nav_state_or_failsafe_changed
|
||||||
|| !(_actuator_armed == actuator_armed_prev)) {
|
|| !(_actuator_armed == actuator_armed_prev)) {
|
||||||
|
|
||||||
// publish actuator_armed first (used by output modules)
|
// 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.timestamp = hrt_absolute_time();
|
||||||
_actuator_armed_pub.publish(_actuator_armed);
|
_actuator_armed_pub.publish(_actuator_armed);
|
||||||
|
|
||||||
|
@ -1795,7 +1756,6 @@ void Commander::run()
|
||||||
updateControlMode();
|
updateControlMode();
|
||||||
|
|
||||||
// vehicle_status publish (after prearm/preflight updates above)
|
// vehicle_status publish (after prearm/preflight updates above)
|
||||||
_vehicle_status.arming_state = _arm_state_machine.getArmState();
|
|
||||||
_vehicle_status.timestamp = hrt_absolute_time();
|
_vehicle_status.timestamp = hrt_absolute_time();
|
||||||
_vehicle_status_pub.publish(_vehicle_status);
|
_vehicle_status_pub.publish(_vehicle_status);
|
||||||
|
|
||||||
|
@ -1822,11 +1782,9 @@ void Commander::run()
|
||||||
|
|
||||||
_status_changed = false;
|
_status_changed = false;
|
||||||
|
|
||||||
_was_armed = _arm_state_machine.isArmed();
|
|
||||||
|
|
||||||
arm_auth_update(hrt_absolute_time(), params_updated);
|
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);
|
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.timestamp >= _vehicle_status.nav_state_timestamp)
|
||||||
&& mission_result.finished) {
|
&& mission_result.finished) {
|
||||||
|
|
||||||
|
@ -1901,6 +1859,10 @@ void Commander::checkForMissionUpdate()
|
||||||
|
|
||||||
bool Commander::getPrearmState() const
|
bool Commander::getPrearmState() const
|
||||||
{
|
{
|
||||||
|
if (_vehicle_status.calibration_enabled) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
|
switch ((PrearmedMode)_param_com_prearm_mode.get()) {
|
||||||
case PrearmedMode::DISABLED:
|
case PrearmedMode::DISABLED:
|
||||||
/* skip prearmed state */
|
/* skip prearmed state */
|
||||||
|
@ -1936,7 +1898,7 @@ void Commander::handlePowerButtonState()
|
||||||
|
|
||||||
if (_power_button_state_sub.copy(&button_state)) {
|
if (_power_button_state_sub.copy(&button_state)) {
|
||||||
if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
|
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); }
|
while (1) { px4_usleep(1); }
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1973,7 +1935,7 @@ void Commander::landDetectorUpdate()
|
||||||
_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
|
_vehicle_land_detected_sub.copy(&_vehicle_land_detected);
|
||||||
|
|
||||||
// Only take actions if armed
|
// Only take actions if armed
|
||||||
if (_arm_state_machine.isArmed()) {
|
if (isArmed()) {
|
||||||
if (!was_landed && _vehicle_land_detected.landed) {
|
if (!was_landed && _vehicle_land_detected.landed) {
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
|
mavlink_log_info(&_mavlink_log_pub, "Landing detected\t");
|
||||||
events::send(events::ID("commander_landing_detected"), events::Log::Info, "Landing detected");
|
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
|
// automatically set or update home position
|
||||||
if (_param_com_home_en.get()) {
|
if (_param_com_home_en.get()) {
|
||||||
// set the home position when taking off, but only if we were previously disarmed
|
// set the home position when taking off
|
||||||
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
|
if (!_vehicle_land_detected.landed) {
|
||||||
if (!_vehicle_land_detected.landed && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
|
|
||||||
if (was_landed) {
|
if (was_landed) {
|
||||||
_home_position.setHomePosition();
|
_home_position.setHomePosition();
|
||||||
|
|
||||||
|
@ -2060,7 +2021,7 @@ void Commander::vtolStatusUpdate()
|
||||||
void Commander::updateTunes()
|
void Commander::updateTunes()
|
||||||
{
|
{
|
||||||
// play arming and battery warning tunes
|
// play arming and battery warning tunes
|
||||||
if (!_arm_tune_played && _arm_state_machine.isArmed()) {
|
if (!_arm_tune_played && isArmed()) {
|
||||||
|
|
||||||
/* play tune when armed */
|
/* play tune when armed */
|
||||||
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
|
set_tune(tune_control_s::TUNE_ID_ARMING_WARNING);
|
||||||
|
@ -2069,6 +2030,7 @@ void Commander::updateTunes()
|
||||||
} else if (!_vehicle_status.usb_connected &&
|
} else if (!_vehicle_status.usb_connected &&
|
||||||
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
(_vehicle_status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
|
||||||
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
(_battery_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
|
||||||
|
|
||||||
/* play tune on battery critical */
|
/* play tune on battery critical */
|
||||||
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
|
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_FAST);
|
||||||
|
|
||||||
|
@ -2077,7 +2039,7 @@ void Commander::updateTunes()
|
||||||
/* play tune on battery warning */
|
/* play tune on battery warning */
|
||||||
set_tune(tune_control_s::TUNE_ID_BATTERY_WARNING_SLOW);
|
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);
|
tune_failsafe(true);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -2085,7 +2047,7 @@ void Commander::updateTunes()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* reset arm_tune_played when disarmed */
|
/* reset arm_tune_played when disarmed */
|
||||||
if (!_arm_state_machine.isArmed()) {
|
if (!isArmed()) {
|
||||||
|
|
||||||
// Notify the user that it is safe to approach the vehicle
|
// Notify the user that it is safe to approach the vehicle
|
||||||
if (_arm_tune_played) {
|
if (_arm_tune_played) {
|
||||||
|
@ -2119,7 +2081,7 @@ void Commander::checkWorkerThread()
|
||||||
void Commander::handleAutoDisarm()
|
void Commander::handleAutoDisarm()
|
||||||
{
|
{
|
||||||
// Auto disarm when landed or kill switch engaged
|
// Auto disarm when landed or kill switch engaged
|
||||||
if (_arm_state_machine.isArmed()) {
|
if (isArmed()) {
|
||||||
|
|
||||||
// Check for auto-disarm on landing or pre-flight
|
// Check for auto-disarm on landing or pre-flight
|
||||||
if (_param_com_disarm_land.get() > 0 || _param_com_disarm_preflight.get() > 0) {
|
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();
|
const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
|
||||||
|
|
||||||
FailsafeBase::State state{};
|
FailsafeBase::State state{};
|
||||||
state.armed = _arm_state_machine.isArmed();
|
state.armed = isArmed();
|
||||||
state.vtol_in_transition_mode = _vehicle_status.in_transition_mode;
|
state.vtol_in_transition_mode = _vehicle_status.in_transition_mode;
|
||||||
state.mission_finished = _mission_result_sub.get().finished;
|
state.mission_finished = _mission_result_sub.get().finished;
|
||||||
state.user_intended_mode = _user_mode_intention.get();
|
state.user_intended_mode = _user_mode_intention.get();
|
||||||
|
@ -2210,13 +2172,6 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||||
|
|
||||||
case FailsafeBase::Action::Terminate:
|
case FailsafeBase::Action::Terminate:
|
||||||
_vehicle_status.nav_state = _vehicle_status.NAVIGATION_STATE_TERMINATION;
|
_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;
|
break;
|
||||||
|
|
||||||
default:
|
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;
|
uint8_t led_color = led_control_s::COLOR_WHITE;
|
||||||
bool set_normal_color = false;
|
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
|
// set mode
|
||||||
if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
|
if (overload && (time_now_us >= _overload_start + overload_warn_delay)) {
|
||||||
led_mode = led_control_s::MODE_BLINK_FAST;
|
led_mode = led_control_s::MODE_BLINK_FAST;
|
||||||
led_color = led_control_s::COLOR_PURPLE;
|
led_color = led_control_s::COLOR_PURPLE;
|
||||||
|
|
||||||
} else if (_arm_state_machine.isArmed()) {
|
} else if (isArmed()) {
|
||||||
led_mode = led_control_s::MODE_ON;
|
led_mode = led_control_s::MODE_ON;
|
||||||
set_normal_color = true;
|
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_mode = led_control_s::MODE_BLINK_FAST;
|
||||||
led_color = led_control_s::COLOR_RED;
|
led_color = led_control_s::COLOR_RED;
|
||||||
|
|
||||||
} else if (_arm_state_machine.isStandby()) {
|
} else {
|
||||||
led_mode = led_control_s::MODE_BREATHE;
|
led_mode = led_control_s::MODE_BREATHE;
|
||||||
set_normal_color = true;
|
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) {
|
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 !defined(CONFIG_ARCH_LEDS) && defined(BOARD_HAS_CONTROL_STATUS_LEDS)
|
||||||
|
|
||||||
if (_arm_state_machine.isArmed()) {
|
if (isArmed()) {
|
||||||
if (_vehicle_status.failsafe) {
|
if (_vehicle_status.failsafe) {
|
||||||
BOARD_ARMED_LED_OFF();
|
BOARD_ARMED_LED_OFF();
|
||||||
|
|
||||||
|
@ -2368,7 +2314,7 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||||
BOARD_ARMED_LED_ON();
|
BOARD_ARMED_LED_ON();
|
||||||
}
|
}
|
||||||
|
|
||||||
} else if (_arm_state_machine.isStandby()) {
|
} else if (_vehicle_status.pre_flight_checks_pass) {
|
||||||
BOARD_ARMED_LED_OFF();
|
BOARD_ARMED_LED_OFF();
|
||||||
|
|
||||||
// ready to arm, blink at 1Hz
|
// 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()
|
void Commander::updateControlMode()
|
||||||
{
|
{
|
||||||
_vehicle_control_mode = {};
|
_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_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
|
||||||
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
||||||
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
|
_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
|
/* only buzz if armed, because else we're driving people nuts indoors
|
||||||
they really need to look at the leds as well. */
|
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();
|
_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 (_failsafe_flags.battery_warning == battery_status_s::BATTERY_WARNING_EMERGENCY) {
|
||||||
#if defined(BOARD_HAS_POWER_CONTROL)
|
#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");
|
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},
|
events::send(events::ID("commander_low_bat_shutdown"), {events::Log::Emergency, events::LogInternal::Warning},
|
||||||
"Dangerously low battery! Shutting system down");
|
"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_above_center = (manual_control_setpoint.throttle > 0.2f);
|
||||||
_is_throttle_low = (manual_control_setpoint.throttle < -0.8f);
|
_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
|
// Abort autonomous mode and switch to position mode if sticks are moved significantly
|
||||||
// but only if actually in air.
|
// but only if actually in air.
|
||||||
if (manual_control_setpoint.sticks_moving
|
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.command = vehicle_command_s::VEHICLE_CMD_DO_PARACHUTE;
|
||||||
vcmd.param1 = static_cast<float>(vehicle_command_s::PARACHUTE_ACTION_RELEASE);
|
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.system_id;
|
||||||
vcmd.source_system = vehicle_status_sub.get().system_id;
|
vcmd.target_system = _vehicle_status.system_id;
|
||||||
vcmd.target_system = vehicle_status_sub.get().system_id;
|
vcmd.source_component = _vehicle_status.component_id;
|
||||||
vcmd.source_component = vehicle_status_sub.get().component_id;
|
|
||||||
vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
|
vcmd.target_component = 161; // MAV_COMP_ID_PARACHUTE
|
||||||
|
|
||||||
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
uORB::Publication<vehicle_command_s> vcmd_pub{ORB_ID(vehicle_command)};
|
||||||
|
|
|
@ -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
|
* Redistribution and use in source and binary forms, with or without
|
||||||
* modification, are permitted provided that the following conditions
|
* modification, are permitted provided that the following conditions
|
||||||
|
@ -34,7 +34,6 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
/* Helper classes */
|
/* Helper classes */
|
||||||
#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
|
|
||||||
#include "failure_detector/FailureDetector.hpp"
|
#include "failure_detector/FailureDetector.hpp"
|
||||||
#include "failsafe/failsafe.h"
|
#include "failsafe/failsafe.h"
|
||||||
#include "Safety.hpp"
|
#include "Safety.hpp"
|
||||||
|
@ -87,6 +86,14 @@
|
||||||
using math::constrain;
|
using math::constrain;
|
||||||
using systemlib::Hysteresis;
|
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;
|
using namespace time_literals;
|
||||||
|
|
||||||
class Commander : public ModuleBase<Commander>, public ModuleParams
|
class Commander : public ModuleBase<Commander>, public ModuleParams
|
||||||
|
@ -116,6 +123,8 @@ public:
|
||||||
void enable_hil();
|
void enable_hil();
|
||||||
|
|
||||||
private:
|
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);
|
void answer_command(const vehicle_command_s &cmd, uint8_t result);
|
||||||
|
|
||||||
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
|
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
|
||||||
|
@ -153,8 +162,6 @@ private:
|
||||||
|
|
||||||
void updateControlMode();
|
void updateControlMode();
|
||||||
|
|
||||||
bool shutdownIfAllowed();
|
|
||||||
|
|
||||||
void send_parachute_command();
|
void send_parachute_command();
|
||||||
|
|
||||||
void checkForMissionUpdate();
|
void checkForMissionUpdate();
|
||||||
|
@ -196,11 +203,9 @@ private:
|
||||||
|
|
||||||
/* Decouple update interval and hysteresis counters, all depends on intervals */
|
/* Decouple update interval and hysteresis counters, all depends on intervals */
|
||||||
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
|
static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};
|
||||||
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
|
|
||||||
|
|
||||||
vehicle_status_s _vehicle_status{};
|
vehicle_status_s _vehicle_status{};
|
||||||
|
|
||||||
ArmStateMachine _arm_state_machine{};
|
|
||||||
Failsafe _failsafe_instance{this};
|
Failsafe _failsafe_instance{this};
|
||||||
FailsafeBase &_failsafe{_failsafe_instance};
|
FailsafeBase &_failsafe{_failsafe_instance};
|
||||||
FailureDetector _failure_detector{this};
|
FailureDetector _failure_detector{this};
|
||||||
|
@ -242,9 +247,6 @@ private:
|
||||||
|
|
||||||
bool _failsafe_user_override_request{false}; ///< override request due to stick movements
|
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 _open_drone_id_system_lost{true};
|
||||||
bool _avoidance_system_lost{false};
|
bool _avoidance_system_lost{false};
|
||||||
bool _onboard_controller_lost{false};
|
bool _onboard_controller_lost{false};
|
||||||
|
@ -257,7 +259,6 @@ private:
|
||||||
bool _is_throttle_low{false};
|
bool _is_throttle_low{false};
|
||||||
|
|
||||||
bool _arm_tune_played{false};
|
bool _arm_tune_played{false};
|
||||||
bool _was_armed{false};
|
|
||||||
bool _have_taken_off_since_arming{false};
|
bool _have_taken_off_since_arming{false};
|
||||||
bool _status_changed{true};
|
bool _status_changed{true};
|
||||||
|
|
||||||
|
|
|
@ -107,28 +107,23 @@ private:
|
||||||
// uint8_t system_status (MAV_STATE) - System status flag.
|
// uint8_t system_status (MAV_STATE) - System status flag.
|
||||||
uint8_t system_status = MAV_STATE_UNINIT;
|
uint8_t system_status = MAV_STATE_UNINIT;
|
||||||
|
|
||||||
switch (vehicle_status.arming_state) {
|
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||||
case vehicle_status_s::ARMING_STATE_ARMED:
|
|
||||||
system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
|
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;
|
system_status = MAV_STATE_STANDBY;
|
||||||
break;
|
|
||||||
|
|
||||||
case vehicle_status_s::ARMING_STATE_SHUTDOWN:
|
|
||||||
system_status = MAV_STATE_POWEROFF;
|
|
||||||
break;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// system_status overrides
|
// system_status overrides
|
||||||
if (actuator_armed.force_failsafe || (actuator_armed.lockdown
|
if (actuator_armed.force_failsafe || (actuator_armed.lockdown
|
||||||
&& vehicle_status.hil_state == vehicle_status_s::HIL_STATE_OFF) || actuator_armed.manual_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) {
|
|| 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_FLIGHT_TERMINATION;
|
||||||
system_status = MAV_STATE_CALIBRATING;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue