|
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
|
|
|
|
half2me /
libant
