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