Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

EKF2: move handling of invalid range into ECL #12988

Merged
merged 2 commits into from
Sep 19, 2019
Merged

EKF2: move handling of invalid range into ECL #12988

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
72c78cb
ekf2: Move handling of invalid range finder data inside ecl library
priseborough Sep 12, 2019
9d504ed
fix code style
dagar Sep 15, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 2 additions & 18 deletions src/modules/ekf2/ekf2_main.cpp
View file
Open in desktop
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
View file
Open in desktop
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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

FYI @hamishwillee , not sure if we have any other documentation on this parameter, but the description was misleading before.

*
* @group EKF2
* @min 0.01
Expand Down