EKF: range_finder parameterize range sensor quality hysteresis time

Signed-off-by: Claudio Micheli <claudio@auterion.com>
Co-authored-by: bresch <brescianimathieu@gmail.com>

EKF/common.h

@@ -291,6 +291,7 @@ struct parameters {
 	float max_vel_for_range_aid{1.0f};	///< maximum ground velocity for which we allow to use the range finder as height source (if range_aid == 1)
 	int32_t range_aid{0};			///< allow switching primary height source to range finder if certain conditions are met
 	float range_aid_innov_gate{1.0f}; 	///< gate size used for innovation consistency checks for range aid fusion
+	float range_valid_quality_s{1.0f};	///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};	///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 
 	// vision position fusion

EKF/control.cpp

@@ -124,11 +124,13 @@ void Ekf::controlFusionModes()
 	// Get range data from buffer and check validity
 	const bool is_rng_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
 	_range_sensor.setDataReadiness(is_rng_data_ready);
-	_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
 
 	// update range sensor angle parameters in case they have changed
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+
+	_range_sensor.runChecks(_imu_sample_delayed.time_us, _R_to_earth);
 	}
 
 	if (_range_sensor.isDataHealthy()) {

EKF/ekf.cpp

@@ -80,6 +80,7 @@ void Ekf::reset()
 	_terrain_initialised = false;
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
+	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
 
 	_control_status.value = 0;
 	_control_status_prev.value = 0;

EKF/sensor_range_finder.hpp

@@ -83,6 +83,10 @@ public:
 		_rng_valid_max_val = max_distance;
 	}
 
+	void setQualityHysteresis(float valid_quality_threshold_s){
+		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
+	}
+
 	float getCosTilt() const { return _cos_tilt_rng_to_earth; }
 
 	void setRange(float rng) { _sample.rng = rng; }
@@ -146,7 +150,7 @@ private:
 	 * Quality check
 	 */
 	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
-	uint64_t _quality_hyst_us{1000000}; 	///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
+	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)
 };
 
 } // namespace sensor