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" |
||
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: |
187 | IDX_ANALOG_ACCNICK = 2+2*2 |
||
188 | IDX_ANALOG_ACCROLL = 2+2*3 |
||
189 | IDX_ANALOG_COMPASS = 2+2*3 |
||
190 | IDX_ANALOG_VOLTAGE = 2+2*9 |
||
191 | |||
192 | def __init__(self, msg): |
||
193 | if (msg.cmd != 'D'): |
||
194 | raise InvalidMsgType |
||
195 | self.msg = msg |
||
196 | |||
197 | def getAccNick(self): |
||
696 | FredericG | 198 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK) |
552 | fredericg | 199 | |
200 | def getAccRoll(self): |
||
696 | FredericG | 201 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL) |
552 | fredericg | 202 | |
203 | def getCompassHeading(self): |
||
696 | FredericG | 204 | return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS) |
552 | fredericg | 205 | |
206 | def getVoltage(self): |
||
696 | FredericG | 207 | return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10 |
552 | fredericg | 208 | |
209 | class VibrationDataMsg: |
||
210 | def __init__(self, msg): |
||
211 | if (msg.cmd != 'F'): |
||
212 | raise InvalidMsgType |
||
213 | self.msg = msg |
||
214 | |||
215 | def getData(self): |
||
216 | data = [] |
||
563 | FredericG | 217 | for i in range(0,50): |
552 | fredericg | 218 | data.append(self.msg.data2SignedInt(2*i)) |
219 | return data |
||
220 | |||
221 | class VersionMsg: |
||
222 | def __init__(self, msg): |
||
223 | if (msg.cmd != 'V'): |
||
224 | raise InvalidMsgType |
||
225 | self.msg = msg |
||
226 | |||
227 | def getVersion(self): |
||
228 | return (self.msg.data[0], self.msg.data[1]) |
||
229 | |||
230 | class MkComm: |
||
653 | FredericG | 231 | ADDRESS_ALL = 0 |
232 | ADDRESS_FC = 1 |
||
233 | ADDRESS_NC = 2 |
||
234 | ADDRESS_MK3MAG = 3 |
||
235 | |||
552 | fredericg | 236 | def __init__(self, printDebugMsg=False): |
237 | #self.logfile = open('mklog.txt', "rbU") |
||
238 | |||
239 | self.serPort = None |
||
240 | self.printDebugMsg = printDebugMsg |
||
241 | |||
242 | msg = MkMsg(address=0, cmd='v', data=[]) |
||
697 | FredericG | 243 | self.getVersionMsgLn = msg.generateUartLine() |
552 | fredericg | 244 | msg = MkMsg(address=0, cmd='d', data=[500]) |
697 | FredericG | 245 | self.getDebugMsgLn = msg.generateUartLine() |
552 | fredericg | 246 | |
247 | |||
248 | def open(self, comPort): |
||
249 | self.serPort = serial.Serial(comPort, 57600, timeout=0.5) |
||
250 | if not self.serPort.isOpen(): |
||
251 | raise IOError("Failed to open serial port") |
||
252 | |||
563 | FredericG | 253 | def close(self): |
254 | self.serPort.close() |
||
255 | |||
552 | fredericg | 256 | def isOpen(self): |
257 | return self.serPort != None |
||
258 | |||
259 | def sendLn(self, ln): |
||
260 | self.serPort.write(ln) |
||
261 | |||
262 | def waitForLn(self): |
||
263 | return self.serPort.readline(eol='\r') |
||
264 | |||
265 | def waitForMsg(self, cmd2wait4): |
||
266 | msg = None |
||
267 | done = False |
||
268 | while (not done): |
||
269 | line = self.waitForLn() |
||
270 | if len(line) == 0: |
||
271 | raise NoResponse(cmd2wait4) |
||
272 | try: |
||
273 | msg = MkMsg(msg=line) |
||
274 | if (msg.cmd == cmd2wait4): |
||
275 | done = True |
||
276 | except InvalidMsg: |
||
277 | if self.printDebugMsg: |
||
278 | print "DebugMsg: \"%s\"" % line[:-1] |
||
279 | pass |
||
280 | return msg |
||
698 | FredericG | 281 | |
282 | def sendMsg(self, msg): |
||
283 | self.sendLn(msg.generateUartLine()) |
||
552 | fredericg | 284 | |
653 | FredericG | 285 | def sendNCRedirectUartFromFC(self): |
286 | self.serPort.flushInput() |
||
287 | msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0]) |
||
698 | FredericG | 288 | self.sendMsg(msg) |
653 | FredericG | 289 | time.sleep(.5) |
696 | FredericG | 290 | # No reply expected... |
291 | |||
292 | def sendSettings(self, settings): |
||
293 | msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings) |
||
698 | FredericG | 294 | self.sendMsg(msg) |
696 | FredericG | 295 | time.sleep(1) |
296 | msg = self.waitForMsg('S') |
||
297 | |||
552 | fredericg | 298 | def getDebugMsg(self): |
299 | self.serPort.flushInput() |
||
300 | self.sendLn(self.getDebugMsgLn) |
||
301 | msg = self.waitForMsg('D') |
||
302 | msg = DebugDataMsg(msg) |
||
303 | return msg |
||
696 | FredericG | 304 | |
305 | def getSettingsMsg(self, index=0xFF): |
||
306 | self.serPort.flushInput() |
||
307 | msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index]) |
||
698 | FredericG | 308 | self.sendMsg(msg) |
696 | FredericG | 309 | msg = self.waitForMsg('Q') |
310 | msg = SettingsMsg(msg) |
||
311 | return msg |
||
552 | fredericg | 312 | |
313 | def getVersionMsg(self): |
||
314 | self.sendLn(self.getVersionMsgLn) |
||
315 | msg = self.waitForMsg('V') |
||
316 | msg = VersionMsg(msg) |
||
317 | return msg |
||
318 | |||
319 | def setMotorTest(self, motorSpeeds): |
||
653 | FredericG | 320 | msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='t', data=motorSpeeds) |
698 | FredericG | 321 | self.sendMsg(msg) |
552 | fredericg | 322 | |
613 | FredericG | 323 | def doVibrationTest(self, nbSamples, channel): |
552 | fredericg | 324 | data = [] |
563 | FredericG | 325 | for i in range(0,(min(nbSamples,1000)/50)): |
653 | FredericG | 326 | msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i]) |
698 | FredericG | 327 | self.sendMsg(msg) |
552 | fredericg | 328 | msg = self.waitForMsg('F') |
329 | msg = VibrationDataMsg(msg) |
||
330 | data += msg.getData() |
||
331 | |||
618 | FredericG | 332 | # FIXE: should be fixed in the FC code |
333 | data[0]=data[1] |
||
552 | fredericg | 334 | return data |
335 | |||
336 | |||
337 | |||
338 | |||
339 | if __name__ == '__main__': |
||
340 | try: |
||
341 | comm = MkComm() |
||
342 | comm.open(comPort="COM5") |
||
343 | |||
344 | msg = comm.getVersionMsg() |
||
345 | print "Version: %d.%d" % msg.getVersion() |
||
696 | FredericG | 346 | |
347 | msg = comm.getSettingsMsg() |
||
348 | print "Index=",msg.getIndex() |
||
349 | print "Name=",msg.getName() |
||
350 | print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P) |
||
351 | print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D) |
||
352 | |||
353 | msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1) |
||
354 | comm.sendSettings(msg.getSettings()) |
||
552 | fredericg | 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") |