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

EKF/common.h

@@ -76,7 +76,7 @@ struct flow_message {
 	uint8_t quality;	///< Quality of Flow data
 	Vector2f flowdata;	///< Optical flow rates about the X and Y body axes (rad/sec)
 	Vector3f gyrodata;	///< Gyro rates about the XYZ body axes (rad/sec)
-	uint32_t dt;		///< integration time of flow samples (sec)
+	uint32_t dt;		///< integration time of flow samples (microseconds)
 };
 
 struct ext_vision_message {

EKF/control.cpp

@@ -401,6 +401,7 @@ void Ekf::controlExternalVisionFusion()
 
 void Ekf::controlOpticalFlowFusion()
 {
+	// TODO: These motion checks run all the time. Pull them out of this function
 	// Check if on ground motion is un-suitable for use of optical flow
 	if (!_control_status.flags.in_air) {
 		// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation

EKF/estimator_interface.cpp

@@ -344,7 +344,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua
 	}
 }
 
-// set optical flow data
+// TODO: Change pointer to constant reference
 void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow)
 {
 	if (!_initialised || _flow_buffer_fail) {
@@ -366,7 +366,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 	if ((time_usec - _time_last_optflow) > _min_obs_interval_us) {
 		// check if enough integration time and fail if integration time is less than 50%
 		// of min arrival interval because too much data is being lost
-		float delta_time = 1e-6f * (float)flow->dt;
+		float delta_time = 1e-6f * (float)flow->dt; // in seconds
 		float delta_time_min = 5e-7f * (float)_min_obs_interval_us;
 		bool delta_time_good = delta_time >= delta_time_min;
 
@@ -407,7 +407,6 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl
 			optflow_sample_new.gyroXYZ = - flow->gyrodata;
 			optflow_sample_new.flowRadXY = -flow->flowdata;
 
-			// convert integration interval to seconds
 			optflow_sample_new.dt = delta_time;
 			_time_last_optflow = time_usec;
 

test/CMakeLists.txt

@@ -41,6 +41,7 @@ set(SRCS
 	test_EKF_ringbuffer.cpp
 	test_EKF_imuSampling.cpp
 	test_AlphaFilter.cpp
+	test_EKF_fusionLogic.cpp
    )
 add_executable(ECL_GTESTS ${SRCS})
 

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 @@ class Baro: public Sensor
 };
 
 } // 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 @@ class Gps: public Sensor
 	~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 @@ class Gps: public Sensor
 };
 
 } // namespace sensor
-} // namespace sensor_simulator::sensor
+} // namespace sensor_simulator