lpe : use vision timestamps to compute measurement delay

src/modules/local_position_estimator/params.c

@@ -219,7 +219,9 @@ PARAM_DEFINE_FLOAT(LPE_EPH_MAX, 3.0f);
 PARAM_DEFINE_FLOAT(LPE_EPV_MAX, 5.0f);
 
 /**
- * Vision delay compensaton
+ * Vision delay compensaton.
+ *
+ * Set to zero to enable automatic compensation from measurement timestamps
  *
  * @group Local Position Estimator
  * @unit sec

src/modules/local_position_estimator/sensors/vision.cpp

@@ -73,6 +73,7 @@ void BlockLocalPositionEstimator::visionCorrect()
 	Matrix<float, n_y_vision, n_y_vision> R;
 	R.setZero();
 
+	// use error estimates from vision topic if available
 	if (_sub_vision_pos.get().eph > _vision_xy_stddev.get()) {
 		R(Y_vision_x, Y_vision_x) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
 		R(Y_vision_y, Y_vision_y) = _sub_vision_pos.get().eph * _sub_vision_pos.get().eph;
@@ -92,7 +93,14 @@ void BlockLocalPositionEstimator::visionCorrect()
 	// vision delayed x
 	uint8_t i_hist = 0;
 
-	if (getDelayPeriods(_vision_delay.get(), &i_hist)  < 0) { return; }
+	float vision_delay = (_timeStamp - _sub_vision_pos.get().timestamp) * 1e-6f; // measurement delay in seconds
+
+	if (vision_delay < 0.0f) { vision_delay = 0.0f; }
+
+	// use auto-calculated delay from measurement if parameter is set to zero
+	if (getDelayPeriods(_vision_delay.get() > 0.0f ? _vision_delay.get() : vision_delay, &i_hist)  < 0) { return; }
+
+	//mavlink_and_console_log_info(&mavlink_log_pub, "[lpe] vision delay : %0.2f ms", double(vision_delay * 1000.0f));
 
 	Vector<float, n_x> x0 = _xDelay.get(i_hist);