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

further improve logic

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2025-02-10 10:13:09 +03:00 committed by Marco Hauswirth
parent 73ade6d05c
commit 806500fc4a
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp
View File

@ -76,25 +76,33 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
if (_is_sample_ready) {
_is_sample_valid = false;
if (_sample.quality == 0) {
_time_bad_quality_us = current_time_us;
_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_last_valid_us;
} else if (current_time_us - _time_bad_quality_us > _quality_hyst_us) {
// We did not receive bad quality data for some time
const bool quality_ok = isQualityOk(current_time_us);
if (isTiltOk() && isDataInRange()) {
updateStuckCheck();
updateFogCheck(getDistBottom(), _sample.time_us);
if (!quality_ok || !isTiltOk() || !isDataInRange()) {
return;
}
if (!_is_stuck && !_is_blocked) {
_is_sample_valid = true;
_time_last_valid_us = _sample.time_us;
}
}
updateStuckCheck();
if (!_is_stuck) {
updateFogCheck(getDistBottom(), _sample.time_us);
}
if (!_is_stuck && !_is_blocked) {
_is_sample_valid = true;
_time_last_valid_us = _sample.time_us;
}
}
}
bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
{
const bool bad_quality = current_time_us - _time_bad_quality_us > _quality_hyst_us;
return bad_quality;
}
void SensorRangeFinder::updateDtDataLpf(uint64_t current_time_us)
{
// Calculate a first order IIR low-pass filtered time of arrival between samples using a 2 second time constant.

2
src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp
View File

@ -131,6 +131,7 @@ private:
bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
bool isDataInRange() const;
bool isQualityOk(uint64_t current_time_us) const;
void updateStuckCheck();
void updateFogCheck(const float dist_bottom, const uint64_t time_us);
@ -184,6 +185,7 @@ private:
float _max_fog_dist{0.f}; //< maximum distance at which the range finder could detect fog (m)
math::MedianFilter<float, 5> _median_dist{};
float _prev_median_dist{0.f};
};
} // namespace sensor