|
|
|
@ -576,10 +576,12 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
@@ -576,10 +576,12 @@ static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_s
|
|
|
|
|
|
|
|
|
|
// check accelerometer delta velocity bias estimates
|
|
|
|
|
param_get(param_find("COM_ARM_EKF_AB"), &test_limit); |
|
|
|
|
for (uint8_t index=13; index<16; index++) { |
|
|
|
|
|
|
|
|
|
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)); |
|
|
|
|
float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f)); |
|
|
|
|
|
|
|
|
|
if (fabsf(status.states[index]) > test_limit + test_uncertainty) { |
|
|
|
|
if (report_fail) { |
|
|
|
|
mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias"); |
|
|
|
|