diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp index 64e529f4bc..818e5812c6 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp @@ -55,7 +55,7 @@ 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, const bool checkGNSS, bool report_failures, const bool prearm, + vehicle_status_flags_s &status_flags, bool report_failures, const bool prearm, const hrt_abstime &time_since_boot) { report_failures = (report_failures && status_flags.condition_system_hotplug_timeout @@ -235,16 +235,21 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu } if (estimator_type == 2) { + bool ekf_healthy = false; + // don't report ekf failures for the first 10 seconds to allow time for the filter to start - bool report_ekf_fail = (time_since_boot > 10_s); + if (time_since_boot > 10_s) { - if (!ekf2Check(mavlink_log_pub, status, false, report_failures && report_ekf_fail, checkGNSS)) { - failed = true; + ekf_healthy = ekf2Check(mavlink_log_pub, status, false, report_failures) && + ekf2CheckSensorBias(mavlink_log_pub, report_failures); + + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, true, ekf_healthy, status); + + } else { + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, true, false, false, status); } - if (!ekf2CheckSensorBias(mavlink_log_pub, report_failures && report_ekf_fail)) { - failed = true; - } + failed |= !ekf_healthy; } /* ---- Failure Detector ---- */ diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp index 4a77a87303..e4ce474d24 100644 --- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp +++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp @@ -78,7 +78,7 @@ 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, const bool checkGNSS, bool reportFailures, const bool prearm, + vehicle_status_flags_s &status_flags, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot); struct arm_requirements_t { @@ -109,7 +109,7 @@ private: static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm); static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, - const bool report_fail, const bool enforce_gps_required); + const bool report_fail); static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail); diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index ae4e57bdb4..43b92032ca 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -45,10 +45,9 @@ using namespace time_literals; bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, - const bool report_fail, const bool enforce_gps_required) + const bool report_fail) { bool success = true; // start with a pass and change to a fail if any test fails - bool ahrs_present = true; int32_t mag_strength_check_enabled = 1; param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled); @@ -65,6 +64,9 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & float mag_test_ratio_limit = 1.f; param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit); + int32_t arm_without_gps = 0; + param_get(param_find("COM_ARM_WO_GPS"), &arm_without_gps); + bool gps_success = true; bool gps_present = true; @@ -74,7 +76,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & const estimator_status_s &status = status_sub.get(); if (status.timestamp == 0) { - ahrs_present = false; + success = false; goto out; } @@ -152,7 +154,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing - if (enforce_gps_required || report_fail) { + { const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS); const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0; @@ -206,7 +208,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } if (message) { - if (enforce_gps_required) { + if (!arm_without_gps) { mavlink_log_critical(mavlink_log_pub, message, " Fail"); } else { @@ -217,7 +219,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & gps_success = false; - if (enforce_gps_required) { + if (!arm_without_gps) { success = false; goto out; } @@ -225,10 +227,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } out: - //PX4_INFO("AHRS CHECK: %s", (success && ahrs_present) ? "OK" : "FAIL"); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, ahrs_present, true, success && ahrs_present, vehicle_status); - set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status); - + set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, !arm_without_gps, gps_success, vehicle_status); return success; } diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index fbde16ecec..7d6bd001a2 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -230,7 +230,7 @@ int Commander::custom_command(int argc, char *argv[]) vehicle_status_flags_sub.copy(&vehicle_status_flags); bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, vehicle_status, vehicle_status_flags, true, true, - true, 30_s); + 30_s); PX4_INFO("Preflight check: %s", preflight_check_res ? "OK" : "FAILED"); bool prearm_check_res = PreFlightCheck::preArmCheck(nullptr, vehicle_status_flags, safety_s{}, @@ -1522,7 +1522,7 @@ 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, _arm_requirements.global_position, false, + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, false, true, hrt_elapsed_time(&_boot_timestamp)); @@ -2615,7 +2615,7 @@ Commander::run() // Evaluate current prearm status if (!_armed.armed && !_status_flags.condition_calibration_enabled) { - bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, true, false, true, 30_s); + bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, 30_s); // skip arm authorization check until actual arming attempt PreFlightCheck::arm_requirements_t arm_req = _arm_requirements; @@ -3711,8 +3711,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, - _arm_requirements.global_position, true, true, hrt_elapsed_time(&_boot_timestamp)); + PreFlightCheck::preflightCheck(&_mavlink_log_pub, _status, _status_flags, true, true, + hrt_elapsed_time(&_boot_timestamp)); } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ee99c0a8e5..afc880ffeb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -133,8 +133,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, - arm_requirements.global_position, true, true, time_since_boot); + preflight_check_ret = PreFlightCheck::preflightCheck(mavlink_log_pub, *status, *status_flags, true, true, + time_since_boot); if (preflight_check_ret) { status_flags->condition_system_sensors_initialized = true; @@ -153,7 +153,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, arm_requirements.global_position, false, status->arming_state != vehicle_status_s::ARMING_STATE_ARMED, + *status_flags, false, status->arming_state != vehicle_status_s::ARMING_STATE_ARMED, time_since_boot); last_preflight_check = hrt_absolute_time();