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 InvalidMsgLen(MkException):

    def __str__(self):

      return "Invalid Message Length"

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.parseUartLine(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 generateUartLine(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 parseUartLine(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 SettingsMsg:

    DATENREVISION   = 80

    IDX_INDEX       = 0

    IDX_STICK_P     = 1 + 19

    IDX_STICK_D     = 1 + 20

    IDX_NAME        = 1 + 90

    def __init__(self, msg):

        if (msg.cmd != 'Q'):

            raise InvalidMsgType

        if len(msg.data) != 105:

            raise InvalidMsgLen

        self.msg = msg

    def getSettings(self):

        return self.msg.data

    def getIndex(self):

        return self.getSetting(SettingsMsg.IDX_INDEX)

    def getSetting(self, settingIndex):

        return self.msg.data[settingIndex]

    def getName(self):

        name = ""

        for i in self.msg.data[SettingsMsg.IDX_NAME:]:

            if i==0:

                break

            name += chr(i)

        return name

    def setSetting(self, settingIndex, value):

        self.msg.data[settingIndex] = value

class DebugDataMsg:

    IDX_ANALOG_ANGLENICK=   2+2*0

    IDX_ANALOG_ANGLEROLL=   2+2*1

    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 getAngleNick(self):

        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLENICK)

    def getAngleRoll(self):

        return self.msg.data2SignedInt(DebugDataMsg.IDX_ANALOG_ANGLEROLL)

    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.generateUartLine()

        msg = MkMsg(address=0, cmd='d', data=[500])

        self.getDebugMsgLn = msg.generateUartLine()

    def open(self, comPort):

        self.serPort = serial.Serial(comPort, 57600, timeout=0.5, rtscts=1)

        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, timeout=0.5):

        msg = None

        done = False

        if self.printDebugMsg: print "[Debug] =>Wait4 %s TO=%.1fs" % (cmd2wait4, timeout)

        startTime = time.clock()

        while (not done):

            line = self.waitForLn()

            try:

                msg = MkMsg(msg=line)

                if self.printDebugMsg: print "[Debug]    msg %s" % msg.cmd

                if (msg.cmd == cmd2wait4):

                    done = True

            except InvalidMsg:

                if self.printDebugMsg: print "[Debug]    no valid msg"

            if ((time.clock()-startTime) > timeout):

                raise NoResponse(cmd2wait4)

        if self.printDebugMsg: print "[Debug]    Done: %.1fs" % (time.clock()-startTime)

        return msg

    def sendMsg(self, msg):

        self.sendLn(msg.generateUartLine())

    def sendNCRedirectUartFromFC(self):

        self.serPort.flushInput()

        msg = MkMsg(address=MkComm.ADDRESS_NC, cmd='u', data=[0])

        self.sendMsg(msg)

        time.sleep(.5)

        # No reply expected...   

    def sendSettings(self, settings):

        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='s', data=settings)

        self.sendMsg(msg)

        #time.sleep(1)

        msg = self.waitForMsg('S', timeout=2)

    def getDebugMsg(self):

        self.serPort.flushInput()

        self.sendLn(self.getDebugMsgLn)

        msg = self.waitForMsg('D')

        msg = DebugDataMsg(msg)

        return msg

    def getSettingsMsg(self, index=0xFF):

        self.serPort.flushInput()

        msg = MkMsg(address=MkComm.ADDRESS_FC, cmd='q', data=[index])

        self.sendMsg(msg)

        msg = self.waitForMsg('Q')

        msg = SettingsMsg(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.sendMsg(msg)

    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.sendMsg(msg)

          msg = self.waitForMsg('F')

          msg = VibrationDataMsg(msg)

          data += msg.getData()

        # FIXE: should be fixed in the FC code

        data[0]=data[1]

        return data

    def recordDbgMsg(self, samplePeriod, nbSamples):

        result = []

        self.serPort.flushInput()

        msg = MkMsg(address=0, cmd='d', data=[int(samplePeriod*100)])

        self.sendMsg(msg)

        for i in range(nbSamples):

            print i

            msg = self.waitForMsg('D', timeout=samplePeriod+1)

            msg = DebugDataMsg(msg)

            result.append(msg)

        return result

if __name__ == '__main__':

    try:

        comm = MkComm()

        comm.printDebugMsg = True

        comm.open(comPort="COM40")

        msg = comm.getVersionMsg()

        print "Version: %d.%d" % msg.getVersion()

#        msg = comm.getSettingsMsg()

#        print "Index=",msg.getIndex()

#        print "Name=",msg.getName()

#        print "StickP=",msg.getSetting(SettingsMsg.IDX_STICK_P)

#        print "StickD=",msg.getSetting(SettingsMsg.IDX_STICK_D)

#        

#        msg.setSetting(SettingsMsg.IDX_STICK_P, msg.getSetting(SettingsMsg.IDX_STICK_P)+1)

#        comm.sendSettings(msg.getSettings())

        messages = comm.recordDbgMsg(0.05, 20)

        for msg in messages:

            print msg.getAngleRoll()

    except Exception,e:

        print

        print "An error occured: ", e

        print

        traceback.print_exc()

        raw_input("Press ENTER, the application will close")