From 61180d8d8ffb3119a34ff22d191906f64a5d8060 Mon Sep 17 00:00:00 2001
From: Matej Karasek <matejkarasek@gmail.com>
Date: Fri, 13 Sep 2024 13:38:20 +0200
Subject: [PATCH 1/7] High Level commander - adding Spiral and linear Go To

---
 cflib/crazyflie/high_level_commander.py | 37 +++++++++++++++++++++----
 1 file changed, 32 insertions(+), 5 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 7966193e3..90f573ff4 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -41,12 +41,13 @@ class HighLevelCommander():
 
     COMMAND_SET_GROUP_MASK = 0
     COMMAND_STOP = 3
-    COMMAND_GO_TO = 4
     COMMAND_START_TRAJECTORY = 5
     COMMAND_DEFINE_TRAJECTORY = 6
     COMMAND_TAKEOFF_2 = 7
     COMMAND_LAND_2 = 8
-
+    COMMAND_SPIRAL = 11
+    COMMAND_GO_TO_2 = 12
+    
     ALL_GROUPS = 0
 
     TRAJECTORY_LOCATION_MEM = 1
@@ -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.COMMAND_GO_TO,
+        self._send_packet(struct.pack('<BBBBfffff',
+                                      self.COMMAND_GO_TO_2,
                                       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):

From 5177a16fbe316e4851002bf2b965696dde3c6d1f Mon Sep 17 00:00:00 2001
From: Matej Karasek <matejkarasek@gmail.com>
Date: Wed, 25 Sep 2024 13:49:48 +0200
Subject: [PATCH 2/7] Revert "High Level commander - adding Spiral and linear
 Go To"

This reverts commit 61180d8d8ffb3119a34ff22d191906f64a5d8060.
---
 cflib/crazyflie/high_level_commander.py | 37 ++++---------------------
 1 file changed, 5 insertions(+), 32 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 90f573ff4..7966193e3 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -41,13 +41,12 @@ class HighLevelCommander():
 
     COMMAND_SET_GROUP_MASK = 0
     COMMAND_STOP = 3
+    COMMAND_GO_TO = 4
     COMMAND_START_TRAJECTORY = 5
     COMMAND_DEFINE_TRAJECTORY = 6
     COMMAND_TAKEOFF_2 = 7
     COMMAND_LAND_2 = 8
-    COMMAND_SPIRAL = 11
-    COMMAND_GO_TO_2 = 12
-    
+
     ALL_GROUPS = 0
 
     TRAJECTORY_LOCATION_MEM = 1
@@ -132,7 +131,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, linear=False,
+    def go_to(self, x, y, z, yaw, duration_s, relative=False,
               group_mask=ALL_GROUPS):
         """
         Go to an absolute or relative position
@@ -143,41 +142,15 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=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('<BBBBfffff',
-                                      self.COMMAND_GO_TO_2,
+        self._send_packet(struct.pack('<BBBfffff',
+                                      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):

From bfe7d2bb16b2621fba327323bbab31c5cc4f46f8 Mon Sep 17 00:00:00 2001
From: Matej Karasek <matejkarasek@gmail.com>
Date: Wed, 25 Sep 2024 13:54:44 +0200
Subject: [PATCH 3/7] adding Spiral and Goto2 allowing linear motion

---
 cflib/crazyflie/high_level_commander.py | 49 +++++++++++++++++++++++++
 1 file changed, 49 insertions(+)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 7966193e3..964289bc6 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -46,6 +46,8 @@ class HighLevelCommander():
     COMMAND_DEFINE_TRAJECTORY = 6
     COMMAND_TAKEOFF_2 = 7
     COMMAND_LAND_2 = 8
+    COMMAND_SPIRAL = 11
+    COMMAND_GO_TO_2 = 12
 
     ALL_GROUPS = 0
 
@@ -151,6 +153,53 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False,
                                       x, y, z,
                                       yaw,
                                       duration_s))
+        
+    def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False,
+              group_mask=ALL_GROUPS):
+        """
+        Go to an absolute or relative position
+
+        :param x: X (m)
+        :param y: Y (m)
+        :param z: Z (m)
+        :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('<BBBBfffff',
+                                      self.COMMAND_GO_TO_2,
+                                      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):

From 3b7d7bbfb1d6d63ce98978f7cbcabc5bd6fa7f90 Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Tue, 1 Oct 2024 07:57:44 +0200
Subject: [PATCH 4/7] fix autopep8 wrapper check

---
 cflib/crazyflie/high_level_commander.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 964289bc6..188f2b1f2 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -153,9 +153,9 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False,
                                       x, y, z,
                                       yaw,
                                       duration_s))
-        
+
     def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False,
-              group_mask=ALL_GROUPS):
+               group_mask=ALL_GROUPS):
         """
         Go to an absolute or relative position
 
@@ -176,9 +176,9 @@ def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False,
                                       x, y, z,
                                       yaw,
                                       duration_s))
-        
+
     def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=False,
-              group_mask=ALL_GROUPS):
+               group_mask=ALL_GROUPS):
         """
         Follow a spiral-like segment (spline approximation of a spiral/arc for <= 90-degree segments)
 

From 5344f6986de6e2b5593b4899497d573fa91e3d7d Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Thu, 3 Oct 2024 18:05:39 +0200
Subject: [PATCH 5/7] Single goto with a version check & limit of spiral
 parameters

---
 cflib/crazyflie/high_level_commander.py | 72 +++++++++++++------------
 1 file changed, 38 insertions(+), 34 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 188f2b1f2..58f607fcc 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -25,7 +25,9 @@
 """
 Used for sending high level setpoints to the Crazyflie
 """
+import math
 import struct
+import warnings
 
 from cflib.crtp.crtpstack import CRTPPacket
 from cflib.crtp.crtpstack import CRTPPort
@@ -133,32 +135,11 @@ 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
 
-        :param x: X (m)
-        :param y: Y (m)
-        :param z: Z (m)
-        :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 group_mask: Mask for which CFs this should apply to
-        """
-        self._send_packet(struct.pack('<BBBfffff',
-                                      self.COMMAND_GO_TO,
-                                      group_mask,
-                                      relative,
-                                      x, y, z,
-                                      yaw,
-                                      duration_s))
-
-    def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False,
-               group_mask=ALL_GROUPS):
-        """
-        Go to an absolute or relative position
-
         :param x: X (m)
         :param y: Y (m)
         :param z: Z (m)
@@ -168,29 +149,52 @@ def go_to2(self, x, y, z, yaw, duration_s, relative=False, linear=False,
         :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('<BBBBfffff',
-                                      self.COMMAND_GO_TO_2,
-                                      group_mask,
-                                      relative,
-                                      linear,
-                                      x, y, z,
-                                      yaw,
-                                      duration_s))
+        if self._cf.platform.get_protocol_version() < 8:
+            if linear:
+                warnings.warning('Linear mode is not supported in protocol version < 8, update your Crazyflie firmware')
+            self._send_packet(struct.pack('<BBBBfffff',
+                                          self.COMMAND_GO_TO,
+                                          group_mask,
+                                          relative,
+                                          x, y, z,
+                                          yaw,
+                                          duration_s))
+        else:
+            self._send_packet(struct.pack('<BBBBfffff',
+                                          self.COMMAND_GO_TO2,
+                                          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 angle: spiral angle (rad), limited to +/- 2pi
+        :param r0: initial radius (m), must be positive
+        :param rF: final radius (m), must be positive
+        :param ascent: altitude gain (m), positive to climb, negative to descent
         :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
         """
+        if angle > 2*math.pi:
+            angle = 2*math.pi
+            warnings.warning('Spiral angle saturated at 2pi as it was too large')
+        elif angle < -2*math.pi:
+            angle = -2*math.pi
+            warnings.warning('Spiral angle saturated at -2pi as it was too small')
+        if r0 < 0:
+            r0 = 0
+            warnings.warning('Initial radius set to 0 as it cannot be negative')
+        if rF < 0:
+            rF = 0
+            warnings.warning('Final radius set to 0 as it cannot be negative')
         self._send_packet(struct.pack('<BBBBfffff',
                                       self.COMMAND_SPIRAL,
                                       group_mask,

From 4348c020c4ba6eafe5ed070c11afcaffc62af3a3 Mon Sep 17 00:00:00 2001
From: Matej Karasek <matejkarasek@gmail.com>
Date: Thu, 3 Oct 2024 19:02:27 +0200
Subject: [PATCH 6/7] bugfixes

---
 cflib/crazyflie/high_level_commander.py | 52 +++++++++++++------------
 1 file changed, 27 insertions(+), 25 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 58f607fcc..4a7792f7f 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -27,7 +27,6 @@
 """
 import math
 import struct
-import warnings
 
 from cflib.crtp.crtpstack import CRTPPacket
 from cflib.crtp.crtpstack import CRTPPort
@@ -151,8 +150,8 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False,
         """
         if self._cf.platform.get_protocol_version() < 8:
             if linear:
-                warnings.warning('Linear mode is not supported in protocol version < 8, update your Crazyflie firmware')
-            self._send_packet(struct.pack('<BBBBfffff',
+                print('Warning: Linear mode is not supported in protocol version < 8, update the firmware of your crazyflie')
+            self._send_packet(struct.pack('<BBBfffff',
                                           self.COMMAND_GO_TO,
                                           group_mask,
                                           relative,
@@ -161,7 +160,7 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False,
                                           duration_s))
         else:
             self._send_packet(struct.pack('<BBBBfffff',
-                                          self.COMMAND_GO_TO2,
+                                          self.COMMAND_GO_TO_2,
                                           group_mask,
                                           relative,
                                           linear,
@@ -183,27 +182,30 @@ def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=Fa
         :param clockwise: true if crazyflie should spiral clockwise instead of counter-clockwise
         :param group_mask: Mask for which CFs this should apply to
         """
-        if angle > 2*math.pi:
-            angle = 2*math.pi
-            warnings.warning('Spiral angle saturated at 2pi as it was too large')
-        elif angle < -2*math.pi:
-            angle = -2*math.pi
-            warnings.warning('Spiral angle saturated at -2pi as it was too small')
-        if r0 < 0:
-            r0 = 0
-            warnings.warning('Initial radius set to 0 as it cannot be negative')
-        if rF < 0:
-            rF = 0
-            warnings.warning('Final radius set to 0 as it cannot be negative')
-        self._send_packet(struct.pack('<BBBBfffff',
-                                      self.COMMAND_SPIRAL,
-                                      group_mask,
-                                      sideways,
-                                      clockwise,
-                                      angle,
-                                      r0, rF,
-                                      ascent,
-                                      duration_s))
+        if self._cf.platform.get_protocol_version() < 8:
+            print('Warning: Spiral command is not supported in protocol version < 8, update the firmware of your crazyflie')
+        else:
+          if angle > 2*math.pi:
+              angle = 2*math.pi
+              print('Warning: Spiral angle saturated at 2pi as it was too large')
+          elif angle < -2*math.pi:
+              angle = -2*math.pi
+              print('Warning: Spiral angle saturated at -2pi as it was too small')
+          if r0 < 0:
+              r0 = 0
+              print('Warning: Initial radius set to 0 as it cannot be negative')
+          if rF < 0:
+              rF = 0
+              print('Warning: Final radius set to 0 as it cannot be negative')
+          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):

From 4efca4d2f70eea98433df2d4d80956779d3b7db8 Mon Sep 17 00:00:00 2001
From: matejkarasek <matejkarasek@gmail.com>
Date: Thu, 3 Oct 2024 19:08:23 +0200
Subject: [PATCH 7/7] fix formating

---
 cflib/crazyflie/high_level_commander.py | 46 ++++++++++++-------------
 1 file changed, 23 insertions(+), 23 deletions(-)

diff --git a/cflib/crazyflie/high_level_commander.py b/cflib/crazyflie/high_level_commander.py
index 4a7792f7f..98ab846d7 100644
--- a/cflib/crazyflie/high_level_commander.py
+++ b/cflib/crazyflie/high_level_commander.py
@@ -150,7 +150,7 @@ def go_to(self, x, y, z, yaw, duration_s, relative=False, linear=False,
         """
         if self._cf.platform.get_protocol_version() < 8:
             if linear:
-                print('Warning: Linear mode is not supported in protocol version < 8, update the firmware of your crazyflie')
+                print('Warning: Linear mode not supported in protocol version < 8, update your crazyflie\'s firmware')
             self._send_packet(struct.pack('<BBBfffff',
                                           self.COMMAND_GO_TO,
                                           group_mask,
@@ -183,29 +183,29 @@ def spiral(self, angle, r0, rF, ascent, duration_s, sideways=False, clockwise=Fa
         :param group_mask: Mask for which CFs this should apply to
         """
         if self._cf.platform.get_protocol_version() < 8:
-            print('Warning: Spiral command is not supported in protocol version < 8, update the firmware of your crazyflie')
+            print('Warning: Spiral command is not supported in protocol version < 8, update your crazyflie\'s firmware')
         else:
-          if angle > 2*math.pi:
-              angle = 2*math.pi
-              print('Warning: Spiral angle saturated at 2pi as it was too large')
-          elif angle < -2*math.pi:
-              angle = -2*math.pi
-              print('Warning: Spiral angle saturated at -2pi as it was too small')
-          if r0 < 0:
-              r0 = 0
-              print('Warning: Initial radius set to 0 as it cannot be negative')
-          if rF < 0:
-              rF = 0
-              print('Warning: Final radius set to 0 as it cannot be negative')
-          self._send_packet(struct.pack('<BBBBfffff',
-                                        self.COMMAND_SPIRAL,
-                                        group_mask,
-                                        sideways,
-                                        clockwise,
-                                        angle,
-                                        r0, rF,
-                                        ascent,
-                                        duration_s))
+            if angle > 2*math.pi:
+                angle = 2*math.pi
+                print('Warning: Spiral angle saturated at 2pi as it was too large')
+            elif angle < -2*math.pi:
+                angle = -2*math.pi
+                print('Warning: Spiral angle saturated at -2pi as it was too small')
+            if r0 < 0:
+                r0 = 0
+                print('Warning: Initial radius set to 0 as it cannot be negative')
+            if rF < 0:
+                rF = 0
+                print('Warning: Final radius set to 0 as it cannot be negative')
+            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):