From ff6577ce5f9d98697da56fc5cc3724e95dd7fac6 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 29 Aug 2019 15:48:46 +0200 Subject: [PATCH] EKF and AirspeedSelector: publish multiple wind estimate topic instances and log them all Signed-off-by: Silvan Fuhrer --- .../airspeed_selector_main.cpp | 23 ++++++------------- src/modules/ekf2/ekf2_main.cpp | 21 +++++++++-------- src/modules/logger/logger.cpp | 2 +- 3 files changed, 20 insertions(+), 26 deletions(-) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 274372ef6e..87adfef289 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -49,6 +49,8 @@ #include #include #include +#include +#include #include #include #include @@ -93,8 +95,8 @@ public: 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_pub {ORB_ID(airspeed_validated)}; /**< airspeed validated topic*/ + uORB::PublicationMulti _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 index c318dd8e42..2fe3228d26 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -283,7 +284,7 @@ private: uORB::Publication _sensor_bias_pub{ORB_ID(sensor_bias)}; uORB::Publication _att_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; - uORB::Publication _wind_pub{ORB_ID(wind_estimate)}; + uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; uORB::PublicationData _vehicle_global_position_pub{ORB_ID(vehicle_global_position)}; uORB::PublicationData _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 ×tamp) { if (_ekf.get_wind_status()) { - float velNE_wind[2]; - _ekf.get_wind_velocity(velNE_wind); - - float wind_var[2]; - _ekf.get_wind_velocity_var(wind_var); - - // Publish wind estimate + // Publish wind estimate only if ekf declares them valid wind_estimate_s wind_estimate{}; + float velNE_wind[2]; + float wind_var[2]; + _ekf.get_wind_velocity(velNE_wind); + _ekf.get_wind_velocity_var(wind_var); + _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 index 0047ba760d..6bfa28a32e 100644 --- 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