-
Notifications
You must be signed in to change notification settings - Fork 908
/
Copy pathcommander.py
206 lines (178 loc) · 7.35 KB
/
commander.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# || ____ _ __
# +------+ / __ )(_) /_______________ _____ ___
# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
# Copyright (C) 2011-2023 Bitcraze AB
#
# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
# as published by the Free Software Foundation; either version 2
# of the License, or (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
"""
Used for sending control setpoints to the Crazyflie
"""
import struct
from cflib.crtp.crtpstack import CRTPPacket
from cflib.crtp.crtpstack import CRTPPort
from cflib.utils.encoding import compress_quaternion
__author__ = 'Bitcraze AB'
__all__ = ['Commander']
SET_SETPOINT_CHANNEL = 0
META_COMMAND_CHANNEL = 1
TYPE_STOP = 0
TYPE_VELOCITY_WORLD = 1
TYPE_ZDISTANCE = 2
TYPE_HOVER = 5
TYPE_FULL_STATE = 6
TYPE_POSITION = 7
TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0
class Commander():
"""
Used for sending control setpoints to the Crazyflie
"""
def __init__(self, crazyflie=None):
"""
Initialize the commander object. By default the commander is in
+-mode (not x-mode).
"""
self._cf = crazyflie
self._x_mode = False
def set_client_xmode(self, enabled):
"""
Enable/disable the client side X-mode. When enabled this recalculates
the setpoints before sending them to the Crazyflie.
"""
self._x_mode = enabled
def send_setpoint(self, roll, pitch, yawrate, thrust):
"""
Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the copter.
The meaning of these values is depended on the mode of the RPYT commander in the firmware.
The roll, pitch and yaw can be set in a rate or absolute mode with parameter group
`flightmode` with variables `stabModeRoll`, `.stabModeRoll` and `.stabModeRoll`.
Default settings are roll, pitch, yawrate and thrust
roll, pitch are in degrees,
yawrate is in degrees/s,
thrust is an integer value ranging from 10001 (next to no power) to 60000 (full power)
"""
if thrust > 0xFFFF or thrust < 0:
raise ValueError('Thrust must be between 0 and 0xFFFF')
if self._x_mode:
roll, pitch = 0.707 * (roll - pitch), 0.707 * (roll + pitch)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER
pk.data = struct.pack('<fffH', roll, -pitch, yawrate, thrust)
self._cf.send_packet(pk)
def send_notify_setpoint_stop(self, remain_valid_milliseconds=0):
"""
Sends a packet so that the priority of the current setpoint to the lowest non-disabled value,
so any new setpoint regardless of source will overwrite it.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.channel = META_COMMAND_CHANNEL
pk.data = struct.pack('<BI', TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP,
remain_valid_milliseconds)
self._cf.send_packet(pk)
def send_stop_setpoint(self):
"""
Send STOP setpoing, stopping the motors and (potentially) falling.
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<B', TYPE_STOP)
self._cf.send_packet(pk)
def send_velocity_world_setpoint(self, vx, vy, vz, yawrate):
"""
Send Velocity in the world frame of reference setpoint with yawrate commands
vx, vy, vz are in m/s
yawrate is in degrees/s
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.channel = SET_SETPOINT_CHANNEL
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):
"""
Control mode where the height is send as an absolute setpoint (intended
to be the distance to the surface under the Crazflie), while giving roll,
pitch and yaw rate commands
roll, pitch are in degrees
yawrate is in degrees/s
zdistance is in meters
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.channel = SET_SETPOINT_CHANNEL
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):
"""
Control mode where the height is send as an absolute setpoint (intended
to be the distance to the surface under the Crazflie), while giving x, y velocity
commands in body-fixed coordinates.
vx, vy are in m/s
yawrate is in degrees/s
zdistance is in meters
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.channel = SET_SETPOINT_CHANNEL
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):
"""
Control mode where the position, velocity, acceleration, orientation and angular
velocity are sent as absolute (world) values.
position [x, y, z] are in m
velocity [vx, vy, vz] are in m/s
acceleration [ax, ay, az] are in m/s^2
orientation [qx, qy, qz, qw] are the quaternion components of the orientation
rollrate, pitchrate, yawrate are in degrees/s
"""
def vector_to_mm_16bit(vec):
return int(vec[0] * 1000), int(vec[1] * 1000), int(vec[2] * 1000)
x, y, z = vector_to_mm_16bit(pos)
vx, vy, vz = vector_to_mm_16bit(vel)
ax, ay, az = vector_to_mm_16bit(acc)
rr, pr, yr = vector_to_mm_16bit([rollrate, pitchrate, yawrate])
orient_comp = compress_quaternion(orientation)
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.data = struct.pack('<BhhhhhhhhhIhhh', TYPE_FULL_STATE,
x, y, z,
vx, vy, vz,
ax, ay, az,
orient_comp,
rr, pr, yr)
self._cf.send_packet(pk)
def send_position_setpoint(self, x, y, z, yaw):
"""
Control mode where the position is sent as absolute (world) x,y,z coordinate in
meter and the yaw is the absolute orientation.
x, y, z are in m
yaw is in degrees
"""
pk = CRTPPacket()
pk.port = CRTPPort.COMMANDER_GENERIC
pk.channel = SET_SETPOINT_CHANNEL
pk.data = struct.pack('<Bffff', TYPE_POSITION,
x, y, z, yaw)
self._cf.send_packet(pk)