Expand test framework and test cases (#685)

* Fix comment

* Ekf wrapper for testing

Add utility function for accessing information in the ekf object

* Add step function for Gps sensor

* Add RangeFinder and Flow to simulated sensors

* Add first fusion logic tests

* Add units to function name

* Use EXPECT_TRUE

* Adding missing qualifiers

* Improve EXPECT_ calls

* Improve naming

EKF/common.h

@@ -76,7 +76,7 @@ struct flow_message {
 	uint8_t quality;	///< Quality of Flow data
 	Vector2f flowdata;	///< Optical flow rates about the X and Y body axes (rad/sec)
 	Vector3f gyrodata;	///< Gyro rates about the XYZ body axes (rad/sec)
-	uint32_t dt;		///< integration time of flow samples (sec)
+	uint32_t dt;		///< integration time of flow samples (microseconds)
 };
 
 struct ext_vision_message {

EKF/control.cpp

@@ -401,6 +401,7 @@ void Ekf::controlExternalVisionFusion()
 
 void Ekf::controlOpticalFlowFusion()
 {
+	// TODO: These motion checks run all the time. Pull them out of this function
 	// Check if on ground motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
 		// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation

EKF/estimator_interface.cpp

@@ -344,7 +344,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua
 	}
 }
 
-// set optical flow data
+// TODO: Change pointer to constant reference
 void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
 {
 	if (!_initialised || _flow_buffer_fail) {
@@ -366,7 +366,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 	if ((time_usec - _time_last_optflow) > _min_obs_interval_us) {
 		// check if enough integration time and fail if integration time is less than 50%
 		// of min arrival interval because too much data is being lost
-		float delta_time = 1e-6f * (float)flow->dt;
+		float delta_time = 1e-6f * (float)flow->dt; // in seconds
 		float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
 		bool delta_time_good = delta_time >= delta_time_min;
 
@@ -407,7 +407,6 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			optflow_sample_new.gyroXYZ = - flow->gyrodata;
 			optflow_sample_new.flowRadXY = -flow->flowdata;
 
-			// convert integration interval to seconds
 			optflow_sample_new.dt = delta_time;
 			_time_last_optflow = time_usec;
 

test/CMakeLists.txt

@@ -41,6 +41,7 @@ set(SRCS
 	test_EKF_ringbuffer.cpp
 	test_EKF_imuSampling.cpp
 	test_AlphaFilter.cpp
+	test_EKF_fusionLogic.cpp
    )
 add_executable(ECL_GTESTS ${SRCS})
 

test/sensor_simulator/CMakeLists.txt

@@ -34,11 +34,14 @@
 
 set(SRCS
 	sensor_simulator.cpp
+	ekf_wrapper.cpp
 	sensor.cpp
 	imu.cpp
 	mag.cpp
 	baro.cpp
 	gps.cpp
+	flow.cpp
+	range_finder.cpp
    )
 
 add_library(ecl_sensor_sim ${SRCS})

test/sensor_simulator/baro.h

@@ -60,4 +60,4 @@ private:
 };
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/ekf_wrapper.cpp

@@ -0,0 +1,71 @@
+#include "ekf_wrapper.h"
+
+EkfWrapper::EkfWrapper(std::shared_ptr<Ekf> ekf):
+_ekf{ekf}
+{
+	_ekf_params = _ekf->getParamHandle();
+}
+
+EkfWrapper::~EkfWrapper()
+{
+}
+
+void EkfWrapper::enableGpsFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_GPS;
+}
+
+void EkfWrapper::disableGpsFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_GPS;
+}
+
+bool EkfWrapper::isIntendingGpsFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.gps;
+}
+
+void EkfWrapper::enableFlowFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_OF;
+}
+
+void EkfWrapper::disableFlowFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_OF;
+}
+
+bool EkfWrapper::isIntendingFlowFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.opt_flow;
+}
+
+Vector3f EkfWrapper::getPosition() const
+{
+	float temp[3];
+	_ekf->get_position(temp);
+	return Vector3f(temp);
+}
+Vector3f EkfWrapper::getVelocity() const
+{
+	float temp[3];
+	_ekf->get_velocity(temp);
+	return Vector3f(temp);
+}
+Vector3f EkfWrapper::getAccelBias() const
+{
+	float temp[3];
+	_ekf->get_accel_bias(temp);
+	return Vector3f(temp);
+}
+
+Vector3f EkfWrapper::getGyroBias() const
+{
+	float temp[3];
+	_ekf->get_gyro_bias(temp);
+	return Vector3f(temp);
+}

test/sensor_simulator/ekf_wrapper.h

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Wrapper class for Ekf object to set behavior or check status
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include <memory>
+#include "EKF/ekf.h"
+#include "EKF/estimator_interface.h"
+
+class EkfWrapper
+{
+public:
+	EkfWrapper(std::shared_ptr<Ekf> ekf);
+	~EkfWrapper();
+
+	void enableGpsFusion();
+
+	void disableGpsFusion();
+
+	bool isIntendingGpsFusion() const;
+
+	void enableFlowFusion();
+
+	void disableFlowFusion();
+
+	bool isIntendingFlowFusion() const;
+
+	Vector3f getPosition() const;
+	Vector3f getVelocity() const;
+	Vector3f getAccelBias() const;
+	Vector3f getGyroBias() const;
+
+
+private:
+	std::shared_ptr<Ekf> _ekf;
+
+	// Pointer to Ekf internal param struct
+	parameters* _ekf_params;
+
+};

test/sensor_simulator/flow.cpp

@@ -0,0 +1,38 @@
+#include "flow.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+Flow::Flow(std::shared_ptr<Ekf> ekf):Sensor(ekf)
+{
+}
+
+Flow::~Flow()
+{
+}
+
+void Flow::send(uint32_t time)
+{
+	_ekf->setOpticalFlowData(time, &_flow_data);
+}
+
+void Flow::setData(const flow_message& flow)
+{
+	_flow_data = flow;
+
+}
+
+flow_message Flow::dataAtRest()
+{
+	flow_message _flow_at_rest;
+	_flow_at_rest.dt = 20000;
+	_flow_at_rest.flowdata = Vector2f{0.0f, 0.0f};
+	_flow_at_rest.gyrodata = Vector3f{0.0f, 0.0f, 0.0f};
+	_flow_at_rest.quality = 255;
+	return _flow_at_rest;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

test/sensor_simulator/flow.h

@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with optical flow data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class Flow: public Sensor
+{
+public:
+	Flow(std::shared_ptr<Ekf> ekf);
+	~Flow();
+
+	void setData(const flow_message& flow);
+	flow_message dataAtRest();
+
+private:
+	flow_message _flow_data;
+
+	void send(uint32_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator

test/sensor_simulator/gps.cpp

@@ -24,5 +24,48 @@ void Gps::setData(const gps_message& gps)
 	_gps_data = gps;
 }
 
+void Gps::stepHeightByMeters(float hgt_change)
+{
+	_gps_data.alt += hgt_change * 1e3f;
+}
+
+void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
+{
+	float hposN_curr;
+	float hposE_curr;
+	map_projection_global_project((float)_gps_data.lat, (float)_gps_data.lon, &hposN_curr, &hposE_curr);
+	Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
+	double lat_new;
+	double lon_new;
+	map_projection_global_reproject(hpos_new(0), hpos_new(1), &lat_new, &lon_new);
+	_gps_data.lon = (uint32_t)lon_new;
+	_gps_data.lat = (uint32_t)lat_new;
+}
+
+
+gps_message Gps::getDefaultGpsData()
+{
+	gps_message gps_data{};
+	gps_data.time_usec = 0;
+	gps_data.lat = 473566094;
+	gps_data.lon = 85190237;
+	gps_data.alt = 422056;
+	gps_data.yaw = 0.0f;
+	gps_data.yaw_offset = 0.0f;
+	gps_data.fix_type = 3;
+	gps_data.eph = 0.5f;
+	gps_data.epv = 0.8f;
+	gps_data.sacc = 0.2f;
+	gps_data.vel_m_s = 0.0;
+	gps_data.vel_ned[0] = 0.0f;
+	gps_data.vel_ned[1] = 0.0f;
+	gps_data.vel_ned[2] = 0.0f;
+	gps_data.vel_ned_valid = 1;
+	gps_data.nsats = 16;
+	gps_data.pdop = 0.0f;
+
+	return gps_data;
+}
+
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/gps.h

@@ -51,6 +51,10 @@ public:
 	~Gps();
 
 	void setData(const gps_message& gps);
+	void stepHeightByMeters(float hgt_change);
+	void stepHorizontalPositionByMeters(Vector2f hpos_change);
+
+	gps_message getDefaultGpsData();
 
 private:
 	gps_message _gps_data;
@@ -60,4 +64,4 @@ private:
 };
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/imu.cpp

@@ -15,7 +15,6 @@ Imu::~Imu()
 
 void Imu::send(uint32_t time)
 {
-	// fill imu sample with stored data
 	imuSample imu_sample;
 	imu_sample.time_us = time;
 	imu_sample.delta_ang_dt = (time - _time_last_data_sent) * 1.e-6f;
@@ -43,4 +42,4 @@ void Imu::setGyroData(const Vector3f& gyro)
 }
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/imu.h

@@ -63,4 +63,4 @@ private:
 };
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/mag.cpp

@@ -26,4 +26,4 @@ void Mag::setData(const Vector3f& mag)
 }
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/mag.h

@@ -60,4 +60,4 @@ private:
 };
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator

test/sensor_simulator/range_finder.cpp

@@ -0,0 +1,28 @@
+#include "range_finder.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+RangeFinder::RangeFinder(std::shared_ptr<Ekf> ekf):Sensor(ekf)
+{
+}
+
+RangeFinder::~RangeFinder()
+{
+}
+
+void RangeFinder::send(uint32_t time)
+{
+	_ekf->setRangeData(time, _range_data, _range_quality);
+}
+
+void RangeFinder::setData(float range_data_meters, int8_t range_quality)
+{
+	_range_data = range_data_meters;
+	_range_quality = range_quality;
+}
+
+} // namespace sensor
+} // namespace sensor_simulator

test/sensor_simulator/range_finder.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with range finder data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include "sensor.h"
+
+namespace sensor_simulator
+{
+namespace sensor
+{
+
+class RangeFinder: public Sensor
+{
+public:
+	RangeFinder(std::shared_ptr<Ekf> ekf);
+	~RangeFinder();
+
+	void setData(float range_data, int8_t range_quality);
+	flow_message dataAtRest();
+
+private:
+	float _range_data;
+	int8_t _range_quality;
+
+	void send(uint32_t time) override;
+
+};
+
+} // namespace sensor
+} // namespace sensor_simulator

test/sensor_simulator/sensor.h

@@ -54,7 +54,7 @@ public:
 
 	void update(uint32_t time);
 
-	void setRate(uint32_t rate){ _update_period = uint32_t(1000000)/rate; }
+	void setRateHz(uint32_t rate){ _update_period = uint32_t(1000000)/rate; }
 
 	bool isRunning() const { return _is_running; }
 

test/sensor_simulator/sensor_simulator.cpp

@@ -3,29 +3,16 @@
 
 SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
 _ekf{ekf},
-_imu{ekf},
-_mag{ekf},
-_baro{ekf},
-_gps{ekf}
+_imu(ekf),
+_mag(ekf),
+_baro(ekf),
+_gps(ekf),
+_flow(ekf),
+_rng(ekf)
 {
-
-	// set default sensor rate in Hz
-	_imu.setRate(250);
-	_mag.setRate(80);
-	_baro.setRate(80);
-	_gps.setRate(5);
-
-	// set default sensor data
-	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
-		     Vector3f{0.0f,0.0f,0.0f});
-	_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
-	_baro.setData(122.2f);
-	_gps.setData(getDefaultGpsData());
-
-	// start default sensor
-	_imu.start();
-	_mag.start();
-	_baro.start();
+	setSensorDataToDefault();
+	setSensorRateToDefault();
+	startBasicSensor();
 }
 
 SensorSimulator::~SensorSimulator()
@@ -33,40 +20,41 @@ SensorSimulator::~SensorSimulator()
 
 }
 
-gps_message SensorSimulator::getDefaultGpsData()
+void SensorSimulator::setSensorDataToDefault()
 {
-	// setup gps message to reasonable default values
-	gps_message gps_data{};
-	gps_data.time_usec = 0;
-	gps_data.lat = 473566094;
-	gps_data.lon = 85190237;
-	gps_data.alt = 422056;
-	gps_data.yaw = 0.0f;
-	gps_data.yaw_offset = 0.0f;
-	gps_data.fix_type = 3;
-	gps_data.eph = 0.5f;
-	gps_data.epv = 0.8f;
-	gps_data.sacc = 0.2f;
-	gps_data.vel_m_s = 0.0;
-	gps_data.vel_ned[0] = 0.0f;
-	gps_data.vel_ned[1] = 0.0f;
-	gps_data.vel_ned[2] = 0.0f;
-	gps_data.vel_ned_valid = 1;
-	gps_data.nsats = 16;
-	gps_data.pdop = 0.0f;
-
-	return gps_data;
+	_imu.setRateHz(250);
+	_mag.setRateHz(80);
+	_baro.setRateHz(80);
+	_gps.setRateHz(5);
+	_flow.setRateHz(50);
+	_rng.setRateHz(30);
+}
+void SensorSimulator::setSensorRateToDefault()
+{
+	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
+		     Vector3f{0.0f,0.0f,0.0f});
+	_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
+	_baro.setData(122.2f);
+	_gps.setData(_gps.getDefaultGpsData());
+	_flow.setData(_flow.dataAtRest());
+	_rng.setData(0.2f, 100);
+}
+void SensorSimulator::startBasicSensor()
+{
+	_imu.start();
+	_mag.start();
+	_baro.start();
 }
 
-void SensorSimulator::run_seconds(float duration_seconds)
+void SensorSimulator::runSeconds(float duration_seconds)
 {
-	run_microseconds( uint32_t(duration_seconds * 1e6f) );
+	runMicroseconds( uint32_t(duration_seconds * 1e6f) );
 }
 
-void SensorSimulator::run_microseconds(uint32_t duration)
+void SensorSimulator::runMicroseconds(uint32_t duration)
 {
 	// simulate in 1000us steps
-	uint32_t start_time = _time;
+	const uint32_t start_time = _time;
 
 	for(;_time < start_time + duration; _time+=1000)
 	{
@@ -74,6 +62,8 @@ void SensorSimulator::run_microseconds(uint32_t duration)
 		_mag.update(_time);
 		_baro.update(_time);
 		_gps.update(_time);
+		_flow.update(_time);
+		_rng.update(_time);
 
 		_ekf->update();
 	}

test/sensor_simulator/sensor_simulator.h

@@ -48,39 +48,47 @@
 #include "mag.h"
 #include "baro.h"
 #include "gps.h"
+#include "flow.h"
+#include "range_finder.h"
 #include "EKF/ekf.h"
 
 using namespace sensor_simulator::sensor;
 
 class SensorSimulator
 {
+
+private:
+	std::shared_ptr<Ekf> _ekf;
+
+	uint32_t _time {0};	// in microseconds
+
+	void setSensorDataToDefault();
+	void setSensorRateToDefault();
+	void startBasicSensor();
+
 public:
 	SensorSimulator(std::shared_ptr<Ekf> ekf);
 	~SensorSimulator();
 
-	void setImuRate(uint32_t rate){ _imu.setRate(rate); }
-	void setMagRate(uint32_t rate){ _mag.setRate(rate); }
-	void setBaroRate(uint32_t rate){ _baro.setRate(rate); }
-	void setGpsRate(uint32_t rate){ _gps.setRate(rate); }
-
-	void run_seconds(float duration_seconds);
-	void run_microseconds(uint32_t duration);
+	void runSeconds(float duration_seconds);
+	void runMicroseconds(uint32_t duration);
 
 	void startGps(){ _gps.start(); }
 	void stopGps(){ _gps.stop(); }
 
-	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
+	void startFlow(){ _flow.start(); }
+	void stopFlow(){ _flow.stop(); }
 
-private:
-	std::shared_ptr<Ekf> _ekf;
+	void startRangeFinder(){ _rng.start(); }
+	void stopRangeFinder(){ _rng.stop(); }
+
+	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
 
 	Imu _imu;
 	Mag _mag;
 	Baro _baro;
 	Gps _gps;
-
-	uint32_t _time {0};	// in microseconds
-
-	gps_message getDefaultGpsData();
+	Flow _flow;
+	RangeFinder _rng;
 
 };

test/test_EKF_basics.cpp

@@ -36,16 +36,19 @@
 #include <memory>
 #include "EKF/ekf.h"
 #include "sensor_simulator/sensor_simulator.h"
-
+#include "sensor_simulator/ekf_wrapper.h"
 
 class EkfInitializationTest : public ::testing::Test {
  public:
 	EkfInitializationTest(): ::testing::Test(),
 	_ekf{std::make_shared<Ekf>()},
-	_sensor_simulator(_ekf) {};
+	_sensor_simulator(_ekf),
+	_ekf_wrapper(_ekf) {};
 
 	std::shared_ptr<Ekf> _ekf;
 	SensorSimulator _sensor_simulator;
+	EkfWrapper _ekf_wrapper;
+
 
 	// Duration of initalization with only providing baro,mag and IMU
 	const uint32_t _init_duration_s{2};
@@ -54,7 +57,7 @@ class EkfInitializationTest : public ::testing::Test {
 	void SetUp() override
 	{
 		_ekf->init(0);
-		_sensor_simulator.run_seconds(_init_duration_s);
+		_sensor_simulator.runSeconds(_init_duration_s);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -68,7 +71,7 @@ TEST_F(EkfInitializationTest, tiltAlign)
 {
 	// GIVEN: reasonable static sensor data for some duration
 	// THEN: EKF should tilt align
-	EXPECT_EQ(true,_ekf->attitude_valid());
+	EXPECT_TRUE(_ekf->attitude_valid());
 }
 
 TEST_F(EkfInitializationTest, initialControlMode)
@@ -108,25 +111,19 @@ TEST_F(EkfInitializationTest, initialControlMode)
 TEST_F(EkfInitializationTest, convergesToZero)
 {
 	// GIVEN: initialized EKF with default IMU, baro and mag input
-	_sensor_simulator.run_seconds(4);
+	_sensor_simulator.runSeconds(4);
 
-	float converged_pos[3];
-	float converged_vel[3];
-	float converged_accel_bias[3];
-	float converged_gyro_bias[3];
-	_ekf->get_position(converged_pos);
-	_ekf->get_velocity(converged_vel);
-	_ekf->get_accel_bias(converged_accel_bias);
-	_ekf->get_gyro_bias(converged_gyro_bias);
+	Vector3f pos = _ekf_wrapper.getPosition();
+	Vector3f vel = _ekf_wrapper.getVelocity();
+	Vector3f accel_bias = _ekf_wrapper.getAccelBias();
+	Vector3f gyro_bias = _ekf_wrapper.getGyroBias();
+	Vector3f ref{0.0f, 0.0f, 0.0f};
 
 	// THEN: EKF should stay or converge to zero
-	for(int i=0; i<3; ++i)
-	{
-		EXPECT_NEAR(0.0f,converged_pos[i],0.001f);
-		EXPECT_NEAR(0.0f,converged_vel[i],0.001f);
-		EXPECT_NEAR(0.0f,converged_accel_bias[i],0.001f);
-		EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f);
-	}
+	EXPECT_TRUE(matrix::isEqual(pos, ref, 0.001f));
+	EXPECT_TRUE(matrix::isEqual(vel, ref, 0.001f));
+	EXPECT_TRUE(matrix::isEqual(accel_bias, ref, 0.001f));
+	EXPECT_TRUE(matrix::isEqual(gyro_bias, ref, 0.001f));
 }
 
 TEST_F(EkfInitializationTest, gpsFusion)
@@ -135,7 +132,7 @@ TEST_F(EkfInitializationTest, gpsFusion)
 	// WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec
 
 	_sensor_simulator.startGps();
-	_sensor_simulator.run_seconds(11);
+	_sensor_simulator.runSeconds(11);
 
 	// THEN: EKF should fuse GPS, but no other position sensor
 	filter_control_status_u control_status;
@@ -171,29 +168,23 @@ TEST_F(EkfInitializationTest, accleBiasEstimation)
 {
 	// GIVEN: initialized EKF with default IMU, baro and mag input for 2s
 	// WHEN: Added more sensor measurements with accel bias and gps measurements
-	Vector3f accel_bias = {0.0f,0.0f,0.1f};
+	Vector3f accel_bias_sim = {0.0f,0.0f,0.1f};
 
 	_sensor_simulator.startGps();
-	_sensor_simulator.setImuBias(accel_bias, Vector3f{0.0f,0.0f,0.0f});
-	_sensor_simulator.run_seconds(10);
+	_sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f});
+	_sensor_simulator.runSeconds(10);
 
-	float converged_pos[3];
-	float converged_vel[3];
-	float converged_accel_bias[3];
-	float converged_gyro_bias[3];
-	_ekf->get_position(converged_pos);
-	_ekf->get_velocity(converged_vel);
-	_ekf->get_accel_bias(converged_accel_bias);
-	_ekf->get_gyro_bias(converged_gyro_bias);
+	Vector3f pos = _ekf_wrapper.getPosition();
+	Vector3f vel = _ekf_wrapper.getVelocity();
+	Vector3f accel_bias = _ekf_wrapper.getAccelBias();
+	Vector3f gyro_bias = _ekf_wrapper.getGyroBias();
+	Vector3f zero{0.0f, 0.0f, 0.0f};
 
-	// THEN: EKF should estimate bias correctelly
-	for(int i=0; i<3; ++i)
-	{
-		EXPECT_NEAR(0.0f,converged_pos[i],0.001f) << "i: " << i;
-		EXPECT_NEAR(0.0f,converged_vel[i],0.001f) << "i: " << i;
-		EXPECT_NEAR(accel_bias(i),converged_accel_bias[i],0.001f) << "i: " << i;
-		EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f) << "i: " << i;
-	}
+	// THEN: EKF should stay or converge to zero
+	EXPECT_TRUE(matrix::isEqual(pos, zero, 0.001f));
+	EXPECT_TRUE(matrix::isEqual(vel, zero, 0.001f));
+	EXPECT_TRUE(matrix::isEqual(accel_bias, accel_bias_sim, 0.001f));
+	EXPECT_TRUE(matrix::isEqual(gyro_bias, zero, 0.001f));
 }
 
 // TODO: Add sampling tests

test/test_EKF_fusionLogic.cpp

@@ -0,0 +1,185 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2019 ECL Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Feeds Ekf with Imu data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "sensor_simulator/sensor_simulator.h"
+#include "sensor_simulator/ekf_wrapper.h"
+
+
+class EkfFusionLogicTest : public ::testing::Test {
+ public:
+
+	EkfFusionLogicTest(): ::testing::Test(),
+	_ekf{std::make_shared<Ekf>()},
+	_sensor_simulator(_ekf),
+	_ekf_wrapper(_ekf) {};
+
+	std::shared_ptr<Ekf> _ekf;
+	SensorSimulator _sensor_simulator;
+	EkfWrapper _ekf_wrapper;
+
+	// Setup the Ekf with synthetic measurements
+	void SetUp() override
+	{
+		_ekf->init(0);
+		_sensor_simulator.runSeconds(5);
+	}
+
+	// Use this method to clean up any memory, network etc. after each test
+	void TearDown() override
+	{
+	}
+};
+
+
+TEST_F(EkfFusionLogicTest, doGpsFusion)
+{
+	// GIVEN: a tilt and heading aligned filter
+	// WHEN: we enable GPS fusion and we send good quality gps data for 11s
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(15);
+
+	// THEN: EKF should intend to fuse GPS
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	// THEN: Local and global position should be valid
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+
+	// WHEN: GPS data is not send for 11s
+	_sensor_simulator.stopGps();
+	_sensor_simulator.runSeconds(11);
+
+	// THEN: EKF should stop to intend to fuse GPS
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: GPS data is send again for 11s
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(11);
+
+	// THEN: EKF should to intend to fuse GPS
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_TRUE(_ekf->global_position_is_valid());
+
+	// // WHEN: clients decides to stop GPS fusion
+	// _ekf_wrapper.disableGpsFusion();
+	// // THEN: EKF should stop to intend to fuse GPS immediately
+	// _sensor_simulator.runMicroseconds(1000);
+	// EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+	// THIS is not happening at the moment
+}
+
+TEST_F(EkfFusionLogicTest, rejectGpsSignalJump)
+{
+	// GIVEN: a tilt and heading aligned filter
+	// WHEN: we enable GPS fusion and we send good quality gps data for 11s
+	_ekf_wrapper.enableGpsFusion();
+	_sensor_simulator.startGps();
+	_sensor_simulator.runSeconds(15);
+
+	// THEN: EKF should intend to fuse GPS
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+
+	// WHEN: Having a big horizontal position Gps jump coming from the Gps Receiver
+	Vector3f pos_old = _ekf_wrapper.getPosition();
+	Vector3f vel_old = _ekf_wrapper.getVelocity();
+	Vector3f accel_bias_old = _ekf_wrapper.getAccelBias();
+	_sensor_simulator._gps.stepHorizontalPositionByMeters(Vector2f{10.0f, 0.0f});
+	_sensor_simulator.runSeconds(2);
+
+	// THEN: The estimate should not change much in the short run
+	//       and GPS fusion should be stopped after a while.
+	Vector3f pos_new = _ekf_wrapper.getPosition();
+	Vector3f vel_new = _ekf_wrapper.getVelocity();
+	Vector3f accel_bias_new = _ekf_wrapper.getAccelBias();
+	EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f));
+	EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f));
+	EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f));
+
+	_sensor_simulator.runSeconds(10);
+	pos_new = _ekf_wrapper.getPosition();
+	vel_new = _ekf_wrapper.getVelocity();
+	accel_bias_new = _ekf_wrapper.getAccelBias();
+	EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f));
+	EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f));
+	EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f));
+	// EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); // What do we expect here?
+}
+
+TEST_F(EkfFusionLogicTest, doFlowFusion)
+{
+	// GIVEN: a tilt and heading aligned filter
+	// WHEN: sending flow data without having the flow fusion enabled
+	//       flow measurement fusion should not be intended.
+	_sensor_simulator.startFlow();
+	_sensor_simulator.runSeconds(3);
+
+	// THEN: EKF should intend to fuse flow measurements
+	EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
+	// THEN: Local and global position should not be valid
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: Flow data is not send and we enable flow fusion
+	_sensor_simulator.stopFlow();
+	_sensor_simulator.runSeconds(3); // TODO: without this line tests fail
+	// probably there are still values in the buffer.
+	_ekf_wrapper.enableFlowFusion();
+	_sensor_simulator.runSeconds(3);
+
+	// THEN: EKF should not intend to fuse flow
+	EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion());
+	// THEN: Local and global position should not be valid
+	EXPECT_FALSE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+
+	// WHEN: Flow data is  send and we enable flow fusion
+	_sensor_simulator.startFlow();
+	_ekf_wrapper.enableFlowFusion();
+	_sensor_simulator.runSeconds(10);
+
+	// THEN: EKF should intend to fuse flow
+	EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
+	// THEN: Local and global position should be valid
+	EXPECT_TRUE(_ekf->local_position_is_valid());
+	EXPECT_FALSE(_ekf->global_position_is_valid());
+}