292 lines
11 KiB
Python
292 lines
11 KiB
Python
#!/usr/bin/python
|
|
|
|
|
|
|
|
# eMPL_client.py
|
|
|
|
# A PC application for use with Embedded MotionApps.
|
|
|
|
# Copyright 2012 InvenSense, Inc. All Rights Reserved.
|
|
|
|
|
|
|
|
import serial, sys, time, string, pygame
|
|
|
|
from ponycube import *
|
|
|
|
|
|
|
|
class eMPL_packet_reader:
|
|
|
|
def __init__(self, port, quat_delegate=None, debug_delegate=None, data_delegate=None ):
|
|
|
|
self.s = serial.Serial(port,115200)
|
|
|
|
self.s.setTimeout(0.1)
|
|
|
|
self.s.setWriteTimeout(0.2)
|
|
|
|
# TODO: Will this break anything?
|
|
|
|
##Client attempts to write to eMPL.
|
|
|
|
#try:
|
|
|
|
#self.s.write("\n")
|
|
|
|
#except serial.serialutil.SerialTimeoutException:
|
|
|
|
#pass # write will timeout if umpl app is already started.
|
|
|
|
|
|
|
|
if quat_delegate:
|
|
|
|
self.quat_delegate = quat_delegate
|
|
|
|
else:
|
|
|
|
self.quat_delegate = empty_packet_delegate()
|
|
|
|
|
|
|
|
if debug_delegate:
|
|
|
|
self.debug_delegate = debug_delegate
|
|
|
|
else:
|
|
|
|
self.debug_delegate = empty_packet_delegate()
|
|
|
|
|
|
|
|
if data_delegate:
|
|
|
|
self.data_delegate = data_delegate
|
|
|
|
else:
|
|
|
|
self.data_delegate = empty_packet_delegate()
|
|
|
|
|
|
|
|
self.packets = []
|
|
|
|
self.length = 0
|
|
|
|
self.previous = None
|
|
|
|
|
|
|
|
def read(self):
|
|
|
|
NUM_BYTES = 23
|
|
|
|
p = None
|
|
|
|
while self.s.inWaiting() >= NUM_BYTES:
|
|
|
|
rs = self.s.read(NUM_BYTES)
|
|
|
|
if ord(rs[0]) == ord('$'):
|
|
|
|
pkt_code = ord(rs[1])
|
|
|
|
if pkt_code == 1:
|
|
|
|
d = debug_packet(rs)
|
|
|
|
self.debug_delegate.dispatch(d)
|
|
|
|
elif pkt_code == 2:
|
|
|
|
p = quat_packet(rs)
|
|
|
|
self.quat_delegate.dispatch(p)
|
|
|
|
elif pkt_code == 3:
|
|
|
|
d = data_packet(rs)
|
|
|
|
self.data_delegate.dispatch(d)
|
|
|
|
else:
|
|
|
|
print "no handler for pkt_code",pkt_code
|
|
|
|
else:
|
|
|
|
c = ' '
|
|
|
|
print "serial misaligned!"
|
|
|
|
while not ord(c) == ord('$'):
|
|
|
|
c = self.s.read(1)
|
|
|
|
self.s.read(NUM_BYTES-1)
|
|
|
|
|
|
|
|
def write(self,a):
|
|
|
|
self.s.write(a)
|
|
|
|
|
|
|
|
def close(self):
|
|
|
|
self.s.close()
|
|
|
|
|
|
|
|
def write_log(self,fname):
|
|
|
|
f = open(fname,'w')
|
|
|
|
for p in self.packets:
|
|
|
|
f.write(p.logfile_line())
|
|
|
|
f.close()
|
|
|
|
|
|
|
|
# =========== PACKET DELEGATES ==========
|
|
|
|
|
|
|
|
class packet_delegate(object):
|
|
|
|
def loop(self,event):
|
|
|
|
print "generic packet_delegate loop w/event",event
|
|
|
|
def dispatch(self,p):
|
|
|
|
print "generic packet_delegate dispatched",p
|
|
|
|
|
|
|
|
class empty_packet_delegate(packet_delegate):
|
|
|
|
def loop(self,event):
|
|
|
|
pass
|
|
|
|
def dispatch(self,p):
|
|
|
|
pass
|
|
|
|
|
|
|
|
class cube_packet_viewer (packet_delegate):
|
|
|
|
def __init__(self):
|
|
|
|
self.screen = Screen(480,400,scale=1.5)
|
|
|
|
self.cube = Cube(30,60,10)
|
|
|
|
self.q = Quaternion(1,0,0,0)
|
|
|
|
self.previous = None # previous quaternion
|
|
|
|
self.latest = None # latest packet (get in dispatch, use in loop)
|
|
|
|
|
|
|
|
def loop(self,event):
|
|
|
|
packet = self.latest
|
|
|
|
if packet:
|
|
|
|
q = packet.to_q().normalized()
|
|
|
|
self.cube.erase(self.screen)
|
|
|
|
self.cube.draw(self.screen,q)
|
|
|
|
pygame.display.flip()
|
|
|
|
self.latest = None
|
|
|
|
|
|
|
|
def dispatch(self,p):
|
|
|
|
if isinstance(p,quat_packet):
|
|
|
|
self.latest = p
|
|
|
|
|
|
|
|
class debug_packet_viewer (packet_delegate):
|
|
|
|
def loop(self,event):
|
|
|
|
pass
|
|
|
|
def dispatch(self,p):
|
|
|
|
assert isinstance(p,debug_packet);
|
|
|
|
p.display()
|
|
|
|
|
|
|
|
class data_packet_viewer (packet_delegate):
|
|
|
|
def loop(self,event):
|
|
|
|
pass
|
|
|
|
def dispatch(self,p):
|
|
|
|
assert isinstance(p,data_packet);
|
|
|
|
p.display()
|
|
|
|
|
|
|
|
# =============== PACKETS =================
|
|
|
|
|
|
|
|
# For 16-bit signed integers.
|
|
|
|
def two_bytes(d1,d2):
|
|
|
|
d = ord(d1)*256 + ord(d2)
|
|
|
|
if d > 32767:
|
|
|
|
d -= 65536
|
|
|
|
return d
|
|
|
|
|
|
|
|
# For 32-bit signed integers.
|
|
|
|
def four_bytes(d1, d2, d3, d4):
|
|
|
|
d = ord(d1)*(1<<24) + ord(d2)*(1<<16) + ord(d3)*(1<<8) + ord(d4)
|
|
|
|
if d > 2147483648:
|
|
|
|
d-= 4294967296
|
|
|
|
return d
|
|
|
|
|
|
|
|
class debug_packet (object):
|
|
|
|
# body of packet is a debug string
|
|
|
|
def __init__(self,l):
|
|
|
|
sss = []
|
|
|
|
for c in l[3:21]:
|
|
|
|
if ord(c) != 0:
|
|
|
|
sss.append(c)
|
|
|
|
self.s = "".join(sss)
|
|
|
|
|
|
|
|
def display(self):
|
|
|
|
sys.stdout.write(self.s)
|
|
|
|
|
|
|
|
class data_packet (object):
|
|
|
|
def __init__(self, l):
|
|
|
|
self.data = [0,0,0,0,0,0,0,0,0]
|
|
|
|
self.type = ord(l[2])
|
|
|
|
if self.type == 0: # accel
|
|
|
|
self.data[0] = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<16)
|
|
|
|
self.data[1] = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<16)
|
|
|
|
self.data[2] = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<16)
|
|
|
|
elif self.type == 1: # gyro
|
|
|
|
self.data[0] = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<16)
|
|
|
|
self.data[1] = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<16)
|
|
|
|
self.data[2] = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<16)
|
|
|
|
elif self.type == 2: # compass
|
|
|
|
self.data[0] = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<16)
|
|
|
|
self.data[1] = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<16)
|
|
|
|
self.data[2] = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<16)
|
|
|
|
elif self.type == 3: # quat
|
|
|
|
self.data[0] = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<30)
|
|
|
|
self.data[1] = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<30)
|
|
|
|
self.data[2] = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<30)
|
|
|
|
self.data[3] = four_bytes(l[15],l[16],l[17],l[18]) * 1.0 / (1<<30)
|
|
|
|
elif self.type == 4: # euler
|
|
|
|
self.data[0] = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<16)
|
|
|
|
self.data[1] = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<16)
|
|
|
|
self.data[2] = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<16)
|
|
|
|
elif self.type == 5: # rot
|
|
|
|
self.data[0] = two_bytes(l[3],l[4]) * 1.0 / (1<<14)
|
|
|
|
self.data[1] = two_bytes(l[5],l[6]) * 1.0 / (1<<14)
|
|
|
|
self.data[2] = two_bytes(l[7],l[8]) * 1.0 / (1<<14)
|
|
|
|
self.data[3] = two_bytes(l[9],l[10]) * 1.0 / (1<<14)
|
|
|
|
self.data[4] = two_bytes(l[11],l[12]) * 1.0 / (1<<14)
|
|
|
|
self.data[5] = two_bytes(l[13],l[14]) * 1.0 / (1<<14)
|
|
|
|
self.data[6] = two_bytes(l[15],l[16]) * 1.0 / (1<<14)
|
|
|
|
self.data[7] = two_bytes(l[17],l[18]) * 1.0 / (1<<14)
|
|
|
|
self.data[8] = two_bytes(l[19],l[20]) * 1.0 / (1<<14)
|
|
|
|
elif self.type == 6: # heading
|
|
|
|
self.data[0] = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<16)
|
|
|
|
else: # unsupported
|
|
|
|
pass
|
|
|
|
|
|
|
|
def display(self):
|
|
|
|
if self.type == 0:
|
|
|
|
print 'accel: %7.3f %7.3f %7.3f' % \
|
|
|
|
(self.data[0], self.data[1], self.data[2])
|
|
|
|
elif self.type == 1:
|
|
|
|
print 'gyro: %9.5f %9.5f %9.5f' % \
|
|
|
|
(self.data[0], self.data[1], self.data[2])
|
|
|
|
elif self.type == 2:
|
|
|
|
print 'compass: %7.4f %7.4f %7.4f' % \
|
|
|
|
(self.data[0], self.data[1], self.data[2])
|
|
|
|
elif self.type == 3:
|
|
|
|
print 'quat: %7.4f %7.4f %7.4f %7.4f' % \
|
|
|
|
(self.data[0], self.data[1], self.data[2], self.data[3])
|
|
|
|
elif self.type == 4:
|
|
|
|
print 'euler: %7.4f %7.4f %7.4f' % \
|
|
|
|
(self.data[0], self.data[1], self.data[2])
|
|
|
|
elif self.type == 5:
|
|
|
|
print 'rotation matrix: \n%7.3f %7.3f %7.3f\n%7.3f %7.3f %7.3f\n%7.3f %7.3f %7.3f' % \
|
|
|
|
(self.data[0], self.data[1], self.data[2], self.data[3], \
|
|
|
|
self.data[4], self.data[5], self.data[6], self.data[7], \
|
|
|
|
self.data[8])
|
|
|
|
elif self.type == 6:
|
|
|
|
print 'heading: %7.4f' % self.data[0]
|
|
|
|
else:
|
|
|
|
print 'what?'
|
|
|
|
|
|
|
|
class quat_packet (object):
|
|
|
|
def __init__(self, l):
|
|
|
|
self.l = l
|
|
|
|
self.q0 = four_bytes(l[3],l[4],l[5],l[6]) * 1.0 / (1<<30)
|
|
|
|
self.q1 = four_bytes(l[7],l[8],l[9],l[10]) * 1.0 / (1<<30)
|
|
|
|
self.q2 = four_bytes(l[11],l[12],l[13],l[14]) * 1.0 / (1<<30)
|
|
|
|
self.q3 = four_bytes(l[15],l[16],l[17],l[18]) * 1.0 / (1<<30)
|
|
|
|
def display_raw(self):
|
|
|
|
l = self.l
|
|
|
|
print "".join(
|
|
|
|
[ str(ord(l[0])), " "] + \
|
|
|
|
[ str(ord(l[1])), " "] + \
|
|
|
|
[ str(ord(a)).ljust(4) for a in
|
|
|
|
[ l[2], l[3], l[4], l[5], l[6], l[7], l[8], l[9], l[10] ] ] + \
|
|
|
|
[ str(ord(a)).ljust(4) for a in
|
|
|
|
[ l[8], l[9], l[10] , l[11], l[12], l[13]] ]
|
|
|
|
)
|
|
|
|
|
|
|
|
def display(self):
|
|
|
|
if 1:
|
|
|
|
print "qs " + " ".join([str(s).ljust(15) for s in
|
|
|
|
[ self.q0, self.q1, self.q2, self.q3 ]])
|
|
|
|
if 0:
|
|
|
|
euler0, euler1, euler2 = self.to_q().get_euler()
|
|
|
|
print "eulers " + " ".join([str(s).ljust(15) for s in
|
|
|
|
[ euler0, euler1, euler2 ]])
|
|
|
|
if 0:
|
|
|
|
euler0, euler1, euler2 = self.to_q().get_euler()
|
|
|
|
print "eulers " + " ".join([str(s).ljust(15) for s in
|
|
|
|
[ (euler0 * 180.0 / 3.14159) - 90 ]])
|
|
|
|
|
|
|
|
def to_q(self):
|
|
|
|
return Quaternion(self.q0, self.q1, self.q2, self.q3)
|
|
|
|
|
|
|
|
# =============== MAIN ======================
|
|
|
|
|
|
|
|
if __name__ == "__main__":
|
|
|
|
if len(sys.argv) == 2:
|
|
|
|
comport = int(sys.argv[1]) - 1
|
|
|
|
else:
|
|
|
|
print "usage: " + sys.argv[0] + " port"
|
|
|
|
sys.exit(-1)
|
|
|
|
|
|
|
|
pygame.init()
|
|
|
|
viewer = cube_packet_viewer()
|
|
|
|
debug = debug_packet_viewer()
|
|
|
|
data = data_packet_viewer()
|
|
|
|
|
|
|
|
reader = eMPL_packet_reader(comport,
|
|
|
|
quat_delegate = viewer,
|
|
|
|
debug_delegate = debug,
|
|
|
|
data_delegate = data)
|
|
|
|
|
|
|
|
while 1:
|
|
|
|
event = pygame.event.poll()
|
|
|
|
# TODO: Allow exit via keystroke.
|
|
|
|
if event.type == pygame.QUIT:
|
|
|
|
viewer.close()
|
|
|
|
break
|
|
|
|
if event.type == pygame.KEYDOWN:
|
|
|
|
reader.write(pygame.key.name(event.key))
|
|
|
|
|
|
|
|
reader.read()
|
|
|
|
viewer.loop(event)
|
|
|
|
debug.loop(event)
|
|
|
|
data.loop(event)
|
|
|
|
|
|
|
|
# TODO: If system load is too high, increase this sleep time.
|
|
|
|
pygame.time.delay(0)
|
|
|
|
|
|
|