ekf2: range fusion: while on the ground the rangefinder quality checks are bypassed and a synthetic measurement is fused using the minimum rangefinder distance. The intention of this logic is to ensure the rangefinder can be fused immediately during takeoff even if it's reporting 0 quality while on the ground. This was not working since the rangefinder _time_bad_quality_us was being updated while on the ground. This results in range fusion being disabled as soon as the in_air flag is set, since the time since the last bad quality value has not exceeded the EKF2_RNG_QLTY_T. This commit moves the logic into the SensorRangeFinder::isQualityOk function where the _time_bad_quality_us is only set once airborne.

boards/ark/pi6x/init/rc.board_defaults

@@ -36,7 +36,6 @@ param set-default EKF2_MULTI_IMU 0
 param set-default EKF2_OF_CTRL 1
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
-param set-default EKF2_RNG_QLTY_T 0.1
 
 param set-default SENS_FLOW_RATE 150
 param set-default SENS_IMU_MODE 1

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -57,7 +57,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		_range_sensor.setQualityHysteresis(_params.ekf2_rng_qlty_t);
 		_range_sensor.setMaxFogDistance(_params.ekf2_rng_fog);
 
-		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
+		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth, _control_status.flags.in_air);
 
 		if (_range_sensor.isDataHealthy()) {
 			// correct the range data for position offset relative to the IMU
@@ -76,17 +76,6 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
 							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, imu_sample.time_us);
 			}
-
-		} else {
-			// If we are supposed to be using range finder data but have bad range measurements
-			// and are on the ground, then synthesise a measurement at the expected on ground value
-			if (!_control_status.flags.in_air
-			    && _range_sensor.isRegularlySendingData()
-			    && _range_sensor.isDataReady()) {
-
-				_range_sensor.setRange(_params.ekf2_min_rng);
-				_range_sensor.setValidity(true); // bypass the checks
-			}
 		}
 
 		_control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent();

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -47,8 +47,10 @@ namespace estimator
 namespace sensor
 {
 
-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth, bool in_air)
 {
+	_in_air = in_air;
+
 	updateSensorToEarthRotation(R_to_earth);
 	updateValidity(current_time_us);
 }
@@ -64,7 +66,7 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 {
 	updateDtDataLpf(current_time_us);
 
-	if (_is_faulty || isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
+	if (isSampleOutOfDate(current_time_us) || !isDataContinuous()) {
 		_is_sample_valid = false;
 		_is_regularly_sending_data = false;
 		return;
@@ -76,7 +78,6 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 	if (_is_sample_ready) {
 		_is_sample_valid = false;
 
-		_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
 
 		if (!isQualityOk(current_time_us) || !isTiltOk() || !isDataInRange()) {
 			return;
@@ -92,8 +93,16 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 	}
 }
 
-bool SensorRangeFinder::isQualityOk(uint64_t current_time_us) const
+bool SensorRangeFinder::isQualityOk(uint64_t current_time_us)
 {
+	// Mark quality as OK while on the ground
+	if (!_in_air) {
+		_sample.rng = _rng_valid_min_val; // set to min val while on ground
+		return true;
+	}
+
+	_time_bad_quality_us = _sample.quality == 0 ? current_time_us : _time_bad_quality_us;
+
 	return current_time_us - _time_bad_quality_us > _quality_hyst_us;
 }
 

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -66,7 +66,7 @@ public:
 	SensorRangeFinder() = default;
 	~SensorRangeFinder() override = default;
 
-	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth);
+	void runChecks(uint64_t current_time_us, const matrix::Dcmf &R_to_earth, bool in_air);
 	bool isHealthy() const override { return _is_sample_valid; }
 	bool isDataHealthy() const override { return _is_sample_ready && _is_sample_valid; }
 	bool isDataReady() const { return _is_sample_ready; }
@@ -120,8 +120,6 @@ public:
 	float getValidMinVal() const { return _rng_valid_min_val; }
 	float getValidMaxVal() const { return _rng_valid_max_val; }
 
-	void setFaulty(bool faulty = true) { _is_faulty = faulty; }
-
 private:
 	void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
 
@@ -131,7 +129,7 @@ private:
 	bool isDataContinuous() const { return _dt_data_lpf < 2e6f; }
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
 	bool isDataInRange() const;
-	bool isQualityOk(uint64_t current_time_us) const;
+	bool isQualityOk(uint64_t current_time_us);
 	void updateStuckCheck();
 	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
 
@@ -141,7 +139,6 @@ private:
 	bool _is_sample_valid{};	///< true if range finder sample retrieved from buffer is valid
 	bool _is_regularly_sending_data{false}; ///< true if the interval between two samples is less than the maximum expected interval
 	uint64_t _time_last_valid_us{};	///< time the last range finder measurement was ready (uSec)
-	bool _is_faulty{false};         ///< the sensor should not be used anymore
 
 	/*
 	 * Stuck check
@@ -177,6 +174,7 @@ private:
 	 */
 	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
 	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
+	bool _in_air{};
 
 	/*
 	 * Fog check

src/modules/ekf2/params_terrain.yaml

@@ -28,7 +28,7 @@ parameters:
           where the range finder may be inside its minimum measurements distance when
           on ground.
       type: float
-      default: 0.1
+      default: 0.01
       min: 0.01
       unit: m
       decimal: 2