diff --git a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp index 8b7fe3cf91..263407fc7a 100644 --- a/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp +++ b/src/modules/commander/Arming/ArmStateMachine/ArmStateMachine.cpp @@ -68,9 +68,12 @@ transition_result_t ArmStateMachine::arming_state_transition(vehicle_status_s &s && !(status.hil_state == vehicle_status_s::HIL_STATE_ON) && (_arm_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE)) { - 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)) { + if (!PreFlightCheck::preflightCheck(mavlink_log_pub, status, status_flags, control_mode, + true, // report_failures + true, // prearm + time_since_boot, + safety_button_available, safety_off, + arm_requirements)) { feedback_provided = true; // Preflight checks report error messages valid_transition = false; } diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index eaa063392d..0999c0e8b4 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -52,7 +52,9 @@ static constexpr unsigned max_mandatory_baro_count = 1; bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, 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) + bool report_failures, const bool prearm, const hrt_abstime &time_since_boot, + const bool safety_button_available, const bool safety_off, + const arm_requirements_t &arm_requirements) { report_failures = (report_failures && !status_flags.calibration_enabled); @@ -226,6 +228,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu failed = failed || !modeCheck(mavlink_log_pub, report_failures, status); failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures); failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags); + failed = failed || !preArmCheck(mavlink_log_pub, status_flags, control_mode, + safety_button_available, safety_off, arm_requirements, status, report_failures); /* Report status */ return !failed; diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 58432d425b..82f4b5ca4d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -56,6 +56,14 @@ public: PreFlightCheck() = default; ~PreFlightCheck() = default; + struct arm_requirements_t { + bool arm_authorization = false; + bool esc_check = false; + bool global_position = false; + bool mission = false; + bool geofence = false; + }; + /** * Runs a preflight check on all sensors to see if they are properly calibrated and healthy * @@ -83,20 +91,9 @@ public: **/ static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, 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; - bool esc_check = false; - bool global_position = false; - bool mission = false; - bool geofence = false; - }; - - static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, - const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off, - const arm_requirements_t &arm_requirements, vehicle_status_s &status, - bool report_fail = true); + bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot, + const bool safety_button_available, const bool safety_off, + const arm_requirements_t &arm_requirements); private: static bool sensorAvailabilityCheck(const bool report_failure, @@ -138,4 +135,8 @@ private: static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail); static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_flags_s &status_flags); + static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, + const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off, + const arm_requirements_t &arm_requirements, vehicle_status_s &status, + const bool report_fail); }; diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp index df7eea0d1a..bb86bd445d 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp @@ -40,7 +40,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const vehicle_control_mode_s &control_mode, const bool safety_button_available, const bool safety_off, - const arm_requirements_t &arm_requirements, vehicle_status_s &status, bool report_fail) + const arm_requirements_t &arm_requirements, vehicle_status_s &status, const bool report_fail) { bool prearm_ok = true; @@ -227,6 +227,5 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st } } - return prearm_ok; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 0ac2e7c755..90b02c21c5 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -298,16 +298,14 @@ int Commander::custom_command(int argc, char *argv[]) bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, vehicle_control_mode, - true, true, 30_s); + true, // report_failures + true, // prearm + 30_s, + false, // safety_buttton_available not known + false, // safety_off not known + PreFlightCheck::arm_requirements_t{}); PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); - bool dummy_safety_button{false}; - bool dummy_safety_off{false}; - bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, vehicle_control_mode, - dummy_safety_button, dummy_safety_off, - PreFlightCheck::arm_requirements_t{}, vehicle_status); - PX4_INFO("Prearm check: %s", prearm_check_res ? "OK" : "FAILED"); - print_health_flags(vehicle_status); return 0; @@ -856,7 +854,12 @@ Commander::Commander() : // run preflight immediately to find all relevant parameters, but don't report PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode, - false, true, hrt_elapsed_time(&_boot_timestamp)); + false, // report_failures + true, // prearm + hrt_elapsed_time(&_boot_timestamp), + false, // safety_buttton_available not known + false, // safety_off not known, + PreFlightCheck::arm_requirements_t{}); } Commander::~Commander() @@ -3004,19 +3007,19 @@ Commander::run() // Evaluate current prearm status (skip during arm -> disarm transition) if (!actuator_armed_prev.armed && !_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) { perf_begin(_preflight_check_perf); - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _vehicle_status, _vehicle_status_flags, - _vehicle_control_mode, - false, true, hrt_elapsed_time(&_boot_timestamp)); - perf_end(_preflight_check_perf); - // 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, _vehicle_status_flags, _vehicle_control_mode, - _safety.isButtonAvailable(), _safety.isSafetyOff(), - arm_req, _vehicle_status, false); + _vehicle_status_flags.pre_flight_checks_pass = PreFlightCheck::preflightCheck(nullptr, _vehicle_status, + _vehicle_status_flags, + _vehicle_control_mode, + false, // report_failures + true, // prearm + hrt_elapsed_time(&_boot_timestamp), + _safety.isButtonAvailable(), _safety.isSafetyOff(), + arm_req); + perf_end(_preflight_check_perf); - _vehicle_status_flags.pre_flight_checks_pass = preflight_check_res && prearm_check_res; set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PREARM_CHECK, true, true, _vehicle_status_flags.pre_flight_checks_pass, _vehicle_status); } @@ -3638,8 +3641,14 @@ void Commander::data_link_check() if (!_arm_state_machine.isArmed() && !_vehicle_status_flags.calibration_enabled) { // make sure to report preflight check failures to a connecting GCS + // skip arm authorization check until actual arming attempt + PreFlightCheck::arm_requirements_t arm_req = _arm_requirements; + arm_req.arm_authorization = false; PreFlightCheck::preflightCheck(&_mavlink_log_pub, _vehicle_status, _vehicle_status_flags, _vehicle_control_mode, - true, false, hrt_elapsed_time(&_boot_timestamp)); + true, // report_failures + false, // prearm + hrt_elapsed_time(&_boot_timestamp), + _safety.isButtonAvailable(), _safety.isSafetyOff(), arm_req); } }