From d3148f2656fb8375e606d4b110e4ec29140f365c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 3 Oct 2020 11:25:59 -0400 Subject: [PATCH] commander: PreFlightCheck param_find all parameters immediately - this ensures the relevant parameters are marked active immediately before parameter sync - fixes https://github.com/PX4/Firmware/issues/15872 --- .../PreFlightCheck/checks/ekf2Check.cpp | 47 +++++++++++-------- .../PreFlightCheck/checks/powerCheck.cpp | 7 ++- 2 files changed, 31 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 51522d523e..80ed02169c 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -46,11 +46,28 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & { bool success = true; // start with a pass and change to a fail if any test fails bool ahrs_present = true; - float test_limit = 1.0f; // pass limit re-used for each test int32_t mag_strength_check_enabled = 1; param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled); + float hgt_test_ratio_limit = 1.f; + param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit); + + float vel_test_ratio_limit = 1.f; + param_get(param_find("COM_ARM_EKF_VEL"), &vel_test_ratio_limit); + + float pos_test_ratio_limit = 1.f; + param_get(param_find("COM_ARM_EKF_POS"), &pos_test_ratio_limit); + + float mag_test_ratio_limit = 1.f; + param_get(param_find("COM_ARM_EKF_YAW"), &mag_test_ratio_limit); + + float ekf_ab_test_limit = 1.f; + param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit); + + float ekf_gb_test_limit = 1.f; + param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit); + bool gps_success = true; bool gps_present = true; @@ -98,9 +115,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // check vertical position innovation test ratio - param_get(param_find("COM_ARM_EKF_HGT"), &test_limit); - - if (status.hgt_test_ratio > test_limit) { + if (status.hgt_test_ratio > hgt_test_ratio_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error"); } @@ -110,9 +125,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // check velocity innovation test ratio - param_get(param_find("COM_ARM_EKF_VEL"), &test_limit); - - if (status.vel_test_ratio > test_limit) { + if (status.vel_test_ratio > vel_test_ratio_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error"); } @@ -122,9 +135,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // check horizontal position innovation test ratio - param_get(param_find("COM_ARM_EKF_POS"), &test_limit); - - if (status.pos_test_ratio > test_limit) { + if (status.pos_test_ratio > pos_test_ratio_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error"); } @@ -134,9 +145,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // check magnetometer innovation test ratio - param_get(param_find("COM_ARM_EKF_YAW"), &test_limit); - - if (status.mag_test_ratio > test_limit) { + if (status.mag_test_ratio > mag_test_ratio_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error"); } @@ -146,17 +155,17 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // check accelerometer delta velocity bias estimates - param_get(param_find("COM_ARM_EKF_AB"), &test_limit); + param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit); for (uint8_t index = 13; index < 16; index++) { // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives // adjust test threshold by 3-sigma float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f)); - if (fabsf(status.states[index]) > test_limit + test_uncertainty) { + if (fabsf(status.states[index]) > ekf_ab_test_limit + test_uncertainty) { if (report_fail) { - PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)test_limit, + PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)status.states[index], (double)ekf_ab_test_limit, (double)test_uncertainty); mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); } @@ -167,10 +176,10 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & } // check gyro delta angle bias estimates - param_get(param_find("COM_ARM_EKF_GB"), &test_limit); + param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit); - if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit - || fabsf(status.states[12]) > test_limit) { + if (fabsf(status.states[10]) > ekf_gb_test_limit || fabsf(status.states[11]) > ekf_gb_test_limit + || fabsf(status.states[12]) > ekf_gb_test_limit) { if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias"); } diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 75f18b82a7..866fc554a7 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp @@ -68,6 +68,9 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta const system_power_s &system_power = system_power_sub.get(); if (hrt_elapsed_time(&system_power.timestamp) < 1_s) { + int32_t required_power_module_count = 0; + param_get(param_find("COM_POWER_COUNT"), &required_power_module_count); + // Check avionics rail voltages (if USB isn't connected) if (!system_power.usb_connected) { float avionics_power_rail_voltage = system_power.voltage5v_v; @@ -94,9 +97,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta const int power_module_count = countSetBits(system_power.brick_valid); - int32_t required_power_module_count = 0; - param_get(param_find("COM_POWER_COUNT"), &required_power_module_count); - if (power_module_count < required_power_module_count) { success = false; @@ -105,7 +105,6 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta power_module_count, required_power_module_count); } } - } } else {