Subversion Repositories Projects

#! /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:

    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 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 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")