File content as of revision 4:8f63393d49fb:

/*
 * 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, http://www.sparkfun.com/products/10736
 */

#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];

AnalogOut aout(p18);

int main() {
    aout = 1.0;
    // 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");
                    
                    //This should be easier to understand than bitshifts. bufIMU is a pointer (arrays are pointers).
                    //We offset it to the start of the desired value. We then typecast bufIMU into a short (16 bit).
                    //This turns bufIMU into a pointer to the desired value. Now we dereference it.
                    //I'm not sure if we'll still need makeCorrect. Adit, check plz <3
                    accX = *((short*)(bufIMU + 1));
                    accY = *((short*)(bufIMU + 3));
                    accZ = *((short*)(bufIMU + 5));
                    
                    gyrX = *((short*)(bufIMU + 7));
                    gyrY = *((short*)(bufIMU + 9));
                    gyrZ = *((short*)(bufIMU + 11));
                    
                    magX = *((short*)(bufIMU + 13));
                    magY = *((short*)(bufIMU + 15));
                    magZ = *((short*)(bufIMU + 17));
                    
                    //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();
}


//So negative numbers that are transferred are in twos complement form
// and the compiler seems to like to use things created by bitwise operators
// in unsigned form so all of the bits are switched and we switch it back and center
// it around 512 which we are using as out zero value
inline void makeCorrect (short* i) {
    if ((*i)>>15) *i = 512 - (~(*i));
    else *i = 512 + *i;
}