1
|
|
|
from abc import abstractmethod |
2
|
|
|
from queue import Queue, Empty |
3
|
|
|
from threading import Lock, Thread, Event |
4
|
|
|
|
5
|
|
|
import math |
6
|
|
|
from serial import Serial, SerialException, SerialTimeoutException |
7
|
|
|
|
8
|
|
|
import usb |
9
|
|
|
import time |
10
|
|
|
from struct import * |
11
|
|
|
import os |
12
|
|
|
|
13
|
|
|
from libAnt.constants import MESSAGE_TX_SYNC, MESSAGE_CHANNEL_BROADCAST_DATA |
14
|
|
|
from libAnt.message import Message, SystemResetMessage |
15
|
|
|
|
16
|
|
|
|
17
|
|
|
class DriverException(Exception): |
18
|
|
|
pass |
19
|
|
|
|
20
|
|
|
class Logger: |
21
|
|
|
def __init__(self, logFile: str): |
22
|
|
|
self._logFile = logFile |
23
|
|
|
self._log = None |
24
|
|
|
|
25
|
|
|
def __enter__(self): |
26
|
|
|
self.open() |
27
|
|
|
return self |
28
|
|
|
|
29
|
|
|
def __exit__(self, exc_type, exc_val, exc_tb): |
30
|
|
|
self.close() |
31
|
|
|
|
32
|
|
|
def open(self): |
33
|
|
|
if self._log is not None: |
34
|
|
|
self.close() |
35
|
|
|
self._log = open(self._logFile, 'wb') |
36
|
|
|
self.onOpen() |
37
|
|
|
|
38
|
|
|
def close(self): |
39
|
|
|
if self._log is not None: |
40
|
|
|
self.beforeClose() |
41
|
|
|
self._log.close() |
42
|
|
|
self.afterClose() |
43
|
|
|
|
44
|
|
|
def log(self, data: bytes): |
45
|
|
|
self._log.write(self.encodeData(data)) |
46
|
|
|
|
47
|
|
|
def onOpen(self): |
48
|
|
|
pass |
49
|
|
|
|
50
|
|
|
def beforeClose(self): |
51
|
|
|
pass |
52
|
|
|
|
53
|
|
|
def afterClose(self): |
54
|
|
|
pass |
55
|
|
|
|
56
|
|
|
def encodeData(self, data): |
57
|
|
|
return data |
58
|
|
|
|
59
|
|
|
class PcapLogger(Logger): |
60
|
|
|
def onOpen(self): |
61
|
|
|
# write pcap global header |
62
|
|
|
magic_number = b'\xD4\xC3\xB2\xA1' |
63
|
|
|
version_major = 2 |
64
|
|
|
version_minor = 4 |
65
|
|
|
thiszone = b'\x00\x00\x00\x00' |
66
|
|
|
sigfigs = b'\x00\x00\x00\x00' |
67
|
|
|
snaplen = b'\xFF\x00\x00\x00' |
68
|
|
|
network = b'\x01\x00\x00\x00' |
69
|
|
|
pcap_global_header = Struct('<4shh4s4s4s4s') |
70
|
|
|
self._log.write( |
71
|
|
|
pcap_global_header.pack(magic_number, version_major, version_minor, thiszone, sigfigs, |
72
|
|
|
snaplen, network)) |
73
|
|
|
|
74
|
|
|
def encodeData(self, data): |
75
|
|
|
timestamp = time.time() |
76
|
|
|
frac, whole = math.modf(timestamp) |
77
|
|
|
|
78
|
|
|
ts_sec = int(whole).to_bytes(4, byteorder='little') |
79
|
|
|
ts_usec = int(frac * 1000 * 1000).to_bytes(4, byteorder='little') |
80
|
|
|
incl_len = len(data) |
81
|
|
|
orig_len = incl_len |
82
|
|
|
|
83
|
|
|
pcap_packet_header = Struct('<4s4sll').pack(ts_sec, ts_usec, incl_len, orig_len) |
84
|
|
|
return pcap_packet_header + data |
85
|
|
|
|
86
|
|
|
class Driver: |
87
|
|
|
""" |
88
|
|
|
The driver provides an interface to read and write raw data to and from an ANT+ capable hardware device |
89
|
|
|
""" |
90
|
|
|
|
91
|
|
|
def __init__(self, logger: Logger = None): |
92
|
|
|
self._lock = Lock() |
93
|
|
|
self._logger = logger |
94
|
|
|
self._openTime = None |
95
|
|
|
|
96
|
|
|
def __enter__(self): |
97
|
|
|
self.open() |
98
|
|
|
return self |
99
|
|
|
|
100
|
|
|
def __exit__(self, exc_type, exc_val, exc_tb): |
101
|
|
|
self.close() |
102
|
|
|
|
103
|
|
|
def isOpen(self) -> bool: |
104
|
|
|
with self._lock: |
105
|
|
|
return self._isOpen() |
106
|
|
|
|
107
|
|
|
def open(self) -> None: |
108
|
|
|
with self._lock: |
109
|
|
|
if not self._isOpen(): |
110
|
|
|
self._openTime = time.time() |
111
|
|
|
if self._logger is not None: |
112
|
|
|
self._logger.open() |
113
|
|
|
self._open() |
114
|
|
|
|
115
|
|
|
def close(self) -> None: |
116
|
|
|
with self._lock: |
117
|
|
|
if self._isOpen: |
118
|
|
|
self._close() |
119
|
|
|
if self._logger is not None: |
120
|
|
|
self._logger.close() |
121
|
|
|
|
122
|
|
|
def reOpen(self) -> None: |
123
|
|
|
with self._lock: |
124
|
|
|
if self._isOpen(): |
125
|
|
|
self._close() |
126
|
|
|
self._open() |
127
|
|
|
|
128
|
|
|
def read(self, timeout=None) -> Message: |
129
|
|
|
# Splits the string into a list of tokens every n characters |
130
|
|
|
def splitN(str1, n): |
131
|
|
|
return [str1[start:start + n] for start in range(0, len(str1), n)] |
132
|
|
|
|
133
|
|
|
if not self.isOpen(): |
134
|
|
|
raise DriverException("Device is closed") |
135
|
|
|
|
136
|
|
|
with self._lock: |
137
|
|
|
while True: |
138
|
|
|
sync = self._read(1, timeout=timeout)[0] |
139
|
|
|
if sync is not MESSAGE_TX_SYNC: |
140
|
|
|
continue |
141
|
|
|
length = self._read(1, timeout=timeout)[0] |
142
|
|
|
type = self._read(1, timeout=timeout)[0] |
143
|
|
|
data = self._read(length, timeout=timeout) |
144
|
|
|
chk = self._read(1, timeout=timeout)[0] |
145
|
|
|
msg = Message(type, data) |
146
|
|
|
|
147
|
|
|
if self._logger: |
148
|
|
|
logMsg = bytearray([sync, length, type]) |
149
|
|
|
logMsg.extend(data) |
150
|
|
|
logMsg.append(chk) |
151
|
|
|
|
152
|
|
|
self._logger.log(bytes(logMsg)) |
153
|
|
|
|
154
|
|
|
if msg.checksum() == chk: |
155
|
|
|
return msg |
156
|
|
|
|
157
|
|
|
def write(self, msg: Message) -> None: |
158
|
|
|
if not self.isOpen(): |
159
|
|
|
raise DriverException("Device is closed") |
160
|
|
|
|
161
|
|
|
with self._lock: |
162
|
|
|
self._write(msg.encode()) |
163
|
|
|
|
164
|
|
|
@abstractmethod |
165
|
|
|
def _isOpen(self) -> bool: |
166
|
|
|
pass |
167
|
|
|
|
168
|
|
|
@abstractmethod |
169
|
|
|
def _open(self) -> None: |
170
|
|
|
pass |
171
|
|
|
|
172
|
|
|
@abstractmethod |
173
|
|
|
def _close(self) -> None: |
174
|
|
|
pass |
175
|
|
|
|
176
|
|
|
@abstractmethod |
177
|
|
|
def _read(self, count: int, timeout=None) -> bytes: |
178
|
|
|
pass |
179
|
|
|
|
180
|
|
|
@abstractmethod |
181
|
|
|
def _write(self, data: bytes) -> None: |
182
|
|
|
pass |
183
|
|
|
|
184
|
|
|
|
185
|
|
|
class SerialDriver(Driver): |
186
|
|
|
""" |
187
|
|
|
An implementation of a serial ANT+ device driver |
188
|
|
|
""" |
189
|
|
|
|
190
|
|
|
def __init__(self, device: str, baudRate: int = 115200, logger: Logger = None): |
191
|
|
|
super().__init__(logger=logger) |
192
|
|
|
self._device = device |
193
|
|
|
self._baudRate = baudRate |
194
|
|
|
self._serial = None |
195
|
|
|
|
196
|
|
|
def __str__(self): |
197
|
|
|
if self.isOpen(): |
198
|
|
|
return self._device + " @ " + str(self._baudRate) |
199
|
|
|
return None |
200
|
|
|
|
201
|
|
|
def _isOpen(self) -> bool: |
202
|
|
|
return self._serial is None |
203
|
|
|
|
204
|
|
|
def _open(self) -> None: |
205
|
|
|
try: |
206
|
|
|
self._serial = Serial(self._device, self._baudRate) |
207
|
|
|
except SerialException as e: |
208
|
|
|
raise DriverException(str(e)) |
209
|
|
|
|
210
|
|
|
if not self._serial.isOpen(): |
211
|
|
|
raise DriverException("Could not open specified device") |
212
|
|
|
|
213
|
|
|
def _close(self) -> None: |
214
|
|
|
self._serial.close() |
215
|
|
|
self._serial = None |
216
|
|
|
|
217
|
|
|
def _read(self, count: int, timeout=None) -> bytes: |
218
|
|
|
return self._serial.read(count, timeout=timeout) |
219
|
|
|
|
220
|
|
|
def _write(self, data: bytes) -> None: |
221
|
|
|
try: |
222
|
|
|
self._serial.write(data) |
223
|
|
|
self._serial.flush() |
224
|
|
|
except SerialTimeoutException as e: |
225
|
|
|
raise DriverException(str(e)) |
226
|
|
|
|
227
|
|
|
|
228
|
|
|
class USBDriver(Driver): |
229
|
|
|
""" |
230
|
|
|
An implementation of a USB ANT+ device driver |
231
|
|
|
""" |
232
|
|
|
|
233
|
|
|
def __init__(self, vid, pid, logger: Logger = None): |
234
|
|
|
super().__init__(logger=logger) |
235
|
|
|
self._idVendor = vid |
236
|
|
|
self._idProduct = pid |
237
|
|
|
self._dev = None |
238
|
|
|
self._epOut = None |
239
|
|
|
self._epIn = None |
240
|
|
|
self._interfaceNumber = None |
241
|
|
|
self._packetSize = 0x20 |
242
|
|
|
self._queue = None |
243
|
|
|
self._loop = None |
244
|
|
|
self._driver_open = False |
245
|
|
|
|
246
|
|
|
def __str__(self): |
247
|
|
|
if self.isOpen(): |
248
|
|
|
return str(self._dev) |
249
|
|
|
return "Closed" |
250
|
|
|
|
251
|
|
|
class USBLoop(Thread): |
252
|
|
|
def __init__(self, ep, packetSize: int, queue: Queue): |
253
|
|
|
super().__init__() |
254
|
|
|
self._stopper = Event() |
255
|
|
|
self._ep = ep |
256
|
|
|
self._packetSize = packetSize |
257
|
|
|
self._queue = queue |
258
|
|
|
|
259
|
|
|
def stop(self) -> None: |
260
|
|
|
self._stopper.set() |
261
|
|
|
|
262
|
|
|
def run(self) -> None: |
263
|
|
|
while not self._stopper.is_set(): |
264
|
|
|
try: |
265
|
|
|
data = self._ep.read(self._packetSize, timeout=1000) |
266
|
|
|
for d in data: |
267
|
|
|
self._queue.put(d) |
268
|
|
|
except usb.core.USBError as e: |
269
|
|
|
if e.errno not in (60, 110) and e.backend_error_code != -116: # Timout errors |
270
|
|
|
self._stopper.set() |
271
|
|
|
# We Put in an invalid byte so threads will realize the device is stopped |
272
|
|
|
self._queue.put(None) |
273
|
|
|
|
274
|
|
|
def _isOpen(self) -> bool: |
275
|
|
|
return self._driver_open |
276
|
|
|
|
277
|
|
|
def _open(self) -> None: |
278
|
|
|
print('USB OPEN START') |
279
|
|
|
try: |
280
|
|
|
# find the first USB device that matches the filter |
281
|
|
|
self._dev = usb.core.find(idVendor=self._idVendor, idProduct=self._idProduct) |
282
|
|
|
|
283
|
|
|
if self._dev is None: |
284
|
|
|
raise DriverException("Could not open specified device") |
285
|
|
|
|
286
|
|
|
# Detach kernel driver |
287
|
|
|
try: |
288
|
|
|
if self._dev.is_kernel_driver_active(0): |
289
|
|
|
try: |
290
|
|
|
self._dev.detach_kernel_driver(0) |
291
|
|
|
except usb.USBError as e: |
292
|
|
|
raise DriverException("Could not detach kernel driver") |
293
|
|
|
except NotImplementedError: |
294
|
|
|
pass # for non unix systems |
295
|
|
|
|
296
|
|
|
# set the active configuration. With no arguments, the first |
297
|
|
|
# configuration will be the active one |
298
|
|
|
self._dev.set_configuration() |
299
|
|
|
|
300
|
|
|
# get an endpoint instance |
301
|
|
|
cfg = self._dev.get_active_configuration() |
302
|
|
|
self._interfaceNumber = cfg[(0, 0)].bInterfaceNumber |
303
|
|
|
interface = usb.util.find_descriptor(cfg, bInterfaceNumber=self._interfaceNumber, |
304
|
|
|
bAlternateSetting=usb.control.get_interface(self._dev, |
305
|
|
|
self._interfaceNumber)) |
306
|
|
|
usb.util.claim_interface(self._dev, self._interfaceNumber) |
307
|
|
|
|
308
|
|
|
self._epOut = usb.util.find_descriptor(interface, custom_match=lambda e: usb.util.endpoint_direction( |
309
|
|
|
e.bEndpointAddress) == usb.ENDPOINT_OUT) |
310
|
|
|
|
311
|
|
|
self._epIn = usb.util.find_descriptor(interface, custom_match=lambda e: usb.util.endpoint_direction( |
312
|
|
|
e.bEndpointAddress) == usb.ENDPOINT_IN) |
313
|
|
|
|
314
|
|
|
if self._epOut is None or self._epIn is None: |
315
|
|
|
raise DriverException("Could not initialize USB endpoint") |
316
|
|
|
|
317
|
|
|
self._queue = Queue() |
318
|
|
|
self._loop = self.USBLoop(self._epIn, self._packetSize, self._queue) |
319
|
|
|
self._loop.start() |
320
|
|
|
self._driver_open = True |
321
|
|
|
print('USB OPEN SUCCESS') |
322
|
|
|
except IOError as e: |
323
|
|
|
self._close() |
324
|
|
|
raise DriverException(str(e)) |
325
|
|
|
|
326
|
|
|
def _close(self) -> None: |
327
|
|
|
print('USB CLOSE START') |
328
|
|
|
if self._loop is not None: |
329
|
|
|
if self._loop.is_alive(): |
330
|
|
|
self._loop.stop() |
331
|
|
|
self._loop.join() |
332
|
|
|
self._loop = None |
333
|
|
|
try: |
334
|
|
|
self._dev.reset() |
335
|
|
|
usb.util.dispose_resources(self._dev) |
336
|
|
|
except: |
337
|
|
|
pass |
338
|
|
|
self._dev = self._epOut = self._epIn = None |
339
|
|
|
self._driver_open = False |
340
|
|
|
print('USB CLOSE END') |
341
|
|
|
|
342
|
|
|
def _read(self, count: int, timeout=None) -> bytes: |
343
|
|
|
data = bytearray() |
344
|
|
|
for i in range(0, count): |
345
|
|
|
b = self._queue.get(timeout=timeout) |
346
|
|
|
if b is None: |
347
|
|
|
self._close() |
348
|
|
|
raise DriverException("Device is closed!") |
349
|
|
|
data.append(b) |
350
|
|
|
return bytes(data) |
351
|
|
|
|
352
|
|
|
def _write(self, data: bytes) -> None: |
353
|
|
|
return self._epOut.write(data) |
354
|
|
|
|
355
|
|
|
|
356
|
|
|
class DummyDriver(Driver): |
357
|
|
|
def __init__(self, logger: Logger = None): |
358
|
|
|
super().__init__(logger=logger) |
359
|
|
|
self._isopen = False |
360
|
|
|
self._data = Queue() |
361
|
|
|
msg1 = Message(MESSAGE_CHANNEL_BROADCAST_DATA, b'\x00\x01\x02\x03\x04\x05\x06\x07').encode() |
362
|
|
|
for b in msg1: |
363
|
|
|
self._data.put(b) |
364
|
|
|
msg2 = Message(MESSAGE_CHANNEL_BROADCAST_DATA, b'\x00\xF1\xF2\xF3\xF4\xF5\xF6\xF7').encode() |
365
|
|
|
for b in msg2: |
366
|
|
|
self._data.put(b) |
367
|
|
|
msg3 = Message(MESSAGE_CHANNEL_BROADCAST_DATA, b'\x00\xF9\xFA\xFB\xFC\xFD\xFE\xFF').encode() |
368
|
|
|
for b in msg3: |
369
|
|
|
self._data.put(b) |
370
|
|
|
|
371
|
|
|
def _isOpen(self) -> bool: |
372
|
|
|
return self._isopen |
373
|
|
|
|
374
|
|
|
def _close(self) -> None: |
375
|
|
|
self._isopen = False |
376
|
|
|
|
377
|
|
|
def _read(self, count: int, timeout=None) -> bytes: |
378
|
|
|
data = bytearray() |
379
|
|
|
for i in range(0, count): |
380
|
|
|
data.append(self._data.get(timeout=timeout)) |
381
|
|
|
return bytes(data) |
382
|
|
|
|
383
|
|
|
def _open(self) -> None: |
384
|
|
|
self._isopen = True |
385
|
|
|
|
386
|
|
|
def _write(self, data: bytes) -> None: |
387
|
|
|
pass |
388
|
|
|
|
389
|
|
|
|
390
|
|
|
class PcapDriver(Driver): |
391
|
|
|
def __init__(self, pcap, logger: Logger = None): |
392
|
|
|
super().__init__(logger=logger) |
393
|
|
|
self._isopen = False |
394
|
|
|
self._pcap = pcap |
395
|
|
|
self._buffer = Queue() |
396
|
|
|
|
397
|
|
|
self._loop = None |
398
|
|
|
|
399
|
|
|
class PcapLoop(Thread): |
400
|
|
|
def __init__(self, pcap, buffer: Queue): |
401
|
|
|
super().__init__() |
402
|
|
|
self._stopper = Event() |
403
|
|
|
self._pcap = pcap |
404
|
|
|
self._buffer = buffer |
405
|
|
|
|
406
|
|
|
def stop(self) -> None: |
407
|
|
|
self._stopper.set() |
408
|
|
|
|
409
|
|
|
def run(self) -> None: |
410
|
|
|
self._pcapfile = open(self._pcap, 'rb') |
411
|
|
|
self._EOF = os.stat(self._pcap).st_size |
412
|
|
|
# move file pointer to first packet header |
413
|
|
|
global_header_length = 24 |
414
|
|
|
self._pcapfile.seek(global_header_length, 0) |
415
|
|
|
|
416
|
|
|
first_ts = 0 |
417
|
|
|
start_time = time.time() |
418
|
|
|
while not self._stopper.is_set(): |
419
|
|
|
if self._pcapfile.tell() is self._EOF: |
420
|
|
|
continue |
421
|
|
|
|
422
|
|
|
ts_sec, = unpack('i', self._pcapfile.read(4)) |
423
|
|
|
ts_usec = unpack('i', self._pcapfile.read(4))[0] / 1000000 |
424
|
|
|
|
425
|
|
|
if first_ts is 0: |
426
|
|
|
first_ts = ts_sec + ts_usec |
427
|
|
|
|
428
|
|
|
ts = ts_sec + ts_usec |
429
|
|
|
send_time = ts - first_ts |
430
|
|
|
elapsed_time = time.time() - start_time |
431
|
|
|
if send_time > (elapsed_time): |
432
|
|
|
sleep_time = send_time - elapsed_time |
433
|
|
|
time.sleep(sleep_time) |
434
|
|
|
|
435
|
|
|
packet_length = unpack('i', self._pcapfile.read(4))[0] |
436
|
|
|
self._pcapfile.seek(4, 1) |
437
|
|
|
for i in range(packet_length): |
438
|
|
|
self._buffer.put(self._pcapfile.read(1)) |
439
|
|
|
|
440
|
|
|
self._pcapfile.close() |
441
|
|
|
|
442
|
|
|
def _isOpen(self) -> bool: |
443
|
|
|
return self._isopen |
444
|
|
|
|
445
|
|
|
def _open(self) -> None: |
446
|
|
|
self._isopen = True |
447
|
|
|
self._loop = self.PcapLoop(self._pcap, self._buffer) |
448
|
|
|
self._loop.start() |
449
|
|
|
|
450
|
|
|
def _close(self) -> None: |
451
|
|
|
self._isopen = False |
452
|
|
|
if self._loop is not None: |
453
|
|
|
if self._loop.is_alive(): |
454
|
|
|
self._loop.stop() |
455
|
|
|
self._loop.join() |
456
|
|
|
self._loop = None |
457
|
|
|
|
458
|
|
|
def _read(self, count: int, timeout=None) -> bytes: |
459
|
|
|
result = bytearray() |
460
|
|
|
|
461
|
|
|
while len(result) < count: |
462
|
|
|
result += self._buffer.get(block=True, timeout=timeout) |
463
|
|
|
|
464
|
|
|
return bytes(result) |
465
|
|
|
|
466
|
|
|
def _write(self, data: bytes) -> None: |
467
|
|
|
pass |
468
|
|
|
|