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