Skip to content

Commit 70d10a1

Browse files
ToveRumarToveRumar
authored
Merge pull request #516 from bitcraze/rik/yawconvention
Use correct yaw convention in motion commander
2 parents 768c58e + 0c63fde commit 70d10a1

File tree

4 files changed

+50
-24
lines changed

4 files changed

+50
-24
lines changed

‎cflib/crazyflie/commander.py

Lines changed: 37 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
Used for sending control setpoints to the Crazyflie
2727
"""
2828
import struct
29+
import warnings
2930

3031
from cflib.crtp.crtpstack import CRTPPacket
3132
from cflib.crtp.crtpstack import CRTPPort
@@ -38,11 +39,14 @@
3839
META_COMMAND_CHANNEL = 1
3940

4041
TYPE_STOP = 0
41-
TYPE_VELOCITY_WORLD = 1
42-
TYPE_ZDISTANCE = 2
43-
TYPE_HOVER = 5
42+
TYPE_VELOCITY_WORLD_LEGACY = 1
43+
TYPE_ZDISTANCE_LEGACY = 2
44+
TYPE_HOVER_LEGACY = 5
4445
TYPE_FULL_STATE = 6
4546
TYPE_POSITION = 7
47+
TYPE_VELOCITY_WORLD = 8
48+
TYPE_ZDISTANCE = 9
49+
TYPE_HOVER = 10
4650

4751
TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0
4852

@@ -122,8 +126,16 @@ def send_velocity_world_setpoint(self, vx, vy, vz, yawrate):
122126
pk = CRTPPacket()
123127
pk.port = CRTPPort.COMMANDER_GENERIC
124128
pk.channel = SET_SETPOINT_CHANNEL
125-
pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD,
126-
vx, vy, vz, yawrate)
129+
if self._cf.platform.get_protocol_version() <= 8:
130+
warnings.warn(
131+
'Using legacy TYPE_VELOCITY_WORLD_LEGACY. Please update your crazyflie-firmware.',
132+
DeprecationWarning,
133+
)
134+
pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD_LEGACY,
135+
vx, vy, vz, -yawrate)
136+
else:
137+
pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD,
138+
vx, vy, vz, yawrate)
127139
self._cf.send_packet(pk)
128140

129141
def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
@@ -139,8 +151,16 @@ def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
139151
pk = CRTPPacket()
140152
pk.port = CRTPPort.COMMANDER_GENERIC
141153
pk.channel = SET_SETPOINT_CHANNEL
142-
pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE,
143-
roll, pitch, yawrate, zdistance)
154+
if self._cf.platform.get_protocol_version() <= 8:
155+
warnings.warn(
156+
'Using legacy TYPE_ZDISTANCE_LEGACY. Please update your crazyflie-firmware.',
157+
DeprecationWarning,
158+
)
159+
pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE_LEGACY,
160+
roll, pitch, -yawrate, zdistance)
161+
else:
162+
pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE,
163+
roll, pitch, yawrate, zdistance)
144164
self._cf.send_packet(pk)
145165

146166
def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
@@ -156,8 +176,16 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
156176
pk = CRTPPacket()
157177
pk.port = CRTPPort.COMMANDER_GENERIC
158178
pk.channel = SET_SETPOINT_CHANNEL
159-
pk.data = struct.pack('<Bffff', TYPE_HOVER,
160-
vx, vy, yawrate, zdistance)
179+
if self._cf.platform.get_protocol_version() <= 8:
180+
warnings.warn(
181+
'Using legacy TYPE_HOVER_LEGACY. Please update your crazyflie-firmware.',
182+
DeprecationWarning,
183+
)
184+
pk.data = struct.pack('<Bffff', TYPE_HOVER_LEGACY,
185+
vx, vy, -yawrate, zdistance)
186+
else:
187+
pk.data = struct.pack('<Bffff', TYPE_HOVER,
188+
vx, vy, yawrate, zdistance)
161189
self._cf.send_packet(pk)
162190

163191
def send_full_state_setpoint(self, pos, vel, acc, orientation, rollrate, pitchrate, yawrate):

‎cflib/positioning/motion_commander.py

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -347,7 +347,7 @@ def start_turn_left(self, rate=RATE):
347347
:param rate: The angular rate (degrees/second)
348348
:return:
349349
"""
350-
self._set_vel_setpoint(0.0, 0.0, 0.0, -rate)
350+
self._set_vel_setpoint(0.0, 0.0, 0.0, rate)
351351

352352
def start_turn_right(self, rate=RATE):
353353
"""
@@ -356,7 +356,7 @@ def start_turn_right(self, rate=RATE):
356356
:param rate: The angular rate (degrees/second)
357357
:return:
358358
"""
359-
self._set_vel_setpoint(0.0, 0.0, 0.0, rate)
359+
self._set_vel_setpoint(0.0, 0.0, 0.0, -rate)
360360

361361
def start_circle_left(self, radius_m, velocity=VELOCITY):
362362
"""
@@ -369,7 +369,7 @@ def start_circle_left(self, radius_m, velocity=VELOCITY):
369369
circumference = 2 * radius_m * math.pi
370370
rate = 360.0 * velocity / circumference
371371

372-
self._set_vel_setpoint(velocity, 0.0, 0.0, -rate)
372+
self._set_vel_setpoint(velocity, 0.0, 0.0, rate)
373373

374374
def start_circle_right(self, radius_m, velocity=VELOCITY):
375375
"""
@@ -382,7 +382,7 @@ def start_circle_right(self, radius_m, velocity=VELOCITY):
382382
circumference = 2 * radius_m * math.pi
383383
rate = 360.0 * velocity / circumference
384384

385-
self._set_vel_setpoint(velocity, 0.0, 0.0, rate)
385+
self._set_vel_setpoint(velocity, 0.0, 0.0, -rate)
386386

387387
def start_linear_motion(self, velocity_x_m, velocity_y_m, velocity_z_m, rate_yaw=0.0):
388388
"""

‎examples/multiranger/multiranger_wall_following.py

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -122,9 +122,7 @@ def handle_range_measurement(range):
122122
'yaw_rate', yaw_rate, 'state_wf', state_wf)
123123

124124
# convert yaw_rate from rad to deg
125-
# the negative sign is because of this ticket:
126-
# https://github.com/bitcraze/crazyflie-lib-python/issues/389
127-
yaw_rate_deg = -1 * degrees(yaw_rate)
125+
yaw_rate_deg = degrees(yaw_rate)
128126

129127
motion_commander.start_linear_motion(
130128
velocity_x, velocity_y, 0, rate_yaw=yaw_rate_deg)

‎test/positioning/test_motion_commander.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -255,7 +255,7 @@ def test_that_it_starts_turn_right(self, _SetPointThread_mock, sleep_mock):
255255

256256
# Assert
257257
thread_mock.set_vel_setpoint.assert_has_calls([
258-
call(0.0, 0.0, 0.0, rate),
258+
call(0.0, 0.0, 0.0, -rate),
259259
])
260260

261261
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):
270270

271271
# Assert
272272
thread_mock.set_vel_setpoint.assert_has_calls([
273-
call(0.0, 0.0, 0.0, -rate),
273+
call(0.0, 0.0, 0.0, rate),
274274
])
275275

276276
def test_that_it_starts_circle_right(
@@ -289,7 +289,7 @@ def test_that_it_starts_circle_right(
289289

290290
# Assert
291291
thread_mock.set_vel_setpoint.assert_has_calls([
292-
call(velocity, 0.0, 0.0, expected_rate),
292+
call(velocity, 0.0, 0.0, -expected_rate),
293293
])
294294

295295
def test_that_it_starts_circle_left(
@@ -308,7 +308,7 @@ def test_that_it_starts_circle_left(
308308

309309
# Assert
310310
thread_mock.set_vel_setpoint.assert_has_calls([
311-
call(velocity, 0.0, 0.0, -expected_rate),
311+
call(velocity, 0.0, 0.0, expected_rate),
312312
])
313313

314314
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):
325325

326326
# Assert
327327
thread_mock.set_vel_setpoint.assert_has_calls([
328-
call(0.0, 0.0, 0.0, rate),
328+
call(0.0, 0.0, 0.0, -rate),
329329
call(0.0, 0.0, 0.0, 0.0)
330330
])
331331
sleep_mock.assert_called_with(turn_time)
@@ -344,7 +344,7 @@ def test_that_it_turns_left(self, _SetPointThread_mock, sleep_mock):
344344

345345
# Assert
346346
thread_mock.set_vel_setpoint.assert_has_calls([
347-
call(0.0, 0.0, 0.0, -rate),
347+
call(0.0, 0.0, 0.0, rate),
348348
call(0.0, 0.0, 0.0, 0.0)
349349
])
350350
sleep_mock.assert_called_with(turn_time)
@@ -368,7 +368,7 @@ def test_that_it_circles_right(self, _SetPointThread_mock, sleep_mock):
368368

369369
# Assert
370370
thread_mock.set_vel_setpoint.assert_has_calls([
371-
call(velocity, 0.0, 0.0, rate),
371+
call(velocity, 0.0, 0.0, -rate),
372372
call(0.0, 0.0, 0.0, 0.0)
373373
])
374374
sleep_mock.assert_called_with(turn_time)
@@ -392,7 +392,7 @@ def test_that_it_circles_left(self, _SetPointThread_mock, sleep_mock):
392392

393393
# Assert
394394
thread_mock.set_vel_setpoint.assert_has_calls([
395-
call(velocity, 0.0, 0.0, -rate),
395+
call(velocity, 0.0, 0.0, rate),
396396
call(0.0, 0.0, 0.0, 0.0)
397397
])
398398
sleep_mock.assert_called_with(turn_time)

0 commit comments

Comments
 (0)