From 3fac51919108abd139511089fe21e1fc37ddce76 Mon Sep 17 00:00:00 2001
From: Matej Karasek <matejkarasek@gmail.com>
Date: Tue, 23 Apr 2024 10:05:01 +0200
Subject: [PATCH] adding linear goto and spiral

---
 cflib/crazyflie/high_level_commander.py | 33 ++++++++++++++++++++++---
 examples/swarm/synchronizedSequence.py  |  7 ++++--
 2 files changed, 35 insertions(+), 5 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 7966193e3..134c7c3b6 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -46,6 +46,7 @@ class HighLevelCommander():
     COMMAND_DEFINE_TRAJECTORY = 6
     COMMAND_TAKEOFF_2 = 7
     COMMAND_LAND_2 = 8
+    COMMAND_SPIRAL = 11
 
     ALL_GROUPS = 0
 
@@ -96,7 +97,7 @@ def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS,
                                       duration_s))
 
     def land(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS,
-             yaw=0.0):
+             yaw=None):
         """
         vertical land from current x-y position to given height
 
@@ -131,7 +132,7 @@ def stop(self, group_mask=ALL_GROUPS):
                                       self.COMMAND_STOP,
                                       group_mask))
 
-    def go_to(self, x, y, z, yaw, duration_s, relative=False,
+    def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False,
               group_mask=ALL_GROUPS):
         """
         Go to an absolute or relative position
@@ -142,15 +143,41 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False,
         :param yaw: Yaw (radians)
         :param duration_s: Time it should take to reach the position (s)
         :param relative: True if x, y, z is relative to the current position
+        :param linear: True to use linear interpolation instead of a smooth polynomial
         :param group_mask: Mask for which CFs this should apply to
         """
-        self._send_packet(struct.pack('<BBBfffff',
+        self._send_packet(struct.pack('<BBBBfffff',
                                       self.COMMAND_GO_TO,
                                       group_mask,
                                       relative,
+                                      linear,
                                       x, y, z,
                                       yaw,
                                       duration_s))
+        
+    def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=False,
+              group_mask=ALL_GROUPS):
+        """
+        Follow a spiral-like segment (spline approximation of a spiral/arc for <= 90-degree segments)
+
+        :param angle: spiral angle (rad)
+        :param r0: initial radius (m)
+        :param rF: final radius (m)
+        :param ascent: altitude gain (m)
+        :param duration_s: time it should take to reach the end of the spiral (s)
+        :param sideways: true if crazyflie should spiral sideways instead of forward
+        :param clockwise: true if crazyflie should spiral clockwise instead of counter-clockwise
+        :param group_mask: Mask for which CFs this should apply to
+        """
+        self._send_packet(struct.pack('<BBBBfffff',
+                                      self.COMMAND_SPIRAL,
+                                      group_mask,
+                                      sideways,
+                                      clockwise,
+                                      angle,
+                                      r0, rF,
+                                      ascent,
+                                      duration_s))
 
     def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False,
                          reversed=False, group_mask=ALL_GROUPS):
diff --git a/examples/swarm/synchronizedSequence.py b/examples/swarm/synchronizedSequence.py
index 17661a0e0..fb7499cc5 100755
--- a/examples/swarm/synchronizedSequence.py
+++ b/examples/swarm/synchronizedSequence.py
@@ -47,7 +47,8 @@
 # Possible commands, all times are in seconds
 Takeoff = namedtuple('Takeoff', ['height', 'time'])
 Land = namedtuple('Land', ['time'])
-Goto = namedtuple('Goto', ['x', 'y', 'z', 'time'])
+Goto = namedtuple('Goto', ['x', 'y', 'z', 'yaw', 'linear', 'time'])
+Spiral = namedtuple('Spiral', ['phi', 'r0', 'rF', 'dz', 'sideways', 'clockwise', 'time'])
 # RGB [0-255], Intensity [0.0-1.0]
 Ring = namedtuple('Ring', ['r', 'g', 'b', 'intensity', 'time'])
 # Reserved for the control loop, do not use in sequence
@@ -138,7 +139,9 @@ def crazyflie_control(scf):
         elif type(command) is Land:
             commander.land(0.0, command.time)
         elif type(command) is Goto:
-            commander.go_to(command.x, command.y, command.z, 0, command.time)
+            commander.go_to(command.x, command.y, command.z, command.yaw, command.time, False, command.linear)
+        elif type(command) is Spiral:
+            commander.spiral(command.phi, command.r0, command.rF, command.dz, command.time, command.sideways, command.clockwise)
         elif type(command) is Ring:
             set_ring_color(cf, command.r, command.g, command.b,
                            command.intensity, command.time)