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

Subversion Repositories Projects

(root)/VibrationTest/branches/ParameterTest/VibrationTest/MkProtocol.py - Rev 706 → 708