ECL Unit Test Clean Up

test/CMakeLists.txt

@@ -33,8 +33,7 @@
 
 include(gtest.cmake)
 
-add_library(SENSOR_SIMULATOR SensorSimulator.cpp)
-target_link_libraries(SENSOR_SIMULATOR ecl_EKF)
+add_subdirectory(SensorSimulator)
 
 set(SRCS
 	main.cpp
@@ -42,7 +41,6 @@ set(SRCS
 	test_EKF_ringbuffer.cpp
 	test_EKF_imuSampling.cpp
 	test_AlphaFilter.cpp
-	test_SensorSimulator.cpp
    )
 add_executable(ECL_GTESTS ${SRCS})
 

test/SensorSimulator.cpp

@@ -1,89 +0,0 @@
-#include "SensorSimulator.h"
-
-namespace simulator
-{
-
-SensorSimulator::SensorSimulator(Ekf* ekf)
-{
-	_ekf = ekf;
-	setGpsMessageToDefaul();
-}
-
-SensorSimulator::~SensorSimulator()
-{
-
-}
-
-void SensorSimulator::setGpsMessageToDefaul()
-{
-	// setup gps message to reasonable default values
-	_gps_message.time_usec = 0;
-	_gps_message.lat = 473566094;
-	_gps_message.lon = 85190237;
-	_gps_message.alt = 422056;
-	_gps_message.yaw = 0.0f;
-	_gps_message.yaw_offset = 0.0f;
-	_gps_message.fix_type = 3;
-	_gps_message.eph = 0.5f;
-	_gps_message.epv = 0.8f;
-	_gps_message.sacc = 0.2f;
-	_gps_message.vel_m_s = 0.0;
-	_gps_message.vel_ned[0] = 0.0f;
-	_gps_message.vel_ned[1] = 0.0f;
-	_gps_message.vel_ned[2] = 0.0f;
-	_gps_message.vel_ned_valid = 1;
-	_gps_message.nsats = 16;
-	_gps_message.pdop = 0.0f;
-}
-
-void SensorSimulator::update_with_const_sensors(uint32_t duration_us,
-		Vector3f ang_vel, Vector3f accel,
-		Vector3f mag_data, float baro_data)
-{
-	// store start time
-	uint32_t start_time_us = _t_us;
-
-	// compute update time step such that we can update the basic sensor at different rates
-	_update_dt_us = gcd(_imu_dt_us, gcd(_mag_dt_us, gcd(_baro_dt_us,_gps_dt_us)));
-
-	// update EKF with synthetic sensor measurements
-	for( ; _t_us < start_time_us+duration_us; _t_us += _update_dt_us)
-	{
-		// Check which sensors update we should do
-		if(_fuse_imu && !(_t_us %_imu_dt_us))
-		{
-			// push imu data into estimator
-			imuSample imu_sample_new;
-			imu_sample_new.time_us = _t_us;
-			imu_sample_new.delta_ang_dt = _imu_dt_us * 1.e-6f;
-			imu_sample_new.delta_ang = ang_vel * imu_sample_new.delta_ang_dt;
-			imu_sample_new.delta_vel_dt = _imu_dt_us * 1.e-6f;
-			imu_sample_new.delta_vel = accel * imu_sample_new.delta_vel_dt;
-
-			_ekf->setIMUData(imu_sample_new);
-			_counter_imu++;
-		}
-		if(_fuse_baro && !(_t_us % _baro_dt_us))
-		{
-			_ekf->setBaroData(_t_us,baro_data);
-			_counter_baro++;
-		}
-		if(_fuse_mag && !(_t_us % _mag_dt_us))
-		{
-			float mag[3];
-			mag_data.copyTo(mag);
-			_ekf->setMagData(_t_us,mag);
-			_counter_mag++;
-		}
-		if(_fuse_gps && !(_t_us % _gps_dt_us))
-		{
-			_gps_message.time_usec = _t_us;
-			_ekf->setGpsData(_t_us,_gps_message);
-			_counter_mag++;
-		}
-
-		_ekf->update();
-	}
-}
-
-} // end of namespace simulator

test/SensorSimulator/Baro.cpp

@@ -0,0 +1,20 @@
+#include "Baro.h"
+
+Baro::Baro(Ekf* ekf):Sensor(ekf)
+{
+}
+
+Baro::~Baro()
+{
+}
+
+void Baro::send(uint32_t time)
+{
+	_ekf->setBaroData(time,_baro_data);
+	_time_last_data_sent = time;
+}
+
+void Baro::setData(float baro)
+{
+	_baro_data = baro;
+}

test/SensorSimulator/Baro.h

@@ -31,38 +31,26 @@
  *
  ****************************************************************************/
 
-#include <gtest/gtest.h>
-#include <math.h>
-#include "SensorSimulator.h"
-#include "EKF/ekf.h"
+/**
+ * Feeds Ekf with Mag data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
 
-using namespace simulator;
+#include "Sensor.h"
 
+class Baro: public Sensor
+{
+public:
+	Baro(Ekf* ekf);
+	~Baro();
 
-class SensorSimulatorTest : public ::testing::Test {
- public:
+	void setData(float baro);
 
-	// Ekf* _ekf;
-	// SensorSimulator* _sensor_simulator;
+private:
+	float _baro_data;
 
-	void SetUp() override
-	{
-		// _ekf = new Ekf();
-		// _ekf->init(0);
-	}
+	void send(uint32_t time);
 
-	void TearDown() override
-	{
-		// delete _sensor_simulator;
-		// delete _ekf;
-	}
 };
 
-TEST_F(SensorSimulatorTest, greatestCommonDivider)
-{
-	// Test gcd function with some example
-	EXPECT_EQ(gcd(uint32_t(3), uint32_t(402)), uint32_t(3));
-	EXPECT_EQ(gcd(uint32_t(3), uint32_t(401)), uint32_t(1));
-
-
-}

test/SensorSimulator/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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 ECL 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.
+#
+############################################################################
+
+
+set(SRCS
+	SensorSimulator.cpp
+	Sensor.cpp
+	Imu.cpp
+	Mag.cpp
+	Baro.cpp
+	Gps.cpp
+   )
+
+add_library(SENSOR_SIMULATOR ${SRCS})
+target_link_libraries(SENSOR_SIMULATOR ecl_EKF)

test/SensorSimulator/Gps.cpp

@@ -0,0 +1,20 @@
+#include "Gps.h"
+
+Gps::Gps(Ekf* ekf):Sensor(ekf)
+{
+}
+
+Gps::~Gps()
+{
+}
+
+void Gps::send(uint32_t time)
+{
+	_gps_data.time_usec = time;
+	_ekf->setGpsData(time, _gps_data);
+}
+
+void Gps::setData(gps_message gps)
+{
+	_gps_data = gps;
+}

test/SensorSimulator/Gps.h

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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 Gps data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include "Sensor.h"
+
+class Gps: public Sensor
+{
+public:
+	Gps(Ekf* ekf);
+	~Gps();
+
+	void setData(gps_message gps);
+
+private:
+	gps_message _gps_data;
+
+	void send(uint32_t time);
+
+};
+

test/SensorSimulator/Imu.cpp

@@ -0,0 +1,39 @@
+#include "Imu.h"
+
+Imu::Imu(Ekf* ekf):Sensor(ekf)
+{
+}
+
+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;
+	imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;
+	imu_sample.delta_vel_dt = (time - _time_last_data_sent) * 1.e-6f;
+	imu_sample.delta_vel = _accel_data * imu_sample.delta_vel_dt;
+
+	_ekf->setIMUData(imu_sample);
+	_time_last_data_sent = time;
+}
+
+void Imu::setData(Vector3f accel, Vector3f gyro)
+{
+	setAccelData(accel);
+	setGyroData(gyro);
+}
+
+void Imu::setAccelData(Vector3f accel)
+{
+	_accel_data = accel;
+}
+
+void Imu::setGyroData(Vector3f gyro)
+{
+	_gyro_data = gyro;
+}

test/SensorSimulator/Imu.h

@@ -0,0 +1,59 @@
+/****************************************************************************
+ *
+ *   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>
+ */
+#pragma once
+
+#include "Sensor.h"
+
+class Imu: public Sensor
+{
+public:
+	Imu(Ekf* ekf);
+	~Imu();
+
+	void setData(Vector3f accel, Vector3f gyro);
+	void setAccelData(Vector3f accel);
+	void setGyroData(Vector3f gyro);
+
+private:
+	Vector3f _accel_data;
+	Vector3f _gyro_data;
+
+	void send(uint32_t time);
+
+};
+

test/SensorSimulator/Mag.cpp

@@ -0,0 +1,22 @@
+#include "Mag.h"
+
+Mag::Mag(Ekf* ekf):Sensor(ekf)
+{
+}
+
+Mag::~Mag()
+{
+}
+
+void Mag::send(uint32_t time)
+{
+	float mag[3];
+	_mag_data.copyTo(mag);
+	_ekf->setMagData(time,mag);
+	_time_last_data_sent = time;
+}
+
+void Mag::setData(Vector3f mag)
+{
+	_mag_data = mag;
+}

test/SensorSimulator/Mag.h

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   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 Mag data
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+#pragma once
+
+#include "Sensor.h"
+
+class Mag: public Sensor
+{
+public:
+	Mag(Ekf* ekf);
+	~Mag();
+
+	void setData(Vector3f mag);
+
+private:
+	Vector3f _mag_data;
+
+	void send(uint32_t time);
+
+};
+

test/SensorSimulator/Sensor.cpp

@@ -0,0 +1,28 @@
+#include "Sensor.h"
+
+Sensor::Sensor(Ekf* ekf)
+{
+	_ekf = ekf;
+}
+
+Sensor::~Sensor()
+{
+}
+
+void Sensor::update(uint32_t time)
+{
+	if(should_send(time))
+	{
+		send(time);
+	}
+}
+
+bool Sensor::should_send(uint32_t time)
+{
+	return _is_running && is_time_to_send(time);
+}
+
+bool Sensor::is_time_to_send(uint32_t time)
+{
+	return (time >= _time_last_data_sent) && ((time - _time_last_data_sent) >= _update_period);
+}

test/SensorSimulator/Sensor.h

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Base class for defining the interface for simulaton of a sensor
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#pragma once
+
+#include "EKF/ekf.h"
+#include <math.h>
+
+class Sensor
+{
+public:
+
+	Sensor(Ekf* ekf);
+	virtual ~Sensor();
+
+	void update(uint32_t time);
+
+	void setRate(uint32_t rate){ _update_period = uint32_t(1000000)/rate; }
+
+	bool isRunning(){ return _is_running; }
+
+	void start(){ _is_running = true; }
+
+	void stop(){ _is_running = false; }
+
+protected:
+
+	Ekf* _ekf;
+	// time in microseconds
+	uint32_t _update_period;
+	uint32_t _time_last_data_sent{0};
+
+	bool _is_running{false};
+
+	bool should_send(uint32_t time);
+
+	// Checks that the right amount time passed since last send data to fulfill rate
+	bool is_time_to_send(uint32_t time);
+
+	// call set*Data function of Ekf
+	virtual void send(uint32_t time) = 0;
+
+};

test/SensorSimulator/SensorSimulator.cpp

@@ -0,0 +1,81 @@
+#include "SensorSimulator.h"
+
+
+SensorSimulator::SensorSimulator(Ekf* ekf):
+_imu{ekf},
+_mag{ekf},
+_baro{ekf},
+_gps{ekf}
+{
+	_ekf = 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();
+}
+
+SensorSimulator::~SensorSimulator()
+{
+
+}
+
+gps_message SensorSimulator::getDefaultGpsData()
+{
+	// 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;
+}
+
+void SensorSimulator::run(uint32_t duration)
+{
+	// simulate in 1000us steps
+	uint32_t start_time = _time;
+
+	for(;_time < start_time + duration; _time+=1000)
+	{
+		_imu.update(_time);
+		_mag.update(_time);
+		_baro.update(_time);
+		_gps.update(_time);
+
+		_ekf->update();
+	}
+}
+
+void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
+{
+	_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,
+		     Vector3f{0.0f,0.0f,0.0f} + gyro_bias);
+}

test/SensorSimulator/SensorSimulator.h

@@ -42,73 +42,41 @@
 
 #pragma once
 
+#include "Imu.h"
+#include "Mag.h"
+#include "Baro.h"
+#include "Gps.h"
 #include "EKF/ekf.h"
-#include <math.h>
 
 
-namespace simulator
-{
-
 class SensorSimulator
 {
 public:
 	SensorSimulator(Ekf* ekf);
 	~SensorSimulator();
 
-	void update_with_const_sensors(uint32_t duration_us,
-		Vector3f ang_vel = Vector3f{0.0f,0.0f,0.0f},
-		Vector3f accel = Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
-		Vector3f mag_data = Vector3f{0.2f, 0.0f, 0.4f},
-		float baro_data = 122.2f);
+	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 setGpsFusionTrue(){ _fuse_gps = true; }
-	void setGpsFusionFalse(){ _fuse_gps = false; }
+	void run(uint32_t duration);
+
+	void startGps(){ _gps.start(); }
+	void stopGps(){ _gps.stop(); }
+
+	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
 
 private:
 	Ekf* _ekf;
 
-	// current time
-	uint32_t _t_us{0};
-
-	// Basics sensors
-	const uint32_t _imu_dt_us{4000};	// 250 Hz	Period between IMU updates
-	const uint32_t _baro_dt_us{12500};	// 80 Hz	Period between barometer updates
-	const uint32_t _mag_dt_us{12500};	// 80 Hz	Period between magnetometer updates
-	const uint32_t _gps_dt_us{200000};	// 5 Hz		Period between GPS updates
-	// const uint32_t _flow_dt_us{20000};	// 50 Hz	Period between Flow updates
-	// const uint32_t _ev_dt_us{40000};	// 25 Hz	Period between external vision updates
-
-	uint32_t _update_dt_us{};			// greatest common divider of all basic sensor periods
-
-
-	// Flags that control if a sensor is fused
-	bool _fuse_imu{true};
-	bool _fuse_baro{true};
-	bool _fuse_mag{true};
-	// Not expected to be fused from beginning
-	bool _fuse_gps{false};
-	// bool _fuse_flow{false};
-	// bool _fuse_ev{false};
-
-	gps_message _gps_message{};
-
-	// used for debugging until now, replace with tests
-	// counter of how many sensor measurement are put into Ekf
-	uint32_t _counter_imu{0};
-	uint32_t _counter_baro{0};
-	uint32_t _counter_mag{0};
-
-
-	void setGpsMessageToDefaul();
+	Imu _imu;
+	Mag _mag;
+	Baro _baro;
+	Gps _gps;
 
+	uint32_t _time {0};	// in microseconds
 
+	gps_message getDefaultGpsData();
 
 };
-
-// Compute greatest common divider
-inline uint32_t gcd(uint32_t a, uint32_t b)
-{
-	return b == 0 ? a : gcd(b, a % b);
-}
-
-} // end of namespace

test/test_EKF_basics.cpp

@@ -34,9 +34,8 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/ekf.h"
-#include "SensorSimulator.h"
+#include "SensorSimulator/SensorSimulator.h"
 
-using namespace simulator;
 
 class EkfInitializationTest : public ::testing::Test {
  public:
@@ -54,7 +53,7 @@ class EkfInitializationTest : public ::testing::Test {
 		_ekf->init(0);
 		_sensor_simulator = new SensorSimulator(_ekf);
 
-		_sensor_simulator->update_with_const_sensors(_init_duration_us);
+		_sensor_simulator->run(_init_duration_us);
 	}
 
 	// Use this method to clean up any memory, network etc. after each test
@@ -111,7 +110,7 @@ TEST_F(EkfInitializationTest, convergesToZero)
 {
 	// GIVEN: initialized EKF with default IMU, baro and mag input for 2s
 	// WHEN: Added more defautl sensor measurements
-	_sensor_simulator->update_with_const_sensors(4000000); // for further 4s
+	_sensor_simulator->run(4000000); // for further 4s
 
 	float converged_pos[3];
 	float converged_vel[3];
@@ -137,8 +136,8 @@ TEST_F(EkfInitializationTest, gpsFusion)
 	// GIVEN: initialized EKF with default IMU, baro and mag input for 2s
 	// WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec
 
-	_sensor_simulator->setGpsFusionTrue();
-	_sensor_simulator->update_with_const_sensors(11000000,Vector3f{0.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}); // for further 3s
+	_sensor_simulator->startGps();
+	_sensor_simulator->run(11000000); // for further 3s
 
 	// THEN: EKF should fuse GPS, but no other position sensor
 	filter_control_status_u control_status;
@@ -176,8 +175,9 @@ TEST_F(EkfInitializationTest, accleBiasEstimation)
 	// WHEN: Added more sensor measurements with accel bias and gps measurements
 	Vector3f accel_bias = {0.0f,0.0f,0.1f};
 
-	_sensor_simulator->setGpsFusionTrue();
-	_sensor_simulator->update_with_const_sensors(10000000,Vector3f{0.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}+accel_bias); // for further 10s
+	_sensor_simulator->startGps();
+	_sensor_simulator->setImuBias(accel_bias, Vector3f{0.0f,0.0f,0.0f});
+	_sensor_simulator->run(10000000);
 
 	float converged_pos[3];
 	float converged_vel[3];