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

only run range validator on regular data

Browse Source
This commit is contained in:
Marco Hauswirth
2025-08-26 15:08:43 +02:00
parent f89f73cf37
commit f2df6c7264
1 changed files with 38 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+3 -1
View File
@@ -51,8 +51,9 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress());
_range_sensor.setDataReadiness(rng_data_ready); _range_sensor.setDataReadiness(rng_data_ready);
if (rng_data_ready) {
// update range sensor angle parameters in case they have changed // update range sensor angle parameters in case they have changed
_range_sensor.setPitchOffset(_params.ekf2_rng_pitch); _range_sensor.setPitchOffset(_params.rng_sens_pitch);
_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
_range_sensor.setQualityHysteresis(_params.range_valid_quality_s); _range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
_range_sensor.setMaxFogDistance(_params.rng_fog); _range_sensor.setMaxFogDistance(_params.rng_fog);
@@ -96,6 +97,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
} }
} }
} }
}
_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();
_control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent()