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

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;
 
 };