Subversion Repositories Projects

Rev

Rev 653 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
552 fredericg 1
#! /usr/bin/env python
2
 
3
#
4
# Mikrokopter Serial protocol
5
#
6
# Author: FredericG
7
# 
8
 
9
import os
10
import glob
11
import serial
12
import time
13
import traceback
14
 
15
class MkException(Exception):
16
    pass
17
 
18
class InvalidMsg(MkException):  
19
    def __str__(self):
20
      return "Invalid message"
21
 
22
class CrcError(MkException):
23
    def __str__(self):
24
      return "CRC error"
25
 
26
class InvalidArguments(MkException):
27
    def __str__(self):
28
      return "Invalid Arguments"
29
 
30
class InvalidMsgType(MkException):
31
    def __str__(self):
32
      return "Invalid Message type"
33
 
34
class NoResponse(MkException):
35
    def __init__(self, cmd):
36
      self.cmd = cmd
37
 
38
    def __str__(self):
39
      return "No Reponse. Waiting for \"%s\" message" % self.cmd
40
 
41
    pass
42
 
43
def calcCrcBytes(str):
44
    crc = 0
45
    for c in str:
46
        crc += ord(c)
47
    crc &= 0xfff
48
    return (chr(crc/64+ord('=')), chr(crc%64+ord('=')))
49
 
50
 
51
class MkMsg:
52
    def __init__(self, msg=None, address=None, cmd=None, data=None):
53
        if (msg != None):
54
            # Create instance based on received message
55
            self.parse(msg)
56
        elif (address != None and cmd != None and data != None):
57
            # Create instance based on address, command and data
58
            self.address = address
59
            self.cmd = cmd
60
            self.data = data
61
        else:
62
            # Cannot create instance
63
            raise InvalidArguments
64
 
65
    def generateMsg(self):
66
        msg = ""
67
 
68
        # make header
69
        msg += '#'
70
        msg += chr(self.address+ord('a'))
71
        msg += self.cmd
72
 
73
        # add data
74
        done = False
75
        i = 0
76
        while (i<len(self.data)) and not done:
77
            a = 0
78
            b = 0
79
            c = 0
80
            try:
81
                a = self.data[i]
82
                b = self.data[i+1]
83
                c = self.data[i+2]
84
                i = i + 3
85
            except IndexError:
86
                done = True
87
            msg += chr(ord('=') + (a >> 2))
88
            msg += chr(ord('=') + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)))
89
            msg += chr(ord('=') + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)))
90
            msg += chr(ord('=') + ( c & 0x3f))
91
 
92
        # add crc and  NL
93
        crc1,crc2 = calcCrcBytes(msg)
94
        msg += crc1 + crc2
95
        msg += '\r'
96
        return msg
97
 
98
 
99
    def parse(self, msg):
100
        if len(msg)<6:
101
            raise InvalidMsg()
102
        if (msg[0] != '#'):
103
            raise InvalidMsg()
104
        if (msg[-1] != '\r'):
105
            raise InvalidMsg()
106
 
107
        self.address = ord(msg[1])
108
        self.cmd = msg[2]
109
 
110
        data64 = map(ord, msg[3:-3])    # last 3 bytes are CRC and \n
111
 
112
        done = False
113
        i = 0
114
        self.data = []
115
        while (i<len(data64)) and not done:
116
            a = 0
117
            b = 0
118
            c = 0
119
            d = 0
120
            try:
121
                a = data64[i] - ord('=')
122
                b = data64[i+1] - ord('=')
123
                c = data64[i+2] - ord('=')
124
                d = data64[i+3] - ord('=')
125
                i = i + 4
126
            except IndexError:
127
                done = True
128
 
129
            self.data.append((a << 2)&0xFF | (b >> 4))
130
            self.data.append(((b & 0x0f) << 4)&0xFF | (c >> 2));
131
            self.data.append(((c & 0x03) << 6)&0xFF | d);
132
 
133
        crc1,crc2 = calcCrcBytes(msg[:-3])
134
        if (crc1 != msg[-3] or crc2 != msg[-2]):
135
            #print msg
136
            raise CrcError
137
 
138
        #print "crc= %x %x %x %x" % ( crc1, crc2, (ord(msg[-3])-ord('=')), (ord(msg[-2])-ord('=')))
139
 
140
 
141
    def data2SignedInt(self, index):
142
        int = self.data[index]+self.data[index+1]*256
143
        if (int > 0xFFFF/2):
144
            int -= 0xFFFF
145
        return int
146
 
147
 
148
class DebugDataMsg:
149
    IDX_ANALOG_ACCNICK  =   2+2*2
150
    IDX_ANALOG_ACCROLL  =   2+2*3
151
    IDX_ANALOG_COMPASS  =   2+2*3
152
    IDX_ANALOG_VOLTAGE  =   2+2*9
153
 
154
    def __init__(self, msg):
155
        if (msg.cmd != 'D'):
156
            raise InvalidMsgType
157
        self.msg = msg
158
 
159
    def getAccNick(self):
160
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK);
161
 
162
    def getAccRoll(self):
163
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL);
164
 
165
    def getCompassHeading(self):
166
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS);
167
 
168
    def getVoltage(self):
169
        return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10;
170
 
171
class VibrationDataMsg:
172
     def __init__(self, msg):
173
        if (msg.cmd != 'F'):
174
            raise InvalidMsgType
175
        self.msg = msg
176
 
177
     def getData(self):
178
        data = []
563 FredericG 179
        for i in range(0,50):
552 fredericg 180
          data.append(self.msg.data2SignedInt(2*i))
181
        return data
182
 
183
class VersionMsg:
184
    def __init__(self, msg):
185
        if (msg.cmd != 'V'):
186
            raise InvalidMsgType
187
        self.msg = msg
188
 
189
    def getVersion(self):
190
        return (self.msg.data[0], self.msg.data[1])
191
 
192
class MkComm:
653 FredericG 193
    ADDRESS_ALL    = 0
194
    ADDRESS_FC     = 1
195
    ADDRESS_NC     = 2
196
    ADDRESS_MK3MAG = 3
197
 
552 fredericg 198
    def __init__(self, printDebugMsg=False):
199
        #self.logfile = open('mklog.txt', "rbU")
200
 
201
        self.serPort = None
202
        self.printDebugMsg = printDebugMsg
203
 
204
        msg = MkMsg(address=0, cmd='v', data=[])
205
        self.getVersionMsgLn = msg.generateMsg()
206
        msg = MkMsg(address=0, cmd='d', data=[500])
207
        self.getDebugMsgLn = msg.generateMsg()
208
 
209
 
210
    def open(self, comPort):
211
        self.serPort = serial.Serial(comPort, 57600, timeout=0.5)
212
        if not self.serPort.isOpen():
213
            raise IOError("Failed to open serial port")
214
 
563 FredericG 215
    def close(self):
216
        self.serPort.close()
217
 
552 fredericg 218
    def isOpen(self):
219
        return self.serPort != None
220
 
221
    def sendLn(self, ln):
222
        self.serPort.write(ln)
223
 
224
    def waitForLn(self):
225
        return self.serPort.readline(eol='\r')
226
 
227
    def waitForMsg(self, cmd2wait4):
228
        msg = None
229
        done = False
230
        while (not done):
231
            line = self.waitForLn()
232
            if len(line) == 0:
233
                raise NoResponse(cmd2wait4)
234
            try:
235
                msg = MkMsg(msg=line)
236
                if (msg.cmd == cmd2wait4):
237
                    done = True
238
            except InvalidMsg:
239
                if self.printDebugMsg:
240
                  print "DebugMsg: \"%s\"" % line[:-1]
241
                pass
242
        return msg
243
 
653 FredericG 244
    def sendNCRedirectUartFromFC(self):
245
        self.serPort.flushInput()
246
        msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
247
        self.sendLn(msg.generateMsg())
248
        time.sleep(.5)
249
        # No reply expected...        
250
 
552 fredericg 251
    def getDebugMsg(self):
252
        self.serPort.flushInput()
253
        self.sendLn(self.getDebugMsgLn)
254
        msg = self.waitForMsg('D')
255
        msg = DebugDataMsg(msg)
256
        return msg
257
 
258
    def getVersionMsg(self):
259
        self.sendLn(self.getVersionMsgLn)
260
        msg = self.waitForMsg('V')
261
        msg = VersionMsg(msg)
262
        return msg
263
 
264
    def setMotorTest(self, motorSpeeds):
653 FredericG 265
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)
552 fredericg 266
        self.sendLn(msg.generateMsg())
267
 
613 FredericG 268
    def doVibrationTest(self, nbSamples, channel):
552 fredericg 269
        data = []
563 FredericG 270
        for i in range(0,(min(nbSamples,1000)/50)):
653 FredericG 271
          msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
552 fredericg 272
          self.sendLn(msg.generateMsg())
273
          msg = self.waitForMsg('F')
274
          msg = VibrationDataMsg(msg)
275
          data += msg.getData()
276
 
618 FredericG 277
        # FIXE: should be fixed in the FC code
278
        data[0]=data[1]
552 fredericg 279
        return data
280
 
281
 
282
 
283
 
284
 
285
 
286
if __name__ == '__main__':
287
    try:
288
 
289
##        file = open('mklog.txt', "rbU")
290
##
291
##        for line in file:
292
##            try:
293
##                msg = MkMsg(msg=line)
294
##                #print "Rec Addr:%02d cmd:%c data:" % (msg.address, msg.cmd), msg.data
295
##                if (msg.cmd == 'D'):
296
##                    #print len(msg.data)
297
##                    msg = DebugDataMsg(msg)
298
##
299
##                    #print "%d %d" % (msg.getAccNick(), msg.getAccRoll())
300
##
301
##                    #val = abs(msg.getAccNick())
302
##                    #print "*************************************************************"[:val/10]
303
##
304
##                    print msg.getCompassHeading()
305
##
306
##            except InvalidMsg:
307
##                pass
308
##
309
##        file.close()
310
##        print "OK"
311
 
312
##        msg = MkMsg(address=1, cmd='a', data=[255, 254, 10, 11, 12])
313
##        print msg.generateMsg()
314
##        msg2 = MkMsg(msg.generateMsg())
315
##        print msg2.address, msg2.cmd, msg2.data
316
 
317
        comm = MkComm()
318
        comm.open(comPort="COM5")
319
 
320
        msg = comm.getVersionMsg()
321
        print "Version: %d.%d" % msg.getVersion()
322
 
323
        comm.doVibrationTest()
324
 
325
#         msg = comm.getVersionMsg()
326
#         print "Version: %d.%d" % msg.getVersion()
327
# 
328
#         comm.setMotorTest([0,0,80,0])
329
#         time.sleep(10)
330
# 
331
#         minN = 0
332
#         maxN = 0
333
#         minR = 0
334
#         maxR = 0
335
# 
336
#         time.clock()
337
#         for i in range(0,1000):
338
#             try:
339
#                 msg = comm.getDebugMsg()
340
#                 n = msg.getAccNick()
341
#                 r = msg.getAccRoll()
342
#                 print "%d,%d" % (n, r)
343
#                 minN = min(minN, n)
344
#                 maxN = max(maxN, n)
345
#                 minR = min(minR, r)
346
#                 maxR = max(maxR, r)
347
#             except Exception:
348
#                 pass
349
# 
350
#         comm.setMotorTest([0,0,0,0])
351
#         print time.clock()
352
#         print maxN-minN,maxR-minR
353
 
354
 
355
 
356
 
357
    except Exception,e:
358
        print
359
        print "An error occured: ", e
360
        print
361
        traceback.print_exc()
362
        raw_input("Press ENTER, the application will close")