fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 17:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF2: move handling of invalid range into ECL (#12988)

Browse Source 
* 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.
This commit is contained in:
Nico van Duijn
2019-09-19 11:29:34 +02:00
committed by Julian Kent
parent fa13f63db6
commit 549a962553
2 changed files with 5 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+2 -18
View File
@@ -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);
src/modules/ekf2/ekf2_params.c
+3 -1
View File
@@ -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