-
Notifications
You must be signed in to change notification settings - Fork 0
/
pollimu.py
executable file
·82 lines (68 loc) · 1.87 KB
/
pollimu.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
#!/usr/bin/python
import time, sys
import lib.motetalk as motetalk
from lib.motetalk import cmd, packetify, setpwm
def startup(m):
m.sendbase(cmd.radio(25))
m.sendbase(cmd.flags(cmd.ledmode_cnt + cmd.notick))
# m.sendheli(cmd.flags(cmd.ledmode_cnt + cmd.tick))
m.sendbase(cmd.mode(cmd.mode_sniff))
def end(m):
m.sendbase(cmd.mode(cmd.mode_spin))
m.end()
def getimu(m, addr):
m.setaddr(addr)
m.flush()
m.loadheli(cmd.mode(cmd.mode_get_imu))
m.sendbase(cmd.mode(cmd.mode_sendrecv))
try:
(arr, t, crc) = m.nextline()
if (arr == False):
errwrite("\n** Error ** ")
errwrite(repr(t) + "\n\n")
return -1, None
elif arr:
if crc:
errwrite("o")
return 1, None
else:
return 0, arr
else:
errwrite("x")
return 1, None
except:
errwrite(repr(sys.exc_info()))
errwrite("\nQuitting... \n")
return -1, None
def errwrite(s):
#sys.stderr.write(s)
#sys.stderr.flush()
return
class motepoll:
num_good = 0
num_bad = 0
num_skip = 0
done = 0
n = 0
oldarr = []
header = "addr len cmd type n sx sy z1 z3 ta mx my mz lx ly lz ti gx gy gz cm ct ca ce cp mychk chk lqi rssi"
fmtstr = '! H b b b H H H H H H h h h H H H H h h h h h h h h B H B B'
def __init__(self):
self.m = motetalk.motetalk(self.fmtstr, self.header, "COM4", debug=False,timeout=0.5)
startup(self.m)
errwrite( "Sniffing...\n")
#print "ts " + header
try:
(arr, t, crc) = self.m.nextline()
(arr, t, crc) = self.m.nextline()
(arr, t, crc) = self.m.nextline()
except:
pass
def get(self, addr):
ret, arr = getimu(self.m, addr)
if (ret == 0):
if (arr['lx'] > 36000): return 'l'
if (arr['lx'] < 32000): return 'r'
if (arr['ly'] > 36000): return 'u'
if (arr['ly'] < 32000): return 'd'
return "x"