commander: Reduce false positives in pre-flight accel bias check

PX4 · Feb 4, 2019 · 6fb7cda · 6fb7cda
1 parent 7ecb04d
commit 6fb7cda
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 1 deletion.
diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp
@@ -576,6 +576,19 @@ 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++) {
+		// 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 (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
+			}
+
+			success = false;
+			goto out;
+		}
+	}
 
 	if (fabsf(status.states[13]) > test_limit || fabsf(status.states[14]) > test_limit
 	    || fabsf(status.states[15]) > test_limit) {

diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
@@ -575,7 +575,7 @@ PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
  * @decimal 4
  * @increment 0.0001
  */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 2.4e-3f);
+PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 1.73e-3f);
 
 /**
  * Maximum value of EKF gyro delta angle bias estimate that will allow arming