/
comm.py
382 lines (330 loc) · 11 KB
/
comm.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
from datetime import datetime
__author__ = 'will'
from messages import *
from serial.tools import list_ports
import os
import time
import threading
import config
import traceback
serial_available = False
bluetooth_available = False
try:
import serial
serial_available = True
except:
pass
try:
import bluetooth
bluetooth_available = True
except Exception, e:
print e
print " bluetooth: ", bluetooth_available
class TimeOutException(BaseException):
pass
class FuncThread(threading.Thread):
def __init__(self, target, *args):
self._target = target
self._args = args
threading.Thread.__init__(self)
def run(self):
self._target(*self._args)
class TelemetryReader():
def __init__(self, window):
self.ser = None
self.sock = None
self.last_mac = None
if serial_available or bluetooth_available:
self.run = True
self.connected = False
self.window = window
self.thread = FuncThread(self.loop)
self.thread.start()
self.attitude = [0, 0, 0]
self.buffer = ""
else:
print("could not load serial module!")
def read_all(self):
if self.ser:
return self.ser.readAll()
return None
def read(self, bytes=1):
if self.ser:
return self.ser.read(bytes)
else:
return self.btread(bytes)
def btread(self,bytes):
newdata = ""
result = None
times = 0
while not result:
if len(self.buffer)<bytes:
try:
newdata = self.sock.recv(10)
except Exception, e:
#print "nothing to receive ", e
time.sleep(0.07)
self.buffer += newdata
if len(self.buffer):
result = self.buffer[:bytes]
self.buffer = self.buffer[bytes:]
times += 1
if times >= 100:
raise TimeOutException()
return result
def write(self, data):
if self.ser:
return self.ser.write(data)
else:
return self.sock.send(data)
def flush_input(self):
return
data = None
if self.ser:
self.ser.flushInput()
else:
try:
#self.sock.set_timeout(0.1)
data = self.sock.recv(1000)
#self.sock.set_timeout(1.0)
except:
pass
print "dropped", data
#print data
def btconnect(self,device_mac):
self.last_mac = device_mac
if self.sock:
self.sock.close()
time.sleep(0.5)
self.sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
self.sock.connect((device_mac, 1))
self.sock.settimeout(1.0)
# try:
# self.sock.recv(1024)
# except Exception, e:
# print e
#self.sock.settimeout(0.1)
self.sock.setblocking(False)
#print dir(self.sock)
print "connected to bluetooth device ", device_mac
self.ser = None
self.connected = True
def find_device(self):
while self.run and not self.connected:
if bluetooth_available:
if self.last_mac:
for i in range(100):
print "lost connection? trying to reconnect to last mac, try:" , i
try:
self.btconnect(self.last_mac)
break
except Exception, e:
print e
print traceback.format_exc()
time.sleep(0.3)
devices_macs = bluetooth.discover_devices()
print "found: " , devices_macs
for device_mac in devices_macs:
if device_mac in config.macs:
self.btconnect(device_mac)
return
for device_mac in devices_macs:
if bluetooth.lookup_name(device_mac) in config.names:
self.btconnect(device_mac)
#time.sleep(2)
return
try:
self.sock = None
self.ser = serial.Serial(self.list_serial_ports()[0], 115200, timeout=1)
print "connected to " , self.ser
self.flush_input()
self.connected = True
return
except Exception as e:
print("could not connect, retrying in 3s\n", e )
time.sleep(3)
def loop(self):
while self.run:
self.find_device()
if self.connected:
try:
while self.connected:
for i in range(10):
if i % 2 == 0:
self.read_gps()
else:
self.read_attitude()
time.sleep(0.05)
except Exception as e:
self.connected = False
print "asd", (e)
def stop(self):
self.run = False
self.sock.close()
def list_serial_ports(self):
# Windows
if os.name == 'nt':
# Scan for available ports.
available = []
for i in range(256):
try:
s = serial.Serial(i)
available.append('COM' + str(i + 1))
s.close()
except serial.SerialException:
pass
return available
else:
# Mac / Linux
return [port[0] for port in list_ports.comports()]
def receiveAnswer(self, expectedCommand):
time.sleep(0.001) #print self.buffer
command = None
while command != expectedCommand:
print self.buffer
if len(self.buffer) > 15:
self.buffer = "$M>" + self.buffer.rsplit("$M>",1)[-1]
header = "000"
#print self.buffer
while "$M>" not in header:
new = ""
try:
new = self.read(1)
except TimeOutException:
print "timeout!"
return None
header += new
if len(header) > 3:
header = header[1:]
size = ord(self.read())
command = ord(self.read())
if command != expectedCommand:
print "wrong command!"
data = []
for i in range(size):
data.append(ord(self.read()))
checksum = 0
checksum ^= size;
checksum ^= command;
for i in data:
checksum ^= i;
receivedChecksum = ord(self.read())
#print 'command' , command
#print 'size' , size
#print 'data' , data
#print checksum, receivedChecksum
if command != expectedCommand:
print( "commands dont match!", command, expectedCommand, len(self.buffer))
if receivedChecksum == checksum: # was not supposed to arrive now, but data is data!
self.try_handle_response(command,data)
#self.flush_input()
return None
if checksum == receivedChecksum:
print data
return data
else:
print ('lost packet!')
return None
def MSPquery(self, command):
self.flush_input()
o = bytearray('$M<')
c = 0
o += chr(0)
c ^= o[3] #no payload
o += chr(command)
c ^= o[4]
o += chr(c)
if self.sock:
o = str(o)
answer = None
while not answer:
#print "writing" , o
self.write(o)
#self.flush_input()
try:
answer = self.receiveAnswer(command)
except TimeOutException:
print "timeout!"
#print answer
return answer
def decode32(self, data):
#print data
result = (data[0] & 0xff) + ((data[1] & 0xff) << 8) + ((data[2] & 0xff) << 16) + ((data[3] & 0xff) << 24)
is_negative = data[3] >= 128
if is_negative:
result -= 2 ** 32
return result
def decode16(self, data):
#print data
result = (data[0] & 0xff) + ((data[1] & 0xff) << 8)
is_negative = data[1] >= 128
if is_negative:
result -= 2 ** 16
return result
def read_gps(self):
answer = self.MSPquery(MSP_RAW_GPS)
if answer:
return self.try_handle_response(MSP_RAW_GPS,answer)
return (0, 0, 0)
def read_attitude(self):
answer = self.MSPquery(MSP_ATTITUDE)
if answer:
return self.try_handle_response(MSP_ATTITUDE, answer)
return (0,0,0)
def try_handle_response(self, command, answer):
if command == MSP_ATTITUDE:
roll = self.decode16(answer[0:2]) / 10.0
pitch = self.decode16(answer[2:4]) / 10.0
mag = self.decode16(answer[4:6])
self.window.set_attitude(roll, pitch, mag)
return roll, pitch, mag
elif command == MSP_RAW_GPS:
lat_list = answer[2:6]
long_list = answer[6:10]
latitude = self.decode32(lat_list) / 10000000.0
longitude = self.decode32(long_list) / 10000000.0
#print longitude,latitude, answer[0:2]
self.window.set_tracked_position(longitude, latitude, self.attitude[2])
self.window.set_data("gps_sats", answer[1])
self.window.set_data("gps_fix", answer[0])
return longitude, latitude, answer[1]
if __name__ == '__main__':
import bluetooth
target_name = "HB01"
target_address = "00:13:03:29:20:30"
#nearby_devices = bluetooth.discover_devices()
# print "founds macs: ", nearby_devices
# for mac in nearby_devices:
# print mac, bluetooth.lookup_name(mac)
# for bdaddr in nearby_devices:
# if target_name == bluetooth.lookup_name(bdaddr):
# target_address = bdaddr
# break
#
# if target_address is not None:
# print "found target bluetooth device with address ", target_address
# else:
# print "could not find target bluetooth device nearby"
#
# print bluetooth.find_service(address=target_address)
port = 1
sock = bluetooth.BluetoothSocket(bluetooth.RFCOMM)
#print sock.bound
#connected = False
sock.connect((target_address, port))
#connected = sock.connected
print 'Connected'
sock.settimeout(1.0)
sock.send('!hw\r')
try:
print sock.recv(100)
except Exception, e:
print "exception " ,e
# print MSPquery(sock, MSP_IDENT)
# print MSPquery(sock, MSP_ATTITUDE)
# print MSPquery(sock, MSP_RAW_GPS)
# print MSPquery(sock, MSP_STATUS)
sock.send("x")
print 'Sent data'
data = sock.recv(1)
print 'received [%s]' % data