Rev 618 |
Go to most recent revision |
Blame |
Last modification |
View Log
| RSS feed
#! /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,50):
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:
ADDRESS_ALL = 0
ADDRESS_FC = 1
ADDRESS_NC = 2
ADDRESS_MK3MAG = 3
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 close(self):
self.serPort.close()
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 sendNCRedirectUartFromFC(self):
self.serPort.flushInput()
msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])
self.sendLn(msg.generateMsg())
time.sleep(.5)
# No reply expected...
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.ADDRESS_FC, cmd='t', data=motorSpeeds)
self.sendLn(msg.generateMsg())
def doVibrationTest(self, nbSamples, channel):
data = []
for i in range(0,(min(nbSamples,1000)/50)):
msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='f', data=[channel, i])
self.sendLn(msg.generateMsg())
msg = self.waitForMsg('F')
msg = VibrationDataMsg(msg)
data += msg.getData()
# FIXE: should be fixed in the FC code
data[0]=data[1]
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")