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