diff --git a/src/lib/ecl b/src/lib/ecl index 6b5f011bc2..fd1ecb7fed 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 6b5f011bc224a45fd57d33e90cc7a136aa075b6c +Subproject commit fd1ecb7fedb8d06574ba7936e9f1bb6da33d98e8 diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index 9d2aab6544..f4e8441180 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -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); } diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 70efd09908..e3533cdba7 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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);