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

Subversion Repositories Projects

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