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

Log height innovation sensor specific

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl

@ -1 +1 @@
Subproject commit 6b5f011bc224a45fd57d33e90cc7a136aa075b6c
Subproject commit fd1ecb7fedb8d06574ba7936e9f1bb6da33d98e8

5
src/modules/ekf2/Utility/PreFlightChecker.cpp
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);
}

6
src/modules/ekf2/ekf2_main.cpp
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);