File content as of revision 1:fdfa313b9cc3:

/*
 * Demo to relay I/O between a computer and the IMU. Make sure to connect the GND wire on the IMU to pin 1 (GND) on the mbed so there's a return current
 * Updated to use interrupts - this will help when we intergrate this code into AVNavControl
 * 9dof razor from sparkfun
 */

#define GYRO_SCALE 14.375  // ticks per degree, http://www.sparkfun.com/datasheets/Sensors/Gyro/PS-ITG-3200-00-01.4.pdf 

#include "mbed.h"

Serial IMU(p9, p10);   // tx, rx
Serial PC(USBTX, USBRX);

DigitalOut myled1(LED1);
DigitalOut myled2(LED2);
DigitalOut myled3(LED3);
DigitalOut myled4(LED4);

bool IMUreadable = false;
bool PCreadable = false;

void readIMU();
void readPC();
inline void makeCorrect(short* i);

short accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ;

// Expected format: {$, accX, accX, accY, accY, accZ, accZ, gyrX, gyrX, gyrY, gyrY, gyrZ, gyrZ, magX, magX, magY, magY, magZ, magZ, #, \r}
int i_IMU;
char bufIMU[21];

int main() {
    // Set up the connection. Read up about parity and stop bits if this is confusing.
    IMU.format(8, Serial::None, 1);
    PC.format(8, Serial::None, 1);
    IMU.baud(57600);
    PC.baud(115200);

    for (int i = 0; i < 80; i++) {
        PC.putc('-');
    }
    PC.printf("\n\r");

    PC.attach(&readPC);
    IMU.attach(&readIMU);

    IMU.putc('6');  //tell the IMU to start sending data in binary mode, if it's not sending data already

    //The main loop
    while (true) {
        __enable_irq();
        if (IMUreadable) {
            myled2 = 1;

            while (IMU.readable()) {
                // This snippet of code should be run whenever a character can be received from the IMU.

                char data = IMU.getc();

                // Start of a new set of data. Reset the counter to the first position in the buffer, and start throwing data in there.
                if (data == '$') {
                    i_IMU = 0;
                    printf("new data\n\r");
                }
                // Something went wrong.
                else if (i_IMU > 21) {
                    printf("\t\t\tIMU error.\n\r");
                    i_IMU = 21;
                }

                // End of the set of data. Parse the buffer
                else if (i_IMU == 21 /*&& bufIMU[0] == '$' && bufIMU[20] == '\n' data == '\n' && i_IMU == 19*/) {
                    printf("Parsing\n\r");
                    
                    //bufIMU contains binary data. Each variable sent by the IMU is a 16-bit integer
                    //broken down into two characters (in bufIMU[]). Here, we reconstitute the original integer by
                    //left-shifting the first character by 8 bits and ORing it with the second character.
                    accX = (bufIMU[1]<<8 | bufIMU[2]);
                    accY = (bufIMU[3]<<8 | bufIMU[4]);
                    accZ = (bufIMU[5]<<8 | bufIMU[6]);

                    gyrX = (bufIMU[7]<<8 | bufIMU[8]);
                    gyrY = (bufIMU[9]<<8 | bufIMU[10]);
                    gyrZ = (bufIMU[11]<<8 | bufIMU[12]);

                    magX = (bufIMU[13]<<8 | bufIMU[14]);
                    magY = (bufIMU[15]<<8 | bufIMU[16]);
                    magZ = (bufIMU[17]<<8 | bufIMU[18]);
                    
                    makeCorrect(&accX);
                    makeCorrect(&accY);
                    makeCorrect(&accZ);

                    makeCorrect(&gyrX);
                    makeCorrect(&gyrY);
                    makeCorrect(&gyrZ);

                    makeCorrect(&magX);
                    makeCorrect(&magY);
                    makeCorrect(&magZ);


                    PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", accX, accY, accZ, gyrX, gyrY, gyrZ, magX, magY, magZ);

                    //accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;

                    /*
                    for (int i = 0; i < 21; i++) {
                        PC.printf("%d  ", bufIMU[i]);
                    }
                    PC.printf("\n\r");
                    */
                    //newIMUData = 1; // Update the flag
                }


                bufIMU[i_IMU] = data;
                i_IMU++;
                //parseNow = (buffer.at(buffer.length() - 1) == '#');
            }
            IMUreadable = false;
            myled2 = 0;
        }
        if (PCreadable) {
            myled1 = 1;
            while (PC.readable()) IMU.putc(PC.getc());
            PCreadable = false;
            myled1 = 0;
        }
        /*if (parseNow) {
            parse(buffer);
            buffer.clear();
            parseNow = false;
        }*/
    }
}

//Interrupt called when there is a character to be read from the PC
//To avoid a livelock, we disable interrupts at the end of the interrupt.
//Then, in the main loop, we read everything from the buffer
void readPC() {
    PCreadable = true;
    __disable_irq();
}

//Interrupt called when there is a character to be read from the IMU
void readIMU() {
    IMUreadable = true;
    __disable_irq();
}


//The bitshift performed always creates a positive integer. Sometimes, the IMU
//values are negative. This fixes that - it centers the value around 512. That is,
//512 is 0 for the IMU variables. Values above it are positive, and values below it are negative.
inline void makeCorrect (short* i) {
    if ((*i)>>15) *i = 512 - (~(*i));
    else *i = 512 + *i;
}