forked from Archive/PX4-Autopilot
wind_estimate message: add identifier for source
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
parent
bc1c8fb73a
commit
59564af860
|
@ -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)
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue