0,0 → 1,352 |
#! /usr/bin/env python |
|
# |
# Mikrokopter Serial protocol |
# |
# Author: FredericG |
# |
|
import os |
import glob |
import serial |
import time |
import traceback |
|
class MkException(Exception): |
pass |
|
class InvalidMsg(MkException): |
def __str__(self): |
return "Invalid message" |
|
class CrcError(MkException): |
def __str__(self): |
return "CRC error" |
|
class InvalidArguments(MkException): |
def __str__(self): |
return "Invalid Arguments" |
|
class InvalidMsgType(MkException): |
def __str__(self): |
return "Invalid Message type" |
|
class NoResponse(MkException): |
def __init__(self, cmd): |
self.cmd = cmd |
|
def __str__(self): |
return "No Reponse. Waiting for \"%s\" message" % self.cmd |
|
pass |
|
def calcCrcBytes(str): |
crc = 0 |
for c in str: |
crc += ord(c) |
crc &= 0xfff |
return (chr(crc/64+ord('=')), chr(crc%64+ord('='))) |
|
|
class MkMsg: |
def __init__(self, msg=None, address=None, cmd=None, data=None): |
if (msg != None): |
# Create instance based on received message |
self.parse(msg) |
elif (address != None and cmd != None and data != None): |
# Create instance based on address, command and data |
self.address = address |
self.cmd = cmd |
self.data = data |
else: |
# Cannot create instance |
raise InvalidArguments |
|
def generateMsg(self): |
msg = "" |
|
# make header |
msg += '#' |
msg += chr(self.address+ord('a')) |
msg += self.cmd |
|
# add data |
done = False |
i = 0 |
while (i<len(self.data)) and not done: |
a = 0 |
b = 0 |
c = 0 |
try: |
a = self.data[i] |
b = self.data[i+1] |
c = self.data[i+2] |
i = i + 3 |
except IndexError: |
done = True |
msg += chr(ord('=') + (a >> 2)) |
msg += chr(ord('=') + (((a & 0x03) << 4) | ((b & 0xf0) >> 4))) |
msg += chr(ord('=') + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6))) |
msg += chr(ord('=') + ( c & 0x3f)) |
|
# add crc and NL |
crc1,crc2 = calcCrcBytes(msg) |
msg += crc1 + crc2 |
msg += '\r' |
return msg |
|
|
def parse(self, msg): |
if len(msg)<6: |
raise InvalidMsg() |
if (msg[0] != '#'): |
raise InvalidMsg() |
if (msg[-1] != '\r'): |
raise InvalidMsg() |
|
self.address = ord(msg[1]) |
self.cmd = msg[2] |
|
data64 = map(ord, msg[3:-3]) # last 3 bytes are CRC and \n |
|
done = False |
i = 0 |
self.data = [] |
while (i<len(data64)) and not done: |
a = 0 |
b = 0 |
c = 0 |
d = 0 |
try: |
a = data64[i] - ord('=') |
b = data64[i+1] - ord('=') |
c = data64[i+2] - ord('=') |
d = data64[i+3] - ord('=') |
i = i + 4 |
except IndexError: |
done = True |
|
self.data.append((a << 2)&0xFF | (b >> 4)) |
self.data.append(((b & 0x0f) << 4)&0xFF | (c >> 2)); |
self.data.append(((c & 0x03) << 6)&0xFF | d); |
|
crc1,crc2 = calcCrcBytes(msg[:-3]) |
if (crc1 != msg[-3] or crc2 != msg[-2]): |
#print msg |
raise CrcError |
|
#print "crc= %x %x %x %x" % ( crc1, crc2, (ord(msg[-3])-ord('=')), (ord(msg[-2])-ord('='))) |
|
|
def data2SignedInt(self, index): |
int = self.data[index]+self.data[index+1]*256 |
if (int > 0xFFFF/2): |
int -= 0xFFFF |
return int |
|
|
class DebugDataMsg: |
IDX_ANALOG_ACCNICK = 2+2*2 |
IDX_ANALOG_ACCROLL = 2+2*3 |
IDX_ANALOG_COMPASS = 2+2*3 |
IDX_ANALOG_VOLTAGE = 2+2*9 |
|
def __init__(self, msg): |
if (msg.cmd != 'D'): |
raise InvalidMsgType |
self.msg = msg |
|
def getAccNick(self): |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCNICK); |
|
def getAccRoll(self): |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ACCROLL); |
|
def getCompassHeading(self): |
return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_COMPASS); |
|
def getVoltage(self): |
return float(self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_VOLTAGE))/10; |
|
class VibrationDataMsg: |
def __init__(self, msg): |
if (msg.cmd != 'F'): |
raise InvalidMsgType |
self.msg = msg |
|
def getData(self): |
data = [] |
for i in range(0,25): |
data.append(self.msg.data2SignedInt(2*i)) |
return data |
|
class VersionMsg: |
def __init__(self, msg): |
if (msg.cmd != 'V'): |
raise InvalidMsgType |
self.msg = msg |
|
def getVersion(self): |
return (self.msg.data[0], self.msg.data[1]) |
|
class MkComm: |
FCADDRESS = 1 |
|
def __init__(self, printDebugMsg=False): |
#self.logfile = open('mklog.txt', "rbU") |
|
self.serPort = None |
self.printDebugMsg = printDebugMsg |
|
msg = MkMsg(address=0, cmd='v', data=[]) |
self.getVersionMsgLn = msg.generateMsg() |
msg = MkMsg(address=0, cmd='d', data=[500]) |
self.getDebugMsgLn = msg.generateMsg() |
|
|
|
def open(self, comPort): |
self.serPort = serial.Serial(comPort, 57600, timeout=0.5) |
if not self.serPort.isOpen(): |
raise IOError("Failed to open serial port") |
|
def isOpen(self): |
return self.serPort != None |
|
def sendLn(self, ln): |
self.serPort.write(ln) |
|
def waitForLn(self): |
return self.serPort.readline(eol='\r') |
|
def waitForMsg(self, cmd2wait4): |
msg = None |
done = False |
while (not done): |
line = self.waitForLn() |
if len(line) == 0: |
raise NoResponse(cmd2wait4) |
try: |
msg = MkMsg(msg=line) |
if (msg.cmd == cmd2wait4): |
done = True |
except InvalidMsg: |
if self.printDebugMsg: |
print "DebugMsg: \"%s\"" % line[:-1] |
pass |
return msg |
|
def getDebugMsg(self): |
self.serPort.flushInput() |
self.sendLn(self.getDebugMsgLn) |
msg = self.waitForMsg('D') |
msg = DebugDataMsg(msg) |
return msg |
|
def getVersionMsg(self): |
self.sendLn(self.getVersionMsgLn) |
msg = self.waitForMsg('V') |
msg = VersionMsg(msg) |
return msg |
|
def setMotorTest(self, motorSpeeds): |
msg = MkMsg(address=MkComm.FCADDRESS, cmd='t', data=motorSpeeds) |
self.sendLn(msg.generateMsg()) |
|
def doVibrationTest(self, motorSpeeds, nbSamples, channel): |
data = [] |
for i in range(0,min(nbSamples,1000)/50): |
self.setMotorTest(motorSpeeds) |
time.sleep(0.05) |
#print i |
msg = MkMsg(address=MkComm.FCADDRESS, cmd='f', data=[channel, i]) |
self.sendLn(msg.generateMsg()) |
msg = self.waitForMsg('F') |
msg = VibrationDataMsg(msg) |
data += msg.getData() |
|
#print data |
return data |
|
|
|
|
|
|
if __name__ == '__main__': |
try: |
|
## file = open('mklog.txt', "rbU") |
## |
## for line in file: |
## try: |
## msg = MkMsg(msg=line) |
## #print "Rec Addr:%02d cmd:%c data:" % (msg.address, msg.cmd), msg.data |
## if (msg.cmd == 'D'): |
## #print len(msg.data) |
## msg = DebugDataMsg(msg) |
## |
## #print "%d %d" % (msg.getAccNick(), msg.getAccRoll()) |
## |
## #val = abs(msg.getAccNick()) |
## #print "*************************************************************"[:val/10] |
## |
## print msg.getCompassHeading() |
## |
## except InvalidMsg: |
## pass |
## |
## file.close() |
## print "OK" |
|
## msg = MkMsg(address=1, cmd='a', data=[255, 254, 10, 11, 12]) |
## print msg.generateMsg() |
## msg2 = MkMsg(msg.generateMsg()) |
## print msg2.address, msg2.cmd, msg2.data |
|
comm = MkComm() |
comm.open(comPort="COM5") |
|
msg = comm.getVersionMsg() |
print "Version: %d.%d" % msg.getVersion() |
|
comm.doVibrationTest() |
|
# msg = comm.getVersionMsg() |
# print "Version: %d.%d" % msg.getVersion() |
# |
# comm.setMotorTest([0,0,80,0]) |
# time.sleep(10) |
# |
# minN = 0 |
# maxN = 0 |
# minR = 0 |
# maxR = 0 |
# |
# time.clock() |
# for i in range(0,1000): |
# try: |
# msg = comm.getDebugMsg() |
# n = msg.getAccNick() |
# r = msg.getAccRoll() |
# print "%d,%d" % (n, r) |
# minN = min(minN, n) |
# maxN = max(maxN, n) |
# minR = min(minR, r) |
# maxR = max(maxR, r) |
# except Exception: |
# pass |
# |
# comm.setMotorTest([0,0,0,0]) |
# print time.clock() |
# print maxN-minN,maxR-minR |
|
|
|
|
except Exception,e: |
print |
print "An error occured: ", e |
print |
traceback.print_exc() |
raw_input("Press ENTER, the application will close") |