Decode information from Magellan Serial 6-DOF input devices

decmagellan.py

#!/usr/bin/env python

#with information from http://paulbourke.net/dataformats/spacemouse/
smkeys=('1','2','3','4','5','6','7','8', '*','L','R','?')
nibble2sm=('0','A','B','3','D','5','6','G','H','9',':','K','<','M','N','?')
sm2nibble=dict(zip(nibble2sm,range(16)))
sm2nibble2=lambda x:ord(x) & 0xf
binary=False

def sm2coord(cstr):
	assert(len(cstr)==4)
	#coord= sm2nibble[cstr[0]] << 12 + sm2nibble[cstr[1]] << 8 +\
	#	sm2nibble[cstr[2]] << 4 +sm2nibble[cstr[3
	coord = ((sm2nibble[cstr[0]])<<12|(sm2nibble[cstr[1]])<<8|\
			(sm2nibble[cstr[2]])<<4|(sm2nibble[cstr[3]]))
	return coord-(1<<15)
		
def int2f(i1):
	return i1*1.0/(1<<9)

def smplus2coord(str1):
    number = (ord(str1[0]) << 6 & 0x03C0) | (ord(str1[1]) & 0x3F)
    if (number>512):
      number -= 1024
    return number
    
def decodesm(str1,binary=False):
	if str1.startswith('e'): #error
		raise 'SM Error'
	elif str1.startswith('d'): #pos and orient
		if binary:
			x = smplus2coord(str1[1:3])
			y = smplus2coord(str1[3:5])
			z = smplus2coord(str1[5:7])
			rx = smplus2coord(str1[7:9])
			ry = smplus2coord(str1[9:11])
			rz = smplus2coord(str1[11:13])
		else:
			x = sm2coord(str1[1:5])
			y = sm2coord(str1[5:9])
			z = sm2coord(str1[9:13])
			rx = sm2coord(str1[13:17])
			ry = sm2coord(str1[17:21])
			rz = sm2coord(str1[21:25])
			
		print '%5d %5d %5d - %5d %5d %5d' % (x,y,z,rx,ry,rz)
		#print [int2f(i) for i in [x,y,z,rx,ry,rz]]
		#print [hex(i) for i in [x,y,z,rx,ry,rz]]
	elif str1.startswith('k'): #keypress
		keys=(sm2nibble[str1[3]] << 8) | (sm2nibble[str1[2]] << 4) | (sm2nibble[str1[1]])  
		#keysstr = ''.join(['1' if keys & (1 << e) else '0' for e in range(12)])
		keystr = ''.join([value if keys & (1 <<e) else '_' for e, value in enumerate(\
			smkeys)])
		print keystr
	elif str1.startswith('m'): #mode
		print 'mode %3d' % sm2nibble[str1[1]]
		#binary=False
	elif str1.startswith('q'): #sensitivity
		print 'gain trans %2d rot %2d' % (sm2nibble[str1[2]],sm2nibble[str1[1]])
#	elif str1.startswith('b'): #beep
	elif str1.startswith('c'): #unknown
		print 'mode %s' % str1
		#binary=True
#	elif str1.startswith('z'): #reset
#	elif str1.startswith('n'): #zero radius
#	elif str1.startswith('v'): #version
	else:
		print str1
	
if __name__ == '__main__':
	#decodesm('dH000H000H000H000H000H000')
	#decodesm('dHAMKGN:NH000H000H000H000')
	#decodesm('k??G')
	import serial,time
	str1=''
	sm=serial.Serial(port='COM3',timeout=0.5)
	time.sleep(1)
	sm.write('vQ\r')
	time.sleep(1)
	#sm.write('m3\r')
	sm.write('c33\r')
	binary=True
	while sm.readable:
		str1+=sm.read()
		if str1.endswith('\r'):
			if str1.startswith('d'):
				print str1[1:].encode('hex')
			decodesm(str1,binary)
			str1=''