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