From 856961ba8525d32b4a4890a473f02c58c987efda Mon Sep 17 00:00:00 2001 From: kamilritz Date: Tue, 21 Jan 2020 14:48:37 +0100 Subject: [PATCH] Update airspeed interface --- EKF/common.h | 2 +- EKF/estimator_interface.cpp | 14 +++++++------- EKF/estimator_interface.h | 2 +- test/sensor_simulator/airspeed.cpp | 7 +++++-- 4 files changed, 14 insertions(+), 11 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 0c08a5c021..e9a3667599 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -140,7 +140,7 @@ struct rangeSample { struct airspeedSample { float true_airspeed; ///< true airspeed measurement (m/sec) - float eas2tas; ///< equivalent to true airspeed factor + float eas2tas; ///< equivalent to true airspeed factor uint64_t time_us; ///< timestamp of the measurement (uSec) }; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 1140258ce7..b8380be3d8 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -291,7 +291,7 @@ void EstimatorInterface::setBaroData(const baroSample &baro_sample) } } -void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas) +void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample) { if (!_initialised || _airspeed_buffer_fail) { return; @@ -309,13 +309,13 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed } // limit data rate to prevent data being lost - if ((time_usec - _time_last_airspeed) > _min_obs_interval_us) { - airspeedSample airspeed_sample_new; - airspeed_sample_new.true_airspeed = true_airspeed; - airspeed_sample_new.eas2tas = eas2tas; - airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000; + if ((airspeed_sample.time_us - _time_last_airspeed) > _min_obs_interval_us) { + _time_last_airspeed = airspeed_sample.time_us; + + airspeedSample airspeed_sample_new = airspeed_sample; + + airspeed_sample_new.time_us -= _params.airspeed_delay_ms * 1000; airspeed_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2; - _time_last_airspeed = time_usec; _airspeed_buffer.push(airspeed_sample_new); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 322521d107..76becbb059 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -183,7 +183,7 @@ public: void setBaroData(const baroSample &baro_sample); - void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas); + void setAirspeedData(const airspeedSample &airspeed_sample); void setRangeData(uint64_t time_usec, float data, int8_t quality); diff --git a/test/sensor_simulator/airspeed.cpp b/test/sensor_simulator/airspeed.cpp index 6c6c174f20..6b7e35979e 100644 --- a/test/sensor_simulator/airspeed.cpp +++ b/test/sensor_simulator/airspeed.cpp @@ -17,8 +17,11 @@ void Airspeed::send(uint64_t time) { if(_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) { - float eas2tas = _true_airspeed_data / _indicated_airspeed_data; - _ekf->setAirspeedData(time, _true_airspeed_data, eas2tas); + airspeedSample airspeed_sample; + airspeed_sample.time_us = time; + airspeed_sample.eas2tas = _true_airspeed_data / _indicated_airspeed_data; + airspeed_sample.true_airspeed = _true_airspeed_data; + _ekf->setAirspeedData(airspeed_sample); } }