From 9b2fb25e52096be1f82b357c839c2e53d6da5224 Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <hoenig@tu-berlin.de>
Date: Thu, 25 Aug 2022 12:53:37 +0200
Subject: [PATCH] High-Level flight: plan shortest rotation to reach the
 desired yaw

* Adds new helper functions (and tests) to math3d:
  normalize_radians and shortest_signed_angle_radians
* For takeoff, landing, and goTo: plan the yaw motion such that always
  the shortest rotation is used.
---
 src/modules/interface/math3d.h | 29 +++++++++++++++++++++++++++++
 src/modules/src/planner.c      | 16 +++++++++++++---
 test_python/test_math3d.py     | 24 ++++++++++++++++++++++++
 3 files changed, 66 insertions(+), 3 deletions(-)

diff --git a/src/modules/interface/math3d.h b/src/modules/interface/math3d.h
index 673d5d6124..260fe3ff62 100644
--- a/src/modules/interface/math3d.h
+++ b/src/modules/interface/math3d.h
@@ -45,6 +45,35 @@ SOFTWARE.
 static inline float fsqr(float x) { return x * x; }
 static inline float radians(float degrees) { return (M_PI_F / 180.0f) * degrees; }
 static inline float degrees(float radians) { return (180.0f / M_PI_F) * radians; }
+
+// Normalize radians to be in range [-pi,pi]
+// See https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
+static inline float normalize_radians(float radians)
+{
+	// Copy the sign of the value in radians to the value of pi.
+	float signed_pi = copysignf(M_PI_F, radians);
+	// Set the value of difference to the appropriate signed value between pi and -pi.
+	radians = fmodf(radians + signed_pi, 2 * M_PI_F) - signed_pi;
+	return radians;
+}
+
+// modulo operation that uses the floored definition (as in Python), rather than
+// the truncated definition used for the % operator in C
+// See https://en.wikipedia.org/wiki/Modulo_operation
+static inline float fmodf_floored(float x, float n)
+{
+	return x - floorf(x / n) * n;
+}
+
+// compute shortest signed angle between two given angles (in range [-pi, pi])
+// See https://stackoverflow.com/questions/1878907/how-can-i-find-the-difference-between-two-angles
+static inline float shortest_signed_angle_radians(float start, float goal)
+{
+	float diff = goal - start;
+	float signed_diff = fmodf_floored(diff + M_PI_F, 2 * M_PI_F) - M_PI_F;
+	return signed_diff;
+}
+
 static inline float clamp(float value, float min, float max) {
   if (value < min) return min;
   if (value > max) return max;
diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c
index f68148935b..1f02a27256 100644
--- a/src/modules/src/planner.c
+++ b/src/modules/src/planner.c
@@ -45,9 +45,14 @@ static void plan_takeoff_or_landing(struct planner *p, struct vec curr_pos, floa
 	struct vec hover_pos = curr_pos;
 	hover_pos.z = hover_height;
 
+	// compute the shortest possible rotation towards 0
+	hover_yaw = normalize_radians(hover_yaw);
+	curr_yaw = normalize_radians(curr_yaw);
+	float goal_yaw = curr_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw);
+
 	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
 		curr_pos,  curr_yaw,  vzero(), 0, vzero(),
-		hover_pos, hover_yaw, vzero(), 0, vzero());
+		hover_pos, goal_yaw, vzero(), 0, vzero());
 }
 
 // ----------------- //
@@ -178,9 +183,14 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r
 		hover_yaw += curr_eval->yaw;
 	}
 
+	// compute the shortest possible rotation towards 0
+	float curr_yaw = normalize_radians(curr_eval->yaw);
+	hover_yaw = normalize_radians(hover_yaw);
+	float goal_yaw = curr_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw);
+
 	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
-		curr_eval->pos, curr_eval->yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc,
-		hover_pos,      hover_yaw,      vzero(),        0,                  vzero());
+		curr_eval->pos, curr_yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc,
+		hover_pos,      goal_yaw,      vzero(),        0,                  vzero());
 
 	p->reversed = false;
 	p->state = TRAJECTORY_STATE_FLYING;
diff --git a/test_python/test_math3d.py b/test_python/test_math3d.py
index ef8ba655ad..6bab10f365 100644
--- a/test_python/test_math3d.py
+++ b/test_python/test_math3d.py
@@ -13,3 +13,27 @@ def test_that_vec_is_converted_to_numpy_array():
     # Assert
     expected = np.array([1, 2, 3])
     assert np.allclose(expected, actual)
+
+
+def test_normalize_radians():
+    # Fixture
+    angles = [-100, -5, 0, np.pi + 0.1, -np.pi - 0.1, 100]
+
+    for angle in angles:
+        # Test
+        actual = cffirmware.normalize_radians(angle)
+        # Assert
+        expected = np.arctan2(np.sin(angle), np.cos(angle))
+        assert np.allclose(expected, actual)
+
+
+def test_shortest_signed_angle_radians():
+    # Fixture
+    angle_pairs = [(-np.pi/2, np.pi), (np.pi/2, np.pi), (np.pi, -np.pi/3)]
+
+    for start, goal in angle_pairs:
+        # Test
+        actual = cffirmware.shortest_signed_angle_radians(start, goal)
+        # Assert
+        expected = np.arctan2(np.sin(goal - start), np.cos(goal - start))
+        assert np.allclose(expected, actual)
\ No newline at end of file