WebSVN - Projects - Diff - Rev 697 and 698 - /VibrationTest/trunk/VibrationTest/mkProto.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):
        msg = None
        done = False
        while (not done):
            line = self.waitForLn()
            if len(line) == 0:
                raise NoResponse(cmd2wait4)
            try:
                msg = MkMsg(msg=line)
                if (msg.cmd == cmd2wait4):
                    done = True
            except InvalidMsg:
                if self.printDebugMsg:
                  print "DebugMsg: \"%s\"" % line[:-1]
                pass
        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')
       
    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.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())
 
 
    except Exception,e:
        print
        print "An error occured: ", e
        print
        traceback.print_exc()
        raw_input("Press ENTER, the application will close")
 

Rev 697	Rev 698
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):
266	msg = None	266	msg = None
267	done = False	267	done = False
268	while (not done):	268	while (not done):
269	line = self.waitForLn()	269	line = self.waitForLn()
270	if len(line) == 0:	270	if len(line) == 0:
271	raise NoResponse(cmd2wait4)	271	raise NoResponse(cmd2wait4)
272	try:	272	try:
273	msg = MkMsg(msg=line)	273	msg = MkMsg(msg=line)
274	if (msg.cmd == cmd2wait4):	274	if (msg.cmd == cmd2wait4):
275	done = True	275	done = True
276	except InvalidMsg:	276	except InvalidMsg:
277	if self.printDebugMsg:	277	if self.printDebugMsg:
278	print "DebugMsg: \"%s\"" % line[:-1]	278	print "DebugMsg: \"%s\"" % line[:-1]
279	pass	279	pass
280	return msg	280	return msg
-		281
-		282	def sendMsg(self, msg):
-		283	self.sendLn(msg.generateUartLine())
281		284
282	def sendNCRedirectUartFromFC(self):	285	def sendNCRedirectUartFromFC(self):
283	self.serPort.flushInput()	286	self.serPort.flushInput()
284	msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])	287	msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
285	self.sendLn(msg.generateUartLine())	288	self.sendMsg(msg)
286	time.sleep(.5)	289	time.sleep(.5)
287	# No reply expected...	290	# No reply expected...
288		291
289	def sendSettings(self, settings):	292	def sendSettings(self, settings):
290	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)	293	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)
291	self.sendLn(msg.generateUartLine())	294	self.sendMsg(msg)
292	time.sleep(1)	295	time.sleep(1)
293	msg = self.waitForMsg('S')	296	msg = self.waitForMsg('S')
294		297
295	def getDebugMsg(self):	298	def getDebugMsg(self):
296	self.serPort.flushInput()	299	self.serPort.flushInput()
297	self.sendLn(self.getDebugMsgLn)	300	self.sendLn(self.getDebugMsgLn)
298	msg = self.waitForMsg('D')	301	msg = self.waitForMsg('D')
299	msg = DebugDataMsg(msg)	302	msg = DebugDataMsg(msg)
300	return msg	303	return msg
301		304
302	def getSettingsMsg(self, index=0xFF):	305	def getSettingsMsg(self, index=0xFF):
303	self.serPort.flushInput()	306	self.serPort.flushInput()
304	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])	307	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])
305	self.sendLn(msg.generateUartLine())	308	self.sendMsg(msg)
306	msg = self.waitForMsg('Q')	309	msg = self.waitForMsg('Q')
307	msg = SettingsMsg(msg)	310	msg = SettingsMsg(msg)
308	return msg	311	return msg
309		312
310	def getVersionMsg(self):	313	def getVersionMsg(self):
311	self.sendLn(self.getVersionMsgLn)	314	self.sendLn(self.getVersionMsgLn)
312	msg = self.waitForMsg('V')	315	msg = self.waitForMsg('V')
313	msg = VersionMsg(msg)	316	msg = VersionMsg(msg)
314	return msg	317	return msg
315		318
316	def setMotorTest(self, motorSpeeds):	319	def setMotorTest(self, motorSpeeds):
317	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)	320	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)
318	self.sendLn(msg.generateUartLine())	321	self.sendMsg(msg)
319		322
320	def doVibrationTest(self, nbSamples, channel):	323	def doVibrationTest(self, nbSamples, channel):
321	data = []	324	data = []
322	for i in range(0,(min(nbSamples,1000)/50)):	325	for i in range(0,(min(nbSamples,1000)/50)):
323	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])	326	msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
324	self.sendLn(msg.generateUartLine())	327	self.sendMsg(msg)
325	msg = self.waitForMsg('F')	328	msg = self.waitForMsg('F')
326	msg = VibrationDataMsg(msg)	329	msg = VibrationDataMsg(msg)
327	data += msg.getData()	330	data += msg.getData()
328		331
329	# FIXE: should be fixed in the FC code	332	# FIXE: should be fixed in the FC code
330	data[0]=data[1]	333	data[0]=data[1]
331	return data	334	return data
332		335
333		-
334		-
335		336
336		337
337		338
338	if __name__ == '__main__':	339	if __name__ == '__main__':
339	try:	340	try:
340	comm = MkComm()	341	comm = MkComm()
341	comm.open(comPort="COM5")	342	comm.open(comPort="COM5")
342		343
343	msg = comm.getVersionMsg()	344	msg = comm.getVersionMsg()
344	print "Version: %d.%d" % msg.getVersion()	345	print "Version: %d.%d" % msg.getVersion()
345		346
346	msg = comm.getSettingsMsg()	347	msg = comm.getSettingsMsg()
347	print "Index=",msg.getIndex()	348	print "Index=",msg.getIndex()
348	print "Name=",msg.getName()	349	print "Name=",msg.getName()
349	print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)	350	print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)
350	print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)	351	print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)
351		352
352	msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)	353	msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)
353	comm.sendSettings(msg.getSettings())	354	comm.sendSettings(msg.getSettings())
354		355
355		356
356	except Exception,e:	357	except Exception,e:
357	print	358	print
358	print "An error occured: ", e	359	print "An error occured: ", e
359	print	360	print
360	traceback.print_exc()	361	traceback.print_exc()
361	raw_input("Press ENTER, the application will close")	362	raw_input("Press ENTER, the application will close")
362		363

Subversion Repositories Projects

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