fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 16:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: add kconfig option to enable/disable optical flow fusion

Browse Source
This commit is contained in:
Daniel Agar
2023-03-14 16:51:49 -04:00
parent 9f8fa99d70
commit 0784901a66
16 changed files with 194 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/Utility/PreFlightChecker.cpp
+8 -1
View File
@@ -82,6 +82,8 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
}
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_is_using_flow_aiding) {
const Vector2f flow_innov = Vector2f(innov.flow);
Vector2f flow_innov_lpf;
@@ -90,6 +92,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_
has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
return has_failed;
}
@@ -143,7 +146,6 @@ bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vecto
void PreFlightChecker::reset()
{
_is_using_gps_aiding = false;
_is_using_flow_aiding = false;
_is_using_ev_pos_aiding = false;
_is_using_ev_vel_aiding = false;
_is_using_baro_hgt_aiding = false;
@@ -162,6 +164,11 @@ void PreFlightChecker::reset()
_filter_rng_hgt_innov.reset();
_filter_ev_hgt_innov.reset();
_filter_heading_innov.reset();
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_is_using_flow_aiding = false;
_filter_flow_x_innov.reset();
_filter_flow_y_innov.reset();
#endif // CONFIG_EKF2_OPTICAL_FLOW
}