Subversion Repositories Projects

Rev

Rev 716 | 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"
696 FredericG 33
 
34
class InvalidMsgLen(MkException):
35
    def __str__(self):
36
      return "Invalid Message Length"
552 fredericg 37
 
38
class NoResponse(MkException):
39
    def __init__(self, cmd):
40
      self.cmd = cmd
41
 
42
    def __str__(self):
43
      return "No Reponse. Waiting for \"%s\" message" % self.cmd
44
 
45
    pass
46
 
47
def calcCrcBytes(str):
48
    crc = 0
49
    for c in str:
50
        crc += ord(c)
51
    crc &= 0xfff
52
    return (chr(crc/64+ord('=')), chr(crc%64+ord('=')))
53
 
54
 
55
class MkMsg:
56
    def __init__(self, msg=None, address=None, cmd=None, data=None):
57
        if (msg != None):
58
            # Create instance based on received message
697 FredericG 59
            self.parseUartLine(msg)
552 fredericg 60
        elif (address != None and cmd != None and data != None):
61
            # Create instance based on address, command and data
62
            self.address = address
63
            self.cmd = cmd
64
            self.data = data
65
        else:
66
            # Cannot create instance
67
            raise InvalidArguments
68
 
697 FredericG 69
    def generateUartLine(self):
552 fredericg 70
        msg = ""
71
 
72
        # make header
73
        msg += '#'
74
        msg += chr(self.address+ord('a'))
75
        msg += self.cmd
76
 
77
        # add data
78
        done = False
79
        i = 0
80
        while (i<len(self.data)) and not done:
81
            a = 0
82
            b = 0
83
            c = 0
84
            try:
85
                a = self.data[i]
86
                b = self.data[i+1]
87
                c = self.data[i+2]
88
                i = i + 3
89
            except IndexError:
90
                done = True
91
            msg += chr(ord('=') + (a >> 2))
92
            msg += chr(ord('=') + (((a & 0x03) << 4) | ((b & 0xf0) >> 4)))
93
            msg += chr(ord('=') + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)))
94
            msg += chr(ord('=') + ( c & 0x3f))
95
 
96
        # add crc and  NL
97
        crc1,crc2 = calcCrcBytes(msg)
98
        msg += crc1 + crc2
99
        msg += '\r'
100
        return msg
101
 
102
 
697 FredericG 103
    def parseUartLine(self, msg):
552 fredericg 104
        if len(msg)<6:
105
            raise InvalidMsg()
106
        if (msg[0] != '#'):
107
            raise InvalidMsg()
108
        if (msg[-1] != '\r'):
109
            raise InvalidMsg()
110
 
111
        self.address = ord(msg[1])
112
        self.cmd = msg[2]
113
 
114
        data64 = map(ord, msg[3:-3])    # last 3 bytes are CRC and \n
115
 
116
        done = False
117
        i = 0
118
        self.data = []
119
        while (i<len(data64)) and not done:
120
            a = 0
121
            b = 0
122
            c = 0
123
            d = 0
124
            try:
125
                a = data64[i] - ord('=')
126
                b = data64[i+1] - ord('=')
127
                c = data64[i+2] - ord('=')
128
                d = data64[i+3] - ord('=')
129
                i = i + 4
130
            except IndexError:
131
                done = True
132
 
133
            self.data.append((a << 2)&0xFF | (b >> 4))
134
            self.data.append(((b & 0x0f) << 4)&0xFF | (c >> 2));
135
            self.data.append(((c & 0x03) << 6)&0xFF | d);
136
 
137
        crc1,crc2 = calcCrcBytes(msg[:-3])
138
        if (crc1 != msg[-3] or crc2 != msg[-2]):
139
            #print msg
140
            raise CrcError
141
 
142
        #print "crc= %x %x %x %x" % ( crc1, crc2, (ord(msg[-3])-ord('=')), (ord(msg[-2])-ord('=')))
143
 
144
 
145
    def data2SignedInt(self, index):
146
        int = self.data[index]+self.data[index+1]*256
147
        if (int > 0xFFFF/2):
148
            int -= 0xFFFF
149
        return int
150
 
696 FredericG 151
class SettingsMsg:
152
    DATENREVISION   = 80
153
    IDX_INDEX       = 0
154
    IDX_STICK_P     = 1 + 19
155
    IDX_STICK_D     = 1 + 20
716 FredericG 156
    IDX_GYRO_P      = 1 + 26
157
    IDX_GYRO_I      = 1 + 27
158
    IDX_GYRO_D      = 1 + 28
159
    IDX_I_FACTOR    = 1 + 35
696 FredericG 160
    IDX_NAME        = 1 + 90
161
 
162
    def __init__(self, msg):
163
        if (msg.cmd != 'Q'):
164
            raise InvalidMsgType
165
        if len(msg.data) != 105:
166
            raise InvalidMsgLen
167
        self.msg = msg
168
 
169
    def getSettings(self):
170
        return self.msg.data
171
 
172
    def getIndex(self):
173
        return self.getSetting(SettingsMsg.IDX_INDEX)
174
 
175
    def getSetting(self, settingIndex):
176
        return self.msg.data[settingIndex]
177
 
178
    def getName(self):
179
        name = ""
180
        for i in self.msg.data[SettingsMsg.IDX_NAME:]:
181
            if i==0:
182
                break
183
            name += chr(i)
184
        return name
185
 
186
    def setSetting(self, settingIndex, value):
187
        self.msg.data[settingIndex] = value
552 fredericg 188
 
696 FredericG 189
 
552 fredericg 190
class DebugDataMsg:
705 FredericG 191
    IDX_ANALOG_ANGLENICK=   2+2*0
192
    IDX_ANALOG_ANGLEROLL=   2+2*1
552 fredericg 193
    IDX_ANALOG_ACCNICK  =   2+2*2
194
    IDX_ANALOG_ACCROLL  =   2+2*3
195
    IDX_ANALOG_COMPASS  =   2+2*3
196
    IDX_ANALOG_VOLTAGE  =   2+2*9
197
 
198
    def __init__(self, msg):
199
        if (msg.cmd != 'D'):
200
            raise InvalidMsgType
201
        self.msg = msg
202
 
705 FredericG 203
    def getAngleNick(self):
204
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLENICK)
205
 
206
    def getAngleRoll(self):
207
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLEROLL)
208
 
552 fredericg 209
    def getAccNick(self):
696 FredericG 210
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK)
552 fredericg 211
 
212
    def getAccRoll(self):
696 FredericG 213
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL)
552 fredericg 214
 
215
    def getCompassHeading(self):
696 FredericG 216
        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS)
552 fredericg 217
 
218
    def getVoltage(self):
696 FredericG 219
        return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10
552 fredericg 220
 
221
class VibrationDataMsg:
222
     def __init__(self, msg):
223
        if (msg.cmd != 'F'):
224
            raise InvalidMsgType
225
        self.msg = msg
226
 
227
     def getData(self):
228
        data = []
563 FredericG 229
        for i in range(0,50):
552 fredericg 230
          data.append(self.msg.data2SignedInt(2*i))
231
        return data
232
 
233
class VersionMsg:
234
    def __init__(self, msg):
235
        if (msg.cmd != 'V'):
236
            raise InvalidMsgType
237
        self.msg = msg
238
 
239
    def getVersion(self):
240
        return (self.msg.data[0], self.msg.data[1])
241
 
242
class MkComm:
653 FredericG 243
    ADDRESS_ALL    = 0
244
    ADDRESS_FC     = 1
245
    ADDRESS_NC     = 2
246
    ADDRESS_MK3MAG = 3
247
 
552 fredericg 248
    def __init__(self, printDebugMsg=False):
249
        #self.logfile = open('mklog.txt', "rbU")
250
 
251
        self.serPort = None
252
        self.printDebugMsg = printDebugMsg
253
 
254
        msg = MkMsg(address=0, cmd='v', data=[])
697 FredericG 255
        self.getVersionMsgLn = msg.generateUartLine()
552 fredericg 256
        msg = MkMsg(address=0, cmd='d', data=[500])
697 FredericG 257
        self.getDebugMsgLn = msg.generateUartLine()
552 fredericg 258
 
259
 
260
    def open(self, comPort):
717 FredericG 261
        self.serPort = serial.Serial(comPort, 57600, timeout=0.5)
552 fredericg 262
        if not self.serPort.isOpen():
263
            raise IOError("Failed to open serial port")
264
 
563 FredericG 265
    def close(self):
266
        self.serPort.close()
267
 
552 fredericg 268
    def isOpen(self):
269
        return self.serPort != None
270
 
271
    def sendLn(self, ln):
272
        self.serPort.write(ln)
273
 
274
    def waitForLn(self):
275
        return self.serPort.readline(eol='\r')
276
 
702 FredericG 277
    def waitForMsg(self, cmd2wait4, timeout=0.5):
552 fredericg 278
        msg = None
279
        done = False
702 FredericG 280
        if self.printDebugMsg: print "[Debug] =>Wait4 %s TO=%.1fs" % (cmd2wait4, timeout)
281
        startTime = time.clock()
552 fredericg 282
        while (not done):
283
            line = self.waitForLn()
284
            try:
285
                msg = MkMsg(msg=line)
702 FredericG 286
                if self.printDebugMsg: print "[Debug]    msg %s" % msg.cmd
552 fredericg 287
                if (msg.cmd == cmd2wait4):
288
                    done = True
289
            except InvalidMsg:
702 FredericG 290
                if self.printDebugMsg: print "[Debug]    no valid msg"
291
 
292
            if ((time.clock()-startTime) > timeout):
293
                raise NoResponse(cmd2wait4)
294
        if self.printDebugMsg: print "[Debug]    Done: %.1fs" % (time.clock()-startTime)
552 fredericg 295
        return msg
698 FredericG 296
 
297
    def sendMsg(self, msg):
298
        self.sendLn(msg.generateUartLine())
552 fredericg 299
 
653 FredericG 300
    def sendNCRedirectUartFromFC(self):
301
        self.serPort.flushInput()
302
        msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
698 FredericG 303
        self.sendMsg(msg)
653 FredericG 304
        time.sleep(.5)
696 FredericG 305
        # No reply expected...   
306
 
307
    def sendSettings(self, settings):
308
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)
698 FredericG 309
        self.sendMsg(msg)
702 FredericG 310
        #time.sleep(1)
311
        msg = self.waitForMsg('S', timeout=2)
696 FredericG 312
 
552 fredericg 313
    def getDebugMsg(self):
314
        self.serPort.flushInput()
315
        self.sendLn(self.getDebugMsgLn)
316
        msg = self.waitForMsg('D')
317
        msg = DebugDataMsg(msg)
318
        return msg
696 FredericG 319
 
320
    def getSettingsMsg(self, index=0xFF):
321
        self.serPort.flushInput()
322
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])
698 FredericG 323
        self.sendMsg(msg)
696 FredericG 324
        msg = self.waitForMsg('Q')
325
        msg = SettingsMsg(msg)
326
        return msg
552 fredericg 327
 
328
    def getVersionMsg(self):
329
        self.sendLn(self.getVersionMsgLn)
330
        msg = self.waitForMsg('V')
331
        msg = VersionMsg(msg)
332
        return msg
333
 
334
    def setMotorTest(self, motorSpeeds):
653 FredericG 335
        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds)
698 FredericG 336
        self.sendMsg(msg)
552 fredericg 337
 
613 FredericG 338
    def doVibrationTest(self, nbSamples, channel):
552 fredericg 339
        data = []
563 FredericG 340
        for i in range(0,(min(nbSamples,1000)/50)):
653 FredericG 341
          msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
698 FredericG 342
          self.sendMsg(msg)
552 fredericg 343
          msg = self.waitForMsg('F')
344
          msg = VibrationDataMsg(msg)
345
          data += msg.getData()
346
 
618 FredericG 347
        # FIXE: should be fixed in the FC code
348
        data[0]=data[1]
552 fredericg 349
        return data
350
 
705 FredericG 351
    def recordDbgMsg(self, samplePeriod, nbSamples):
352
        result = []
353
        self.serPort.flushInput()
354
        msg = MkMsg(address=0, cmd='d', data=[int(samplePeriod*100)])
355
        self.sendMsg(msg)
717 FredericG 356
        start = time.clock()
705 FredericG 357
        for i in range(nbSamples):
358
            msg = self.waitForMsg('D', timeout=samplePeriod+1)
359
            msg = DebugDataMsg(msg)
360
            result.append(msg)
717 FredericG 361
        print "Time = ", (time.clock()-start)
362
        self.getDebugMsg();
705 FredericG 363
        return result
552 fredericg 364
 
365
 
717 FredericG 366
import copy
552 fredericg 367
 
368
if __name__ == '__main__':
369
    try:
370
        comm = MkComm()
702 FredericG 371
        comm.printDebugMsg = True
717 FredericG 372
        comm.open(comPort="COM4")
552 fredericg 373
 
374
        msg = comm.getVersionMsg()
375
        print "Version: %d.%d" % msg.getVersion()
696 FredericG 376
 
717 FredericG 377
        msg = MkMsg(address=2, cmd='f', data=[7, 50])
378
        comm.sendMsg(msg)
379
 
380
 
381
#        while True:
382
#            msg = comm.getSettingsMsg()
383
#            print "Index=",msg.getIndex()
384
#            print "Name=",msg.getName()
385
#            print
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())
402
#            raw_input("Press ENTER to end test")
403
#            
404
#            print "Normal Settings..."
405
#            comm.sendSettings(msg.getSettings())
406
 
407
 
705 FredericG 408
#        msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)
409
#        comm.sendSettings(msg.getSettings())
696 FredericG 410
 
717 FredericG 411
#        messages = comm.recordDbgMsg(0.05, 20)
412
#        for msg in messages:
413
#            print msg.getAngleRoll()
702 FredericG 414
 
705 FredericG 415
 
717 FredericG 416
 
417
        comm.close()
552 fredericg 418
 
419
    except Exception,e:
420
        print
421
        print "An error occured: ", e
422
        print
423
        traceback.print_exc()
717 FredericG 424
#        raw_input("Press ENTER, the application will close")