From 4fb2d1b02e1592a8744d9043c3cec24c19ef8f49 Mon Sep 17 00:00:00 2001
From: kamilritz <kritz@ethz.ch>
Date: Tue, 21 Jan 2020 14:49:49 +0100
Subject: [PATCH] Update range finder interface

---
 EKF/estimator_interface.cpp            | 12 ++++++------
 EKF/estimator_interface.h              |  2 +-
 test/sensor_simulator/range_finder.cpp |  7 ++++---
 test/sensor_simulator/range_finder.h   |  3 +--
 4 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp
index 7b8d21418f..3399ee47da 100644
--- a/EKF/estimator_interface.cpp
+++ b/EKF/estimator_interface.cpp
@@ -320,7 +320,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 	}
 }
 
-void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t quality)
+void EstimatorInterface::setRangeData(const rangeSample& range_sample)
 {
 	if (!_initialised || _range_buffer_fail) {
 		return;
@@ -338,12 +338,12 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua
 	}
 
 	// limit data rate to prevent data being lost
-	if ((time_usec - _time_last_range) > _min_obs_interval_us) {
+	if ((range_sample.time_us - _time_last_range) > _min_obs_interval_us) {
+		_time_last_range = range_sample.time_us;
+
 		rangeSample range_sample_new;
-		range_sample_new.rng = data;
-		range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
-		range_sample_new.quality = quality;
-		_time_last_range = time_usec;
+		range_sample_new.time_us -= _params.range_delay_ms * 1000;
+		range_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
 
 		_range_buffer.push(range_sample_new);
 	}
diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h
index e548b96ccf..a88cc24c03 100644
--- a/EKF/estimator_interface.h
+++ b/EKF/estimator_interface.h
@@ -185,7 +185,7 @@ public:
 
 	void setAirspeedData(const airspeedSample &airspeed_sample);
 
-	void setRangeData(uint64_t time_usec, float data, int8_t quality);
+	void setRangeData(const rangeSample& range_sample);
 
 	// if optical flow sensor gyro delta angles are not available, set gyroXYZ vector fields to NaN and the EKF will use its internal delta angle data instead
 	void setOpticalFlowData(uint64_t time_usec, flow_message *flow);
diff --git a/test/sensor_simulator/range_finder.cpp b/test/sensor_simulator/range_finder.cpp
index 130f43c6ca..07eb8ca128 100644
--- a/test/sensor_simulator/range_finder.cpp
+++ b/test/sensor_simulator/range_finder.cpp
@@ -15,13 +15,14 @@ RangeFinder::~RangeFinder()
 
 void RangeFinder::send(uint64_t time)
 {
-	_ekf->setRangeData(time, _range_data, _range_quality);
+	_range_sample.time_us = time;
+	_ekf->setRangeData(_range_sample);
 }
 
 void RangeFinder::setData(float range_data_meters, int8_t range_quality)
 {
-	_range_data = range_data_meters;
-	_range_quality = range_quality;
+	_range_sample.rng = range_data_meters;
+	_range_sample.quality = range_quality;
 }
 
 } // namespace sensor
diff --git a/test/sensor_simulator/range_finder.h b/test/sensor_simulator/range_finder.h
index 63d9c44d05..a53dc04caa 100644
--- a/test/sensor_simulator/range_finder.h
+++ b/test/sensor_simulator/range_finder.h
@@ -54,8 +54,7 @@ public:
 	flow_message dataAtRest();
 
 private:
-	float _range_data{0.0f};
-	int8_t _range_quality;
+	rangeSample _range_sample {};
 
 	void send(uint64_t time) override;