WebSVN - Projects - Blame - Rev 697 - /VibrationTest/trunk/VibrationTest/mkProto.py

Rev	Author	Line No.	Line
552	fredericg	1	#! /usr/bin/env python
		2
		3	#
		4	# Mikrokopter Serial protocol
		5	#
		6	# Author: FredericG
		7	#
		8
		9	import os
		10	import glob
		11	import serial
		12	import time
		13	import traceback
		14
		15	class MkException(Exception):
		16	pass
		17
		18	class InvalidMsg(MkException):
		19	def __str__(self):
		20	return "Invalid message"
		21
		22	class CrcError(MkException):
		23	def __str__(self):
		24	return "CRC error"
		25
		26	class InvalidArguments(MkException):
		27	def __str__(self):
		28	return "Invalid Arguments"
		29
		30	class InvalidMsgType(MkException):
		31	def __str__(self):
		32	return "Invalid Message type"
696	FredericG	33
		34	class InvalidMsgLen(MkException):
		35	def __str__(self):
		36	return "Invalid Message Length"
552	fredericg	37
		38	class NoResponse(MkException):
		39	def __init__(self, cmd):
		40	self.cmd = cmd
		41
		42	def __str__(self):
		43	return "No Reponse. Waiting for \"%s\" message" % self.cmd
		44
		45	pass
		46
		47	def calcCrcBytes(str):
		48	crc = 0
		49	for c in str:
		50	crc += ord(c)
		51	crc &= 0xfff
		52	return (chr(crc/64+ord('=')), chr(crc%64+ord('=')))
		53
		54
		55	class MkMsg:
		56	def __init__(self, msg=None, address=None, cmd=None, data=None):
		57	if (msg != None):
		58	# Create instance based on received message
697	FredericG	59	self.parseUartLine(msg)
552	fredericg	60	elif (address != None and cmd != None and data != None):
		61	# Create instance based on address, command and data
		62	self.address = address
		63	self.cmd = cmd
		64	self.data = data
		65	else:
		66	# Cannot create instance
		67	raise InvalidArguments
		68
697	FredericG	69	def generateUartLine(self):
552	fredericg	70	msg = ""
		71
		72	# make header
		73	msg += '#'
		74	msg += chr(self.address+ord('a'))
		75	msg += self.cmd
		76
		77	# add data
		78	done = False
		79	i = 0
		80	while (i<len(self.data)) and not done:
		81	a = 0
		82	b = 0
		83	c = 0
		84	try:
		85	a = self.data[i]
		86	b = self.data[i+1]
		87	c = self.data[i+2]
		88	i = i + 3
		89	except IndexError:
		90	done = True
		91	msg += chr(ord('=') + (a >> 2))
		92	msg += chr(ord('=') + (((a & 0x03) << 4) \| ((b & 0xf0) >> 4)))
		93	msg += chr(ord('=') + (((b & 0x0f) << 2) \| ((c & 0xc0) >> 6)))
		94	msg += chr(ord('=') + ( c & 0x3f))
		95
		96	# add crc and NL
		97	crc1,crc2 = calcCrcBytes(msg)
		98	msg += crc1 + crc2
		99	msg += '\r'
		100	return msg
		101
		102
697	FredericG	103	def parseUartLine(self, msg):
552	fredericg	104	if len(msg)<6:
		105	raise InvalidMsg()
		106	if (msg[0] != '#'):
		107	raise InvalidMsg()
		108	if (msg[-1] != '\r'):
		109	raise InvalidMsg()
		110
		111	self.address = ord(msg[1])
		112	self.cmd = msg[2]
		113
		114	data64 = map(ord, msg[3:-3]) # last 3 bytes are CRC and \n
		115
		116	done = False
		117	i = 0
		118	self.data = []
		119	while (i<len(data64)) and not done:
		120	a = 0
		121	b = 0
		122	c = 0
		123	d = 0
		124	try:
		125	a = data64[i] - ord('=')
		126	b = data64[i+1] - ord('=')
		127	c = data64[i+2] - ord('=')
		128	d = data64[i+3] - ord('=')
		129	i = i + 4
		130	except IndexError:
		131	done = True
		132
		133	self.data.append((a << 2)&0xFF \| (b >> 4))
		134	self.data.append(((b & 0x0f) << 4)&0xFF \| (c >> 2));
		135	self.data.append(((c & 0x03) << 6)&0xFF \| d);
		136
		137	crc1,crc2 = calcCrcBytes(msg[:-3])
		138	if (crc1 != msg[-3] or crc2 != msg[-2]):
		139	#print msg
		140	raise CrcError
		141
		142	#print "crc= %x %x %x %x" % ( crc1, crc2, (ord(msg[-3])-ord('=')), (ord(msg[-2])-ord('=')))
		143
		144
		145	def data2SignedInt(self, index):
		146	int = self.data[index]+self.data[index+1]*256
		147	if (int > 0xFFFF/2):
		148	int -= 0xFFFF
		149	return int
		150
696	FredericG	151	class SettingsMsg:
		152	DATENREVISION = 80
		153	IDX_INDEX = 0
		154	IDX_STICK_P = 1 + 19
		155	IDX_STICK_D = 1 + 20
		156	IDX_NAME = 1 + 90
		157
		158	def __init__(self, msg):
		159	if (msg.cmd != 'Q'):
		160	raise InvalidMsgType
		161	if len(msg.data) != 105:
		162	raise InvalidMsgLen
		163	self.msg = msg
		164
		165	def getSettings(self):
		166	return self.msg.data
		167
		168	def getIndex(self):
		169	return self.getSetting(SettingsMsg.IDX_INDEX)
		170
		171	def getSetting(self, settingIndex):
		172	return self.msg.data[settingIndex]
		173
		174	def getName(self):
		175	name = ""
		176	for i in self.msg.data[SettingsMsg.IDX_NAME:]:
		177	if i==0:
		178	break
		179	name += chr(i)
		180	return name
		181
		182	def setSetting(self, settingIndex, value):
		183	self.msg.data[settingIndex] = value
552	fredericg	184
696	FredericG	185
552	fredericg	186	class DebugDataMsg:
		187	IDX_ANALOG_ACCNICK = 2+2*2
		188	IDX_ANALOG_ACCROLL = 2+2*3
		189	IDX_ANALOG_COMPASS = 2+2*3
		190	IDX_ANALOG_VOLTAGE = 2+2*9
		191
		192	def __init__(self, msg):
		193	if (msg.cmd != 'D'):
		194	raise InvalidMsgType
		195	self.msg = msg
		196
		197	def getAccNick(self):
696	FredericG	198	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK)
552	fredericg	199
		200	def getAccRoll(self):
696	FredericG	201	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL)
552	fredericg	202
		203	def getCompassHeading(self):
696	FredericG	204	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS)
552	fredericg	205
		206	def getVoltage(self):
696	FredericG	207	return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10
552	fredericg	208
		209	class VibrationDataMsg:
		210	def __init__(self, msg):
		211	if (msg.cmd != 'F'):
		212	raise InvalidMsgType
		213	self.msg = msg
		214
		215	def getData(self):
		216	data = []
563	FredericG	217	for i in range(0,50):
552	fredericg	218	data.append(self.msg.data2SignedInt(2*i))
		219	return data
		220
		221	class VersionMsg:
		222	def __init__(self, msg):
		223	if (msg.cmd != 'V'):
		224	raise InvalidMsgType
		225	self.msg = msg
		226
		227	def getVersion(self):
		228	return (self.msg.data[0], self.msg.data[1])
		229
		230	class MkComm:
653	FredericG	231	ADDRESS_ALL = 0
		232	ADDRESS_FC = 1
		233	ADDRESS_NC = 2
		234	ADDRESS_MK3MAG = 3
		235
552	fredericg	236	def __init__(self, printDebugMsg=False):
		237	#self.logfile = open('mklog.txt', "rbU")
		238
		239	self.serPort = None
		240	self.printDebugMsg = printDebugMsg
		241
		242	msg = MkMsg(address=0, cmd='v', data=[])
697	FredericG	243	self.getVersionMsgLn = msg.generateUartLine()
552	fredericg	244	msg = MkMsg(address=0, cmd='d', data=[500])
697	FredericG	245	self.getDebugMsgLn = msg.generateUartLine()
552	fredericg	246
		247
		248	def open(self, comPort):
		249	self.serPort = serial.Serial(comPort, 57600, timeout=0.5)
		250	if not self.serPort.isOpen():
		251	raise IOError("Failed to open serial port")
		252
563	FredericG	253	def close(self):
		254	self.serPort.close()
		255
552	fredericg	256	def isOpen(self):
		257	return self.serPort != None
		258
		259	def sendLn(self, ln):
		260	self.serPort.write(ln)
		261
		262	def waitForLn(self):
		263	return self.serPort.readline(eol='\r')
		264
		265	def waitForMsg(self, cmd2wait4):
		266	msg = None
		267	done = False
		268	while (not done):
		269	line = self.waitForLn()
		270	if len(line) == 0:
		271	raise NoResponse(cmd2wait4)
		272	try:
		273	msg = MkMsg(msg=line)
		274	if (msg.cmd == cmd2wait4):
		275	done = True
		276	except InvalidMsg:
		277	if self.printDebugMsg:
		278	print "DebugMsg: \"%s\"" % line[:-1]
		279	pass
		280	return msg
		281
653	FredericG	282	def sendNCRedirectUartFromFC(self):
		283	self.serPort.flushInput()
		284	msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
697	FredericG	285	self.sendLn(msg.generateUartLine())
653	FredericG	286	time.sleep(.5)
696	FredericG	287	# No reply expected...
		288
		289	def sendSettings(self, settings):
		290	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)
697	FredericG	291	self.sendLn(msg.generateUartLine())
696	FredericG	292	time.sleep(1)
		293	msg = self.waitForMsg('S')
		294
552	fredericg	295	def getDebugMsg(self):
		296	self.serPort.flushInput()
		297	self.sendLn(self.getDebugMsgLn)
		298	msg = self.waitForMsg('D')
		299	msg = DebugDataMsg(msg)
		300	return msg
696	FredericG	301
		302	def getSettingsMsg(self, index=0xFF):
		303	self.serPort.flushInput()
		304	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])
697	FredericG	305	self.sendLn(msg.generateUartLine())
696	FredericG	306	msg = self.waitForMsg('Q')
		307	msg = SettingsMsg(msg)
		308	return msg
552	fredericg	309
		310	def getVersionMsg(self):
		311	self.sendLn(self.getVersionMsgLn)
		312	msg = self.waitForMsg('V')
		313	msg = VersionMsg(msg)
		314	return msg
		315
		316	def setMotorTest(self, motorSpeeds):
653	FredericG	317	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)
697	FredericG	318	self.sendLn(msg.generateUartLine())
552	fredericg	319
613	FredericG	320	def doVibrationTest(self, nbSamples, channel):
552	fredericg	321	data = []
563	FredericG	322	for i in range(0,(min(nbSamples,1000)/50)):
653	FredericG	323	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
697	FredericG	324	self.sendLn(msg.generateUartLine())
552	fredericg	325	msg = self.waitForMsg('F')
		326	msg = VibrationDataMsg(msg)
		327	data += msg.getData()
		328
618	FredericG	329	# FIXE: should be fixed in the FC code
		330	data[0]=data[1]
552	fredericg	331	return data
		332
		333
		334
		335
		336
		337
		338	if __name__ == '__main__':
		339	try:
		340	comm = MkComm()
		341	comm.open(comPort="COM5")
		342
		343	msg = comm.getVersionMsg()
		344	print "Version: %d.%d" % msg.getVersion()
696	FredericG	345
		346	msg = comm.getSettingsMsg()
		347	print "Index=",msg.getIndex()
		348	print "Name=",msg.getName()
		349	print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)
		350	print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)
		351
		352	msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)
		353	comm.sendSettings(msg.getSettings())
552	fredericg	354
		355
		356	except Exception,e:
		357	print
		358	print "An error occured: ", e
		359	print
		360	traceback.print_exc()
		361	raw_input("Press ENTER, the application will close")

Subversion Repositories Projects

(root)/VibrationTest/trunk/VibrationTest/mkProto.py - Rev 697