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

wind_estimator fix true airspeed usage

Browse Source
This commit is contained in:
Daniel Agar
2019-05-06 15:25:33 -04:00
committed by GitHub
parent 317e8cda64
commit 3d22da9cbe
1 changed files with 5 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/wind_estimator/wind_estimator_main.cpp
View File

@@ -196,7 +196,6 @@ WindEstimatorModule::cycle()
bool lpos_valid = false;
bool att_valid = false;
bool airspeed_valid = false;
const hrt_abstime time_now_usec = hrt_absolute_time();
@@ -229,14 +228,12 @@ WindEstimatorModule::cycle()
airspeed_s airspeed = {};
if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) {
airspeed_valid = (time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0);
}
if ((time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0)) {
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}};
if (airspeed_valid) {
Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}};
// airspeed fusion
_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)});
// airspeed fusion
_wind_estimator.fuse_airspeed(time_now_usec, airspeed.true_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)});
}
}
// if we fused either airspeed or sideslip we publish a wind_estimate message