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

fake measurement while on ground if signal quality is 0

Browse Source
This commit is contained in:
Jacob Dahl 2025-09-22 17:01:25 -08:00
parent 5598581c4e
commit 50d3b7e1ff
1 changed files with 9 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/ekf2/EKF/aid_sources/range_finder/jake_range_height_control.cpp
View File

@ -67,16 +67,19 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
float cosine_max_tilt = 0.9659; // 10 degrees
_range_sensor.setCosMaxTilt(cosine_max_tilt);
_rng_consistency_check.setGate(_params.ekf2_rng_k_gate);
// Update sensor to earth rotation
_range_sensor.updateSensorToEarthRotation(_R_to_earth);
// TODO: refactor into validity_checks()
// Gate sample consumption on these checks
bool quality_ok = sample.quality > 0; // TODO: what about unknown? (-1)
bool quality_ok = sample.quality > 0;
// While on ground fake a measurement at ground level if the signal quality is bad
if (!quality_ok && !_control_status.flags.in_air) {
sample.quality = 100;
quality_ok = true;
sample.range = _range_sensor.getValidMinVal();
}
bool tilt_ok = _range_sensor.isTiltOk();
bool range_ok = sample.range <= _range_sensor.getValidMaxVal() && sample.range >= _range_sensor.getValidMinVal();
// - Not stuck value
// - Not fog detected
// If quality, tilt, or value are outside of bounds -- throw away measurement
if (!quality_ok || !tilt_ok || !range_ok) {