Skip to content

Commit

Permalink
EKF2: move handling of invalid range into ECL (#12988)
Browse files Browse the repository at this point in the history
* ekf2: Move handling of invalid range finder data inside ecl library

The ecl library EKF is able to use vehicle motion and in-air status to better determine when the default on-ground range finder reading can be used.
The description for the EKF2_MIN_RNG parameter has been updated to make its use clearer.
  • Loading branch information
nicovanduijn authored and Julian Kent committed Sep 19, 2019
1 parent fa13f63 commit 549a962
Show file tree
Hide file tree
Showing 2 changed files with 5 additions and 19 deletions.
20 changes: 2 additions & 18 deletions src/modules/ekf2/ekf2_main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 3 additions & 1 deletion src/modules/ekf2/ekf2_params.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 549a962

Please sign in to comment.