-
Notifications
You must be signed in to change notification settings - Fork 5
Expand file tree
/
Copy pathinstruction.py
More file actions
178 lines (153 loc) · 6.48 KB
/
Copy pathinstruction.py
File metadata and controls
178 lines (153 loc) · 6.48 KB
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
import crc8
import crc16
import time
import datetime
messageStart = 0x00cc
wifiMessage = 0x001a
videoRateQuery = 0x0028
lightMessage = 0x0035
flightMessage = 0x0056
logMessage = 0x1050
videoEncoderRateCommand = 0x0020
videoStartCommand = 0x0025
exposureCommand = 0x0034
timeCommand = 0x0046
stickCommand = 0x0050
takeoffCommand = 0x0054
landCommand = 0x0055
flipCommand = 0x005c
throwtakeoffCommand = 0x005d
palmLandCommand = 0x005e
bounceCommand = 0x1053
FlipFront = 0
FlipLeft = 1
FlipBack = 2
FlipRight = 3
FlipForwardLeft = 4
FlipBackLeft = 5
FlipBackRight = 6
FlipForwardRight = 7
seq = 0
rx, ry, lx, ly = 0, 0, 0, 0
throttle = 0
def create_packet(cmd, pkt, length):
l = length + 11
array_bytes = []
array_bytes.append(messageStart.to_bytes(1,byteorder='little'))
array_bytes.append((l << 3).to_bytes(1,byteorder='little'))
array_bytes.append((0).to_bytes(1,byteorder='little'))
digested= crc8.calculate_crc8(array_bytes).to_bytes(1,byteorder='little')
array_bytes.append(digested)
array_bytes.append((pkt).to_bytes(1,byteorder='little'))
instructions=(cmd).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return array_bytes
def take_off():
global seq
array_bytes = create_packet(takeoffCommand,0x68, 0)
seq += 1
instructions=seq.to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
instructions=(crc16.calculate_crc16(array_bytes)).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return b''.join(array_bytes)
def land():
global seq
array_bytes = create_packet(landCommand,0x68, 1)
seq += 1
instructions=seq.to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
array_bytes.append((0).to_bytes(1,byteorder='little'))
instructions=(crc16.calculate_crc16(array_bytes)).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return b''.join(array_bytes)
def start_video():
array_bytes = create_packet(videoStartCommand,0x60, 0)
instructions=(0).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
instructions=(crc16.calculate_crc16(array_bytes)).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return b''.join(array_bytes)
def set_videoencoder_rate(rate):
global seq
array_bytes = create_packet(videoEncoderRateCommand,0x68, 1)
seq += 1
instructions=seq.to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
array_bytes.append(rate.to_bytes(1,byteorder='little'))
instructions=(crc16.calculate_crc16(array_bytes)).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return b''.join(array_bytes)
def connection_string(port):
instructions=port.to_bytes(2,byteorder='little')
array_bytes=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
array_bytes=b'conn_req:'+b''.join(array_bytes)
return array_bytes
def up(val):
global ly
ly = float(val) / 100.0
def down(val):
global ly
ly = float(val) / 100.0 * -1
def forward(val):
global ry
ry = float(val) / 100.0
def backward(val):
global ry
ry = float(val) / 100.0 * -1
def right(val):
global rx
rx = float(val) / 100.0
def left(val):
global rx
rx = float(val) / 100.0 * -1
def clockwise(val):
global lx
lx = float(val) / 100.0
def counter_clockwise(val):
global lx
lx = float(val) / 100.0 * -1
def flip(direction):
global seq
array_bytes = create_packet(flipCommand,0x70, 1)
seq += 1
instructions=seq.to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
array_bytes.append(direction.to_bytes(1,byteorder='little'))
instructions=(crc16.calculate_crc16(array_bytes)).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return b''.join(array_bytes)
def front_flip():
return flip(FlipFront)
def back_flip():
return flip(FlipBack)
def right_flip():
return flip(FlipRight)
def left_flip():
return flip(FlipLeft)
def send_stickcommand():
array_bytes = create_packet(stickCommand, 0x60, 11)
instructions=(0).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
axis1 = int(660.0*rx + 1024.0)
axis2 = int(660.0*ry + 1024.0)
axis3 = int(660.0*ly + 1024.0)
axis4 = int(660.0*lx + 1024.0)
axis5 = int(throttle)
packed = (axis1)&0x7FF | (axis2&0x7FF)<<11 | (0x7FF&axis3)<<22 | (0x7FF&axis4)<<33 | (axis5)<<44
array_bytes.append((0xFF&packed).to_bytes(1,byteorder='little'))
array_bytes.append((packed>>8&0xFF).to_bytes(1,byteorder='little'))
array_bytes.append((packed>>16&0xFF).to_bytes(1,byteorder='little'))
array_bytes.append((packed>>24&0xFF).to_bytes(1,byteorder='little'))
array_bytes.append((packed>>32&0xFF).to_bytes(1,byteorder='little'))
array_bytes.append((packed>>40&0xFF).to_bytes(1,byteorder='little'))
now = datetime.datetime.now()
array_bytes.append((now.hour).to_bytes(1,byteorder='little'))
array_bytes.append((now.minute).to_bytes(1,byteorder='little'))
array_bytes.append((now.second).to_bytes(1,byteorder='little'))
array_bytes.append((int(time.time() * 10)&0xff).to_bytes(1,byteorder='little'))
array_bytes.append((int(time.time() * 10)>>8).to_bytes(4,byteorder='little')[0].to_bytes(1,byteorder='little'))
instructions=(crc16.calculate_crc16(array_bytes)).to_bytes(2,byteorder='little')
array_bytes+=[instructions[0].to_bytes(1,byteorder='little'),instructions[1].to_bytes(1,byteorder='little')]
return b''.join(array_bytes)
print(take_off())