WebSVN - Projects - Blame - Rev 705 - /VibrationTest/trunk/VibrationTest/MkProtocol.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:
705	FredericG	187	IDX_ANALOG_ANGLENICK= 2+2*0
		188	IDX_ANALOG_ANGLEROLL= 2+2*1
552	fredericg	189	IDX_ANALOG_ACCNICK = 2+2*2
		190	IDX_ANALOG_ACCROLL = 2+2*3
		191	IDX_ANALOG_COMPASS = 2+2*3
		192	IDX_ANALOG_VOLTAGE = 2+2*9
		193
		194	def __init__(self, msg):
		195	if (msg.cmd != 'D'):
		196	raise InvalidMsgType
		197	self.msg = msg
		198
705	FredericG	199	def getAngleNick(self):
		200	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLENICK)
		201
		202	def getAngleRoll(self):
		203	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLEROLL)
		204
552	fredericg	205	def getAccNick(self):
696	FredericG	206	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK)
552	fredericg	207
		208	def getAccRoll(self):
696	FredericG	209	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL)
552	fredericg	210
		211	def getCompassHeading(self):
696	FredericG	212	return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS)
552	fredericg	213
		214	def getVoltage(self):
696	FredericG	215	return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10
552	fredericg	216
		217	class VibrationDataMsg:
		218	def __init__(self, msg):
		219	if (msg.cmd != 'F'):
		220	raise InvalidMsgType
		221	self.msg = msg
		222
		223	def getData(self):
		224	data = []
563	FredericG	225	for i in range(0,50):
552	fredericg	226	data.append(self.msg.data2SignedInt(2*i))
		227	return data
		228
		229	class VersionMsg:
		230	def __init__(self, msg):
		231	if (msg.cmd != 'V'):
		232	raise InvalidMsgType
		233	self.msg = msg
		234
		235	def getVersion(self):
		236	return (self.msg.data[0], self.msg.data[1])
		237
		238	class MkComm:
653	FredericG	239	ADDRESS_ALL = 0
		240	ADDRESS_FC = 1
		241	ADDRESS_NC = 2
		242	ADDRESS_MK3MAG = 3
		243
552	fredericg	244	def __init__(self, printDebugMsg=False):
		245	#self.logfile = open('mklog.txt', "rbU")
		246
		247	self.serPort = None
		248	self.printDebugMsg = printDebugMsg
		249
		250	msg = MkMsg(address=0, cmd='v', data=[])
697	FredericG	251	self.getVersionMsgLn = msg.generateUartLine()
552	fredericg	252	msg = MkMsg(address=0, cmd='d', data=[500])
697	FredericG	253	self.getDebugMsgLn = msg.generateUartLine()
552	fredericg	254
		255
		256	def open(self, comPort):
		257	self.serPort = serial.Serial(comPort, 57600, timeout=0.5)
		258	if not self.serPort.isOpen():
		259	raise IOError("Failed to open serial port")
		260
563	FredericG	261	def close(self):
		262	self.serPort.close()
		263
552	fredericg	264	def isOpen(self):
		265	return self.serPort != None
		266
		267	def sendLn(self, ln):
		268	self.serPort.write(ln)
		269
		270	def waitForLn(self):
		271	return self.serPort.readline(eol='\r')
		272
702	FredericG	273	def waitForMsg(self, cmd2wait4, timeout=0.5):
552	fredericg	274	msg = None
		275	done = False
702	FredericG	276	if self.printDebugMsg: print "[Debug] =>Wait4 %s TO=%.1fs" % (cmd2wait4, timeout)
		277	startTime = time.clock()
552	fredericg	278	while (not done):
		279	line = self.waitForLn()
		280	try:
		281	msg = MkMsg(msg=line)
702	FredericG	282	if self.printDebugMsg: print "[Debug] msg %s" % msg.cmd
552	fredericg	283	if (msg.cmd == cmd2wait4):
		284	done = True
		285	except InvalidMsg:
702	FredericG	286	if self.printDebugMsg: print "[Debug] no valid msg"
		287
		288	if ((time.clock()-startTime) > timeout):
		289	raise NoResponse(cmd2wait4)
		290	if self.printDebugMsg: print "[Debug] Done: %.1fs" % (time.clock()-startTime)
552	fredericg	291	return msg
698	FredericG	292
		293	def sendMsg(self, msg):
		294	self.sendLn(msg.generateUartLine())
552	fredericg	295
653	FredericG	296	def sendNCRedirectUartFromFC(self):
		297	self.serPort.flushInput()
		298	msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
698	FredericG	299	self.sendMsg(msg)
653	FredericG	300	time.sleep(.5)
696	FredericG	301	# No reply expected...
		302
		303	def sendSettings(self, settings):
		304	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)
698	FredericG	305	self.sendMsg(msg)
702	FredericG	306	#time.sleep(1)
		307	msg = self.waitForMsg('S', timeout=2)
696	FredericG	308
552	fredericg	309	def getDebugMsg(self):
		310	self.serPort.flushInput()
		311	self.sendLn(self.getDebugMsgLn)
		312	msg = self.waitForMsg('D')
		313	msg = DebugDataMsg(msg)
		314	return msg
696	FredericG	315
		316	def getSettingsMsg(self, index=0xFF):
		317	self.serPort.flushInput()
		318	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])
698	FredericG	319	self.sendMsg(msg)
696	FredericG	320	msg = self.waitForMsg('Q')
		321	msg = SettingsMsg(msg)
		322	return msg
552	fredericg	323
		324	def getVersionMsg(self):
		325	self.sendLn(self.getVersionMsgLn)
		326	msg = self.waitForMsg('V')
		327	msg = VersionMsg(msg)
		328	return msg
		329
		330	def setMotorTest(self, motorSpeeds):
653	FredericG	331	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)
698	FredericG	332	self.sendMsg(msg)
552	fredericg	333
613	FredericG	334	def doVibrationTest(self, nbSamples, channel):
552	fredericg	335	data = []
563	FredericG	336	for i in range(0,(min(nbSamples,1000)/50)):
653	FredericG	337	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
698	FredericG	338	self.sendMsg(msg)
552	fredericg	339	msg = self.waitForMsg('F')
		340	msg = VibrationDataMsg(msg)
		341	data += msg.getData()
		342
618	FredericG	343	# FIXE: should be fixed in the FC code
		344	data[0]=data[1]
552	fredericg	345	return data
		346
705	FredericG	347	def recordDbgMsg(self, samplePeriod, nbSamples):
		348	result = []
		349	self.serPort.flushInput()
		350	msg = MkMsg(address=0, cmd='d', data=[int(samplePeriod*100)])
		351	self.sendMsg(msg)
		352	for i in range(nbSamples):
		353	print i
		354	msg = self.waitForMsg('D', timeout=samplePeriod+1)
		355	msg = DebugDataMsg(msg)
		356	result.append(msg)
		357	return result
552	fredericg	358
		359
		360
		361	if __name__ == '__main__':
		362	try:
		363	comm = MkComm()
702	FredericG	364	comm.printDebugMsg = True
552	fredericg	365	comm.open(comPort="COM5")
		366
		367	msg = comm.getVersionMsg()
		368	print "Version: %d.%d" % msg.getVersion()
696	FredericG	369
705	FredericG	370	# msg = comm.getSettingsMsg()
		371	# print "Index=",msg.getIndex()
		372	# print "Name=",msg.getName()
		373	# print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)
		374	# print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)
		375	#
		376	# msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)
		377	# comm.sendSettings(msg.getSettings())
696	FredericG	378
705	FredericG	379	messages = comm.recordDbgMsg(0.05, 20)
		380	for msg in messages:
		381	print msg.getAngleRoll()
702	FredericG	382
705	FredericG	383
552	fredericg	384
		385	except Exception,e:
		386	print
		387	print "An error occured: ", e
		388	print
		389	traceback.print_exc()
		390	raw_input("Press ENTER, the application will close")

Subversion Repositories Projects

(root)/VibrationTest/trunk/VibrationTest/MkProtocol.py @ 831 - Rev 705