From d53f532079efc7d777ef3020d66cd7a331a2d7a8 Mon Sep 17 00:00:00 2001 From: Roman Date: Fri, 12 Aug 2016 09:00:41 +0200 Subject: [PATCH] fixed computation of range measurement timestamp Signed-off-by: Roman --- EKF/estimator_interface.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index fdd2e0117c..deeebb9dd7 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -241,9 +241,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float *data) rangeSample range_sample_new = {}; range_sample_new.rng = *data; rng = *data; - range_sample_new.time_us -= _params.range_delay_ms * 1000; - - range_sample_new.time_us = time_usec; + range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000; _time_last_range = time_usec; _range_buffer.push(range_sample_new);