IMU driver for microblaze

IMUInterface.c

#include "xparameters.h"
#include "xiic.h"
#include "platform.h"
#include "Sleep.h"

static const u8 kI2CAddress = 0x68;
static volatile u32* kI2CBase = (u32*) XPAR_IIC_0_BASEADDR;

typedef enum {
    kIRQStatus = 0x020 >> 2,
    kSoftReset = 0x040 >> 2,
    kControl   = 0x100 >> 2,
    kStatus    = 0x104 >> 2,
    kTxFIFO    = 0x108 >> 2,
    kRxFIFO    = 0x10C >> 2,
} Registers;

typedef enum {
	kArbitrationLost   = 1 << 0,
	kTxErrorRxComplete = 1 << 1,
	kRxFIFOFull        = 1 << 3,
	kBusNotBusy        = 1 << 4,
	kAddressedAsSlave  = 1 << 5,
} I2CIRQStatus;

typedef enum {
	kBusBusy = 1 << 2,
	kRxFIFOEmpty = 1 << 6,
	kTxFIFOEmpty = 1 << 7,
} I2CStatus;

typedef enum {
	kEnable        = 1 << 0,
	kTxFIFOReset   = 1 << 1,
	kMasterMode    = 1 << 2,
	kTxMode        = 1 << 3,
	kTxNACK         = 1 << 4,
	kRepeatedStart = 1 << 5,
} I2CControl;

typedef enum {
	kStartRead  = 0x101,
	kStartWrite = 0x100,
	kStop       = 0x200,
} I2CFIFO;

int IsI2CBitSet (Registers inRegister, int inMask) {
	return (kI2CBase [inRegister] & inMask) != 0;
}

int IsIRQStatusBitSet (I2CIRQStatus inMask) {
	return IsI2CBitSet (kIRQStatus, inMask);
}

int IsStatusBitSet (I2CStatus inMask) {
	return IsI2CBitSet (kStatus, inMask);
}

int IsTransmitting () {
	return IsI2CBitSet (kControl, kTxMode);
}

int IsIOPending () {
    if (IsStatusBitSet (kBusBusy)) {
        return 1;
    }

    if (IsTransmitting ()) {
    	return !IsStatusBitSet (kTxFIFOEmpty);
    }
    else {
    	return !IsIRQStatusBitSet (kTxErrorRxComplete);
    }

    return 0;
}

int HaveIOError () {
    if (IsIRQStatusBitSet (kArbitrationLost)) {
        return 1;
    }

    if (IsTransmitting ()) {
    	return IsIRQStatusBitSet (kTxErrorRxComplete);
    }

    return 0;
}

int WaitForTxCompletion () {
    unsigned maxWait = 10000;
    while (maxWait && IsIOPending () && !HaveIOError ()) {
        --maxWait;
    }

    return (HaveIOError () || maxWait == 0) ? XST_FAILURE : XST_SUCCESS;
}

u8 Receive (u8* outBuffer, u8 inSize) {
    u8 received = 0;
    while (!IsStatusBitSet (kRxFIFOEmpty)) {
        outBuffer [received++] = kI2CBase [kRxFIFO];
        if (received == inSize) {
            break;
        }
    }

    return received;
}

int WaitForRxCompletion (u8* outBuffer, u8 inSize) {
    u8 received = 0;
    unsigned maxWait = 10000;

    while (maxWait && IsIOPending () && !HaveIOError ()) {
        received += Receive (outBuffer + received, inSize - received);
        --maxWait;
    }

    Receive (outBuffer + received, inSize - received);

    return maxWait != 0 ? XST_SUCCESS : XST_FAILURE;
}

void ClearErrorBits () {
    int haveError = 0;
	if (IsIRQStatusBitSet (kArbitrationLost)) {
        //Reset arbitration lost bit
        kI2CBase [kIRQStatus] |= kArbitrationLost;
        haveError = 1;
    }

    if (IsIRQStatusBitSet (kTxErrorRxComplete)) {
        kI2CBase [kIRQStatus] |= kTxErrorRxComplete;
        if (IsTransmitting()) {
        	haveError = 1;
        }
    }

    if (haveError) {
    	kI2CBase [kControl] |= kTxFIFOReset;
    	kI2CBase [kControl] &= ~kTxFIFOReset;
    }
}

void StartTransAction (u8 inRead) {
    ClearErrorBits ();

    if (inRead) {
        kI2CBase [kControl] &= ~kTxMode; //Rx mode
        kI2CBase [kTxFIFO] = kStartRead | (kI2CAddress << 1);
    }
    else {
        kI2CBase [kControl] |= kTxMode;
        kI2CBase [kTxFIFO] = kStartWrite | (kI2CAddress << 1);
    }
}

int SetIMURegister (u8 inRegister, u8 inValue) {
	int result = XST_FAILURE;
	StartTransAction (0);

    kI2CBase [kTxFIFO] = inRegister;
	kI2CBase [kTxFIFO] = kStop | inValue;
	result = WaitForTxCompletion ();

    return result;
}

int ReadIMUData (u8 inStartRegister, u8* outBuffer, u8 inSize) {
    StartTransAction (0);
    kI2CBase [kTxFIFO] = inStartRegister;
    kI2CBase [kControl] |= kRepeatedStart; //Repeated start (must be set before starting transaction)
    StartTransAction (1);
    kI2CBase [kTxFIFO] = kStop | inSize;

    int result = XST_FAILURE;
    if (WaitForRxCompletion (outBuffer, inSize) == XST_SUCCESS) {
        result = XST_SUCCESS;
    }

    kI2CBase [kControl] &= ~kRepeatedStart; //Disable repeated start

    return result;
}

int GetIMURegister (u8 inRegister, u8* outValue) {
	return ReadIMUData (inRegister, outValue, 1);
}

int IsIMUPresent () {
    u8 whoami = 0;
    int status;

    status = GetIMURegister (0x75, &whoami);
    if (status != XST_SUCCESS || whoami != 0x68) {
        return 0;
    }

    return 1;
}

void InitializeIMUInterface () {
	kI2CBase [kSoftReset] = 0xA;
	MilliSleep (1);

    kI2CBase [kControl] |= kEnable;
    kI2CBase [kControl] |= kMasterMode;
}

IMUInterface.h

#ifndef IMUINTERFACE_H_
#define IMUINTERFACE_H_
#include "xil_types.h"

void InitializeIMUInterface ();
int IsIMUPresent ();
int GetIMURegister (u8 inRegister, u8* outValue);
int ReadIMUData (u8 inStartRegister, u8* outBuffer, u8 inSize);
int SetIMURegister (u8 inRegister, u8 inValue);

#endif /* IMUINTERFACE_H_ */

Sleep.c

 #include "xparameters.h"

static const unsigned kCyclesPerLoop = 11;
s
void MilliSleep (unsigned inMS) {
	const unsigned kOverHead =  100;
	unsigned waitClockCycles =  inMS * (XPAR_CPU_CORE_CLOCK_FREQ_HZ / 1e3);
	if (waitClockCycles > kOverHead) {
		volatile unsigned loops = ((waitClockCycles - kOverHead) + (kCyclesPerLoop >> 1)) / kCyclesPerLoop;
		while (loops) {
			--loops;
		}
	}
}

Sleep.h

#ifndef SLEEP_H_
#define SLEEP_H_

void MilliSleep (unsigned inMillisecs);

#endif /* SLEEP_H_ */

Custom polling microblaze I2C driver for the Invensens MPU9150.