From 3cafedd4c1ccdb716fe984b2c03a9a7e94be183d Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Sat, 30 Jan 2016 12:46:01 -0800 Subject: [PATCH] EKF: start storing samples only when ekf is initialised --- EKF/estimator_base.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index e30325dfcf..890b1c2dae 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -119,7 +119,7 @@ void EstimatorBase::setMagData(uint64_t time_usec, float *data) void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps) { - if(!collect_gps(time_usec, gps)) { + if(!collect_gps(time_usec, gps) || !_initialised) { return; } @@ -150,7 +150,9 @@ void EstimatorBase::setGpsData(uint64_t time_usec, struct gps_message *gps) void EstimatorBase::setBaroData(uint64_t time_usec, float *data) { - + if(!collect_baro(time_usec, data) || !_initialised) { + return; + } if (time_usec - _time_last_baro > 70000) { baroSample baro_sample_new; @@ -168,6 +170,9 @@ void EstimatorBase::setBaroData(uint64_t time_usec, float *data) void EstimatorBase::setAirspeedData(uint64_t time_usec, float *data) { + if(!collect_airspeed(time_usec, data) || !_initialised) { + return; + } if (time_usec > _time_last_airspeed) { airspeedSample airspeed_sample_new; airspeed_sample_new.airspeed = *data; @@ -183,13 +188,17 @@ void EstimatorBase::setAirspeedData(uint64_t time_usec, float *data) // set range data void EstimatorBase::setRangeData(uint64_t time_usec, float *data) { - + if(!collect_range(time_usec, data) || !_initialised) { + return; + } } // set optical flow data void EstimatorBase::setOpticalFlowData(uint64_t time_usec, float *data) { - + if(!collect_opticalflow(time_usec, data) || !_initialised) { + return; + } } bool EstimatorBase::initialise_interface(uint64_t timestamp)