Merge pull request #516 from bitcraze/rik/yawconvention

Use correct yaw convention in motion commander

Author: ToveRumar

cflib/crazyflie/commander.py

@@ -26,6 +26,7 @@
 Used for sending control setpoints to the Crazyflie
 """
 import struct
+import warnings
 
 from cflib.crtp.crtpstack import CRTPPacket
 from cflib.crtp.crtpstack import CRTPPort
@@ -38,11 +39,14 @@
 META_COMMAND_CHANNEL = 1
 
 TYPE_STOP = 0
-TYPE_VELOCITY_WORLD = 1
-TYPE_ZDISTANCE = 2
-TYPE_HOVER = 5
+TYPE_VELOCITY_WORLD_LEGACY = 1
+TYPE_ZDISTANCE_LEGACY = 2
+TYPE_HOVER_LEGACY = 5
 TYPE_FULL_STATE = 6
 TYPE_POSITION = 7
+TYPE_VELOCITY_WORLD = 8
+TYPE_ZDISTANCE = 9
+TYPE_HOVER = 10
 
 TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0
 
@@ -122,8 +126,16 @@ def send_velocity_world_setpoint(self, vx, vy, vz, yawrate):
         pk = CRTPPacket()
         pk.port = CRTPPort.COMMANDER_GENERIC
         pk.channel = SET_SETPOINT_CHANNEL
-        pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD,
-                              vx, vy, vz, yawrate)
+        if self._cf.platform.get_protocol_version() <= 8:
+            warnings.warn(
+                'Using legacy TYPE_VELOCITY_WORLD_LEGACY. Please update your crazyflie-firmware.',
+                DeprecationWarning,
+            )
+            pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD_LEGACY,
+                                  vx, vy, vz, -yawrate)
+        else:
+            pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD,
+                                  vx, vy, vz, yawrate)
         self._cf.send_packet(pk)
 
     def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
@@ -139,8 +151,16 @@ def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
         pk = CRTPPacket()
         pk.port = CRTPPort.COMMANDER_GENERIC
         pk.channel = SET_SETPOINT_CHANNEL
-        pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE,
-                              roll, pitch, yawrate, zdistance)
+        if self._cf.platform.get_protocol_version() <= 8:
+            warnings.warn(
+                'Using legacy TYPE_ZDISTANCE_LEGACY. Please update your crazyflie-firmware.',
+                DeprecationWarning,
+            )
+            pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE_LEGACY,
+                                  roll, pitch, -yawrate, zdistance)
+        else:
+            pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE,
+                                  roll, pitch, yawrate, zdistance)
         self._cf.send_packet(pk)
 
     def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
@@ -156,8 +176,16 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
         pk = CRTPPacket()
         pk.port = CRTPPort.COMMANDER_GENERIC
         pk.channel = SET_SETPOINT_CHANNEL
-        pk.data = struct.pack('<Bffff', TYPE_HOVER,
-                              vx, vy, yawrate, zdistance)
+        if self._cf.platform.get_protocol_version() <= 8:
+            warnings.warn(
+                'Using legacy TYPE_HOVER_LEGACY. Please update your crazyflie-firmware.',
+                DeprecationWarning,
+            )
+            pk.data = struct.pack('<Bffff', TYPE_HOVER_LEGACY,
+                                  vx, vy, -yawrate, zdistance)
+        else:
+            pk.data = struct.pack('<Bffff', TYPE_HOVER,
+                                  vx, vy, yawrate, zdistance)
         self._cf.send_packet(pk)
 
     def send_full_state_setpoint(self, pos, vel, acc, orientation, rollrate, pitchrate, yawrate):

cflib/positioning/motion_commander.py

@@ -347,7 +347,7 @@ def start_turn_left(self, rate=RATE):
         :param rate: The angular rate (degrees/second)
         :return:
         """
-        self._set_vel_setpoint(0.0, 0.0, 0.0, -rate)
+        self._set_vel_setpoint(0.0, 0.0, 0.0, rate)
 
     def start_turn_right(self, rate=RATE):
         """
@@ -356,7 +356,7 @@ def start_turn_right(self, rate=RATE):
         :param rate: The angular rate (degrees/second)
         :return:
         """
-        self._set_vel_setpoint(0.0, 0.0, 0.0, rate)
+        self._set_vel_setpoint(0.0, 0.0, 0.0, -rate)
 
     def start_circle_left(self, radius_m, velocity=VELOCITY):
         """
@@ -369,7 +369,7 @@ def start_circle_left(self, radius_m, velocity=VELOCITY):
         circumference = 2 * radius_m * math.pi
         rate = 360.0 * velocity / circumference
 
-        self._set_vel_setpoint(velocity, 0.0, 0.0, -rate)
+        self._set_vel_setpoint(velocity, 0.0, 0.0, rate)
 
     def start_circle_right(self, radius_m, velocity=VELOCITY):
         """
@@ -382,7 +382,7 @@ def start_circle_right(self, radius_m, velocity=VELOCITY):
         circumference = 2 * radius_m * math.pi
         rate = 360.0 * velocity / circumference
 
-        self._set_vel_setpoint(velocity, 0.0, 0.0, rate)
+        self._set_vel_setpoint(velocity, 0.0, 0.0, -rate)
 
     def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw=0.0):
         """

examples/multiranger/multiranger_wall_following.py

@@ -122,9 +122,7 @@ def handle_range_measurement(range):
                               'yaw_rate', yaw_rate, 'state_wf', state_wf)
 
                         # convert yaw_rate from rad to deg
-                        # the negative sign is because of this ticket:
-                        #    https://github.com/bitcraze/crazyflie-lib-python/issues/389
-                        yaw_rate_deg = -1 * degrees(yaw_rate)
+                        yaw_rate_deg = degrees(yaw_rate)
 
                         motion_commander.start_linear_motion(
                             velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg)

test/positioning/test_motion_commander.py

@@ -255,7 +255,7 @@ def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock):
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(0.0, 0.0, 0.0, rate),
+            call(0.0, 0.0, 0.0, -rate),
         ])
 
     def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock):
@@ -270,7 +270,7 @@ def test_that_it_starts_turn_left(self, _SetPointThread_mock, sleep_mock):
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(0.0, 0.0, 0.0, -rate),
+            call(0.0, 0.0, 0.0, rate),
         ])
 
     def test_that_it_starts_circle_right(
@@ -289,7 +289,7 @@ def test_that_it_starts_circle_right(
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(velocity, 0.0, 0.0, expected_rate),
+            call(velocity, 0.0, 0.0, -expected_rate),
         ])
 
     def test_that_it_starts_circle_left(
@@ -308,7 +308,7 @@ def test_that_it_starts_circle_left(
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(velocity, 0.0, 0.0, -expected_rate),
+            call(velocity, 0.0, 0.0, expected_rate),
         ])
 
     def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock):
@@ -325,7 +325,7 @@ def test_that_it_turns_right(self, _SetPointThread_mock, sleep_mock):
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(0.0, 0.0, 0.0, rate),
+            call(0.0, 0.0, 0.0, -rate),
             call(0.0, 0.0, 0.0, 0.0)
         ])
         sleep_mock.assert_called_with(turn_time)
@@ -344,7 +344,7 @@ def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock):
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(0.0, 0.0, 0.0, -rate),
+            call(0.0, 0.0, 0.0, rate),
             call(0.0, 0.0, 0.0, 0.0)
         ])
         sleep_mock.assert_called_with(turn_time)
@@ -368,7 +368,7 @@ def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock):
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(velocity, 0.0, 0.0, rate),
+            call(velocity, 0.0, 0.0, -rate),
             call(0.0, 0.0, 0.0, 0.0)
         ])
         sleep_mock.assert_called_with(turn_time)
@@ -392,7 +392,7 @@ def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock):
 
         # Assert
         thread_mock.set_vel_setpoint.assert_has_calls([
-            call(velocity, 0.0, 0.0, -rate),
+            call(velocity, 0.0, 0.0, rate),
             call(0.0, 0.0, 0.0, 0.0)
         ])
         sleep_mock.assert_called_with(turn_time)