From 861634534687b5d43752e6ad82eefe60c071fdf3 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 10 Nov 2020 10:07:50 -0500
Subject: [PATCH] ekf2: move estimator_sensor_bias publication to method

 - only publish if there's a change in bias
 - publish current estimated mag bias, including saved mag bias
---
 src/modules/ekf2/EKF2.cpp | 100 ++++++++++++++++++++++++++------------
 src/modules/ekf2/EKF2.hpp |   5 ++
 2 files changed, 73 insertions(+), 32 deletions(-)

diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 2f51e17bee..c35192389c 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -891,38 +891,6 @@ void EKF2::Run()
 			status.timestamp = _replay_mode ? now : hrt_absolute_time();
 			_estimator_status_pub.publish(status);
 
-			// estimator_sensor_bias
-			if (status.filter_fault_flags == 0) {
-				// publish all corrected sensor readings and bias estimates after mag calibration is updated above
-				estimator_sensor_bias_s bias;
-				bias.timestamp_sample = imu_sample_new.time_us;
-
-				// take device ids from sensor_selection_s if not using specific vehicle_imu_s
-				bias.gyro_device_id = _device_id_gyro;
-				bias.accel_device_id = _device_id_accel;
-				bias.mag_device_id = _device_id_mag;
-
-				_ekf.getGyroBias().copyTo(bias.gyro_bias);
-				_ekf.getAccelBias().copyTo(bias.accel_bias);
-
-				bias.mag_bias[0] = _last_valid_mag_cal[0];
-				bias.mag_bias[1] = _last_valid_mag_cal[1];
-				bias.mag_bias[2] = _last_valid_mag_cal[2];
-
-				bias.gyro_bias_variance[0] = states.covariances[10];
-				bias.gyro_bias_variance[1] = states.covariances[11];
-				bias.gyro_bias_variance[2] = states.covariances[12];
-				bias.accel_bias_variance[0] = states.covariances[13];
-				bias.accel_bias_variance[1] = states.covariances[14];
-				bias.accel_bias_variance[2] = states.covariances[15];
-				bias.mag_bias_variance[0] = states.covariances[19];
-				bias.mag_bias_variance[1] = states.covariances[20];
-				bias.mag_bias_variance[2] = states.covariances[21];
-
-				bias.timestamp = _replay_mode ? now : hrt_absolute_time();
-				_estimator_sensor_bias_pub.publish(bias);
-			}
-
 			{
 				/* Check and save learned magnetometer bias estimates */
 
@@ -992,6 +960,7 @@ void EKF2::Run()
 				}
 			}
 
+			PublishSensorBias(now);
 			PublishWindEstimate(now);
 			PublishYawEstimatorStatus(now);
 
@@ -1312,6 +1281,73 @@ void EKF2::PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu)
 	_odometry_pub.publish(odom);
 }
 
+void EKF2::PublishSensorBias(const hrt_abstime &timestamp)
+{
+	// estimator_sensor_bias
+	estimator_sensor_bias_s bias{};
+	bias.timestamp_sample = timestamp;
+
+	const Vector3f gyro_bias{_ekf.getGyroBias()};
+	const Vector3f accel_bias{_ekf.getAccelBias()};
+
+	float states[24];
+	_ekf.getStateAtFusionHorizonAsVector().copyTo(states);
+	const Vector3f mag_bias {
+		states[19] + _param_ekf2_magbias_x.get(),
+		states[20] + _param_ekf2_magbias_y.get(),
+		states[21] + _param_ekf2_magbias_z.get(),
+	};
+
+	// only publish on change
+	if ((gyro_bias - _last_gyro_bias).longerThan(0.001f)
+	    || (accel_bias - _last_accel_bias).longerThan(0.001f)
+	    || (mag_bias - _last_mag_bias).longerThan(0.001f)) {
+
+		float covariances[24];
+		_ekf.covariances_diagonal().copyTo(covariances);
+
+		// take device ids from sensor_selection_s if not using specific vehicle_imu_s
+		if (_device_id_gyro != 0) {
+			bias.gyro_device_id = _device_id_gyro;
+
+			gyro_bias.copyTo(bias.gyro_bias);
+
+			bias.gyro_bias_variance[0] = covariances[10];
+			bias.gyro_bias_variance[1] = covariances[11];
+			bias.gyro_bias_variance[2] = covariances[12];
+
+			_last_gyro_bias = gyro_bias;
+		}
+
+		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_variance[0] = covariances[13];
+			bias.accel_bias_variance[1] = covariances[14];
+			bias.accel_bias_variance[2] = covariances[15];
+
+			_last_accel_bias = accel_bias;
+		}
+
+		if (_device_id_mag != 0) {
+			bias.mag_device_id = _device_id_mag;
+
+			mag_bias.copyTo(bias.mag_bias);
+
+			bias.mag_bias_variance[0] = covariances[19];
+			bias.mag_bias_variance[1] = covariances[20];
+			bias.mag_bias_variance[2] = covariances[21];
+
+			_last_mag_bias = mag_bias;
+		}
+
+		bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+		_estimator_sensor_bias_pub.publish(bias);
+	}
+}
+
 void EKF2::PublishYawEstimatorStatus(const hrt_abstime &timestamp)
 {
 	static_assert(sizeof(yaw_estimator_status_s::yaw) / sizeof(float) == N_MODELS_EKFGSF,
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index c1b7037387..96b7e2b800 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -138,6 +138,7 @@ private:
 	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishOpticalFlowVel(const hrt_abstime &timestamp, const optical_flow_s &optical_flow);
 	void PublishOdometry(const hrt_abstime &timestamp, const imuSample &imu);
+	void PublishSensorBias(const hrt_abstime &timestamp);
 	void PublishWindEstimate(const hrt_abstime &timestamp);
 	void PublishYawEstimatorStatus(const hrt_abstime &timestamp);
 
@@ -191,6 +192,10 @@ private:
 
 	Vector3f _last_local_position_for_gpos{};
 
+	Vector3f _last_accel_bias{};
+	Vector3f _last_gyro_bias{};
+	Vector3f _last_mag_bias{};
+
 	uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
 	uORB::Subscription _ev_odom_sub{ORB_ID(vehicle_visual_odometry)};