fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:50:34 +08: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
src/modules/ekf2/EKF2Selector.cpp
+25
View File
@@ -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;