From 0f91378eebef30358ab547ec4fba87d12ae11ef9 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Mon, 16 Jan 2017 12:20:54 +0530 Subject: [PATCH] lpe : use vision timestamps to compute measurement delay --- src/modules/local_position_estimator/params.c | 4 +++- .../local_position_estimator/sensors/vision.cpp | 10 +++++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c index 97390a1243..72ed4b624c 100644 --- a/src/modules/local_position_estimator/params.c +++ b/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 diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 8edb4e2699..8193c33257 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -73,6 +73,7 @@ void BlockLocalPositionEstimator::visionCorrect() Matrix 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 x0 = _xDelay.get(i_hist);