-
Notifications
You must be signed in to change notification settings - Fork 2
/
hk310-filter.py
286 lines (224 loc) · 7.21 KB
/
hk310-filter.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
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
#******************************************************************************
#
# hk310-filter.py
#
# Modifies CH3 data in the HobbyKing HK310 or Turnigy X-3S transmitter
# on the fly.
#
#******************************************************************************
#
# Author: Werner Lane
# E-mail: [email protected]
#
#******************************************************************************
import serial
import sys
import time
values = [
0, # 1 bit
0x3f, # 6 bits
0x1f, # 5 bits
0x1f # 5 bits
]
SYNC_VALUE_NOMINAL = 0xa40
new_ch3 = values[0]
disableLoop = 0
cur_value = 0
def prepareValues():
# Sanitize values
for i, _ in enumerate(values):
if i == 0:
values[i] = values[i] & 0x01
elif i == 1:
values[i] = values[i] & 0x3f
elif i>= 2:
values[i] = values[i] & 0x1f
new = values[i]
old = values[i - 1]
# Maximize the difference between adjacent values
# This results in a minimum difference of 0x200
if abs(old - new) < abs(old - (new | (0x400 >> 5))):
new = new | (0x400 >> 5)
values[i] = new
for i, _ in enumerate(values):
print "%03x" % nextValue(i)
c = 0
def nextValue(idx):
global values
global c
if idx == 0:
rx_desired = SYNC_VALUE_NOMINAL
else:
rx_desired = values[idx] << 5
#rx_desired = c << 5
#c += 1
#if c > 0x3f:
# c = 0
rx_desired += 0x20 # Add 0x20 to avoid tiny pulse durations
rx_desired += (rx_desired >> 8)
rx = 2720 - 1 - rx_desired
return rx * 16 / 17
prepareValues()
def crc16_ccitt(crc, byte):
""" Add a byte to the CRC16-CCITT checksum.
"""
crc = crc ^ byte << 8
for _ in range(8):
if crc & 0x8000:
crc = crc << 1 ^ 0x1021
else:
crc = crc << 1
return crc & 0xffff
def hk310_filter(port):
global new_ch3
global cur_value
global values
try:
s = serial.Serial(port, 19200)
except serial.SerialException, e:
print("Unable to open port %s.\nError message: %s" % (port, e))
sys.exit(0)
STATE_WAIT_FOR_SYNC = 0
STATE_SYNC2 = 1
STATE_SYNC3 = 2
STATE_PAYLOAD3 = 3
STATE_PAYLOAD4 = 4
STATE_PAYLOAD5 = 5
STATE_PAYLOAD6 = 6
STATE_PAYLOAD7 = 7
STATE_PAYLOAD8 = 8
STATE_CRC_H = 9
STATE_CRC_L = 10
STATE_GARBAGE1 = 11
STATE_GARBAGE2 = 12
STATE_CHECKSUM_H = 13
STATE_CHECKSUM_L = 14
STATE_SKIP = 99
count = 0
state = STATE_WAIT_FOR_SYNC
while True:
b = s.read(1)
if len(b) == 0:
sys.exit(0)
b = ord(b)
if state == STATE_WAIT_FOR_SYNC:
if b == 0xff:
state = STATE_SYNC2
elif state == STATE_SYNC2:
if b == 0x55:
state = STATE_SYNC3
elif b != 0xff:
state = STATE_WAIT_FOR_SYNC
elif state == STATE_SYNC3:
if b == 0xaa:
state = STATE_PAYLOAD3
elif b == 0xff:
state = STATE_SYNC2
elif state == STATE_PAYLOAD3:
crc16 = crc16_ccitt(0, b)
checksum = b
if b == 0xaa:
state = STATE_PAYLOAD4
else:
state = STATE_SKIP
skip = 11
new_crc16 = crc16_ccitt(0, b)
new_checksum = b
elif state == STATE_PAYLOAD4:
checksum = checksum + b
crc16 = crc16_ccitt(crc16, b)
if (b >> 4) == 0xa:
# High nibble of steering
state = STATE_PAYLOAD5
else:
state = STATE_SKIP
skip = 10
new_checksum = new_checksum + b
new_crc16 = crc16_ccitt(new_crc16, b)
elif state == STATE_PAYLOAD5:
checksum = checksum + b
crc16 = crc16_ccitt(crc16, b)
t, ch3 = (b >> 4), (b & 0x0f);
# High nibble of throttle and CH3
ch3 = new_ch3 >> 8
t, ch3 = (t & 0x0f), (ch3 & 0x0f)
b = (t << 4) + ch3
state = STATE_PAYLOAD6
new_checksum = new_checksum + b
new_crc16 = crc16_ccitt(new_crc16, b)
elif state == STATE_PAYLOAD6:
checksum = checksum + b
crc16 = crc16_ccitt(crc16, b)
# Low byte of steering
state = STATE_PAYLOAD7
new_checksum = new_checksum + b
new_crc16 = crc16_ccitt(new_crc16, b)
elif state == STATE_PAYLOAD7:
checksum = checksum + b
crc16 = crc16_ccitt(crc16, b)
# Low byte of throttle
state = STATE_PAYLOAD8
new_checksum = new_checksum + b
new_crc16 = crc16_ccitt(new_crc16, b)
elif state == STATE_PAYLOAD8:
checksum = checksum + b
crc16 = crc16_ccitt(crc16, b)
# Low byte of CH3
b = new_ch3 & 0xff
state = STATE_CRC_H
new_checksum = new_checksum + b
new_crc16 = crc16_ccitt(new_crc16, b)
elif state == STATE_CRC_H:
checksum = checksum + b
received_crc16 = b << 8
state = STATE_CRC_L
b = new_crc16 >> 8
new_checksum = new_checksum + b
elif state == STATE_CRC_L:
checksum = checksum + b
received_crc16 = received_crc16 + b
if crc16 != received_crc16:
print "WARNING: incoming CRC16 mismatch!"
b = new_crc16 & 0xff
state = STATE_GARBAGE1
new_checksum = new_checksum + b
elif state == STATE_GARBAGE1:
state = STATE_GARBAGE2
elif state == STATE_GARBAGE2:
state = STATE_CHECKSUM_H
elif state == STATE_CHECKSUM_H:
received_checksum = b << 8
b = new_checksum >> 8
state = STATE_CHECKSUM_L
elif state == STATE_CHECKSUM_L:
received_checksum = received_checksum + b
if checksum != received_checksum:
print "WARNING: incoming checksum mismatch!"
b = new_checksum & 0xff
state = STATE_WAIT_FOR_SYNC
#print "Packet successfully filtered"
if count < 2:
count += 1
else:
count = 0;
if not disableLoop:
cur_value = cur_value + 1
if cur_value >= len(values):
cur_value = 0
new_ch3 = nextValue(cur_value)
elif state == STATE_SKIP:
skip = skip - 1
if skip == 0:
state = STATE_WAIT_FOR_SYNC
s.write(chr(b))
if __name__ == '__main__':
try:
port = sys.argv[1]
except IndexError:
port = '/dev/ttyUSB0'
try:
hk310_filter(port)
except KeyboardInterrupt:
print ""
#print "Average time between commands: %d ms" % (avgValue / avgCount)
sys.exit(0)