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: |
||
| 193 | FCADDRESS = 1 |
||
| 194 | |||
| 195 | def __init__(self, printDebugMsg=False): |
||
| 196 | #self.logfile = open('mklog.txt', "rbU") |
||
| 197 | |||
| 198 | self.serPort = None |
||
| 199 | self.printDebugMsg = printDebugMsg |
||
| 200 | |||
| 201 | msg = MkMsg(address=0, cmd='v', data=[]) |
||
| 202 | self.getVersionMsgLn = msg.generateMsg() |
||
| 203 | msg = MkMsg(address=0, cmd='d', data=[500]) |
||
| 204 | self.getDebugMsgLn = msg.generateMsg() |
||
| 205 | |||
| 206 | |||
| 207 | |||
| 208 | def open(self, comPort): |
||
| 209 | self.serPort = serial.Serial(comPort, 57600, timeout=0.5) |
||
| 210 | if not self.serPort.isOpen(): |
||
| 211 | raise IOError("Failed to open serial port") |
||
| 212 | |||
| 563 | FredericG | 213 | def close(self): |
| 214 | self.serPort.close() |
||
| 215 | |||
| 552 | fredericg | 216 | def isOpen(self): |
| 217 | return self.serPort != None |
||
| 218 | |||
| 219 | def sendLn(self, ln): |
||
| 220 | self.serPort.write(ln) |
||
| 221 | |||
| 222 | def waitForLn(self): |
||
| 223 | return self.serPort.readline(eol='\r') |
||
| 224 | |||
| 225 | def waitForMsg(self, cmd2wait4): |
||
| 226 | msg = None |
||
| 227 | done = False |
||
| 228 | while (not done): |
||
| 229 | line = self.waitForLn() |
||
| 230 | if len(line) == 0: |
||
| 231 | raise NoResponse(cmd2wait4) |
||
| 232 | try: |
||
| 233 | msg = MkMsg(msg=line) |
||
| 234 | if (msg.cmd == cmd2wait4): |
||
| 235 | done = True |
||
| 236 | except InvalidMsg: |
||
| 237 | if self.printDebugMsg: |
||
| 238 | print "DebugMsg: \"%s\"" % line[:-1] |
||
| 239 | pass |
||
| 240 | return msg |
||
| 241 | |||
| 242 | def getDebugMsg(self): |
||
| 243 | self.serPort.flushInput() |
||
| 244 | self.sendLn(self.getDebugMsgLn) |
||
| 245 | msg = self.waitForMsg('D') |
||
| 246 | msg = DebugDataMsg(msg) |
||
| 247 | return msg |
||
| 248 | |||
| 249 | def getVersionMsg(self): |
||
| 250 | self.sendLn(self.getVersionMsgLn) |
||
| 251 | msg = self.waitForMsg('V') |
||
| 252 | msg = VersionMsg(msg) |
||
| 253 | return msg |
||
| 254 | |||
| 255 | def setMotorTest(self, motorSpeeds): |
||
| 256 | msg = MkMsg(address=MkComm.FCADDRESS, cmd='t', data=motorSpeeds) |
||
| 257 | self.sendLn(msg.generateMsg()) |
||
| 258 | |||
| 259 | def doVibrationTest(self, motorSpeeds, nbSamples, channel): |
||
| 260 | data = [] |
||
| 563 | FredericG | 261 | for i in range(0,(min(nbSamples,1000)/50)): |
| 610 | FredericG | 262 | #self.setMotorTest(motorSpeeds) |
| 552 | fredericg | 263 | time.sleep(0.05) |
| 264 | #print i |
||
| 265 | msg = MkMsg(address=MkComm.FCADDRESS, cmd='f', data=[channel, i]) |
||
| 266 | self.sendLn(msg.generateMsg()) |
||
| 267 | msg = self.waitForMsg('F') |
||
| 268 | msg = VibrationDataMsg(msg) |
||
| 269 | data += msg.getData() |
||
| 270 | |||
| 271 | #print data |
||
| 272 | return data |
||
| 273 | |||
| 274 | |||
| 275 | |||
| 276 | |||
| 277 | |||
| 278 | |||
| 279 | if __name__ == '__main__': |
||
| 280 | try: |
||
| 281 | |||
| 282 | ## file = open('mklog.txt', "rbU") |
||
| 283 | ## |
||
| 284 | ## for line in file: |
||
| 285 | ## try: |
||
| 286 | ## msg = MkMsg(msg=line) |
||
| 287 | ## #print "Rec Addr:%02d cmd:%c data:" % (msg.address, msg.cmd), msg.data |
||
| 288 | ## if (msg.cmd == 'D'): |
||
| 289 | ## #print len(msg.data) |
||
| 290 | ## msg = DebugDataMsg(msg) |
||
| 291 | ## |
||
| 292 | ## #print "%d %d" % (msg.getAccNick(), msg.getAccRoll()) |
||
| 293 | ## |
||
| 294 | ## #val = abs(msg.getAccNick()) |
||
| 295 | ## #print "*************************************************************"[:val/10] |
||
| 296 | ## |
||
| 297 | ## print msg.getCompassHeading() |
||
| 298 | ## |
||
| 299 | ## except InvalidMsg: |
||
| 300 | ## pass |
||
| 301 | ## |
||
| 302 | ## file.close() |
||
| 303 | ## print "OK" |
||
| 304 | |||
| 305 | ## msg = MkMsg(address=1, cmd='a', data=[255, 254, 10, 11, 12]) |
||
| 306 | ## print msg.generateMsg() |
||
| 307 | ## msg2 = MkMsg(msg.generateMsg()) |
||
| 308 | ## print msg2.address, msg2.cmd, msg2.data |
||
| 309 | |||
| 310 | comm = MkComm() |
||
| 311 | comm.open(comPort="COM5") |
||
| 312 | |||
| 313 | msg = comm.getVersionMsg() |
||
| 314 | print "Version: %d.%d" % msg.getVersion() |
||
| 315 | |||
| 316 | comm.doVibrationTest() |
||
| 317 | |||
| 318 | # msg = comm.getVersionMsg() |
||
| 319 | # print "Version: %d.%d" % msg.getVersion() |
||
| 320 | # |
||
| 321 | # comm.setMotorTest([0,0,80,0]) |
||
| 322 | # time.sleep(10) |
||
| 323 | # |
||
| 324 | # minN = 0 |
||
| 325 | # maxN = 0 |
||
| 326 | # minR = 0 |
||
| 327 | # maxR = 0 |
||
| 328 | # |
||
| 329 | # time.clock() |
||
| 330 | # for i in range(0,1000): |
||
| 331 | # try: |
||
| 332 | # msg = comm.getDebugMsg() |
||
| 333 | # n = msg.getAccNick() |
||
| 334 | # r = msg.getAccRoll() |
||
| 335 | # print "%d,%d" % (n, r) |
||
| 336 | # minN = min(minN, n) |
||
| 337 | # maxN = max(maxN, n) |
||
| 338 | # minR = min(minR, r) |
||
| 339 | # maxR = max(maxR, r) |
||
| 340 | # except Exception: |
||
| 341 | # pass |
||
| 342 | # |
||
| 343 | # comm.setMotorTest([0,0,0,0]) |
||
| 344 | # print time.clock() |
||
| 345 | # print maxN-minN,maxR-minR |
||
| 346 | |||
| 347 | |||
| 348 | |||
| 349 | |||
| 350 | except Exception,e: |
||
| 351 | print |
||
| 352 | print "An error occured: ", e |
||
| 353 | print |
||
| 354 | traceback.print_exc() |
||
| 355 | raw_input("Press ENTER, the application will close") |