fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 19:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Log height innovation sensor specific

Browse Source
This commit is contained in:
kamilritz
2019-12-03 13:01:43 +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/lib/ecl
+1 -1
Submodule src/lib/ecl updated: 6b5f011bc2...fd1ecb7fed
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);
}
src/modules/ekf2/ekf2_main.cpp
+6
View File
@@ -1610,6 +1610,8 @@ void Ekf2::Run()
_ekf.getGpsVelPosInnov(&innovations.gps_hvel[0], innovations.gps_vvel, &innovations.gps_hpos[0],
innovations.gps_vpos);
_ekf.getEvVelPosInnov(&innovations.ev_hvel[0], innovations.ev_vvel, &innovations.ev_hpos[0], innovations.ev_vpos);
_ekf.getBaroHgtInnov(innovations.baro_vpos);
_ekf.getRngHgtInnov(innovations.rng_vpos);
_ekf.getAuxVelInnov(&innovations.aux_hvel[0]);
_ekf.getFlowInnov(&innovations.flow[0]);
_ekf.getHeadingInnov(innovations.heading);
@@ -1626,6 +1628,8 @@ void Ekf2::Run()
innovation_var.gps_vpos);
_ekf.getEvVelPosInnovVar(&innovation_var.ev_hvel[0], innovation_var.ev_vvel, &innovation_var.ev_hpos[0],
innovation_var.ev_vpos);
_ekf.getBaroHgtInnovVar(innovation_var.baro_vpos);
_ekf.getRngHgtInnovVar(innovation_var.rng_vpos);
_ekf.getAuxVelInnovVar(&innovation_var.aux_hvel[0]);
_ekf.getFlowInnovVar(&innovation_var.flow[0]);
_ekf.getHeadingInnovVar(innovation_var.heading);
@@ -1642,6 +1646,8 @@ void Ekf2::Run()
test_ratios.gps_vpos);
_ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0],
test_ratios.ev_vpos);
_ekf.getBaroHgtInnovRatio(test_ratios.baro_vpos);
_ekf.getRngHgtInnovRatio(test_ratios.rng_vpos);
_ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]);
_ekf.getFlowInnovRatio(test_ratios.flow[0]);
_ekf.getHeadingInnovRatio(test_ratios.heading);