EKF: Relax minimum required range finder measurement rate

PX4 · May 7, 2018 · 868bc01 · 868bc01
1 parent 02963a8
commit 868bc01
Showing 1 changed file with 6 additions and 5 deletions.
diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp
@@ -207,16 +207,17 @@ void Ekf::get_hagl_innov_var(float *hagl_innov_var)
 // check that the range finder data is continuous
 void Ekf::checkRangeDataContinuity()
 {
-	// update range data continuous flag (2Hz ie 500 ms)
+	// update range data continuous flag (1Hz ie 1000 ms)
 	/* Timing in micro seconds */
 
-	/* Apply a 1.0 sec low pass filter to the time delta from the last range finder updates */
-	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - _dt_update) + _dt_update *
+	/* Apply a 2.0 sec low pass filter to the time delta from the last range finder updates */
+	float alpha = 0.5f * _dt_update;
+	_dt_last_range_update_filt_us = _dt_last_range_update_filt_us * (1.0f - alpha) + alpha *
 					(_time_last_imu - _time_last_range);
 
-	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 1e6f);
+	_dt_last_range_update_filt_us = fminf(_dt_last_range_update_filt_us, 2e6f);
 
-	if (_dt_last_range_update_filt_us < 5e5f) {
+	if (_dt_last_range_update_filt_us < 1e6f) {
 		_range_data_continuous = true;
 
 	} else {