From 33fb9d0c620c0011257e1b8b7236cf19c3d612b0 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 7 Aug 2020 17:29:43 +0200
Subject: [PATCH] Commander: update home position yaw in case of EKF yaw reset

This makes sure the drone lands in the correct orientation
during RTL even if the initial yaw at takeoff was wrong.
---
 src/modules/commander/Commander.cpp | 28 ++++++++++++++++++++++++++++
 src/modules/commander/Commander.hpp |  4 ++++
 2 files changed, 32 insertions(+)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 6125b5dabb..042d7af676 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1237,6 +1237,31 @@ Commander::set_home_position_alt_only()
 	return false;
 }
 
+void
+Commander::updateHomePositionYaw(float yaw)
+{
+	home_position_s home = _home_pub.get();
+
+	home.yaw = yaw;
+	home.timestamp = hrt_absolute_time();
+
+	_home_pub.update(home);
+}
+
+void
+Commander::checkEkfResetCounters()
+{
+	if (_attitude_sub.get().quat_reset_counter != _quat_reset_counter) {
+		const float delta_psi = matrix::Eulerf(matrix::Quatf(_attitude_sub.get().delta_q_reset)).psi();
+
+		if (!_home_pub.get().manual_home) {
+			updateHomePositionYaw(matrix::wrap_pi(_home_pub.get().yaw + delta_psi));
+		}
+
+		_quat_reset_counter = _attitude_sub.get().quat_reset_counter;
+	}
+}
+
 void
 Commander::run()
 {
@@ -2254,6 +2279,8 @@ Commander::run()
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
+		checkEkfResetCounters();
+
 		// automatically set or update home position
 		if (!_home_pub.get().manual_home) {
 			const vehicle_local_position_s &local_position = _local_position_sub.get();
@@ -4028,6 +4055,7 @@ void Commander::estimator_check()
 
 	_local_position_sub.update();
 	_global_position_sub.update();
+	_attitude_sub.update();
 
 	const vehicle_local_position_s &lpos = _local_position_sub.get();
 	const vehicle_global_position_s &gpos = _global_position_sub.get();
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 6878093e3f..90d7ba4067 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -155,6 +155,8 @@ private:
 
 	bool set_home_position();
 	bool set_home_position_alt_only();
+	void updateHomePositionYaw(float yaw);
+	void checkEkfResetCounters();
 
 	void update_control_mode();
 
@@ -360,6 +362,7 @@ private:
 	hrt_abstime	_timestamp_engine_healthy{0}; ///< absolute time when engine was healty
 
 	uint32_t	_counter{0};
+	uint8_t		_quat_reset_counter{0};
 
 	bool		_status_changed{true};
 	bool		_arm_tune_played{false};
@@ -420,6 +423,7 @@ private:
 	uORB::SubscriptionData<offboard_control_mode_s>		_offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::SubscriptionData<vehicle_attitude_s>		_attitude_sub{ORB_ID(vehicle_attitude)};
 
 	// Publications
 	uORB::Publication<actuator_armed_s>			_armed_pub{ORB_ID(actuator_armed)};