diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 4eb742c26268..31402bc82dc6 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1118,28 +1118,12 @@ void Ekf2::run() } if (_range_finder_sub_index >= 0) { - bool range_finder_updated = _range_finder_subs[_range_finder_sub_index].updated(); - if (range_finder_updated) { + if (_range_finder_subs[_range_finder_sub_index].updated()) { distance_sensor_s range_finder; if (_range_finder_subs[_range_finder_sub_index].copy(&range_finder)) { - // check distance sensor data quality - // TODO - move this check inside the ecl library - if (range_finder.signal_quality == 0) { - // use rng_gnd_clearance if on ground - if (_ekf.get_in_air_status()) { - range_finder_updated = false; - - } else { - range_finder.current_distance = _param_ekf2_min_rng.get(); - } - } - - if (range_finder_updated) { - // TODO: Nico's pr will add proper quality handling, for now we put it to unkown - _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance, -1.f); - } + _ekf.setRangeData(range_finder.timestamp, range_finder.current_distance, range_finder.signal_quality); // Save sensor limits reported by the rangefinder _ekf.set_rangefinder_limits(range_finder.min_distance, range_finder.max_distance); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 7ace0d0dd122..dcc8461d4444 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -689,7 +689,9 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_SFE, 0.05f); PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); /** - * Minimum valid range for the range finder + * Expected range finder reading when on ground. + * + * If the vehicle is on ground, is not moving as determined by the motion test controlled by EKF2_MOVE_TEST and the range finder is returning invalid or no data, then an assumed range value of EKF2_MIN_RNG will be used by the terrain estimator so that a terrain height estimate is avilable at the start of flight in situations where the range finder may be inside its minimum measurements distance when on ground. * * @group EKF2 * @min 0.01