EKF: add baro bias estimator using GNSS altitude

src/modules/ekf2/EKF2.cpp

@@ -202,6 +202,7 @@ bool EKF2::multi_init(int imu, int mag)
 	_estimator_status_flags_pub.advertise();
 	_estimator_visual_odometry_aligned_pub.advertised();
 	_yaw_est_pub.advertise();
+	_baro_bias_estimate_pub.advertise();
 
 	bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
 
@@ -505,6 +506,7 @@ void EKF2::Run()
 			PublishInnovationTestRatios(now);
 			PublishInnovationVariances(now);
 			PublishYawEstimatorStatus(now);
+			PublishBaroBiasEstimate(now);
 
 			UpdateMagCalibration(now);
 
@@ -548,6 +550,21 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 	}
 }
 
+void EKF2::PublishBaroBiasEstimate(const hrt_abstime &timestamp)
+{
+
+	baro_bias_estimate_s bbe{};
+	bbe.timestamp = timestamp;
+	bbe.timestamp_sample = timestamp;
+	bbe.baro_device_id = _device_id_baro;
+	_ekf.getBaroBiasEstimatorStatus(bbe.bias,
+					bbe.bias_var,
+					bbe.innov,
+					bbe.innov_var,
+					bbe.innov_test_ratio);
+	_baro_bias_estimate_pub.publish(bbe);
+}
+
 void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)
 {
 	// publish GPS drift data only when updated to minimise overhead