def read_debug(self):
NUM_BYTES = 23
p = None
time.sleep(0.005)
while self.s.inWaiting() >= NUM_BYTES:
#time.sleep(0.01)
if self.index_accel >= self.SIZE:
break
rs = self.s.read(NUM_BYTES)
if ord(rs[0]) == ord('$'):
pkt_code = ord(rs[1])
#print "."
print "\r",#+str(pkt_code),
if pkt_code == 1:
d = debug_packet(rs)
d.display()
elif pkt_code == 2:
p = quat_packet(rs)
self.quat_list[self.index_quat] = p
self.index_quat = self.index_quat + 1
#p.display()
print "+",
elif pkt_code == 3:
d = data_packet(rs)
#d.display()
self.data = d.data
datatype = d.type
if datatype ==0:
#self.index = self.index+1
#print self.index
self.accel_list[self.index_accel] = d
self.x_list[self.index_accel] = d.data[0]
self.y_list[self.index_accel] = d.data[1]
self.z_list[self.index_accel] = d.data[2]
vec = [d.data[0] , d.data[1], d.data[2]]
vec = vec - self.gravity
norm = np.linalg.norm(vec)
norm = norm-1
self.mag_list[self.index_accel] = norm
self.index_accel = self.index_accel +1
print "-",
sys.stdout.flush()
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)
print d
#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)
#d.display()3
#self.data_delegate.dispatch(d)
else:
sss = "no handler for pkt_code",pkt_code
print sss
#f_file.write(sss + '\n')
else:
c = ' '
print "serial misaligned!"
while not ord(c) == ord('$'):
c = self.s.read(1)
self.s.read(NUM_BYTES-1)
