WebSVN - Projects - Diff - Rev 702 and 705 - /VibrationTest/branches/ParameterTest/VibrationTest/MkProtocol.py


#! /usr/bin/env python
 
#
# Mikrokopter Serial protocol
#
# Author: FredericG
# 
 
import os
import glob
import serial
import time
import traceback
 
class MkException(Exception):
    pass
 
class InvalidMsg(MkException):  
    def __str__(self):
      return "Invalid message"
 
class CrcError(MkException):
    def __str__(self):
      return "CRC error"
 
class InvalidArguments(MkException):
    def __str__(self):
      return "Invalid Arguments"
 
class InvalidMsgType(MkException):
    def __str__(self):
      return "Invalid Message type"
 
class InvalidMsgLen(MkException):
    def __str__(self):
      return "Invalid Message Length"
 
class NoResponse(MkException):
    def __init__(self, cmd):
      self.cmd = cmd
     
    def __str__(self):
      return "No Reponse. Waiting for \"%s\" message" % self.cmd
     
    pass
 
def calcCrcBytes(str):
    crc = 0
    for c in str:
        crc += ord(c)
    crc &= 0xfff
    return (chr(crc/64+ord('=')), chr(crc%64+ord('=')))
 
 
class MkMsg:
    def __init__(self, msg=None, address=None, cmd=None, data=None):
        if (msg != None):
            # Create instance based on received message
            self.parseUartLine(msg)
        elif (address != None and cmd != None and data != None):
            # Create instance based on address, command and data
            self.address = address
            self.cmd = cmd
            self.data = data
        else:
            # Cannot create instance
            raise InvalidArguments
 
    def generateUartLine(self):
        msg = ""
 
        # make header
        msg += '#'
        msg += chr(self.address+ord('a'))
        msg += self.cmd
 
        # add data
        done = False
        i = 0
        while (i<len(self.data)) and not done:
            a = 0
            b = 0
            c = 0
            try:
                a = self.data[i]
                b = self.data[i+1]
                c = self.data[i+2]
                i = i + 3
            except IndexError:
                done = True
            msg += chr(ord('=') + (a >> 2))
            msg += chr(ord('=') + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)))
            msg += chr(ord('=') + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)))
            msg += chr(ord('=') + ( c & 0x3f))
 
        # add crc and  NL
        crc1,crc2 = calcCrcBytes(msg)
        msg += crc1 + crc2
        msg += '\r'
        return msg
 
 
    def parseUartLine(self, msg):
        if len(msg)<6:
            raise InvalidMsg()
        if (msg[0] != '#'):
            raise InvalidMsg()
        if (msg[-1] != '\r'):
            raise InvalidMsg()
 
        self.address = ord(msg[1])
        self.cmd = msg[2]
 
        data64 = map(ord, msg[3:-3])    # last 3 bytes are CRC and \n
 
        done = False
        i = 0
        self.data = []
        while (i<len(data64)) and not done:
            a = 0
            b = 0
            c = 0
            d = 0
            try:
                a = data64[i] - ord('=')
                b = data64[i+1] - ord('=')
                c = data64[i+2] - ord('=')
                d = data64[i+3] - ord('=')
                i = i + 4
            except IndexError:
                done = True
 
            self.data.append((a << 2)&0xFF | (b >> 4))
            self.data.append(((b & 0x0f) << 4)&0xFF | (c >> 2));
            self.data.append(((c & 0x03) << 6)&0xFF | d);
 
        crc1,crc2 = calcCrcBytes(msg[:-3])
        if (crc1 != msg[-3] or crc2 != msg[-2]):
            #print msg
            raise CrcError
       
        #print "crc= %x %x %x %x" % ( crc1, crc2, (ord(msg[-3])-ord('=')), (ord(msg[-2])-ord('=')))
 
 
    def data2SignedInt(self, index):
        int = self.data[index]+self.data[index+1]*256
        if (int > 0xFFFF/2):
            int -= 0xFFFF
        return int
 
class SettingsMsg:
    DATENREVISION   = 80
    IDX_INDEX       = 0
    IDX_STICK_P     = 1 + 19
    IDX_STICK_D     = 1 + 20
    IDX_NAME        = 1 + 90
   
    def __init__(self, msg):
        if (msg.cmd != 'Q'):
            raise InvalidMsgType
        if len(msg.data) != 105:
            raise InvalidMsgLen
        self.msg = msg
       
    def getSettings(self):
        return self.msg.data
       
    def getIndex(self):
        return self.getSetting(SettingsMsg.IDX_INDEX)
   
    def getSetting(self, settingIndex):
        return self.msg.data[settingIndex]
   
    def getName(self):
        name = ""
        for i in self.msg.data[SettingsMsg.IDX_NAME:]:
            if i==0:
                break
            name += chr(i)
        return name
   
    def setSetting(self, settingIndex, value):
        self.msg.data[settingIndex] = value
 
 
class DebugDataMsg:
    IDX_ANALOG_ANGLENICK=   2+2*0
    IDX_ANALOG_ANGLEROLL=   2+2*1
    IDX_ANALOG_ACCNICK  =   2+2*2
    IDX_ANALOG_ACCROLL  =   2+2*3
    IDX_ANALOG_COMPASS  =   2+2*3
    IDX_ANALOG_VOLTAGE  =   2+2*9
 
    def __init__(self, msg):
        if (msg.cmd != 'D'):
            raise InvalidMsgType
        self.msg = msg
 
    def getAngleNick(self):
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLENICK)
 
    def getAngleRoll(self):
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLEROLL)
 
    def getAccNick(self):
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK)
 
    def getAccRoll(self):
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL)
 
    def getCompassHeading(self):
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS)
 
    def getVoltage(self):
        return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10
 
class VibrationDataMsg:
     def __init__(self, msg):
        if (msg.cmd != 'F'):
            raise InvalidMsgType
        self.msg = msg
       
     def getData(self):
        data = []
        for i in range(0,50):
          data.append(self.msg.data2SignedInt(2*i))
        return data
 
class VersionMsg:
    def __init__(self, msg):
        if (msg.cmd != 'V'):
            raise InvalidMsgType
        self.msg = msg
 
    def getVersion(self):
        return (self.msg.data[0], self.msg.data[1])
 
class MkComm:
    ADDRESS_ALL    = 0
    ADDRESS_FC     = 1
    ADDRESS_NC     = 2
    ADDRESS_MK3MAG = 3
   
    def __init__(self, printDebugMsg=False):
        #self.logfile = open('mklog.txt', "rbU")
 
        self.serPort = None
        self.printDebugMsg = printDebugMsg
 
        msg = MkMsg(address=0, cmd='v', data=[])
        self.getVersionMsgLn = msg.generateUartLine()
        msg = MkMsg(address=0, cmd='d', data=[500])
        self.getDebugMsgLn = msg.generateUartLine()
       
 
    def open(self, comPort):
        self.serPort = serial.Serial(comPort, 57600, timeout=0.5)
        if not self.serPort.isOpen():
            raise IOError("Failed to open serial port")
 
    def close(self):
        self.serPort.close()
       
    def isOpen(self):
        return self.serPort != None
 
    def sendLn(self, ln):
        self.serPort.write(ln)
 
    def waitForLn(self):
        return self.serPort.readline(eol='\r')
 
    def waitForMsg(self, cmd2wait4, timeout=0.5):
        msg = None
        done = False
        if self.printDebugMsg: print "[Debug] =>Wait4 %s TO=%.1fs" % (cmd2wait4, timeout)
        startTime = time.clock()
        while (not done):
            line = self.waitForLn()
            try:
                msg = MkMsg(msg=line)
                if self.printDebugMsg: print "[Debug]    msg %s" % msg.cmd
                if (msg.cmd == cmd2wait4):
                    done = True
            except InvalidMsg:
                if self.printDebugMsg: print "[Debug]    no valid msg"
               
            if ((time.clock()-startTime) > timeout):
                raise NoResponse(cmd2wait4)
        if self.printDebugMsg: print "[Debug]    Done: %.1fs" % (time.clock()-startTime)
        return msg
   
    def sendMsg(self, msg):
        self.sendLn(msg.generateUartLine())
 
    def sendNCRedirectUartFromFC(self):
        self.serPort.flushInput()
        msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
        self.sendMsg(msg)
        time.sleep(.5)
        # No reply expected...   
       
    def sendSettings(self, settings):
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)
        self.sendMsg(msg)
        #time.sleep(1)
        msg = self.waitForMsg('S', timeout=2)
       
    def getDebugMsg(self):
        self.serPort.flushInput()
        self.sendLn(self.getDebugMsgLn)
        msg = self.waitForMsg('D')
        msg = DebugDataMsg(msg)
        return msg
   
    def getSettingsMsg(self, index=0xFF):
        self.serPort.flushInput()
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])
        self.sendMsg(msg)
        msg = self.waitForMsg('Q')
        msg = SettingsMsg(msg)
        return msg
 
    def getVersionMsg(self):
        self.sendLn(self.getVersionMsgLn)
        msg = self.waitForMsg('V')
        msg = VersionMsg(msg)
        return msg
 
    def setMotorTest(self, motorSpeeds):
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)
        self.sendMsg(msg)
       
    def doVibrationTest(self, nbSamples, channel):
        data = []
        for i in range(0,(min(nbSamples,1000)/50)):
          msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
          self.sendMsg(msg)
          msg = self.waitForMsg('F')
          msg = VibrationDataMsg(msg)
          data += msg.getData()
           
        # FIXE: should be fixed in the FC code
        data[0]=data[1]
        return data
   
    def recordDbgMsg(self, samplePeriod, nbSamples):
        result = []
        self.serPort.flushInput()
        msg = MkMsg(address=0, cmd='d', data=[int(samplePeriod*100)])
        self.sendMsg(msg)
        for i in range(nbSamples):
            print i
            msg = self.waitForMsg('D', timeout=samplePeriod+1)
            msg = DebugDataMsg(msg)
            result.append(msg)
        return result
 
 
 
if __name__ == '__main__':
    try:
        comm = MkComm()
        comm.printDebugMsg = True
        comm.open(comPort="COM5")
       
        msg = comm.getVersionMsg()
        print "Version: %d.%d" % msg.getVersion()
       
#        msg = comm.getSettingsMsg()
#        print "Index=",msg.getIndex()
#        print "Name=",msg.getName()
#        print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)
#        print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)
#        
#        msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)
#        comm.sendSettings(msg.getSettings())
       
        messages = comm.recordDbgMsg(0.05, 20)
        for msg in messages:
 
 
 
 
            print msg.getAngleRoll()
       
       
 
    except Exception,e:
        print
        print "An error occured: ", e
        print
        traceback.print_exc()
        raw_input("Press ENTER, the application will close")
 

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

Subversion Repositories Projects

(root)/VibrationTest/branches/ParameterTest/VibrationTest/MkProtocol.py @ 708 - Rev 702 → 705