Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

ekf2-flow: only allow flow when in range

Browse Source 
Also, as the flow makes the link between range and horizontal velocity,
only allow it to start if at least one of the two is known. Otherwise
the EKF will struggle to estimate both values at the same time.
This commit is contained in:
bresch
2024-07-15 14:38:45 +02:00
committed by Daniel Agar
parent 8e5f28f834
commit 9d6c2baa90
1 changed files with 3 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp
+3 -2
View File
@@ -116,7 +116,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
&& (_control_status.flags.inertial_dead_reckoning // is doing inertial dead-reckoning so must constrain drift urgently
|| isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow));
const bool is_within_max_sensor_dist = getHagl() <= _flow_max_distance;
const bool is_within_sensor_dist = (getHagl() >= _flow_min_distance) && (getHagl() <= _flow_max_distance);
const bool is_magnitude_good = flow_sample.flow_rate.isAllFinite()
&& !flow_sample.flow_rate.longerThan(_flow_max_rate)
@@ -124,12 +124,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
const bool continuing_conditions_passing = (_params.flow_ctrl == 1)
&& _control_status.flags.tilt_align
&& is_within_max_sensor_dist;
&& is_within_sensor_dist;
const bool starting_conditions_passing = continuing_conditions_passing
&& is_quality_good
&& is_magnitude_good
&& is_tilt_good
&& (isTerrainEstimateValid() || isHorizontalAidingActive())
&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
// If the height is relative to the ground, terrain height cannot be observed.