SensorSimulator

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