yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

select single system-wide wind estimate message (current best)

Browse Source 
- 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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-02-20 19:15:01 +01:00
committed by GitHub
parent 9d65e9a980
commit 0ea8104344
17 changed files with 119 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
src/modules/ekf2/EKF2.cpp
View File

@@ -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 &timestamp)
{
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);
}
}