Subversion Repositories Projects

#! /usr/bin/env python

#

# Mikrokopter Vibration Test

#

# Author: FredericG

# 

import mkProto

import time

import traceback

import getopt

import sys

CHECK_VERSION = False

CHANNEL_NAMES = ["GyroYaw", "GyroRoll", "GyroNick", "Pressure", "Batt", "AccTop", "AccRoll", "AccNick"]

def usage():

    print

    print "VibrationTest.py COMPORT MOTORS SPEEDS CHANNELS [-m MINSPEED] [-s NBSAMPLES] [-n NAME] [-d FILENAME] [-v]"

    print " COMPORT         Serial port to use. e.g. COM4"

    print " MOTORS          Motors to activate during test. Multiple motors can be used at the same time. e.g. 1,2,3,4"

    print " SPEEDS          Indicates at what speeds the motors need to be tested. "

    print "                   Format 1:  e.g. 50,110,140            Tests at speeds 50, 110 and 140"

    print "                   Format 2:  e.g. 100-200:50            Tests at speeds 100, 150 and 200"

    print " CHANNELS        Channels to monitor. e.g. 5,6,7"

    for i,channelName in enumerate(CHANNEL_NAMES):

      print "                   Channel %d = %s" % (i, channelName)

    print " -m MINSPEED     Minimum speed of the motor(s)"

    print " -s NBSAMPLES    Number of samples"

    print " -n NAME         Name of the test"

    print " -d FILENAME     File to which the measured values will be logged in"

    print " -v              Verbose"          

    print

def exit_usage():

    usage()

    sys.exit(2)

if __name__ == '__main__':

    #print sys.argv

    if len(sys.argv)<5:

        exit_usage()

    parComPort = sys.argv[1]

    parMotors = sys.argv[2].split(',')

    parChannels = sys.argv[4].split(',')

    par = sys.argv[3]

    if par.count("-") == 1:

        # assume from-to:step format

        par = par.split("-")

        if len(par) != 2:

            exit_usage()

        par[1] = par[1].split(":")

        if len(par[1]) != 2:

            exit_usage()

        parSpeeds = range(int(par[0]),int(par[1][0])+int(par[1][1]),int(par[1][1]))

    else:

        parSpeeds = par.split(',')

    try:

        opts, args = getopt.getopt(sys.argv[5:], "s:n:d:m:v")

    except Exception, err:

        print str(err) # will print something like "option -a not recognized"

        usage()

        sys.exit(2)

    parVerbose = False

    parFileName = None

    parNbSamples = 400

    parMinSpeed = 25

    parTestName = ""

    for o, a in opts:

        if o == "-v":

            parVerbose = True

        elif o == "-d":

            parFileName = a

        elif o == "-s":

            parNbSamples = int(a)

        elif o == "-m":

            parMinSpeed = int(a)

        elif o == "-n":

            parTestName = a

        else:

            assert False, "unhandled option %s" % (o)

    if parVerbose:

        print "comPort  =", parComPort

        print "motors   =", parMotors

        print "minSpeed =", parMinSpeed

        print "speeds   =", parSpeeds

        print "channels =", parChannels

        print "nbSamples=", parNbSamples

        print "fileName =", parFileName

        print "testName =", parTestName

    try:

        parMotors = map(int, parMotors)

        parSpeeds = map(int, parSpeeds)

        parChannels = map(int, parChannels)

        if parVerbose:

          print "Opening comPort... "

        mk = mkProto.MkComm()

        mk.open(comPort=parComPort)

        time.sleep(.1)

        msg = mk.getVersionMsg()

        version = msg.getVersion()

        if parVerbose:

          print "Version: %d.%d" % version

        if CHECK_VERSION and version != (0,74):

          print "INVALID VERSION", version

          sys.exit(2)

        msg = mk.getDebugMsg()

        voltage = msg.getVoltage()

        if (voltage == 0):

          minVoltage = 0

        else:

          if (voltage > 4.2*3):

            minVoltage = 4*3.5

          else:

            minVoltage = 3*3.5

        if parVerbose:

          print "Voltage: %2.1fV" % msg.getVoltage()

          print "Minimum Voltage: %2.1fV" % minVoltage

        motorStartSpeeds = [0,0,0,0]

        if parVerbose:

          print "Starting motor(s) (speed=%d)... " % (parMinSpeed),

        for motor in parMotors:

            motorStartSpeeds[motor-1] = parMinSpeed

        for i in range(10):

          mk.setMotorTest(motorStartSpeeds)

          time.sleep(.1)

        if parVerbose:

          print "OK"

        for speed in parSpeeds:

          if len(parChannels)>1:

            print

          if parVerbose:

            print "Setting speed to %3d ..." % speed

          motorSpeeds = [0,0,0,0]  

          for motor in parMotors:

            motorSpeeds[motor-1] = speed

          for i in range(0,10):

            time.sleep(.1)

            mk.setMotorTest(motorSpeeds)

          for channel in parChannels:

            mk.setMotorTest(motorSpeeds)

            channelName = CHANNEL_NAMES[channel]

            if parVerbose:

              print "Getting data... " ,

            data = mk.doVibrationTest(motorSpeeds, parNbSamples, channel)

            #data = (0,1,2)

            minval = min(data[1:])

            maxval = max(data[1:])

            #if (maxval-minval)>100: print data

            time.sleep(.1)

            msg = mk.getDebugMsg()

            voltage = msg.getVoltage()

            if voltage<minVoltage:

              print "VOLTAGE TOO LOW, TEST ABORTED"

              sys.exit(2)

            pp = maxval-minval;  

            print "%10s Speed=%3d U=%2.1fV Channel=%-10s Min=%3d Max=%3d pp=%3d" % (parTestName,  speed, voltage, channelName, minval, maxval, pp),

            print "*"*(min(pp,200)/5)

            if parFileName != None:

              try:

                logfile = open(parFileName, "r")

                newFile = False

              except:

                newFile = True

              if newFile:

                logfile = open(parFileName, "w")

                if parVerbose:

                  print "Writing result to %s ..." % parFileName,

                logfile.write("%s %d %s\n" % (parTestName, speed, channelName))

                for value in data[1:]:

                  logfile.write("%d\n" % value)

                logfile.close()

                if parVerbose:  

                  print "OK"

              else:

                if parVerbose:

                  print "Appending result to %s ..." % parFileName,

                prevData = []

                for line in logfile:

                  prevData.append(line[:-1])

                logfile.close()

                logfile = open(parFileName, "w")

                logfile.write("%s,%s %d %s\n" % (prevData[0], parTestName, speed, channelName))

                i = 1

                for value in data[1:]:

                  logfile.write("%s,%d\n" % (prevData[i], value))

                  i += 1

                logfile.close()

                if parVerbose:

                  print "OK"

        mk.close()

    except Exception,e:

      print

      print "== ERROR ==: \"%s\"" % e

      if parVerbose:

        print

        print "Traceback:"

        traceback.print_exc()

        print

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

      print