WebSVN - Projects - Diff - Rev 699 and 702 - /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_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 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
   
 
 
 
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())
       
        comm.doVibrationTest(100,5)
 
 
    except Exception,e:
        print
        print "An error occured: ", e
        print
        traceback.print_exc()
        raw_input("Press ENTER, the application will close")
 

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

Subversion Repositories Projects

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