estimated IMU bias preflight checks

- commander preflightcheck use estimator_sensor_bias message instead of EKF state index magic number
 - ekf2 publish estimated bias limits in estimator_sensor_bias
 - preflightcheck only error if bias estimate exceeds half of configured limit (delete COM_ARM_EKF_AB and COM_ARM_EKF_GB parameters)

ROMFS/px4fmu_common/init.d/airframes/4071_ifo

@@ -98,7 +98,6 @@ then
 	param set BAT_V_DIV 10.14
 	param set BAT_A_PER_V 18.18
 	#param set CBRK_IO_SAFETY 22027
-	param set COM_ARM_EKF_AB 0.005
 	param set COM_DISARM_LAND 2
 
 	# Filter settings

ROMFS/px4fmu_common/init.d/airframes/4072_draco

@@ -77,7 +77,6 @@ then
 
 	param set BAT_SOURCE 0
 	param set CBRK_IO_SAFETY 22027
-	#param set COM_ARM_EKF_AB 0.005
 	#param set COM_DISARM_LAND 3
 
 	# Filter settings

ROMFS/px4fmu_common/init.d/airframes/4073_ifo-s

@@ -107,7 +107,6 @@ then
 	param set BAT_V_DIV 10.14
 	param set BAT_A_PER_V 18.18
 	#param set CBRK_IO_SAFETY 22027
-	param set COM_ARM_EKF_AB 0.005
 	param set COM_DISARM_LAND 2
 
 	# Filter settings

ROMFS/px4fmu_common/init.d/airframes/4250_teal

@@ -49,7 +49,6 @@ then
 	# sensor calibration
 	param set CAL_MAG_SIDES 63
 	param set SENS_BOARD_ROT 0
-	param set COM_ARM_EKF_AB 0.0032
 
 	# circuit breakers
 	param set CBRK_IO_SAFETY 22027

msg/estimator_sensor_bias.msg

@@ -3,22 +3,25 @@
 # scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available).
 #
 
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp                # time since system start (microseconds)
 uint64 timestamp_sample         # the timestamp of the raw data (microseconds)
 
 # In-run bias estimates (subtract from uncorrected data)
 
-uint32 gyro_device_id		# unique device ID for the sensor that does not change between power cycles
+uint32 gyro_device_id           # unique device ID for the sensor that does not change between power cycles
-float32[3] gyro_bias		# gyroscope in-run bias in body frame (rad/s)
+float32[3] gyro_bias            # gyroscope in-run bias in body frame (rad/s)
+float32 gyro_bias_limit         # magnitude of maximum gyroscope in-run bias in body frame (rad/s)
 float32[3] gyro_bias_variance
 bool gyro_bias_valid
 
-uint32 accel_device_id		# unique device ID for the sensor that does not change between power cycles
+uint32 accel_device_id          # unique device ID for the sensor that does not change between power cycles
-float32[3] accel_bias		# accelerometer in-run bias in body frame (m/s^2)
+float32[3] accel_bias           # accelerometer in-run bias in body frame (m/s^2)
+float32 accel_bias_limit        # magnitude of maximum accelerometer in-run bias in body frame (m/s^2)
 float32[3] accel_bias_variance
 bool accel_bias_valid
 
-uint32 mag_device_id		# unique device ID for the sensor that does not change between power cycles
+uint32 mag_device_id            # unique device ID for the sensor that does not change between power cycles
-float32[3] mag_bias		# magnetometer in-run bias in body frame (Gauss)
+float32[3] mag_bias             # magnetometer in-run bias in body frame (Gauss)
+float32 mag_bias_limit          # magnitude of maximum magnetometer in-run bias in body frame (Gauss)
 float32[3] mag_bias_variance
 bool mag_bias_valid

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp

@@ -232,7 +232,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 			failed = true;
 		}
 
-		if (!ekf2CheckStates(mavlink_log_pub, report_failures && report_ekf_fail)) {
+		if (!ekf2CheckSensorBias(mavlink_log_pub, report_failures && report_ekf_fail)) {
 			failed = true;
 		}
 	}

src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp

@@ -111,7 +111,7 @@ private:
 	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);
 
-	static bool ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail);
+	static bool ekf2CheckSensorBias(orb_advert_t *mavlink_log_pub, const bool report_fail);
 
 	static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
 					 const bool prearm);

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -39,9 +39,11 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/estimator_selector_status.h>
-#include <uORB/topics/estimator_states.h>
+#include <uORB/topics/estimator_sensor_bias.h>
 #include <uORB/topics/estimator_status.h>
 
+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)
 {
@@ -69,7 +71,6 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	// Get estimator status data if available and exit with a fail recorded if not
 	uORB::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status), estimator_selector_status_sub.get().primary_instance};
-	status_sub.update();
 	const estimator_status_s &status = status_sub.get();
 
 	if (status.timestamp == 0) {
@@ -231,52 +232,55 @@ out:
 	return success;
 }
 
-bool PreFlightCheck::ekf2CheckStates(orb_advert_t *mavlink_log_pub, const bool report_fail)
+bool PreFlightCheck::ekf2CheckSensorBias(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::SubscriptionData<estimator_selector_status_s> estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
-	uORB::Subscription states_sub{ORB_ID(estimator_states), estimator_selector_status_sub.get().primary_instance};
+	uORB::SubscriptionData<estimator_sensor_bias_s> estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias), estimator_selector_status_sub.get().primary_instance};
+	const estimator_sensor_bias_s &bias = estimator_sensor_bias_sub.get();
 
-	estimator_states_s states;
+	if (hrt_elapsed_time(&bias.timestamp) < 30_s) {
 
-	if (states_sub.copy(&states)) {
+		// check accelerometer bias estimates
+		if (bias.accel_bias_valid) {
+			const float ekf_ab_test_limit = 0.5f * bias.accel_bias_limit;
 
-		// check accelerometer delta velocity bias estimates
+			for (uint8_t axis_index = 0; axis_index < 3; axis_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
-			// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+				// adjust test threshold by 3-sigma
-			// adjust test threshold by 3-sigma
+				const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.accel_bias_variance[axis_index], 0.0f));
-			float test_uncertainty = 3.0f * sqrtf(fmaxf(states.covariances[index], 0.0f));
 
-			if (fabsf(states.states[index]) > ekf_ab_test_limit + test_uncertainty) {
+				if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
+					if (report_fail) {
+						PX4_ERR("accel bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
+							(double)bias.accel_bias[axis_index], (double)ekf_ab_test_limit, (double)test_uncertainty);
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
+					}
 
-				if (report_fail) {
+					return false;
-					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");
 				}
-
-				return false;
 			}
 		}
 
-		// check gyro delta angle bias estimates
+		// check gyro bias estimates
+		if (bias.gyro_bias_valid) {
+			const float ekf_gb_test_limit = 0.5f * bias.gyro_bias_limit;
 
+			for (uint8_t axis_index = 0; axis_index < 3; axis_index++) {
+				// allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
+				// adjust test threshold by 3-sigma
+				const float test_uncertainty = 3.0f * sqrtf(fmaxf(bias.gyro_bias_variance[axis_index], 0.0f));
 
-		if (fabsf(states.states[10]) > ekf_gb_test_limit
+				if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
-		    || fabsf(states.states[11]) > ekf_gb_test_limit
+					if (report_fail) {
-		    || fabsf(states.states[12]) > ekf_gb_test_limit) {
+						PX4_ERR("gyro bias (axis %d): |%.8f| > %.8f + %.8f", axis_index,
+							(double)bias.gyro_bias[axis_index], (double)ekf_gb_test_limit, (double)test_uncertainty);
+						mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
+					}
 
-			if (report_fail) {
+					return false;
-				mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
+				}
 			}
-
-			return false;
 		}
 	}
 

src/modules/commander/commander_params.c

@@ -556,31 +556,6 @@ PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f);
  */
 PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
 
-/**
- * Maximum value of EKF accelerometer delta velocity bias estimate that will allow arming.
- * Note: ekf2 will limit the delta velocity bias estimate magnitude to be less than EKF2_ABL_LIM * FILTER_UPDATE_PERIOD_MS * 0.001 so this parameter must be less than that to be useful.
- *
- * @group Commander
- * @unit m/s
- * @min 0.001
- * @max 0.01
- * @decimal 4
- * @increment 0.0001
- */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_AB, 0.0022f);
-
-/**
- * Maximum value of EKF gyro delta angle bias estimate that will allow arming
- *
- * @group Commander
- * @unit rad
- * @min 0.0001
- * @max 0.0017
- * @decimal 4
- * @increment 0.0001
- */
-PARAM_DEFINE_FLOAT(COM_ARM_EKF_GB, 0.0011f);
-
 /**
  * Maximum accelerometer inconsistency between IMU units that will allow arming
  *

src/modules/ekf2/EKF2.cpp

@@ -884,6 +884,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 		if (_device_id_gyro != 0) {
 			bias.gyro_device_id = _device_id_gyro;
 			gyro_bias.copyTo(bias.gyro_bias);
+			bias.gyro_bias_limit = math::radians(20.f); // 20 degrees/s see Ekf::constrainStates()
 			_ekf.getGyroBiasVariance().copyTo(bias.gyro_bias_variance);
 			bias.gyro_bias_valid = true;
 
@@ -893,6 +894,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 		if ((_device_id_accel != 0) && !(_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS)) {
 			bias.accel_device_id = _device_id_accel;
 			accel_bias.copyTo(bias.accel_bias);
+			bias.accel_bias_limit = _params->acc_bias_lim;
 			_ekf.getAccelBiasVariance().copyTo(bias.accel_bias_variance);
 			bias.accel_bias_valid = !_ekf.fault_status_flags().bad_acc_bias;
 
@@ -902,6 +904,7 @@ void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
 		if (_device_id_mag != 0) {
 			bias.mag_device_id = _device_id_mag;
 			mag_bias.copyTo(bias.mag_bias);
+			bias.mag_bias_limit = 0.5f; // 0.5 Gauss see Ekf::constrainStates()
 			_mag_cal_last_bias_variance.copyTo(bias.mag_bias_variance);
 			bias.mag_bias_valid = _mag_cal_available;