1
|
|
|
import threading |
2
|
|
|
from queue import Queue, Empty |
3
|
|
|
from time import sleep |
4
|
|
|
|
5
|
|
|
from libAnt.drivers.driver import Driver |
6
|
|
|
from libAnt.message import * |
7
|
|
|
|
8
|
|
|
|
9
|
|
|
class Network: |
10
|
|
|
def __init__(self, key: bytes = b'\x00' * 8, name: str = None): |
11
|
|
|
self.key = key |
12
|
|
|
self.name = name |
13
|
|
|
self.number = 0 |
14
|
|
|
|
15
|
|
|
def __str__(self): |
16
|
|
|
return self.name |
17
|
|
|
|
18
|
|
|
|
19
|
|
|
class Pump(threading.Thread): |
20
|
|
|
def __init__(self, driver: Driver, initMessages, out: Queue, onSucces, onFailure): |
21
|
|
|
super().__init__() |
22
|
|
|
self._stopper = threading.Event() |
23
|
|
|
self._driver = driver |
24
|
|
|
self._out = out |
25
|
|
|
self._initMessages = initMessages |
26
|
|
|
self._waiters = [] |
27
|
|
|
self._onSuccess = onSucces |
28
|
|
|
self._onFailure = onFailure |
29
|
|
|
|
30
|
|
|
def stop(self): |
31
|
|
|
self._driver.abort() |
32
|
|
|
self._stopper.set() |
33
|
|
|
|
34
|
|
|
def stopped(self): |
35
|
|
|
return self._stopper.isSet() |
36
|
|
|
|
37
|
|
|
def run(self): |
38
|
|
|
while not self.stopped(): |
39
|
|
|
try: |
40
|
|
|
with self._driver as d: |
41
|
|
|
# Startup |
42
|
|
|
rst = SystemResetMessage() |
43
|
|
|
self._waiters.append(rst) |
44
|
|
|
d.write(rst) |
45
|
|
|
for m in self._initMessages: |
46
|
|
|
self._waiters.append(m) |
47
|
|
|
d.write(m) |
48
|
|
|
|
49
|
|
|
while not self.stopped(): |
50
|
|
|
# Write |
51
|
|
|
try: |
52
|
|
|
outMsg = self._out.get(block=False) |
53
|
|
|
self._waiters.append(outMsg) |
54
|
|
|
d.write(outMsg) |
55
|
|
|
except Empty: |
56
|
|
|
pass |
57
|
|
|
|
58
|
|
|
# Read |
59
|
|
|
try: |
60
|
|
|
msg = d.read(timeout=1) |
61
|
|
|
if msg.type == MESSAGE_CHANNEL_EVENT: |
62
|
|
|
# This is a response to our outgoing message |
63
|
|
|
for w in self._waiters: |
64
|
|
|
if w.type == msg.content[1]: # ACK |
65
|
|
|
self._waiters.remove(w) |
66
|
|
|
# TODO: Call waiter callback from tuple (waiter, callback) |
67
|
|
|
break |
68
|
|
|
elif msg.type == MESSAGE_CHANNEL_BROADCAST_DATA: |
69
|
|
|
bmsg = BroadcastMessage(msg.type, msg.content).build(msg.content) |
70
|
|
|
self._onSuccess(bmsg) |
71
|
|
|
except Empty: |
72
|
|
|
pass |
73
|
|
|
except Exception as e: |
74
|
|
|
self._onFailure(e) |
75
|
|
|
except: |
76
|
|
|
pass |
77
|
|
|
self._waiters.clear() |
78
|
|
|
sleep(1) |
79
|
|
|
|
80
|
|
|
|
81
|
|
|
class Node: |
82
|
|
|
def __init__(self, driver: Driver, name: str = None): |
83
|
|
|
self._driver = driver |
84
|
|
|
self._name = name |
85
|
|
|
self._out = Queue() |
86
|
|
|
self._init = [] |
87
|
|
|
self._pump = None |
88
|
|
|
self._configMessages = Queue() |
89
|
|
|
|
90
|
|
|
def __enter__(self): |
91
|
|
|
return self |
92
|
|
|
|
93
|
|
|
def __exit__(self, exc_type, exc_val, exc_tb): |
94
|
|
|
self.stop() |
95
|
|
|
|
96
|
|
|
def start(self, onSuccess, onFailure): |
97
|
|
|
if not self.isRunning(): |
98
|
|
|
self._pump = Pump(self._driver, self._init, self._out, onSuccess, onFailure) |
99
|
|
|
self._pump.start() |
100
|
|
|
|
101
|
|
|
def enableRxScanMode(self, networkKey=ANTPLUS_NETWORK_KEY, channelType=CHANNEL_TYPE_ONEWAY_RECEIVE, |
102
|
|
|
frequency: int = 2457, rxTimestamp: bool = True, rssi: bool = True, channelId: bool = True): |
103
|
|
|
self._init.append(SystemResetMessage()) |
104
|
|
|
self._init.append(SetNetworkKeyMessage(0, networkKey)) |
105
|
|
|
self._init.append(AssignChannelMessage(0, channelType)) |
106
|
|
|
self._init.append(SetChannelIdMessage(0)) |
107
|
|
|
self._init.append(SetChannelRfFrequencyMessage(0, frequency)) |
108
|
|
|
self._init.append(EnableExtendedMessagesMessage()) |
109
|
|
|
self._init.append(LibConfigMessage(rxTimestamp, rssi, channelId)) |
110
|
|
|
self._init.append(OpenRxScanModeMessage()) |
111
|
|
|
|
112
|
|
|
def stop(self): |
113
|
|
|
if self.isRunning(): |
114
|
|
|
self._pump.stop() |
115
|
|
|
self._pump.join() |
116
|
|
|
|
117
|
|
|
def isRunning(self): |
118
|
|
|
if self._pump is None: |
119
|
|
|
return False |
120
|
|
|
return self._pump.is_alive() |
121
|
|
|
|
122
|
|
|
def getCapabilities(self): |
123
|
|
|
pass |
124
|
|
|
|