select single system-wide wind estimate message (current best)

- publish wind estimate only from EKF, and wind speeds from airspeed selector to new separate topic (airspeed_wind)
 - rename message wind_estimate to wind
 - publish wind from currently used ekf instance (ekf2selector)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/ekf2/EKF2.cpp

@@ -55,6 +55,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_local_position_pub(multi_mode ? ORB_ID(estimator_local_position) : ORB_ID(vehicle_local_position)),
 	_global_position_pub(multi_mode ? ORB_ID(estimator_global_position) : ORB_ID(vehicle_global_position)),
 	_odometry_pub(multi_mode ? ORB_ID(estimator_odometry) : ORB_ID(vehicle_odometry)),
+	_wind_pub(multi_mode ? ORB_ID(estimator_wind) : ORB_ID(wind)),
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_min_obs_dt(_params->sensor_interval_min_ms),
 	_param_ekf2_mag_delay(_params->mag_delay_ms),
@@ -1112,25 +1113,23 @@ void EKF2::PublishWindEstimate(const hrt_abstime &timestamp)
 {
 	if (_ekf.get_wind_status()) {
 		// Publish wind estimate only if ekf declares them valid
-		wind_estimate_s wind_estimate{};
-		wind_estimate.timestamp_sample = timestamp;
+		wind_s wind{};
+		wind.timestamp_sample = timestamp;
 
 		const Vector2f wind_vel = _ekf.getWindVelocity();
 		const Vector2f wind_vel_var = _ekf.getWindVelocityVariance();
-		_ekf.getAirspeedInnov(wind_estimate.tas_innov);
-		_ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var);
-		_ekf.getBetaInnov(wind_estimate.beta_innov);
-		_ekf.getBetaInnovVar(wind_estimate.beta_innov_var);
+		_ekf.getAirspeedInnov(wind.tas_innov);
+		_ekf.getAirspeedInnovVar(wind.tas_innov_var);
+		_ekf.getBetaInnov(wind.beta_innov);
+		_ekf.getBetaInnovVar(wind.beta_innov_var);
 
-		wind_estimate.windspeed_north = wind_vel(0);
-		wind_estimate.windspeed_east = wind_vel(1);
-		wind_estimate.variance_north = wind_vel_var(0);
-		wind_estimate.variance_east = wind_vel_var(1);
-		wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
-		wind_estimate.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
-		wind_estimate.source = wind_estimate_s::SOURCE_EKF;
+		wind.windspeed_north = wind_vel(0);
+		wind.windspeed_east = wind_vel(1);
+		wind.variance_north = wind_vel_var(0);
+		wind.variance_east = wind_vel_var(1);
+		wind.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
 
-		_wind_pub.publish(wind_estimate);
+		_wind_pub.publish(wind);
 	}
 }
 

src/modules/ekf2/EKF2.hpp

@@ -85,7 +85,7 @@
 #include <uORB/topics/vehicle_magnetometer.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/wind.h>
 #include <uORB/topics/yaw_estimator_status.h>
 
 #include "Utility/PreFlightChecker.hpp"
@@ -254,13 +254,14 @@ private:
 	uORB::PublicationMulti<estimator_status_flags_s>     _estimator_status_flags_pub{ORB_ID(estimator_status_flags)};
 	uORB::PublicationMulti<vehicle_odometry_s>           _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)};
 	uORB::PublicationMulti<yaw_estimator_status_s>       _yaw_est_pub{ORB_ID(yaw_estimator_status)};
-	uORB::PublicationMulti<wind_estimate_s>              _wind_pub{ORB_ID(wind_estimate)};
 
 	// publications with topic dependent on multi-mode
 	uORB::PublicationMulti<vehicle_attitude_s>           _attitude_pub;
 	uORB::PublicationMulti<vehicle_local_position_s>     _local_position_pub;
 	uORB::PublicationMulti<vehicle_global_position_s>    _global_position_pub;
 	uORB::PublicationMulti<vehicle_odometry_s>           _odometry_pub;
+	uORB::PublicationMulti<wind_s>              _wind_pub;
+
 
 	PreFlightChecker _preflt_checker;
 

src/modules/ekf2/EKF2Selector.cpp

@@ -122,6 +122,7 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
 		PublishVehicleAttitude(true);
 		PublishVehicleLocalPosition(true);
 		PublishVehicleGlobalPosition(true);
+		PublishWindEstimate(true);
 
 		return true;
 	}
@@ -498,6 +499,25 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
 	}
 }
 
+void EKF2Selector::PublishWindEstimate(bool reset)
+{
+	wind_s wind;
+
+	if (_instance[_selected_instance].estimator_wind_sub.copy(&wind)) {
+		if (reset) {
+			// ensure monotonically increasing timestamp_sample through reset
+			wind.timestamp_sample = max(wind.timestamp_sample, _wind_last.timestamp_sample);
+		}
+
+		// save last primary wind
+		_wind_last = wind;
+
+		// republish with current timestamp
+		wind.timestamp = hrt_absolute_time();
+		_wind_pub.publish(wind);
+	}
+}
+
 void EKF2Selector::Run()
 {
 	// re-schedule as backup timeout
@@ -651,6 +671,11 @@ void EKF2Selector::Run()
 		PublishVehicleGlobalPosition();
 	}
 
+	// selected estimator_wind -> wind
+	if (_instance[_selected_instance].estimator_wind_sub.updated()) {
+		PublishWindEstimate();
+	}
+
 	// selected estimator_odometry -> vehicle_odometry
 	if (_instance[_selected_instance].estimator_odometry_sub.updated()) {
 		vehicle_odometry_s vehicle_odometry;

src/modules/ekf2/EKF2Selector.hpp

@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/wind.h>
 
 #if CONSTRAINED_MEMORY
 # define EKF2_MAX_INSTANCES 2
@@ -80,6 +81,7 @@ private:
 	void PublishVehicleAttitude(bool reset = false);
 	void PublishVehicleLocalPosition(bool reset = false);
 	void PublishVehicleGlobalPosition(bool reset = false);
+	void PublishWindEstimate(bool reset = false);
 	bool SelectInstance(uint8_t instance);
 
 	// Update the error scores for all available instances
@@ -94,6 +96,7 @@ private:
 			estimator_local_position_sub{ORB_ID(estimator_local_position), i},
 			estimator_global_position_sub{ORB_ID(estimator_global_position), i},
 			estimator_odometry_sub{ORB_ID(estimator_odometry), i},
+			estimator_wind_sub{ORB_ID(estimator_wind), i},
 			instance(i)
 		{}
 
@@ -103,6 +106,7 @@ private:
 		uORB::Subscription estimator_local_position_sub;
 		uORB::Subscription estimator_global_position_sub;
 		uORB::Subscription estimator_odometry_sub;
+		uORB::Subscription estimator_wind_sub;
 
 		uint64_t timestamp_sample_last{0};
 
@@ -195,6 +199,9 @@ private:
 	uint8_t _lat_lon_reset_counter{0};
 	uint8_t _alt_reset_counter{0};
 
+	// wind estimate
+	wind_s _wind_last{};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
 
@@ -205,6 +212,7 @@ private:
 	uORB::Publication<vehicle_global_position_s>   _vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
 	uORB::Publication<vehicle_local_position_s>    _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
 	uORB::Publication<vehicle_odometry_s>          _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
+	uORB::Publication<wind_s>             _wind_pub{ORB_ID(wind)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::EKF2_SEL_ERR_RED>) _param_ekf2_sel_err_red,