add basic gps yaw fusion test

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