From 0ea810434442543d70fcc5fcc12ff864cbf37628 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Sat, 20 Feb 2021 19:15:01 +0100 Subject: [PATCH] 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 --- msg/CMakeLists.txt | 9 ++++--- msg/{wind_estimate.msg => airspeed_wind.msg} | 9 +++---- msg/tools/uorb_rtps_message_ids.yaml | 7 ++++- msg/wind.msg | 16 +++++++++++ .../airspeed_validator/AirspeedValidator.cpp | 4 +-- .../airspeed_validator/AirspeedValidator.hpp | 4 +-- .../airspeed_selector_main.cpp | 16 +++++------ src/modules/ekf2/EKF2.cpp | 27 +++++++++---------- src/modules/ekf2/EKF2.hpp | 5 ++-- src/modules/ekf2/EKF2Selector.cpp | 25 +++++++++++++++++ src/modules/ekf2/EKF2Selector.hpp | 8 ++++++ src/modules/logger/logged_topics.cpp | 4 ++- src/modules/mavlink/streams/ALTITUDE.hpp | 2 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 16 +++++------ src/modules/mavlink/streams/WIND_COV.hpp | 18 ++++++------- src/modules/navigator/rtl.cpp | 8 +++--- src/modules/navigator/rtl.h | 4 +-- 17 files changed, 119 insertions(+), 63 deletions(-) rename msg/{wind_estimate.msg => airspeed_wind.msg} (74%) create mode 100644 msg/wind.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index bf80aa6fbf..388b8ea70e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -41,6 +41,7 @@ set(msg_files adc_report.msg airspeed.msg airspeed_validated.msg + airspeed_wind.msg battery_status.msg camera_capture.msg camera_trigger.msg @@ -66,12 +67,12 @@ set(msg_files follow_target.msg generator_status.msg geofence_result.msg - gimbal_device_set_attitude.msg - gimbal_manager_set_attitude.msg - gimbal_manager_set_manual_control.msg gimbal_device_attitude_status.msg gimbal_device_information.msg + gimbal_device_set_attitude.msg gimbal_manager_information.msg + gimbal_manager_set_attitude.msg + gimbal_manager_set_manual_control.msg gimbal_manager_status.msg gps_dump.msg gps_inject_data.msg @@ -181,7 +182,7 @@ set(msg_files vehicle_trajectory_waypoint.msg vtol_vehicle_status.msg wheel_encoders.msg - wind_estimate.msg + wind.msg yaw_estimator_status.msg ) diff --git a/msg/wind_estimate.msg b/msg/airspeed_wind.msg similarity index 74% rename from msg/wind_estimate.msg rename to msg/airspeed_wind.msg index e92de9f75f..03033bbbdd 100644 --- a/msg/wind_estimate.msg +++ b/msg/airspeed_wind.msg @@ -16,8 +16,7 @@ float32 beta_innov_var # Sideslip measurement innovation variance uint8 source # source of wind estimate -uint8 SOURCE_EKF = 0 # wind estimate source is the EKF -uint8 SOURCE_AS_BETA_ONLY = 1 # wind estimate from airspeed selector, only based on synthetic sideslip fusion -uint8 SOURCE_AS_SENSOR_1 = 2 # combined synthetic sideslip and airspeed fusion (data from first airspeed sensor) -uint8 SOURCE_AS_SENSOR_2 = 3 # combined synthetic sideslip and airspeed fusion (data from second airspeed sensor) -uint8 SOURCE_AS_SENSOR_3 = 4 # combined synthetic sideslip and airspeed fusion (data from third airspeed sensor) +uint8 SOURCE_AS_BETA_ONLY = 0 # wind estimate only based on synthetic sideslip fusion +uint8 SOURCE_AS_SENSOR_1 = 1 # combined synthetic sideslip and airspeed fusion (data from first airspeed sensor) +uint8 SOURCE_AS_SENSOR_2 = 2 # combined synthetic sideslip and airspeed fusion (data from second airspeed sensor) +uint8 SOURCE_AS_SENSOR_3 = 3 # combined synthetic sideslip and airspeed fusion (data from third airspeed sensor) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 2fc8682e85..94655e8b2b 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -225,7 +225,7 @@ rtps: receive: true - msg: vtol_vehicle_status id: 105 - - msg: wind_estimate + - msg: wind id: 106 - msg: collision_constraints id: 107 @@ -329,6 +329,8 @@ rtps: id: 156 - msg: gimbal_manager_set_manual_control id: 157 + - msg: airspeed_wind + id: 158 ########## multi topics: begin ########## - msg: actuator_controls_0 id: 170 @@ -431,4 +433,7 @@ rtps: - msg: actuator_controls_5 id: 202 alias: actuator_controls + - msg: estimator_wind + id: 203 + alias: wind ########## multi topics: end ########## diff --git a/msg/wind.msg b/msg/wind.msg new file mode 100644 index 0000000000..ff8b6f4535 --- /dev/null +++ b/msg/wind.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated + +float32 tas_innov # True airspeed innovation +float32 tas_innov_var # True airspeed innovation variance + +float32 beta_innov # Sideslip measurement innovation +float32 beta_innov_var # Sideslip measurement innovation variance + +# TOPICS wind estimator_wind diff --git a/src/lib/airspeed_validator/AirspeedValidator.cpp b/src/lib/airspeed_validator/AirspeedValidator.cpp index 75f80e9fbe..7b26acf6af 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.cpp +++ b/src/lib/airspeed_validator/AirspeedValidator.cpp @@ -86,10 +86,10 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air } // this function returns the current states of the wind estimator to be published in the airspeed module -wind_estimate_s +airspeed_wind_s AirspeedValidator::get_wind_estimator_states(uint64_t timestamp) { - wind_estimate_s wind_est = {}; + airspeed_wind_s wind_est = {}; wind_est.timestamp = timestamp; float wind[2]; diff --git a/src/lib/airspeed_validator/AirspeedValidator.hpp b/src/lib/airspeed_validator/AirspeedValidator.hpp index 5a02d3bd1a..8e1bc5e4d5 100644 --- a/src/lib/airspeed_validator/AirspeedValidator.hpp +++ b/src/lib/airspeed_validator/AirspeedValidator.hpp @@ -40,7 +40,7 @@ #include #include -#include +#include using matrix::Dcmf; @@ -86,7 +86,7 @@ public: bool get_airspeed_valid() { return _airspeed_valid; } float get_CAS_scale() {return _CAS_scale;} - wind_estimate_s get_wind_estimator_states(uint64_t timestamp); + airspeed_wind_s get_wind_estimator_states(uint64_t timestamp); // setters wind estimator parameters void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); } diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 981fce9033..daf3aa0126 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -60,7 +60,7 @@ #include #include #include -#include +#include using namespace time_literals; @@ -103,7 +103,7 @@ private: }; 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) */ + uORB::PublicationMulti _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}, {ORB_ID(airspeed_wind)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */ orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/ uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -129,7 +129,7 @@ private: vtol_vehicle_status_s _vtol_vehicle_status {}; WindEstimator _wind_estimator_sideslip; /**< wind estimator instance only fusing sideslip */ - wind_estimate_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ + airspeed_wind_s _wind_estimate_sideslip {}; /**< wind estimate message for wind estimator instance only fusing sideslip */ int32_t _number_of_airspeed_sensors{0}; /**< number of airspeed sensors in use (detected during initialization)*/ int32_t _prev_number_of_airspeed_sensors{0}; /**< number of airspeed sensors in previous loop (to detect a new added sensor)*/ @@ -489,7 +489,7 @@ void AirspeedModule::update_wind_estimator_sideslip() _wind_estimate_sideslip.beta_innov = _wind_estimator_sideslip.get_beta_innov(); _wind_estimate_sideslip.beta_innov_var = _wind_estimator_sideslip.get_beta_innov_var(); _wind_estimate_sideslip.tas_scale = _wind_estimator_sideslip.get_tas_scale(); - _wind_estimate_sideslip.source = wind_estimate_s::SOURCE_AS_BETA_ONLY; + _wind_estimate_sideslip.source = airspeed_wind_s::SOURCE_AS_BETA_ONLY; } void AirspeedModule::update_ground_minus_wind_airspeed() @@ -602,16 +602,16 @@ void AirspeedModule::select_airspeed_and_publish() // 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(_time_now_usec); + airspeed_wind_s wind_est = _airspeed_validator[i].get_wind_estimator_states(_time_now_usec); if (i == 0) { - wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_1; + wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_1; } else if (i == 1) { - wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_2; + wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_2; } else { - wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_3; + wind_est.source = airspeed_wind_s::SOURCE_AS_SENSOR_3; } _wind_est_pub[i + 1].publish(wind_est); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index f9e106b5af..2cfdc6f689 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/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 ×tamp) { 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); } } diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 42a88e4590..1952630a09 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -85,7 +85,7 @@ #include #include #include -#include +#include #include #include "Utility/PreFlightChecker.hpp" @@ -254,13 +254,14 @@ private: uORB::PublicationMulti _estimator_status_flags_pub{ORB_ID(estimator_status_flags)}; uORB::PublicationMulti _estimator_visual_odometry_aligned_pub{ORB_ID(estimator_visual_odometry_aligned)}; uORB::PublicationMulti _yaw_est_pub{ORB_ID(yaw_estimator_status)}; - uORB::PublicationMulti _wind_pub{ORB_ID(wind_estimate)}; // publications with topic dependent on multi-mode uORB::PublicationMulti _attitude_pub; uORB::PublicationMulti _local_position_pub; uORB::PublicationMulti _global_position_pub; uORB::PublicationMulti _odometry_pub; + uORB::PublicationMulti _wind_pub; + PreFlightChecker _preflt_checker; diff --git a/src/modules/ekf2/EKF2Selector.cpp b/src/modules/ekf2/EKF2Selector.cpp index c79616bd0e..898dbc87f5 100644 --- a/src/modules/ekf2/EKF2Selector.cpp +++ b/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; diff --git a/src/modules/ekf2/EKF2Selector.hpp b/src/modules/ekf2/EKF2Selector.hpp index 38cce2859e..fc63297f36 100644 --- a/src/modules/ekf2/EKF2Selector.hpp +++ b/src/modules/ekf2/EKF2Selector.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #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_pub{ORB_ID(vehicle_global_position)}; uORB::Publication _vehicle_local_position_pub{ORB_ID(vehicle_local_position)}; uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _wind_pub{ORB_ID(wind)}; DEFINE_PARAMETERS( (ParamFloat) _param_ekf2_sel_err_red, diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index ac427f9828..29fe3e37ac 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -113,6 +113,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_status"); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); + add_topic("wind", 1000); // Control allocation topics add_topic("vehicle_angular_acceleration_setpoint", 20); @@ -123,6 +124,7 @@ void LoggedTopics::add_default_topics() // multi topics add_topic_multi("actuator_outputs", 100, 3); + add_topic_multi("airspeed_wind", 1000); add_topic_multi("logger_status", 0, 2); add_topic_multi("multirotor_motor_limits", 1000, 2); add_topic_multi("rate_ctrl_status", 200, 2); @@ -149,8 +151,8 @@ void LoggedTopics::add_default_topics() add_topic_multi("estimator_status", 200, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_status_flags", 0, MAX_ESTIMATOR_INSTANCES); add_topic_multi("estimator_visual_odometry_aligned", 200, MAX_ESTIMATOR_INSTANCES); + add_topic_multi("estimator_wind", 1000, MAX_ESTIMATOR_INSTANCES); add_topic_multi("yaw_estimator_status", 1000, MAX_ESTIMATOR_INSTANCES); - add_topic_multi("wind_estimate", 1000); // published by both ekf2 and airspeed_selector // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 300, 2); diff --git a/src/modules/mavlink/streams/ALTITUDE.hpp b/src/modules/mavlink/streams/ALTITUDE.hpp index 78aba36aa4..45eb67c9f9 100644 --- a/src/modules/mavlink/streams/ALTITUDE.hpp +++ b/src/modules/mavlink/streams/ALTITUDE.hpp @@ -35,7 +35,7 @@ #define ALTITUDE_HPP #include -#include +#include class MavlinkStreamAltitude : public MavlinkStream { diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index a1e913f54a..90e83cddb4 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -134,7 +134,7 @@ private: updated |= write_tecs_status(&msg); updated |= write_vehicle_status(&msg); updated |= write_vehicle_status_flags(&msg); - updated |= write_wind_estimate(&msg); + updated |= write_wind(&msg); if (updated) { msg.timestamp = t / 1000; @@ -477,9 +477,9 @@ private: return false; } - bool write_wind_estimate(mavlink_high_latency2_t *msg) + bool write_wind(mavlink_high_latency2_t *msg) { - wind_estimate_s wind; + wind_s wind; if (_wind_sub.update(&wind)) { msg->wind_heading = static_cast(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east, @@ -506,7 +506,7 @@ private: update_local_position(); update_gps(); update_vehicle_status(); - update_wind_estimate(); + update_wind(); } void update_airspeed() @@ -586,9 +586,9 @@ private: } } - void update_wind_estimate() + void update_wind() { - wind_estimate_s wind; + wind_s wind; if (_wind_sub.update(&wind)) { _windspeed.add_value(sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east), @@ -642,7 +642,7 @@ private: uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _status_flags_sub{ORB_ID(vehicle_status_flags)}; uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; - uORB::Subscription _wind_sub{ORB_ID(wind_estimate)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/mavlink/streams/WIND_COV.hpp b/src/modules/mavlink/streams/WIND_COV.hpp index ec816e183f..192af05073 100644 --- a/src/modules/mavlink/streams/WIND_COV.hpp +++ b/src/modules/mavlink/streams/WIND_COV.hpp @@ -35,7 +35,7 @@ #define WIND_COV_HPP #include -#include +#include class MavlinkStreamWindCov : public MavlinkStream { @@ -50,29 +50,29 @@ public: unsigned get_size() override { - return _wind_estimate_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; + return _wind_sub.advertised() ? MAVLINK_MSG_ID_WIND_COV_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0; } private: explicit MavlinkStreamWindCov(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::Subscription _wind_estimate_sub{ORB_ID(wind_estimate)}; + uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; bool send() override { - wind_estimate_s wind_estimate; + wind_s wind; - if (_wind_estimate_sub.update(&wind_estimate)) { + if (_wind_sub.update(&wind)) { mavlink_wind_cov_t msg{}; - msg.time_usec = wind_estimate.timestamp; + msg.time_usec = wind.timestamp; - msg.wind_x = wind_estimate.windspeed_north; - msg.wind_y = wind_estimate.windspeed_east; + msg.wind_x = wind.windspeed_north; + msg.wind_y = wind.windspeed_east; msg.wind_z = 0.0f; - msg.var_horiz = wind_estimate.variance_north + wind_estimate.variance_east; + msg.var_horiz = wind.variance_north + wind.variance_east; msg.var_vert = 0.0f; vehicle_local_position_s lpos{}; diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 62b339a627..286f12ca7b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -670,12 +670,12 @@ void RTL::get_rtl_xy_z_speed(float &xy, float &z) matrix::Vector2f RTL::get_wind() { - _wind_estimate_sub.update(); + _wind_sub.update(); matrix::Vector2f wind; - if (hrt_absolute_time() - _wind_estimate_sub.get().timestamp < 1_s) { - wind(0) = _wind_estimate_sub.get().windspeed_north; - wind(1) = _wind_estimate_sub.get().windspeed_east; + if (hrt_absolute_time() - _wind_sub.get().timestamp < 1_s) { + wind(0) = _wind_sub.get().windspeed_north; + wind(1) = _wind_sub.get().windspeed_east; } return wind; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 6b5cd4118d..3718ac724d 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #include @@ -169,7 +169,7 @@ private: param_t _param_rtl_xy_speed{PARAM_INVALID}; param_t _param_rtl_descent_speed{PARAM_INVALID}; - uORB::SubscriptionData _wind_estimate_sub{ORB_ID(wind_estimate)}; + uORB::SubscriptionData _wind_sub{ORB_ID(wind)}; uORB::Publication _rtl_flight_time_pub{ORB_ID(rtl_flight_time)}; };