ekf2: set horizon using specific parameter

Some sensors can have their delay included in the timestamp. In this
case, the buffer cannot be sized using the _DELAY parameter.

src/modules/ekf2/EKF/common.h

@@ -265,7 +265,7 @@ struct parameters {
 	int32_t height_sensor_ref{static_cast<int32_t>(HeightSensor::BARO)};
 	int32_t position_sensor_ref{static_cast<int32_t>(PositionSensor::GNSS)};
 
-	int32_t sensor_interval_max_ms{10};     ///< maximum time of arrival difference between non IMU sensor updates. Sets the size of the observation buffers. (mSec)
+	float delay_max_ms{110.f};              ///< maximum time delay of all the aiding sensors. Sets the size of the observation buffers. (mSec)
 
 	// input noise
 	float gyro_noise{1.5e-2f};              ///< IMU angular rate noise used for covariance prediction (rad/sec)

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -516,71 +516,15 @@ void EstimatorInterface::setDragData(const imuSample &imu)
 
 bool EstimatorInterface::initialise_interface(uint64_t timestamp)
 {
-	// find the maximum time delay the buffers are required to handle
-
-	// it's reasonable to assume that aux velocity device has low delay. TODO: check the delay only if the aux device is used
-	float max_time_delay_ms = _params.sensor_interval_max_ms;
-
-	// aux vel
-#if defined(CONFIG_EKF2_AUXVEL)
-	max_time_delay_ms = math::max(_params.auxvel_delay_ms, max_time_delay_ms);
-#endif // CONFIG_EKF2_AUXVEL
-
-#if defined(CONFIG_EKF2_BAROMETER)
-	// using baro
-	if (_params.baro_ctrl > 0) {
-		max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_AIRSPEED)
-	// using airspeed
-	if (_params.arsp_thr > FLT_EPSILON) {
-		max_time_delay_ms = math::max(_params.airspeed_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_AIRSPEED
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-	// mag mode
-	if (_params.mag_fusion_type != MagFuseType::NONE) {
-		max_time_delay_ms = math::max(_params.mag_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_MAGNETOMETER
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-	// using range finder
-	if ((_params.rng_ctrl != static_cast<int32_t>(RngCtrl::DISABLED))) {
-		max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_RANGE_FINDER
-
-#if defined(CONFIG_EKF2_GNSS)
-	if (_params.gnss_ctrl > 0) {
-		max_time_delay_ms = math::max(_params.gps_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_GNSS
-
-#if defined(CONFIG_EKF2_OPTICAL_FLOW)
-	if (_params.flow_ctrl > 0) {
-		max_time_delay_ms = math::max(_params.flow_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_OPTICAL_FLOW
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	if (_params.ev_ctrl > 0) {
-		max_time_delay_ms = math::max(_params.ev_delay_ms, max_time_delay_ms);
-	}
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
 	const float filter_update_period_ms = _params.filter_update_interval_us / 1000.f;
 
 	// calculate the IMU buffer length required to accomodate the maximum delay with some allowance for jitter
-	_imu_buffer_length = math::max(2, (int)ceilf(max_time_delay_ms / filter_update_period_ms));
+	_imu_buffer_length = math::max(2, (int)ceilf(_params.delay_max_ms / filter_update_period_ms));
 
 	// set the observation buffer length to handle the minimum time of arrival between observations in combination
 	// with the worst case delay from current time to ekf fusion time
 	// allow for worst case 50% extension of the ekf fusion time horizon delay due to timing jitter
-	const float ekf_delay_ms = max_time_delay_ms * 1.5f;
+	const float ekf_delay_ms = _params.delay_max_ms * 1.5f;
 	_obs_buffer_length = roundf(ekf_delay_ms / filter_update_period_ms);
 
 	// limit to be no longer than the IMU buffer (we can't process data faster than the EKF prediction rate)

src/modules/ekf2/EKF2.cpp

@@ -64,6 +64,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 #endif // CONFIG_EKF2_WIND
 	_params(_ekf.getParamHandle()),
 	_param_ekf2_predict_us(_params->filter_update_interval_us),
+	_param_ekf2_delay_max(_params->delay_max_ms),
 	_param_ekf2_imu_ctrl(_params->imu_ctrl),
 #if defined(CONFIG_EKF2_AUXVEL)
 	_param_ekf2_avel_delay(_params->auxvel_delay_ms),
@@ -437,46 +438,6 @@ void EKF2::Run()
 
 #endif // CONFIG_EKF2_AIRSPEED
 
-#if defined(CONFIG_EKF2_BAROMETER)
-
-		// if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output
-		if (_params->baro_ctrl == 1) {
-			float sens_baro_rate = 0.f;
-
-			if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) {
-				if (sens_baro_rate > 0) {
-					float interval_ms = roundf(1000.f / sens_baro_rate);
-
-					if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
-						PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
-						_params->sensor_interval_max_ms = interval_ms;
-					}
-				}
-			}
-		}
-
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-
-		// if using mag ensure sensor interval minimum is sufficient to accommodate system averaged mag output
-		if (_params->mag_fusion_type != MagFuseType::NONE) {
-			float sens_mag_rate = 0.f;
-
-			if (param_get(param_find("SENS_MAG_RATE"), &sens_mag_rate) == PX4_OK) {
-				if (sens_mag_rate > 0) {
-					float interval_ms = roundf(1000.f / sens_mag_rate);
-
-					if (PX4_ISFINITE(interval_ms) && (interval_ms > _params->sensor_interval_max_ms)) {
-						PX4_DEBUG("updating sensor_interval_max_ms %.3f -> %.3f", (double)_params->sensor_interval_max_ms, (double)interval_ms);
-						_params->sensor_interval_max_ms = interval_ms;
-					}
-				}
-			}
-		}
-
-#endif // CONFIG_EKF2_MAGNETOMETER
-
 		_ekf.updateParameters();
 	}
 
@@ -822,6 +783,82 @@ void EKF2::VerifyParams()
 	}
 
 #endif // CONFIG_EKF2_MAGNETOMETER
+
+	float delay_max = _param_ekf2_delay_max.get();
+
+#if defined(CONFIG_EKF2_AUXVEL)
+
+	if (_param_ekf2_avel_delay.get() > delay_max) {
+		delay_max = _param_ekf2_avel_delay.get();
+	}
+
+#endif // CONFIG_EKF2_AUXVEL
+
+#if defined(CONFIG_EKF2_BAROMETER)
+
+	if (_param_ekf2_baro_delay.get() > delay_max) {
+		delay_max = _param_ekf2_baro_delay.get();
+	}
+
+#endif // CONFIG_EKF2_BAROMETER
+
+#if defined(CONFIG_EKF2_AIRSPEED)
+
+	if (_param_ekf2_asp_delay.get() > delay_max) {
+		delay_max = _param_ekf2_asp_delay.get();
+	}
+
+#endif // CONFIG_EKF2_AIRSPEED
+
+#if defined(CONFIG_EKF2_MAGNETOMETER)
+
+	if (_param_ekf2_mag_delay.get() > delay_max) {
+		delay_max = _param_ekf2_mag_delay.get();
+	}
+
+#endif // CONFIG_EKF2_MAGNETOMETER
+
+#if defined(CONFIG_EKF2_RANGE_FINDER)
+
+	if (_param_ekf2_rng_delay.get() > delay_max) {
+		delay_max = _param_ekf2_rng_delay.get();
+	}
+
+#endif // CONFIG_EKF2_RANGE_FINDER
+
+#if defined(CONFIG_EKF2_GNSS)
+
+	if (_param_ekf2_gps_delay.get() > delay_max) {
+		delay_max = _param_ekf2_gps_delay.get();
+	}
+
+#endif // CONFIG_EKF2_GNSS
+
+#if defined(CONFIG_EKF2_OPTICAL_FLOW)
+
+	if (_param_ekf2_of_delay.get() > delay_max) {
+		delay_max = _param_ekf2_of_delay.get();
+	}
+
+#endif // CONFIG_EKF2_OPTICAL_FLOW
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+
+	if (_param_ekf2_ev_delay.get() > delay_max) {
+		delay_max = _param_ekf2_ev_delay.get();
+	}
+
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+	if (delay_max > _param_ekf2_delay_max.get()) {
+		/* EVENT
+		 * @description EKF2_DELAY_MAX({1}ms) is too small compared to the maximum sensor delay ({2})
+		 */
+		events::send<float, float>(events::ID("nf_delay_max_too_small"), events::Log::Warning,
+					   "EKF2_DELAY_MAX increased to {2}ms, please reboot", _param_ekf2_delay_max.get(),
+					   delay_max);
+		_param_ekf2_delay_max.commit_no_notification(delay_max);
+	}
 }
 
 void EKF2::PublishAidSourceStatus(const hrt_abstime &timestamp)

src/modules/ekf2/EKF2.hpp

@@ -498,6 +498,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamExtInt<px4::params::EKF2_PREDICT_US>) _param_ekf2_predict_us,
+		(ParamExtFloat<px4::params::EKF2_DELAY_MAX>) _param_ekf2_delay_max,
 		(ParamExtInt<px4::params::EKF2_IMU_CTRL>) _param_ekf2_imu_ctrl,
 
 #if defined(CONFIG_EKF2_AUXVEL)

src/modules/ekf2/module.yaml

@@ -29,6 +29,18 @@ parameters:
       default: 7
       min: 0
       max: 7
+    EKF2_DELAY_MAX:
+      description:
+        short: Maximum delay of all the aiding sensors
+        long: Defines the delay between the current time and the delayed-time horizon.
+          This value should be at least as large as the largest EKF2_XXX_DELAY parameter.
+      type: float
+      default: 200
+      min: 0
+      max: 1000
+      unit: ms
+      reboot_required: true
+      decimal: 1
     EKF2_MAG_DELAY:
       description:
         short: Magnetometer measurement delay relative to IMU measurements