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

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

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