fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 23:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Log height innovation sensor specific

Browse Source
This commit is contained in:
kamilritz
2019-12-05 11:29:29 +01:00
committed by Julian Kent
parent 61b569245b
commit 5d0965e83a
3 changed files with 10 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/Utility/PreFlightChecker.cpp
+3 -2
View File
@@ -95,14 +95,15 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
{
const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel));
const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); // only temporary solution
const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim);
}
bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
{
const float hgt_innov = innov.gps_vpos; // HACK: height innovation independent of sensor is published on gps_vpos
const float hgt_innov = fmaxf(fabsf(innov.gps_vpos), fmaxf(fabs(innov.ev_vpos),
fabs(innov.rng_vpos))); // only temporary solution
const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim);
return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim);
}