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

ArmStateMachine: simplify how preflight checks are called

This commit is contained in:
Matthias Grob 2022-06-09 14:13:38 +02:00 committed by Daniel Agar
parent 5dc3fecac0
commit 0f41a5e385
1 changed files with 12 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -53,55 +53,30 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s
transition_result_t ret = TRANSITION_DENIED;
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 == _arm_state) {
ret = TRANSITION_NOT_CHANGED;
} else {
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);
feedback_provided = true;
}
// Check that we have a valid state transition
bool valid_transition = arming_transitions[new_arming_state][_arm_state];
if (valid_transition) {
// We have a good transition. Now perform any secondary validation.
if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
// 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)) {
// Do not perform pre-arm checks if coming from in air restore
// Allow if vehicle_status_s::HIL_STATE_ON
if (_arm_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_button_available, safety_off,
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 (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, true, true, time_since_boot)
|| !PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety_button_available, safety_off,
arm_requirements, status)) {
feedback_provided = true; // Preflight checks report error messages
valid_transition = false;
}
}
if (hil_enabled) {
if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
/* enforce lockdown in HIL */
armed.lockdown = true;