imu.py

#!/usr/bin/python
"""
  Light weight wrapper for Chrobotics UM6 IMU
  usage:
     ./imu.py <task> [<replay log file> [F|FF]]
"""
# https://www.chrobotics.com/docs/UM6_datasheet.pdf
# http://wiki.ros.org/um6

import sys
import serial
import struct
from logit import *

# page 42
UM6_GYRO_RAW_XY = 0x56
UM6_GYRO_PROC_XY = 0x5e
UM6_MAG_PROC_XY = 0x60
UM6_EULER_PHI_THETA = 0x62
UM6_TEMPERATURE = 0x76

class IMU:
    def __init__( self, com ):
        self.com = com
        self.temperature = None
        self.config()

    def sendPacket( self, messageType, data = "" ):
        pass

    def readPacket( self ):
        data = self.com.read(7) # minimal size
        while not data.startswith( "snp" ):
            print ord(data[0])
            data = data[1:] + self.com.read(1)
        pt = ord(data[3])
        hasData = pt & 0x80
        isBatch = pt & 0x40
        batchLen = (pt >> 2) & 0x0F
        if hasData:
            if isBatch:
                if batchLen > 0:
                    data += self.com.read( 4 * batchLen ) # number of 4-bytes registers
            else:
                data += self.com.read( 4 ) # single register
        chsum = sum([ord(x) for x in data[:-2]])
        assert struct.unpack(">H",data[-2:])[0] == chsum, (data[-2:], hex(chsum))
        addr = ord(data[4])
        assert addr in [0x76, 0x56, 0x5c, 0x5e, 0x60, 0x62], hex(addr) # visible in current dump
        if addr == UM6_TEMPERATURE:
            self.temperature = struct.unpack_from(">f", data, 5)[0]
            print self.temperature
        return data
        

    def config( self ):
        pass

    def update( self ):
        packet = self.readPacket()
        print "".join(["%02X" % ord(x) for x in packet])


def testUsage( com ):
    "log available data"
    imu = IMU( com )
    for i in xrange(100):
        imu.update()


if __name__ == "__main__":
    if len(sys.argv) < 2:
        print __doc__
        sys.exit(1)
    filename = None
    com = None
    if len(sys.argv) > 2:
        replayAssert = True
        filename = sys.argv[2]
        if len(sys.argv) > 3:
            assert sys.argv[3] in ['F','FF']
            if sys.argv[2] == 'F':
                replayAssert = False
            else:
                com = ReplyLogInputsOnly( filename )
    if filename:
        if com == None:
            com = ReplayLog( filename, assertWrite=replayAssert )
    else:
        com = LogIt( serial.Serial( '/dev/ttyUSB0', 115200 ), prefix='logs/imu' )
    testUsage( com )

# vim: expandtab sw=4 ts=4