ekf2: baro bias publish minor cleanup

- naming consistency (estimator prefix as "namespace")
 - only publish if baro is available and bias is changing as a small logging optimization
 - avoid unnecessary copying (get const reference to status directly)
 - trivial code style fixes

.ci/Jenkinsfile-hardware

@@ -902,6 +902,7 @@ void printTopics() {
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_event_flags"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_global_position"'
   sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_innovation_test_ratios"'

msg/CMakeLists.txt

@@ -44,7 +44,6 @@ set(msg_files
 	airspeed.msg
 	airspeed_validated.msg
 	airspeed_wind.msg
-	baro_bias_estimate.msg
 	battery_status.msg
 	camera_capture.msg
 	camera_status.msg
@@ -59,9 +58,9 @@ set(msg_files
 	distance_sensor.msg
 	ekf2_timestamps.msg
 	ekf_gps_drift.msg
-	event.msg
 	esc_report.msg
 	esc_status.msg
+	estimator_baro_bias.msg
 	estimator_event_flags.msg
 	estimator_innovations.msg
 	estimator_optical_flow_vel.msg
@@ -70,6 +69,7 @@ set(msg_files
 	estimator_states.msg
 	estimator_status.msg
 	estimator_status_flags.msg
+	event.msg
 	follow_target.msg
 	generator_status.msg
 	geofence_result.msg

msg/tools/uorb_rtps_message_ids.yaml

@@ -343,7 +343,7 @@ rtps:
     id: 159
   - msg: event
     id: 160
-  - msg: baro_bias_estimate
+  - msg: estimator_baro_bias
     id: 161
   - msg: internal_combustion_engine_status
     id: 162

src/modules/ekf2/EKF/ekf.h

@@ -298,7 +298,8 @@ public:
 	// returns false when data is not available
 	bool getDataEKFGSF(float *yaw_composite, float *yaw_variance, float yaw[N_MODELS_EKFGSF],
 			   float innov_VN[N_MODELS_EKFGSF], float innov_VE[N_MODELS_EKFGSF], float weight[N_MODELS_EKFGSF]);
-	void getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio);
+
+	const BaroBiasEstimator::status &getBaroBiasEstimatorStatus() const { return _baro_b_est.getStatus(); }
 
 private:
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -1320,9 +1320,8 @@ void Ekf::updateBaroHgtOffset()
 
 		// apply a 10 second first order low pass filter to baro offset
 		const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias();
-;
+
-		const float offset_rate_correction =  0.1f * (unbiased_baro + _state.pos(2) -
+		const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset);
-								_baro_hgt_offset);
 		_baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f);
 	}
 }
@@ -1348,7 +1347,7 @@ void Ekf::updateBaroHgtBias()
 	}
 
 	if (_gps_data_ready && !_gps_hgt_intermittent
-	    && _gps_checks_passed &&_NED_origin_initialised
+	    && _gps_checks_passed && _NED_origin_initialised
 	    && !_baro_hgt_faulty) {
 		// Use GPS altitude as a reference to compute the baro bias measurement
 		const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset)
@@ -1358,16 +1357,6 @@ void Ekf::updateBaroHgtBias()
 	}
 }
 
-void Ekf::getBaroBiasEstimatorStatus(float &bias, float &bias_var, float &innov, float &innov_var, float &innov_test_ratio)
-{
-	BaroBiasEstimator::status status = _baro_b_est.getStatus();
-	bias = status.bias;
-	bias_var = status.bias_var;
-	innov = status.innov;
-	innov_var = status.innov_var;
-	innov_test_ratio = status.innov_test_ratio;
-}
-
 Vector3f Ekf::getVisionVelocityInEkfFrame() const
 {
 	Vector3f vel;

src/modules/ekf2/EKF2.cpp

@@ -192,6 +192,7 @@ bool EKF2::multi_init(int imu, int mag)
 
 	_ekf2_timestamps_pub.advertise();
 	_ekf_gps_drift_pub.advertise();
+	_estimator_baro_bias_pub.advertise();
 	_estimator_innovation_test_ratios_pub.advertise();
 	_estimator_innovation_variances_pub.advertise();
 	_estimator_innovations_pub.advertise();
@@ -202,7 +203,6 @@ 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);
 
@@ -497,6 +497,7 @@ void EKF2::Run()
 			PublishWindEstimate(now);
 
 			// publish status/logging messages
+			PublishBaroBias(now);
 			PublishEkfDriftMetrics(now);
 			PublishEventFlags(now);
 			PublishStates(now);
@@ -506,7 +507,6 @@ void EKF2::Run()
 			PublishInnovationTestRatios(now);
 			PublishInnovationVariances(now);
 			PublishYawEstimatorStatus(now);
-			PublishBaroBiasEstimate(now);
 
 			UpdateMagCalibration(now);
 
@@ -550,19 +550,26 @@ void EKF2::PublishAttitude(const hrt_abstime &timestamp)
 	}
 }
 
-void EKF2::PublishBaroBiasEstimate(const hrt_abstime &timestamp)
+void EKF2::PublishBaroBias(const hrt_abstime &timestamp)
 {
+	if (_device_id_baro != 0) {
+		const BaroBiasEstimator::status &status = _ekf.getBaroBiasEstimatorStatus();
 
-	baro_bias_estimate_s bbe{};
+		if (fabsf(status.bias - _last_baro_bias_published) > 0.001f) {
-	bbe.timestamp = timestamp;
+			estimator_baro_bias_s baro_bias{};
-	bbe.timestamp_sample = timestamp;
+			baro_bias.timestamp_sample = timestamp;
-	bbe.baro_device_id = _device_id_baro;
+			baro_bias.baro_device_id = _device_id_baro;
-	_ekf.getBaroBiasEstimatorStatus(bbe.bias,
+			baro_bias.bias = status.bias;
-					bbe.bias_var,
+			baro_bias.bias_var = status.bias_var;
-					bbe.innov,
+			baro_bias.innov = status.innov;
-					bbe.innov_var,
+			baro_bias.innov_var = status.innov_var;
-					bbe.innov_test_ratio);
+			baro_bias.innov_test_ratio = status.innov_test_ratio;
-	_baro_bias_estimate_pub.publish(bbe);
+			baro_bias.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
+			_estimator_baro_bias_pub.publish(baro_bias);
+
+			_last_baro_bias_published = status.bias;
+		}
+	}
 }
 
 void EKF2::PublishEkfDriftMetrics(const hrt_abstime &timestamp)

src/modules/ekf2/EKF2.hpp

@@ -41,13 +41,15 @@
 #ifndef EKF2_HPP
 #define EKF2_HPP
 
+#include "EKF/ekf.h"
+#include "Utility/PreFlightChecker.hpp"
+
 #include "EKF2Selector.hpp"
 
 #include <float.h>
 
 #include <containers/LockGuard.hpp>
 #include <drivers/drv_hrt.h>
-#include "EKF/ekf.h"
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <px4_platform_common/defines.h>
@@ -62,10 +64,10 @@
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/topics/airspeed.h>
-#include <uORB/topics/baro_bias_estimate.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/ekf2_timestamps.h>
 #include <uORB/topics/ekf_gps_drift.h>
+#include <uORB/topics/estimator_baro_bias.h>
 #include <uORB/topics/estimator_event_flags.h>
 #include <uORB/topics/estimator_innovations.h>
 #include <uORB/topics/estimator_optical_flow_vel.h>
@@ -92,7 +94,6 @@
 #include <uORB/topics/wind.h>
 #include <uORB/topics/yaw_estimator_status.h>
 
-#include "Utility/PreFlightChecker.hpp"
 
 extern pthread_mutex_t ekf2_module_mutex;
 
@@ -130,7 +131,7 @@ private:
 	void Run() override;
 
 	void PublishAttitude(const hrt_abstime &timestamp);
-	void PublishBaroBiasEstimate(const hrt_abstime &timestamp);
+	void PublishBaroBias(const hrt_abstime &timestamp);
 	void PublishEkfDriftMetrics(const hrt_abstime &timestamp);
 	void PublishEventFlags(const hrt_abstime &timestamp);
 	void PublishGlobalPosition(const hrt_abstime &timestamp);
@@ -218,6 +219,7 @@ private:
 	Vector3f _last_accel_bias_published{};
 	Vector3f _last_gyro_bias_published{};
 	Vector3f _last_mag_bias_published{};
+	float _last_baro_bias_published{};
 
 	float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
 
@@ -258,9 +260,9 @@ private:
 	uint32_t _filter_warning_event_changes{0};
 	uint32_t _filter_information_event_changes{0};
 
-	uORB::PublicationMulti<baro_bias_estimate_s>         _baro_bias_estimate_pub{ORB_ID(baro_bias_estimate)};
 	uORB::PublicationMulti<ekf2_timestamps_s>            _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
 	uORB::PublicationMulti<ekf_gps_drift_s>              _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
+	uORB::PublicationMulti<estimator_baro_bias_s>        _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
 	uORB::PublicationMulti<estimator_innovations_s>      _estimator_innovations_pub{ORB_ID(estimator_innovations)};

src/modules/logger/logged_topics.cpp

@@ -138,7 +138,6 @@ void LoggedTopics::add_default_topics()
 #else
 	static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
 	add_topic("estimator_selector_status");
-	add_topic_multi("baro_bias_estimate", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);
@@ -146,6 +145,7 @@ void LoggedTopics::add_default_topics()
 #endif
 
 	add_topic_multi("ekf_gps_drift", 1000, MAX_ESTIMATOR_INSTANCES);
+	add_topic_multi("estimator_baro_bias", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_event_flags", 0, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_innovation_test_ratios", 500, MAX_ESTIMATOR_INSTANCES);
 	add_topic_multi("estimator_innovation_variances", 500, MAX_ESTIMATOR_INSTANCES);