From 7d7200cf2fb6a6788fc9608c4cd405cb1476b72f Mon Sep 17 00:00:00 2001
From: David Jablonski <dayjaby@gmail.com>
Date: Sat, 2 May 2020 10:05:20 +0200
Subject: [PATCH] Added yaw to CMD_DO_SET_HOME

---
 src/modules/commander/Commander.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 671a528508..a6f0e6c885 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -74,6 +74,7 @@
 #include <math.h>
 #include <float.h>
 #include <cstring>
+#include <matrix/math.hpp>
 
 #include <uORB/topics/mavlink_log.h>
 
@@ -1030,6 +1031,8 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				}
 
 			} else {
+				float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
+				yaw = PX4_ISFINITE(yaw) ? yaw : NAN;
 				const double lat = cmd.param5;
 				const double lon = cmd.param6;
 				const float alt = cmd.param7;
@@ -1053,7 +1056,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						float home_y;
 						map_projection_project(&ref_pos, lat, lon, &home_x, &home_y);
 						const float home_z = -(alt - local_pos.ref_alt);
-						fillLocalHomePos(home, home_x, home_y, home_z, 0.f);
+						fillLocalHomePos(home, home_x, home_y, home_z, yaw);
 
 						/* mark home position as set */
 						_status_flags.condition_home_position_valid = _home_pub.update(home);