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

ekf2: don't fuse optical flow samples when the current distance to the ground is larger than the reported maximum flow sensor distance

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
Roman Bapst
2024-06-12 03:20:40 +02:00
committed by GitHub
parent 5dd76332ba
commit 0ce64e1b92
1 changed files with 5 additions and 1 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
+5 -1
View File
@@ -55,9 +55,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
&& !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate); && !_flow_sample_delayed.flow_rate.longerThan(_flow_max_rate);
const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt);
// don't enforce this condition if terrain estimate is not valid as we have logic below to coast through bad range finder data
const bool is_within_max_sensor_dist = isTerrainEstimateValid() ? (_terrain_vpos - _state.pos(2) <= _flow_max_distance) : true;
if (is_quality_good if (is_quality_good
&& is_magnitude_good && is_magnitude_good
&& is_tilt_good) { && is_tilt_good
&& is_within_max_sensor_dist) {
// compensate for body motion to give a LOS rate // compensate for body motion to give a LOS rate
calcOptFlowBodyRateComp(imu_delayed); calcOptFlowBodyRateComp(imu_delayed);
_flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy(); _flow_rate_compensated = _flow_sample_delayed.flow_rate - _flow_sample_delayed.gyro_rate.xy();