From a4ad82695857559c518a3623e319529a901ff4ee Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 3 May 2016 13:16:19 +0200
Subject: [PATCH] Loiter: Face travel direction when performing reposition

---
 src/modules/navigator/loiter.cpp | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 51bfdb0fd8..e56355393d 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -45,6 +45,8 @@
 #include <math.h>
 #include <fcntl.h>
 
+#include <geo/geo.h>
+
 #include <systemlib/mavlink_log.h>
 #include <systemlib/err.h>
 
@@ -117,6 +119,15 @@ Loiter::reposition()
 		memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
 		pos_sp_triplet->next.valid = false;
 
+		// set yaw
+
+		// calculate direction the vehicle should point to.
+		pos_sp_triplet->current.yaw = get_bearing_to_next_waypoint(
+				_navigator->get_global_position()->lat,
+				_navigator->get_global_position()->lon,
+				pos_sp_triplet->current.lat,
+				pos_sp_triplet->current.lon);
+
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
 		_navigator->set_position_setpoint_triplet_updated();