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

ekf2: new kconfig to enable/disable GNSS (enabled by default)

Browse Source
This commit is contained in:
Daniel Agar
2023-10-11 14:02:34 -04:00
committed by GitHub
parent 2d78383296
commit d2b3e7fe16
24 changed files with 476 additions and 331 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/optical_flow_control.cpp
+17 -7
View File
@@ -142,15 +142,25 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
// New optical flow data is available and is ready to be fused when the midpoint of the sample falls behind the fusion time horizon
if (_flow_data_ready) {
// Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching
const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
// Check if we are in-air and require optical flow to control position drift
const bool is_flow_required = _control_status.flags.in_air
bool is_flow_required = _control_status.flags.in_air
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow)
|| (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim))); // is using GPS, but GPS is bad
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
#if defined(CONFIG_EKF2_GNSS)
// check if using GPS, but GPS is bad
if (_control_status.flags.gps) {
if (_control_status.flags.in_air && !is_flow_required) {
// Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching
const float gps_err_norm_lim = _control_status.flags.opt_flow ? 0.7f : 1.0f;
if (_gps_error_norm > gps_err_norm_lim) {
is_flow_required = true;
}
}
}
#endif // CONFIG_EKF2_GNSS
// inhibit use of optical flow if motion is unsuitable and we are not reliant on it for flight navigation
const bool preflight_motion_not_ok = !_control_status.flags.in_air