From ad447ab2235d3004eedfbd06bd4b9d2cb4375dd2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 24 Jan 2022 21:00:02 -0500 Subject: [PATCH] commander: respect control mode for prearm requirements - preflight tolerate ekf2 warnings if not in an attitude/velocity/position mode (eg manual or acro) --- .../Arming/PreFlightCheck/PreFlightCheck.cpp | 12 +++- .../Arming/PreFlightCheck/PreFlightCheck.hpp | 7 ++- .../PreFlightCheck/checks/preArmCheck.cpp | 36 +++++++++++ src/modules/commander/Commander.cpp | 59 +++++++++---------- .../state_machine_helper_test.cpp | 5 +- .../commander/state_machine_helper.cpp | 12 ++-- src/modules/commander/state_machine_helper.h | 3 +- 7 files changed, 90 insertions(+), 44 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 259af6b878..2b9f6ff0b7 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -55,8 +55,8 @@ static constexpr unsigned max_mandatory_baro_count = 1; static constexpr unsigned max_optional_baro_count = 4; bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm, - const hrt_abstime &time_since_boot) + vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, + bool report_failures, const bool prearm, const hrt_abstime &time_since_boot) { report_failures = (report_failures && status_flags.condition_system_hotplug_timeout && !status_flags.condition_calibration_enabled); @@ -256,7 +256,13 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); } - failed |= !ekf_healthy; + + if (control_mode.flag_control_attitude_enabled + || control_mode.flag_control_velocity_enabled + || control_mode.flag_control_position_enabled) { + // healthy estimator only required for dependent control modes + failed |= !ekf_healthy; + } } /* ---- Failure Detector ---- */ diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 315b483e45..242fbf58ae 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -41,8 +41,8 @@ #pragma once #include +#include #include - #include #include @@ -78,8 +78,8 @@ public: * true if the system power should be checked **/ static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, - vehicle_status_flags_s &status_flags, bool reportFailures, const bool prearm, - const hrt_abstime &time_since_boot); + vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, + bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot); struct arm_requirements_t { bool arm_authorization = false; @@ -90,6 +90,7 @@ public: }; static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const vehicle_control_mode_s &control_mode, const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail = true); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index d0d113ece8..fe14df4345 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -39,10 +39,46 @@ #include bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const vehicle_control_mode_s &control_mode, const safety_s &safety, const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail) { bool prearm_ok = true; + // rate control mode require valid angular velocity + if (control_mode.flag_control_rates_enabled && !status_flags.condition_angular_velocity_valid) { + if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! angular velocity invalid"); } + + prearm_ok = false; + } + + // attitude control mode require valid attitude + if (control_mode.flag_control_attitude_enabled && !status_flags.condition_attitude_valid) { + if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! attitude invalid"); } + + prearm_ok = false; + } + + // velocity control mode require valid velocity + if (control_mode.flag_control_velocity_enabled && !status_flags.condition_local_velocity_valid) { + if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! velocity invalid"); } + + prearm_ok = false; + } + + // position control mode require valid position + if (control_mode.flag_control_position_enabled && !status_flags.condition_local_position_valid) { + if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! position invalid"); } + + prearm_ok = false; + } + + // manual control mode require valid manual control (rc) + if (control_mode.flag_control_manual_enabled && status.rc_signal_lost) { + if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! manual control lost"); } + + prearm_ok = false; + } + // USB not connected if (!status_flags.circuit_breaker_engaged_usb_check && status_flags.usb_connected) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Flying with USB is not safe"); } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 53fdda3e65..6178a32194 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -274,11 +274,16 @@ int Commander::custom_command(int argc, char *argv[]) vehicle_status_flags_s vehicle_status_flags{}; vehicle_status_flags_sub.copy(&vehicle_status_flags); - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, true, true, - 30_s); + uORB::Subscription vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; + vehicle_control_mode_s vehicle_control_mode{}; + vehicle_control_mode_sub.copy(&vehicle_control_mode); + + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, + vehicle_control_mode, + true, true, 30_s); PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); - bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, safety_s{}, + bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode, safety_s{}, PreFlightCheck::arm_requirements_t{}, vehicle_status); PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); @@ -468,7 +473,8 @@ extern "C" __EXPORT int commander_main(int argc, char *argv[]) bool Commander::shutdown_if_allowed() { - return TRANSITION_DENIED != arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_SHUTDOWN, + return TRANSITION_DENIED != 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); } @@ -716,15 +722,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_ } } - transition_result_t arming_res = arming_state_transition(_status, - _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); + transition_result_t arming_res = 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); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(&_mavlink_log_pub, "Armed by %s\t", arm_disarm_reason_str(calling_reason)); @@ -763,14 +764,9 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f } } - transition_result_t arming_res = arming_state_transition(_status, - _safety, - vehicle_status_s::ARMING_STATE_STANDBY, - _armed, - false, - &_mavlink_log_pub, - _status_flags, - _arm_requirements, + transition_result_t arming_res = 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); if (arming_res == TRANSITION_CHANGED) { @@ -1334,7 +1330,8 @@ Commander::handle_command(const vehicle_command_s &cmd) } else { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED == arming_state_transition(_status, safety_s{}, vehicle_status_s::ARMING_STATE_INIT, _armed, + if (TRANSITION_DENIED == 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 30_s, // time since boot not relevant for switching to ARMING_STATE_INIT @@ -1988,9 +1985,8 @@ Commander::run() arm_auth_init(&_mavlink_log_pub, &_status.system_id); // run preflight immediately to find all relevant parameters, but don't report - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, false, - true, - hrt_elapsed_time(&_boot_timestamp)); + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode, + false, true, hrt_elapsed_time(&_boot_timestamp)); while (!should_exit()) { @@ -2347,7 +2343,7 @@ Commander::run() /* If in INIT state, try to proceed to STANDBY state */ if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { - arming_state_transition(_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, _armed, + 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); @@ -2942,13 +2938,14 @@ Commander::run() // Evaluate current prearm status if (!_armed.armed && !_status_flags.condition_calibration_enabled) { - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, - hrt_elapsed_time(&_boot_timestamp)); + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, _vehicle_control_mode, + false, true, hrt_elapsed_time(&_boot_timestamp)); // skip arm authorization check until actual arming attempt PreFlightCheck::arm_requirements_t arm_req = _arm_requirements; arm_req.arm_authorization = false; - bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _safety, arm_req, _status, false); + bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, _status_flags, _vehicle_control_mode, _safety, arm_req, + _status, false); set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, (preflight_check_res && prearm_check_res), _status); @@ -3584,8 +3581,8 @@ void Commander::data_link_check() if (!_armed.armed && !_status_flags.condition_calibration_enabled) { // make sure to report preflight check failures to a connecting GCS - PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, false, - hrt_elapsed_time(&_boot_timestamp)); + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, _vehicle_control_mode, + true, false, hrt_elapsed_time(&_boot_timestamp)); } } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8d74a4643c..e3b331417f 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -296,11 +296,14 @@ bool StateMachineHelperTest::armingStateTransitionTest() status_flags.circuit_breaker_engaged_power_check = true; safety.safety_switch_available = test->safety_switch_available; safety.safety_off = test->safety_off; + armed.armed = test->current_state.armed; armed.ready_to_arm = test->current_state.ready_to_arm; + vehicle_control_mode_s control_mode{}; + // Attempt transition - transition_result_t result = arming_state_transition(status, safety, test->requested_state, armed, + 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, diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 10174b823a..5de1b94d09 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -154,7 +154,8 @@ 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 safety_s &safety, +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, @@ -186,8 +187,8 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe if (fRunPreArmChecks && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) && !hil_enabled) { - preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, true, true, - time_since_boot); + preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, + true, true, time_since_boot); if (preflight_check_ret) { status_flags.condition_system_sensors_initialized = true; @@ -206,7 +207,7 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe if ((last_preflight_check == 0) || (hrt_elapsed_time(&last_preflight_check) > 1000 * 1000)) { status_flags.condition_system_sensors_initialized = PreFlightCheck::preflightCheck(mavlink_log_pub, status, - status_flags, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, + status_flags, control_mode, false, status.arming_state != vehicle_status_s::ARMING_STATE_ARMED, time_since_boot); last_preflight_check = hrt_absolute_time(); @@ -228,7 +229,8 @@ transition_result_t arming_state_transition(vehicle_status_s &status, const safe if (fRunPreArmChecks && preflight_check_ret) { // only bother running prearm if preflight was successful - prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, safety, arm_requirements, status); + prearm_check_ret = PreFlightCheck::preArmCheck(mavlink_log_pub, status_flags, control_mode, safety, arm_requirements, + status); } if (!preflight_check_ret || !prearm_check_ret) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a7c5e2d709..b189cb21fc 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -108,7 +108,8 @@ enum RCLossExceptionBits { }; transition_result_t -arming_state_transition(vehicle_status_s &status, const safety_s &safety, const arming_state_t new_arming_state, +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);