ECL Unit Test Clean Up

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

@@ -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 Baro: public Sensor
+{
+public:
+	Baro(Ekf* ekf);
+	~Baro();
+
+	void setData(float baro);
+
+private:
+	float _baro_data;
+
+	void send(uint32_t time);
+
+};
+

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

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * This class is providing methods to feed the ECL EKF with measurement.
+ * It takes a pointer to the Ekf object and will manipulate the object
+ * by call set*Data functions.
+ * It simulates the time to allow for sensor data being set at certain rate
+ * and also calls the update method of the EKF
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#pragma once
+
+#include "Imu.h"
+#include "Mag.h"
+#include "Baro.h"
+#include "Gps.h"
+#include "EKF/ekf.h"
+
+
+class SensorSimulator
+{
+public:
+	SensorSimulator(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(uint32_t duration);
+
+	void startGps(){ _gps.start(); }
+	void stopGps(){ _gps.stop(); }
+
+	void setImuBias(Vector3f accel_bias, Vector3f gyro_bias);
+
+private:
+	Ekf* _ekf;
+
+	Imu _imu;
+	Mag _mag;
+	Baro _baro;
+	Gps _gps;
+
+	uint32_t _time {0};	// in microseconds
+
+	gps_message getDefaultGpsData();
+
+};