SensorSimulator

test/CMakeLists.txt

@@ -33,15 +33,19 @@
 
 include(gtest.cmake)
 
+add_library(SENSOR_SIMULATOR SensorSimulator.cpp)
+target_link_libraries(SENSOR_SIMULATOR ecl_EKF)
+
 set(SRCS
 	main.cpp
 	test_EKF_basics.cpp
 	test_EKF_ringbuffer.cpp
 	test_EKF_imuSampling.cpp
 	test_AlphaFilter.cpp
+	test_SensorSimulator.cpp
    )
 add_executable(ECL_GTESTS ${SRCS})
 
-target_link_libraries(ECL_GTESTS gtest_main ecl_EKF)
+target_link_libraries(ECL_GTESTS gtest_main ecl_EKF SENSOR_SIMULATOR)
 
 add_test(NAME ECL_GTESTS COMMAND ECL_GTESTS)

test/SensorSimulator.cpp

@@ -0,0 +1,89 @@
+#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.h

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   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 "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 setGpsFusionTrue(){ _fuse_gps = true; }
+	void setGpsFusionFalse(){ _fuse_gps = false; }
+
+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();
+
+
+
+};
+
+// 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