add basic gps yaw fusion test

test/CMakeLists.txt

@@ -45,6 +45,7 @@ set(SRCS
 	test_AlphaFilter.cpp
 	test_EKF_fusionLogic.cpp
 	test_EKF_initialization.cpp
+	test_EKF_gps_yaw.cpp
 	test_EKF_gps.cpp
 	test_EKF_externalVision.cpp
 	test_EKF_airspeed.cpp

test/sensor_simulator/ekf_wrapper.cpp

@@ -75,6 +75,23 @@ bool EkfWrapper::isIntendingGpsFusion() const
 	return control_status.flags.gps;
 }
 
+void EkfWrapper::enableGpsHeadingFusion()
+{
+	_ekf_params->fusion_mode |= MASK_USE_GPSYAW;
+}
+
+void EkfWrapper::disableGpsHeadingFusion()
+{
+	_ekf_params->fusion_mode &= ~MASK_USE_GPSYAW;
+}
+
+bool EkfWrapper::isIntendingGpsHeadingFusion() const
+{
+	filter_control_status_u control_status;
+	_ekf->get_control_mode(&control_status.value);
+	return control_status.flags.gps_yaw;
+}
+
 void EkfWrapper::enableFlowFusion()
 {
 	_ekf_params->fusion_mode |= MASK_USE_OF;
@@ -165,7 +182,21 @@ Eulerf EkfWrapper::getEulerAngles() const
 	return Eulerf(_ekf->getQuaternion());
 }
 
+float EkfWrapper::getYawAngle() const
+{
+	const Eulerf euler(_ekf->getQuaternion());
+	return euler(2);
+}
+
 matrix::Vector<float, 4> EkfWrapper::getQuaternionVariance() const
 {
 	return matrix::Vector<float, 4>(_ekf->orientation_covariances().diag());
 }
+
+int EkfWrapper::getQuaternionResetCounter() const
+{
+	float tmp[4];
+	uint8_t counter;
+	_ekf->get_quat_reset(tmp, &counter);
+	return static_cast<int>(counter);
+}

test/sensor_simulator/ekf_wrapper.h

@@ -64,6 +64,10 @@ public:
 	void disableGpsFusion();
 	bool isIntendingGpsFusion() const;
 
+	void enableGpsHeadingFusion();
+	void disableGpsHeadingFusion();
+	bool isIntendingGpsHeadingFusion() const;
+
 	void enableFlowFusion();
 	void disableFlowFusion();
 	bool isIntendingFlowFusion() const;
@@ -86,7 +90,9 @@ public:
 	bool isWindVelocityEstimated() const;
 
 	Eulerf getEulerAngles() const;
+	float getYawAngle() const;
 	matrix::Vector<float, 4> getQuaternionVariance() const;
+	int getQuaternionResetCounter() const;
 
 private:
 	std::shared_ptr<Ekf> _ekf;

test/sensor_simulator/gps.cpp

@@ -44,6 +44,10 @@ void Gps::setVelocity(const Vector3f& vel)
 	_gps_data.vel_ned = vel;
 }
 
+void Gps::setYaw(float yaw) {
+	_gps_data.yaw = yaw;
+}
+
 void Gps::setFixType(int n)
 {
 	_gps_data.fix_type = n;

test/sensor_simulator/gps.h

@@ -57,6 +57,7 @@ public:
 	void setLatitude(int32_t lat);
 	void setLongitude(int32_t lon);
 	void setVelocity(const Vector3f& vel);
+	void setYaw(float yaw);
 	void setFixType(int n);
 	void setNumberOfSatellites(int n);
 	void setPdop(float pdop);

test/test_EKF_gps_yaw.cpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test the gps yaw fusion
+ * @author Kamil Ritz <ka.ritz@hotmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "EKF/ekf.h"
+#include "sensor_simulator/sensor_simulator.h"
+#include "sensor_simulator/ekf_wrapper.h"
+#include "test_helper/reset_logging_checker.h"
+
+class EkfGpsHeadingTest : public ::testing::Test {
+ public:
+
+	EkfGpsHeadingTest(): ::testing::Test(),
+	_ekf{std::make_shared<Ekf>()},
+	_sensor_simulator(_ekf),
+	_ekf_wrapper(_ekf) {};
+
+	std::shared_ptr<Ekf> _ekf;
+	SensorSimulator _sensor_simulator;
+	EkfWrapper _ekf_wrapper;
+
+	// Setup the Ekf with synthetic measurements
+	void SetUp() override
+	{
+		_ekf->init(0);
+		_sensor_simulator.runSeconds(2);
+		_ekf_wrapper.enableGpsFusion();
+		_sensor_simulator.startGps();
+		_sensor_simulator.runSeconds(11);
+	}
+};
+
+TEST_F(EkfGpsHeadingTest, fusionStartWithReset)
+{
+	// GIVEN:EKF that fuses GPS
+
+	// WHEN: enabling GPS heading fusion and heading difference is bigger than 15 degrees
+	const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(20.f);
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_ekf_wrapper.enableGpsHeadingFusion();
+	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
+	_sensor_simulator.runSeconds(0.2);
+
+	// THEN: GPS heading fusion should have started;
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+
+	// AND: a reset to GPS heading is performed
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+	EXPECT_NEAR(_ekf_wrapper.getYawAngle(), gps_heading, 0.001);
+
+	// WHEN: GPS heading is disabled
+	_sensor_simulator._gps.stop();
+	_sensor_simulator.runSeconds(11);
+
+	// THEN: after a while the fusion should be stopped
+	// TODO: THIS IS NOT HAPPENING
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+}
+
+TEST_F(EkfGpsHeadingTest, fusionStartWithoutReset)
+{
+	// GIVEN:EKF that fuses GPS
+
+	// WHEN: enabling GPS heading fusion and heading difference is smaller than 15 degrees
+	const float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_ekf_wrapper.enableGpsHeadingFusion();
+	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
+	_sensor_simulator.runSeconds(0.2);
+
+	// THEN: GPS heading fusion should have started;
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+
+	// AND: no reset to GPS heading should be performed
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+
+	// TODO: investigate why heading is not converging exactly to GPS heading
+}