diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp index 079063e3e7c8..024d6b04d75e 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp @@ -47,11 +47,22 @@ 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); + bool gps_success = true; bool gps_present = true; @@ -99,9 +110,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"); } @@ -111,9 +120,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"); } @@ -123,9 +130,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"); } @@ -135,9 +140,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"); } @@ -229,6 +232,12 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s & bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail) { + float ekf_ab_test_limit = 1.0f; // pass limit re-used for each test + param_get(param_find("COM_ARM_EKF_AB"), &ekf_ab_test_limit); + + float ekf_gb_test_limit = 1.0f; // pass limit re-used for each test + param_get(param_find("COM_ARM_EKF_GB"), &ekf_gb_test_limit); + // Get estimator states data if available and exit with a fail recorded if not uORB::Subscription states_sub{ORB_ID(estimator_states)}; estimator_states_s states; @@ -236,18 +245,15 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r if (states_sub.copy(&states)) { // check accelerometer delta velocity bias estimates - float test_limit = 1.0f; // pass limit re-used for each test - param_get(param_find("COM_ARM_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(states.covariances[index], 0.0f)); - if (fabsf(states.states[index]) > test_limit + test_uncertainty) { + if (fabsf(states.states[index]) > ekf_ab_test_limit + test_uncertainty) { if (report_fail) { - PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)test_limit, + PX4_ERR("state %d: |%.8f| > %.8f + %.8f", index, (double)states.states[index], (double)ekf_ab_test_limit, (double)test_uncertainty); mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); } @@ -257,11 +263,11 @@ bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool r } // check gyro delta angle bias estimates - param_get(param_find("COM_ARM_EKF_GB"), &test_limit); - if (fabsf(states.states[10]) > test_limit - || fabsf(states.states[11]) > test_limit - || fabsf(states.states[12]) > test_limit) { + + if (fabsf(states.states[10]) > ekf_gb_test_limit + || fabsf(states.states[11]) > ekf_gb_test_limit + || fabsf(states.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/imuConsistencyCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp index 05bcdc16f00d..12379d41255c 100644 --- a/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp +++ b/src/modules/commander/Arming/PreFlightCheck/checks/imuConsistencyCheck.cpp @@ -44,15 +44,18 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status) { + float accel_test_limit = 1.f; + param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit); + + float gyro_test_limit = 1.f; + param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit); + // Get sensor_preflight data if available and exit with a fail recorded if not uORB::SubscriptionData sensors_status_imu_sub{ORB_ID(sensors_status_imu)}; const sensors_status_imu_s &imu = sensors_status_imu_sub.get(); // Use the difference between IMU's to detect a bad calibration. // If a single IMU is fitted, the value being checked will be zero so this check will always pass. - float accel_test_limit = 1.f; - param_get(param_find("COM_ARM_IMU_ACC"), &accel_test_limit); - for (unsigned i = 0; i < (sizeof(imu.accel_inconsistency_m_s_s) / sizeof(imu.accel_inconsistency_m_s_s[0])); i++) { if (imu.accel_device_ids[i] != 0) { if (imu.accel_device_ids[i] == imu.accel_device_id_primary) { @@ -87,9 +90,6 @@ bool PreFlightCheck::imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_ } // Fail if gyro difference greater than 5 deg/sec and notify if greater than 2.5 deg/sec - float gyro_test_limit = 1.f; - param_get(param_find("COM_ARM_IMU_GYR"), &gyro_test_limit); - for (unsigned i = 0; i < (sizeof(imu.gyro_inconsistency_rad_s) / sizeof(imu.gyro_inconsistency_rad_s[0])); i++) { if (imu.gyro_device_ids[i] != 0) { if (imu.gyro_device_ids[i] == imu.gyro_device_id_primary) { diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/powerCheck.cpp index 52d13e254104..3ad148a8c0da 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 (system_power.timestamp != 0) { + 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 {