dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

AirspeedValidator: fuse true airspeed before sideslip (#742) 

- the states can be initialised more accurately using an airspeed measurement
and the wind estimator uses the first measurement to initialise

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst 2020-11-11 11:12:51 +03:00
parent d44e537084
commit 842d98d572
1 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/lib/airspeed_validator/AirspeedValidator.cpp
View File

@ -77,12 +77,14 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
Quatf q(att_q);
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q);
// airspeed fusion (with raw TAS)
const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)});
// sideslip fusion
_wind_estimator.fuse_beta(time_now_usec, vI, q);
}
}