fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 10:23: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-21 11:25:34 -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
}
src/modules/ekf2/Utility/PreFlightChecker.hpp
+16 -7
View File
@@ -75,7 +75,9 @@ public:
void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
#endif // CONFIG_EKF2_OPTICAL_FLOW
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
@@ -150,7 +152,6 @@ private:
bool _can_observe_heading_in_flight{};
bool _is_using_gps_aiding{};
bool _is_using_flow_aiding{};
bool _is_using_ev_pos_aiding{};
bool _is_using_ev_vel_aiding{};
@@ -164,8 +165,6 @@ private:
InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec)
InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec)
InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad)
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
// Preflight low pass filter height innovation (m)
InnovationLpf _filter_baro_hgt_innov;
@@ -173,6 +172,18 @@ private:
InnovationLpf _filter_rng_hgt_innov;
InnovationLpf _filter_ev_hgt_innov;
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool _is_using_flow_aiding {};
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
// Maximum permissible flow innovation to pass pre-flight checks
static constexpr float _flow_innov_test_lim = 0.5f;
// Preflight flow innovation spike limit (rad)
static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
#endif // CONFIG_EKF2_OPTICAL_FLOW
// Preflight low pass filter time constant inverse (1/sec)
static constexpr float _innov_lpf_tau_inv = 0.2f;
// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
@@ -183,13 +194,11 @@ private:
static constexpr float _nav_heading_innov_test_lim = 0.25f;
// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
static constexpr float _heading_innov_test_lim = 0.52f;
// Maximum permissible flow innovation to pass pre-flight checks
static constexpr float _flow_innov_test_lim = 0.5f;
// Preflight velocity innovation spike limit (m/sec)
static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
// Preflight position innovation spike limit (m)
static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;
// Preflight flow innovation spike limit (rad)
static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
};
#endif // !EKF2_PREFLIGHTCHECKER_HPP