From 7c3a2f839bd679c07d1209a88a03c6c33e253fa0 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 21 Jan 2020 14:50:21 +0100 Subject: [PATCH] Update range finder interface --- EKF/estimator_interface.cpp | 13 +++++-------- EKF/estimator_interface.h | 2 +- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 3399ee47da..c3b203bb15 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -452,7 +452,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message } } -void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &velocity, const Vector3f &variance) +void EstimatorInterface::setAuxVelData(const auxVelSample& auxvel_sample) { if (!_initialised || _auxvel_buffer_fail) { return; @@ -470,16 +470,13 @@ void EstimatorInterface::setAuxVelData(uint64_t time_usec, const Vector3f &veloc } // limit data rate to prevent data being lost - if ((time_usec - _time_last_auxvel) > _min_obs_interval_us) { + if ((auxvel_sample.time_us - _time_last_auxvel) > _min_obs_interval_us) { + _time_last_auxvel = auxvel_sample.time_us; - auxVelSample auxvel_sample_new; - auxvel_sample_new.time_us = time_usec - _params.auxvel_delay_ms * 1000; + auxVelSample auxvel_sample_new = auxvel_sample; + auxvel_sample_new.time_us -= _params.auxvel_delay_ms * 1000; auxvel_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _time_last_auxvel = time_usec; - - auxvel_sample_new.vel = velocity; - auxvel_sample_new.velVar = variance; _auxvel_buffer.push(auxvel_sample_new); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index a88cc24c03..06a03154b0 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -193,7 +193,7 @@ public: // set external vision position and attitude data void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); - void setAuxVelData(uint64_t time_usec, const Vector3f &vel, const Vector3f &variance); + void setAuxVelData(const auxVelSample& auxvel_sample); // return a address to the parameters struct // in order to give access to the application