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

vehicle_status: add latest arming/disarming reason 

Makes it easier to debug.
This commit is contained in:
Beat Küng 2020-08-04 10:55:26 +02:00 committed by Daniel Agar
parent 1f5d0cd2d0
commit 5f8c6512b3
6 changed files with 106 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
msg/vehicle_status.msg
View File

@ -94,3 +94,22 @@ uint8 failure_detector_status # Bitmask containing FailureDetector status [0,
uint32 onboard_control_sensors_present
uint32 onboard_control_sensors_enabled
uint32 onboard_control_sensors_health
uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0
uint8 ARM_DISARM_REASON_RC_STICK = 1
uint8 ARM_DISARM_REASON_RC_SWITCH = 2
uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3
uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4
uint8 ARM_DISARM_REASON_MISSION_START = 5
uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6
uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7
uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8
uint8 ARM_DISARM_REASON_KILL_SWITCH = 9
uint8 ARM_DISARM_REASON_LOCKDOWN = 10
uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11
uint8 ARM_DISARM_REASON_SHUTDOWN = 12
uint8 ARM_DISARM_REASON_UNIT_TEST = 13
uint8 latest_arming_reason
uint8 latest_disarming_reason

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

@ -424,11 +424,12 @@ bool Commander::shutdown_if_allowed()
{
return TRANSITION_DENIED != arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN,
&armed, false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags, _arm_requirements,
hrt_elapsed_time(&_boot_timestamp));
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN);
}
transition_result_t
Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local, const char *armedBy)
Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub_local,
arm_disarm_reason_t calling_reason)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
@ -442,10 +443,42 @@ Commander::arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink
mavlink_log_pub_local,
&status_flags,
_arm_requirements,
hrt_elapsed_time(&_boot_timestamp));
hrt_elapsed_time(&_boot_timestamp), calling_reason);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "ARMED" : "DISARMED", armedBy);
const char *reason = "";
switch (calling_reason) {
case arm_disarm_reason_t::TRANSITION_TO_STANDBY: reason = ""; break;
case arm_disarm_reason_t::RC_STICK: reason = "RC"; break;
case arm_disarm_reason_t::RC_SWITCH: reason = "RC (switch)"; break;
case arm_disarm_reason_t::COMMAND_INTERNAL: reason = "internal command"; break;
case arm_disarm_reason_t::COMMAND_EXTERNAL: reason = "external command"; break;
case arm_disarm_reason_t::MISSION_START: reason = "mission start"; break;
case arm_disarm_reason_t::SAFETY_BUTTON: reason = "safety button"; break;
case arm_disarm_reason_t::AUTO_DISARM_LAND: reason = "landing"; break;
case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: reason = "auto preflight disarming"; break;
case arm_disarm_reason_t::KILL_SWITCH: reason = "kill-switch"; break;
case arm_disarm_reason_t::LOCKDOWN: reason = "lockdown"; break;
case arm_disarm_reason_t::FAILURE_DETECTOR: reason = "failure detector"; break;
case arm_disarm_reason_t::SHUTDOWN: reason = "shutdown request"; break;
case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break;
}
mavlink_log_info(mavlink_log_pub_local, "%s by %s", arm ? "Armed" : "Disarmed", reason);
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
@ -736,7 +769,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
}
}
transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub, "Arm/Disarm component command");
transition_result_t arming_res = arm_disarm(cmd_arms, !enforce, &mavlink_log_pub,
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL));
if (arming_res == TRANSITION_DENIED) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
@ -949,7 +983,7 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
// switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(*status_local, commander_state_s::MAIN_STATE_AUTO_MISSION, status_flags,
&_internal_state))
&& (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, "Mission start command"))) {
&& (TRANSITION_DENIED != arm_disarm(true, true, &mavlink_log_pub, arm_disarm_reason_t::MISSION_START))) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -1495,7 +1529,7 @@ Commander::run()
}
if (safety_disarm_allowed) {
if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, "Safety button")) {
if (TRANSITION_CHANGED == arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::SAFETY_BUTTON)) {
_status_changed = true;
}
}
@ -1615,7 +1649,8 @@ Commander::run()
}
if (_auto_disarm_landed.get_state()) {
arm_disarm(false, true, &mavlink_log_pub, "Auto disarm initiated");
arm_disarm(false, true, &mavlink_log_pub,
(_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT));
}
}
@ -1624,10 +1659,10 @@ Commander::run()
if (_auto_disarm_killed.get_state()) {
if (armed.manual_lockdown) {
arm_disarm(false, true, &mavlink_log_pub, "Kill-switch still engaged");
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::KILL_SWITCH);
} else {
arm_disarm(false, true, &mavlink_log_pub, "System in lockdown");
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::LOCKDOWN);
}
}
@ -1663,7 +1698,8 @@ Commander::run()
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp));
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY);
if (arming_ret == TRANSITION_DENIED) {
/* do not complain if not allowed into standby */
@ -1922,7 +1958,8 @@ Commander::run()
if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed,
true /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp));
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
}
_stick_off_counter++;
@ -1976,7 +2013,8 @@ Commander::run()
} else if (status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &armed,
!in_arming_grace_period /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp));
&mavlink_log_pub, &status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
if (arming_ret != TRANSITION_CHANGED) {
px4_usleep(100000);
@ -2182,7 +2220,7 @@ Commander::run()
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
arm_disarm(false, true, &mavlink_log_pub, "Failure detector");
arm_disarm(false, true, &mavlink_log_pub, arm_disarm_reason_t::FAILURE_DETECTOR);
_status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "ESCs did not respond to arm request");
}
@ -3419,7 +3457,8 @@ void *commander_low_prio_loop(void *arg)
if (TRANSITION_DENIED == arming_state_transition(&status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, &armed,
false /* fRunPreArmChecks */, &mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_INIT
30_s) // time since boot not relevant for switching to ARMING_STATE_INIT
30_s, // time since boot not relevant for switching to ARMING_STATE_INIT
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL))
) {
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED, command_ack_pub);
@ -3514,7 +3553,8 @@ void *commander_low_prio_loop(void *arg)
false /* fRunPreArmChecks */,
&mavlink_log_pub, &status_flags,
PreFlightCheck::arm_requirements_t{}, // arming requirements not relevant for switching to ARMING_STATE_STANDBY
30_s); // time since boot not relevant for switching to ARMING_STATE_STANDBY
30_s, // time since boot not relevant for switching to ARMING_STATE_STANDBY
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL));
} else {
tune_negative(true);

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

@ -113,7 +113,8 @@ public:
private:
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub, const char *armedBy);
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, orb_advert_t *mavlink_log_pub,
arm_disarm_reason_t calling_reason);
void battery_status_check();

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

@ -305,8 +305,8 @@ bool StateMachineHelperTest::armingStateTransitionTest()
nullptr /* no mavlink_log_pub */,
&status_flags,
arm_req,
2e6 /* 2 seconds after boot, everything should be checked */
);
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);

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

@ -105,7 +105,7 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
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)
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");
@ -216,12 +216,20 @@ transition_result_t arming_state_transition(vehicle_status_s *status, const safe
// 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) {
armed->armed_time_ms = hrt_absolute_time() / 1000;

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

@ -97,11 +97,28 @@ enum class position_nav_loss_actions_t {
extern const char *const arming_state_names[];
enum class arm_disarm_reason_t {
TRANSITION_TO_STANDBY = 0,
RC_STICK = 1,
RC_SWITCH = 2,
COMMAND_INTERNAL = 3,
COMMAND_EXTERNAL = 4,
MISSION_START = 5,
SAFETY_BUTTON = 6,
AUTO_DISARM_LAND = 7,
AUTO_DISARM_PREFLIGHT = 8,
KILL_SWITCH = 9,
LOCKDOWN = 10,
FAILURE_DETECTOR = 11,
SHUTDOWN = 12,
UNIT_TEST = 13
};
transition_result_t
arming_state_transition(vehicle_status_s *status, 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);
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,