Go to most recent revision | Details | 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") |