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