WebSVN - Projects - Diff - Rev 716 and 717 - /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)
        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)
        start = time.clock()
        for i in range(nbSamples):
 
            msg = self.waitForMsg('D', timeout=samplePeriod+1)
            msg = DebugDataMsg(msg)
            result.append(msg)
        print "Time = ", (time.clock()-start)
        self.getDebugMsg();
        return result
 
 
import copy
 
if __name__ == '__main__':
    try:
        comm = MkComm()
        comm.printDebugMsg = True
        comm.open(comPort="COM4")
       
        msg = comm.getVersionMsg()
        print "Version: %d.%d" % msg.getVersion()
       
        msg = MkMsg(address=2, cmd='f', data=[7, 50])
        comm.sendMsg(msg)
       
       
#        while True:
#            msg = comm.getSettingsMsg()
#            print "Index=",msg.getIndex()
#            print "Name=",msg.getName()
#            print
#            print "StickP =",msg.getSetting(SettingsMsg.IDX_STICK_P)
#            print "StickD =",msg.getSetting(SettingsMsg.IDX_STICK_D)
#            print
#            print "GyroP =",msg.getSetting(SettingsMsg.IDX_GYRO_P)
#            print "GyroI =",msg.getSetting(SettingsMsg.IDX_GYRO_I)
#            print "GyroD =",msg.getSetting(SettingsMsg.IDX_GYRO_D)
#            print "Gyro I Factor =",msg.getSetting(SettingsMsg.IDX_I_FACTOR)
#            
#            testSettings = copy.deepcopy(msg)
#            testSettings.setSetting(SettingsMsg.IDX_STICK_D, 0)
#            
#            testSettings.setSetting(SettingsMsg.IDX_GYRO_P, 0)
#            testSettings.setSetting(SettingsMsg.IDX_GYRO_I, 50)
#    
#            print "Test Settings..."
#            comm.sendSettings(testSettings.getSettings())
#            raw_input("Press ENTER to end test")
#            
#            print "Normal Settings..."
#            comm.sendSettings(msg.getSettings())
       
       
#        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()
       
       
       
        comm.close()
 
    except Exception,e:
        print
        print "An error occured: ", e
        print
        traceback.print_exc()
#        raw_input("Press ENTER, the application will close")
 

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

Subversion Repositories Projects

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