diff --git a/msg/wind_estimate.msg b/msg/wind_estimate.msg index f6894d914c..e92de9f75f 100644 --- a/msg/wind_estimate.msg +++ b/msg/wind_estimate.msg @@ -13,3 +13,11 @@ float32 tas_scale # Estimated true airspeed scale factor float32 beta_innov # Sideslip measurement innovation 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) diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index cc7f8b9f2a..ea25062514 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -486,6 +486,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; } void AirspeedModule::update_ground_minus_wind_airspeed() @@ -593,6 +594,17 @@ 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); + + if (i == 0) { + wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_1; + + } else if (i == 1) { + wind_est.source = wind_estimate_s::SOURCE_AS_SENSOR_2; + + } else { + wind_est.source = wind_estimate_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 be1dfe6762..c26265ccbc 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1028,6 +1028,7 @@ void EKF2::PublishWindEstimate(const hrt_abstime ×tamp) 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_pub.publish(wind_estimate); }