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

Commander: separate out arm state machine to class 

Pure refactoring and just the first step to avoid conflicts on the way.
This commit is contained in:
Matthias Grob 2022-03-31 12:00:42 +02:00
parent 6e9c673262
commit 074080c816
10 changed files with 367 additions and 224 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

217
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp Normal file
View File

@ -0,0 +1,217 @@
/****************************************************************************
*
* 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];
events::px4::enums::arming_state_t ArmStateMachine::eventArmingState(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;
}
transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &status,
const vehicle_control_mode_s &control_mode, const safety_s &safety,
const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason)
{
// Double check that our static arrays are still valid
static_assert(vehicle_status_s::ARMING_STATE_INIT == 0, "ARMING_STATE_INIT == 0");
static_assert(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1,
"ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1");
transition_result_t ret = TRANSITION_DENIED;
arming_state_t current_arming_state = status.arming_state;
bool feedback_provided = false;
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/*
* Get sensing state if necessary
*/
bool preflight_check_ret = true;
/* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !hil_enabled) {
preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
true, true, time_since_boot);
if (preflight_check_ret) {
status_flags.system_sensors_initialized = true;
}
feedback_provided = true;
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status_flags.system_sensors_initialized
&& fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
&& !hil_enabled) {
if ((_last_preflight_check == 0) || (hrt_elapsed_time(&_last_preflight_check) > 1000 * 1000)) {
status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status,
status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
time_since_boot);
_last_preflight_check = hrt_absolute_time();
}
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON
if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
bool prearm_check_ret = true;
if (fRunPreArmChecks && preflight_check_ret) {
// only bother running prearm if preflight was successful
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
status);
}
if (!preflight_check_ret || !prearm_check_ret) {
// the prearm and preflight checks already print the rejection reason
feedback_provided = true;
valid_transition = false;
}
}
}
}
if (hil_enabled) {
/* enforce lockdown in HIL */
armed.lockdown = true;
status_flags.system_sensors_initialized = true;
/* recover from a prearm fail */
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
// HIL can always go to standby
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
}
}
if (!hil_enabled &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
// Sensors need to be initialized for STANDBY state, except for HIL
if (!status_flags.system_sensors_initialized) {
feedback_provided = true;
valid_transition = false;
}
}
// Finish up the state transition
if (valid_transition) {
bool was_armed = armed.armed;
armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
ret = TRANSITION_CHANGED;
status.arming_state = new_arming_state;
if (was_armed && !armed.armed) { // disarm transition
status.latest_disarming_reason = (uint8_t)calling_reason;
} else if (!was_armed && armed.armed) { // arm transition
status.latest_arming_reason = (uint8_t)calling_reason;
}
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
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",
arming_state_names[status.arming_state], arming_state_names[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}",
eventArmingState(status.arming_state), eventArmingState(new_arming_state));
}
}
return ret;
}

82
src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.hpp Normal file
View File

@ -0,0 +1,82 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../PreFlightCheck/PreFlightCheck.hpp"
#include <drivers/drv_hrt.h>
#include <px4_platform_common/events.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
#include "../../state_machine_helper.h" // TODO: get independent of transition_result_t
using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
class ArmStateMachine
{
public:
ArmStateMachine() = default;
~ArmStateMachine() = default;
transition_result_t
arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety,
const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
private:
static inline events::px4::enums::arming_state_t eventArmingState(uint8_t arming_state);
hrt_abstime _last_preflight_check = 0; ///< initialize so it gets checked immediately
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static constexpr bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX]
= {
// INIT, STANDBY, ARMED, STANDBY_ERROR, SHUTDOWN, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */ true, true, true, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_SHUTDOWN */ true, true, false, true, true, true },
{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false }, // NYI
};
};

39
src/modules/commander/Arming/ArmStateMachine/CMakeLists.txt Normal file
View File

@ -0,0 +1,39 @@
############################################################################
#
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_library(ArmStateMachine
ArmStateMachine.cpp
)
target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(ArmStateMachine PUBLIC PreFlightCheck)

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

@ -31,6 +31,7 @@
#
############################################################################
add_subdirectory(PreFlightCheck)
add_subdirectory(ArmAuthorization)
add_subdirectory(ArmStateMachine)
add_subdirectory(HealthFlags)
add_subdirectory(PreFlightCheck)

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

@ -62,6 +62,7 @@ px4_add_module(
hysteresis
PreFlightCheck
ArmAuthorization
ArmStateMachine
HealthFlags
sensor_calibration
world_magnetic_model

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

@ -479,7 +479,7 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[])
bool Commander::shutdown_if_allowed()
{
return TRANSITION_DENIED != arming_state_transition(_status, _vehicle_control_mode, _safety,
return TRANSITION_DENIED != _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
vehicle_status_s::ARMING_STATE_SHUTDOWN,
_armed, false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::shutdown);
@ -732,7 +732,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
}
}
transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety,
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
vehicle_status_s::ARMING_STATE_ARMED, _armed, run_preflight_checks,
&_mavlink_log_pub, _status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
calling_reason);
@ -774,7 +774,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
}
}
transition_result_t arming_res = arming_state_transition(_status, _vehicle_control_mode, _safety,
transition_result_t arming_res = _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
vehicle_status_s::ARMING_STATE_STANDBY, _armed, false,
&_mavlink_log_pub, _status_flags, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason);
@ -1368,7 +1368,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else {
/* try to go to INIT/PREFLIGHT arming state */
if (TRANSITION_DENIED == arming_state_transition(_status, _vehicle_control_mode, safety_s{},
if (TRANSITION_DENIED == _arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, safety_s{},
vehicle_status_s::ARMING_STATE_INIT, _armed,
false /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
@ -2375,10 +2375,11 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */
if (!_status_flags.calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_state_transition(_status, _vehicle_control_mode, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::transition_to_standby);
_arm_state_machine.arming_state_transition(_status, _vehicle_control_mode, _safety,
vehicle_status_s::ARMING_STATE_STANDBY, _armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, _status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::transition_to_standby);
}
/* start mission result check */

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

@ -34,6 +34,7 @@
#pragma once
/* Helper classes */
#include "Arming/ArmStateMachine/ArmStateMachine.hpp"
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "Safety.hpp"
@ -294,6 +295,7 @@ private:
static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s}; /**< wait for hotplug sensors to come online for upto 8 seconds */
static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
ArmStateMachine _arm_state_machine{};
PreFlightCheck::arm_requirements_t _arm_requirements{};
hrt_abstime _valid_distance_sensor_time_us{0}; /**< Last time that distance sensor data arrived (usec) */

22
src/modules/commander/commander_tests/state_machine_helper_test.cpp
View File

@ -42,6 +42,7 @@
#include "../state_machine_helper.h"
#include <unit_test.h>
#include "../Arming/ArmStateMachine/ArmStateMachine.hpp"
#include "../Arming/PreFlightCheck/PreFlightCheck.hpp"
class StateMachineHelperTest : public UnitTest
@ -50,6 +51,8 @@ public:
StateMachineHelperTest() = default;
~StateMachineHelperTest() override = default;
ArmStateMachine _arm_state_machine{};
bool run_tests() override;
private:
@ -303,13 +306,18 @@ bool StateMachineHelperTest::armingStateTransitionTest()
vehicle_control_mode_s control_mode{};
// Attempt transition
transition_result_t result = arming_state_transition(status, control_mode, safety, test->requested_state, armed,
true /* enable pre-arm checks */,
nullptr /* no mavlink_log_pub */,
status_flags,
arm_req,
2e6, /* 2 seconds after boot, everything should be checked */
arm_disarm_reason_t::unit_test);
transition_result_t result = _arm_state_machine.arming_state_transition(
status,
control_mode,
safety,
test->requested_state,
armed,
true /* enable pre-arm checks */,
nullptr /* no mavlink_log_pub */,
status_flags,
arm_req,
2e6, /* 2 seconds after boot, everything should be checked */
arm_disarm_reason_t::unit_test);
// Validate result of transition
ut_compare(test->assertMsg, test->expected_transition_result, result);

196
src/modules/commander/state_machine_helper.cpp
View File

@ -61,22 +61,6 @@ static constexpr const char reason_no_datalink[] = "no datalink";
static constexpr const char reason_no_rc_and_no_datalink[] = "no RC and no datalink";
static constexpr const char reason_no_gps[] = "no GPS";
// 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 const 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
};
// You can index into the array with an arming_state_t in order to get its textual representation
const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"INIT",
@ -87,28 +71,6 @@ const char *const arming_state_names[vehicle_status_s::ARMING_STATE_MAX] = {
"IN_AIR_RESTORE",
};
static inline events::px4::enums::arming_state_t eventArmingState(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;
}
// You can index into the array with an navigation_state_t in order to get its textual representation
const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"MANUAL",
@ -135,8 +97,6 @@ const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
"ORBIT"
};
static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately
void set_link_loss_nav_state(vehicle_status_s &status, actuator_armed_s &armed,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state, link_loss_actions_t link_loss_act,
const float ll_delay);
@ -159,162 +119,6 @@ void reset_offboard_loss_globals(actuator_armed_s &armed, const bool old_failsaf
const offboard_loss_actions_t offboard_loss_act,
const offboard_loss_rc_actions_t offboard_loss_rc_act);
transition_result_t arming_state_transition(vehicle_status_s &status,
const vehicle_control_mode_s &control_mode, const safety_s &safety,
const arming_state_t new_arming_state, actuator_armed_s &armed, const bool fRunPreArmChecks,
orb_advert_t *mavlink_log_pub, vehicle_status_flags_s &status_flags,
const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason)
{
// 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;
arming_state_t current_arming_state = status.arming_state;
bool feedback_provided = false;
const bool hil_enabled = (status.hil_state == vehicle_status_s::HIL_STATE_ON);
/* only check transition if the new state is actually different from the current one */
if (new_arming_state == current_arming_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
/*
* Get sensing state if necessary
*/
bool preflight_check_ret = true;
/* only perform the pre-arm check if we have to */
if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
&& !hil_enabled) {
preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode,
true, true, time_since_boot);
if (preflight_check_ret) {
status_flags.system_sensors_initialized = true;
}
feedback_provided = true;
}
/* re-run the pre-flight check as long as sensors are failing */
if (!status_flags.system_sensors_initialized
&& fRunPreArmChecks
&& ((new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY))
&& !hil_enabled) {
if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) {
status_flags.system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status,
status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED,
time_since_boot);
last_preflight_check = hrt_absolute_time();
}
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][status.arming_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON
if (status.arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE) {
bool prearm_check_ret = true;
if (fRunPreArmChecks && preflight_check_ret) {
// only bother running prearm if preflight was successful
prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements,
status);
}
if (!preflight_check_ret || !prearm_check_ret) {
// the prearm and preflight checks already print the rejection reason
feedback_provided = true;
valid_transition = false;
}
}
}
}
if (hil_enabled) {
/* enforce lockdown in HIL */
armed.lockdown = true;
status_flags.system_sensors_initialized = true;
/* recover from a prearm fail */
if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
status.arming_state = vehicle_status_s::ARMING_STATE_STANDBY;
}
// HIL can always go to standby
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
valid_transition = true;
}
}
if (!hil_enabled &&
(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
(status.arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR)) {
// Sensors need to be initialized for STANDBY state, except for HIL
if (!status_flags.system_sensors_initialized) {
feedback_provided = true;
valid_transition = false;
}
}
// Finish up the state transition
if (valid_transition) {
bool was_armed = armed.armed;
armed.armed = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED);
armed.ready_to_arm = (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)
|| (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY);
ret = TRANSITION_CHANGED;
status.arming_state = new_arming_state;
if (was_armed && !armed.armed) { // disarm transition
status.latest_disarming_reason = (uint8_t)calling_reason;
} else if (!was_armed && armed.armed) { // arm transition
status.latest_arming_reason = (uint8_t)calling_reason;
}
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
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",
arming_state_names[status.arming_state], arming_state_names[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}",
eventArmingState(status.arming_state), eventArmingState(new_arming_state));
}
}
return ret;
}
transition_result_t
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state)

12
src/modules/commander/state_machine_helper.h
View File

@ -44,13 +44,10 @@
#include <drivers/drv_hrt.h>
#include "Arming/PreFlightCheck/PreFlightCheck.hpp"
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/commander_state.h>
#include <uORB/topics/vehicle_status_flags.h>
#include <px4_platform_common/events.h>
@ -106,21 +103,12 @@ enum class position_nav_loss_actions_t {
extern const char *const arming_state_names[];
extern const char *const nav_state_names[];
using arm_disarm_reason_t = events::px4::enums::arm_disarm_reason_t;
enum RCLossExceptionBits {
RCL_EXCEPT_MISSION = (1 << 0),
RCL_EXCEPT_HOLD = (1 << 1),
RCL_EXCEPT_OFFBOARD = (1 << 2)
};
transition_result_t
arming_state_transition(vehicle_status_s &status, const vehicle_control_mode_s &control_mode, const safety_s &safety,
const arming_state_t new_arming_state,
actuator_armed_s &armed, const bool fRunPreArmChecks, orb_advert_t *mavlink_log_pub,
vehicle_status_flags_s &status_flags, const PreFlightCheck::arm_requirements_t &arm_requirements,
const hrt_abstime &time_since_boot, arm_disarm_reason_t calling_reason);
transition_result_t
main_state_transition(const vehicle_status_s &status, const main_state_t new_main_state,
const vehicle_status_flags_s &status_flags, commander_state_s &internal_state);