AP_NavEKF3: ground clearance fusion fix

Author: rmackay9

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -25,6 +25,8 @@ void NavEKF3_core::readRangeFinder(void)
     if (_rng == nullptr) {
         return;
     }
+
+    // update global range-on-ground, used for all rangefinders
     rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f);
 
     // limit update rate to maximum allowed by data buffers
@@ -35,23 +37,35 @@ void NavEKF3_core::readRangeFinder(void)
 
         // store samples and sample time into a ring buffer if valid
         // use data from two range finders if available
-
+        float range_distance = 0.0f;
         for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
             const auto *sensor = _rng->get_backend(sensorIndex);
             if (sensor == nullptr) {
                 continue;
             }
-            if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
-                rngMeasIndex[sensorIndex] ++;
-                if (rngMeasIndex[sensorIndex] > 2) {
-                    rngMeasIndex[sensorIndex] = 0;
-                }
-                storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
-                storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance();
+            if (sensor->orientation() != ROTATION_PITCH_270) {
+                // only consume downward facing rangefinder data
+                continue;
+            }
+            if (sensor->status() == AP_DAL_RangeFinder::Status::Good) {
+                // get the current range measurement
+                range_distance = sensor->distance();
+            } else if (onGround && sensor->status() == AP_DAL_RangeFinder::Status::OutOfRangeLow) {
+                // use ground clearange range if on ground
+                range_distance = rngOnGnd;
             } else {
+                // ignore out-of-range data
                 continue;
             }
 
+            // place distance in the range data buffer
+            rngMeasIndex[sensorIndex]++;
+            if (rngMeasIndex[sensorIndex] > 2) {
+                rngMeasIndex[sensorIndex] = 0;
+            }
+            storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
+            storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = range_distance;
+
             // check for three fresh samples
             bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
             for (uint8_t index = 0; index <= 2; index++) {
@@ -91,18 +105,6 @@ void NavEKF3_core::readRangeFinder(void)
 
                 // indicate we have updated the measurement
                 rngValidMeaTime_ms = imuSampleTime_ms;
-
-            } else if (onGround && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
-                // before takeoff we assume on-ground range value if there is no data
-                rangeDataNew.time_ms = imuSampleTime_ms;
-                rangeDataNew.rng = rngOnGnd;
-
-                // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
-                storedRange.push(rangeDataNew);
-
-                // indicate we have updated the measurement
-                rngValidMeaTime_ms = imuSampleTime_ms;
-
             }
         }
     }