EKF and AirspeedSelector: publish multiple wind estimate topic instan…

…ces and log them all Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
PX4 · Aug 30, 2019 · 098d71f · 098d71f
1 parent 43fdcd7
commit 098d71f
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 24 deletions.
diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp
@@ -49,6 +49,8 @@
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/mavlink_log.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/vehicle_acceleration.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -93,8 +95,8 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 private:
 	static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */
 
-	orb_advert_t	_airspeed_validated_pub {nullptr};			/**< airspeed validated topic*/
-	orb_advert_t 	_wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {};							/**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
+	uORB::Publication<airspeed_validated_s> _airspeed_validated_pub {ORB_ID(airspeed_validated)};			/**< airspeed validated topic*/
+	uORB::PublicationMulti<wind_estimate_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
 	orb_advert_t 	_mavlink_log_pub {nullptr}; 						/**< mavlink log topic*/
 
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
@@ -181,15 +183,6 @@ AirspeedModule::~AirspeedModule()
 {
 	ScheduleClear();
 
-	for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
-		if (_wind_est_pub[i] != nullptr) {
-			orb_unadvertise(_wind_est_pub[i]);
-		}
-	}
-
-	orb_unadvertise(_airspeed_validated_pub);
-
-
 	perf_free(_perf_elapsed);
 	perf_free(_perf_interval);
 
@@ -524,17 +517,15 @@ void AirspeedModule::select_airspeed_and_publish()
 	}
 
 	/* publish airspeed validated topic */
-	int instance;
-	orb_publish_auto(ORB_ID(airspeed_validated), &_airspeed_validated_pub, &airspeed_validated, &instance,
-			 ORB_PRIO_DEFAULT);
+	_airspeed_validated_pub.publish(airspeed_validated);
 
 	/* publish sideslip-only-fusion wind topic */
-	orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[0], &_wind_estimate_sideslip, &instance, ORB_PRIO_LOW);
+	_wind_est_pub[0].publish(_wind_estimate_sideslip);
 
 	/* publish the wind estimator states from all airspeed validators */
 	for (int i = 0; i < _number_of_airspeed_sensors; i++) {
 		wind_estimate_s wind_est = _airspeed_validator[i].get_wind_estimator_states(hrt_absolute_time());
-		orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub[i + 1], &wind_est, &instance, ORB_PRIO_LOW);
+		_wind_est_pub[i + 1].publish(wind_est);
 	}
 
 }

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
@@ -51,6 +51,7 @@
 #include <px4_tasks.h>
 #include <px4_time.h>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/distance_sensor.h>
@@ -283,7 +284,7 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 	uORB::Publication<sensor_bias_s>			_sensor_bias_pub{ORB_ID(sensor_bias)};
 	uORB::Publication<vehicle_attitude_s>			_att_pub{ORB_ID(vehicle_attitude)};
 	uORB::Publication<vehicle_odometry_s>			_vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
-	uORB::Publication<wind_estimate_s>			_wind_pub{ORB_ID(wind_estimate)};
+	uORB::PublicationMulti<wind_estimate_s>			_wind_pub{ORB_ID(wind_estimate)};
 	uORB::PublicationData<vehicle_global_position_s>	_vehicle_global_position_pub{ORB_ID(vehicle_global_position)};
 	uORB::PublicationData<vehicle_local_position_s>		_vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
 
@@ -1757,20 +1758,22 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime
 bool Ekf2::publish_wind_estimate(const hrt_abstime &timestamp)
 {
 	if (_ekf.get_wind_status()) {
+		// Publish wind estimate only if ekf declares them valid
+		wind_estimate_s wind_estimate{};
 		float velNE_wind[2];
-		_ekf.get_wind_velocity(velNE_wind);
-
 		float wind_var[2];
+		_ekf.get_wind_velocity(velNE_wind);
 		_ekf.get_wind_velocity_var(wind_var);
-
-		// Publish wind estimate
-		wind_estimate_s wind_estimate{};
+		_ekf.get_airspeed_innov(&wind_estimate.tas_innov);
+		_ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var);
+		_ekf.get_beta_innov(&wind_estimate.beta_innov);
+		_ekf.get_beta_innov_var(&wind_estimate.beta_innov_var);
 		wind_estimate.timestamp = timestamp;
 		wind_estimate.windspeed_north = velNE_wind[0];
 		wind_estimate.windspeed_east = velNE_wind[1];
 		wind_estimate.variance_north = wind_var[0];
 		wind_estimate.variance_east = wind_var[1];
-		wind_estimate.tas_scale = 1.0f; //fix to 1 as scale not estimated in ekf
+		wind_estimate.tas_scale = 0.0f; //leave at 0 as scale is not estimated in ekf
 
 		_wind_pub.publish(wind_estimate);
 

diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
@@ -549,13 +549,13 @@ void Logger::add_default_topics()
 	add_topic("vehicle_status", 200);
 	add_topic("vehicle_status_flags");
 	add_topic("vtol_vehicle_status", 200);
-	add_topic("wind_estimate", 200);
 
 	add_topic_multi("actuator_outputs", 100);
 	add_topic_multi("battery_status", 500);
 	add_topic_multi("distance_sensor", 100);
 	add_topic_multi("telemetry_status");
 	add_topic_multi("vehicle_gps_position");
+	add_topic_multi("wind_estimate", 200);
 
 #ifdef CONFIG_ARCH_BOARD_PX4_SITL