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

ekf2: add kconfig option to enable/disable range finder fusion (and terrain estimator)

Browse Source
This commit is contained in:
Daniel Agar
2023-03-21 11:25:34 -04:00
parent 0784901a66
commit 32a5bd32ad
17 changed files with 343 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/Utility/PreFlightChecker.cpp
+8 -2
View File
@@ -117,11 +117,15 @@ bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s
has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_is_using_rng_hgt_aiding) {
const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
#endif // CONFIG_EKF2_RANGE_FINDER
if (_is_using_ev_hgt_aiding) {
const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
@@ -150,7 +154,6 @@ void PreFlightChecker::reset()
_is_using_ev_vel_aiding = false;
_is_using_baro_hgt_aiding = false;
_is_using_gps_hgt_aiding = false;
_is_using_rng_hgt_aiding = false;
_is_using_ev_hgt_aiding = false;
_has_heading_failed = false;
_has_horiz_vel_failed = false;
@@ -161,10 +164,13 @@ void PreFlightChecker::reset()
_filter_vel_d_innov.reset();
_filter_baro_hgt_innov.reset();
_filter_gps_hgt_innov.reset();
_filter_rng_hgt_innov.reset();
_filter_ev_hgt_innov.reset();
_filter_heading_innov.reset();
#if defined(CONFIG_EKF2_RANGE_FINDER)
_is_using_rng_hgt_aiding = false;
_filter_rng_hgt_innov.reset();
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_is_using_flow_aiding = false;
src/modules/ekf2/Utility/PreFlightChecker.hpp
+7 -2
View File
@@ -83,7 +83,9 @@ public:
void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
#if defined(CONFIG_EKF2_RANGE_FINDER)
void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
#endif // CONFIG_EKF2_RANGE_FINDER
void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
bool hasHeadingFailed() const { return _has_heading_failed; }
@@ -157,7 +159,6 @@ private:
bool _is_using_baro_hgt_aiding{};
bool _is_using_gps_hgt_aiding{};
bool _is_using_rng_hgt_aiding{};
bool _is_using_ev_hgt_aiding{};
// Low-pass filters for innovation pre-flight checks
@@ -169,9 +170,13 @@ private:
// Preflight low pass filter height innovation (m)
InnovationLpf _filter_baro_hgt_innov;
InnovationLpf _filter_gps_hgt_innov;
InnovationLpf _filter_rng_hgt_innov;
InnovationLpf _filter_ev_hgt_innov;
#if defined(CONFIG_EKF2_RANGE_FINDER)
bool _is_using_rng_hgt_aiding {};
InnovationLpf _filter_rng_hgt_innov;
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool _is_using_flow_aiding {};
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)