File content as of revision 4:8f63393d49fb:

#include "mbed.h"
#include "imu.h"

/* 
 * IMU data averages, flat surface
 * 20.138444, -15.096182, 232.355290, -29.691927, -14.679685, 45.097056, -75.690469, 0.737977, -159.505392
 */

IMU::IMU(int baud, PinName tx, PinName rx, Serial* pc) {
    p_pc = pc;
    p_pc->printf("Initializing IMU...\n");
    p_device = new Serial(tx, rx);
    p_device->baud(baud);
    p_device->format(8, Serial::None, 1);
    readable = false;
    i_IMU = 0;
    accX = accY = accZ = gyrX = gyrY = gyrZ = magX = magY = magZ = 0;
    p_device->putc('4'); //Tell the IMU to start sending binary data, if it isn't already
    newData = false;
}

IMU::~IMU() {
    if (p_device != NULL){
        delete p_device;    //This prevents memory leaks and keeps the port accessible for future use.
    }
}

//Wrapper function to write a character to the IMU
void IMU::putc(char c) {
    p_device->putc(c);
}


//Wrapper function to attach a callback function to the RX interrupt of the IMU Serial object
void IMU::attach(void (*fptr)(void)) {
    p_pc->putc('a');
    p_device->attach(fptr);
}

//Gets all data from the IMU read buffer. If we should parse, parse.
void IMU::getData() {
    readable = false;
    //p_pc->printf("HELLO");
    __enable_irq(); 
    //NVIC_EnableIRQ(UART1_IRQn);
    while (p_device->readable()) {
        //p_pc->printf("readable");
        // This snippet of code should be run whenever a character can be received from the IMU.

        char data = p_device->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;
            //p_pc->printf("new data\n\r");
        }
        // Something went wrong.
        else if (i_IMU > 21) {
            for (int i = 0; i < 20; i++) {
                p_pc->printf("\n\r");
            }
            //p_pc->printf("\t\t\t\t\t\t\t\t\t**\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*/) {
            parse();
        }


        bufIMU[i_IMU] = data;
        i_IMU++;
        //parseNow = (buffer.at(buffer.length() - 1) == '#');
    }
}

// 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 IMU::makeCorrect(short* i) {
    if ((*i)>>15) *i = 0-(~(*i)+1);
    else *i = 0+*i;
}

/*
void IMU::attach(void) {
    p_device->attach(this->readIMU);
}*/

void IMU::parse() {
    //p_pc->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.
    
    //Don't use this. It doesn't work. 
    //Kevin said this didn't work because of endianness. The Cortex M3 has a macro to reverse.
    //See http://mbed.org/forum/helloworld/topic/1573/?page=1#comment-7818
    /*accX = *((short*)(bufIMU + 2));
    accY = *((short*)(bufIMU + 4));
    accZ = *((short*)(bufIMU + 6));

    gyrX = *((short*)(bufIMU + 8));
    gyrY = *((short*)(bufIMU + 10));
    gyrZ = *((short*)(bufIMU + 12));

    magX = *((short*)(bufIMU + 14));
    magY = *((short*)(bufIMU + 16));
    magZ = *((short*)(bufIMU + 18));*/

    //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);
    
    newData = true;
    
    //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
}


//Callback called when there is a character to be read from the IMU
void readIMU() {
    imu.readable = true;
    __disable_irq();
    //NVIC_DisableIRQ(UART1_IRQn);
}

int main() {
    PC.format(8, Serial::None, 1);
    PC.baud(115200);

    for (int i = 0; i < 80; i++) {
        PC.putc('-');
    }
    PC.printf("\n\r");
    imu.attach(&readIMU);
    //imu.p_device->attach(&readIMU);
    imu.putc('6');
    
    PC.printf("Waiting for data.");
    
    // Calibration variables
    long long sumAccX = 0, sumAccY = 0, sumAccZ = 0;
    long long sumGyrX = 0, sumGyrY = 0, sumGyrZ = 0;
    long long sumMagX = 0, sumMagY = 0, sumMagZ = 0;
    int num = 0;
    
    while (true){
        if (imu.readable) {
            //PC.printf("fun");
            imu.getData();
            if (imu.newData) {
                PC.printf("Data: %d, %d, %d, %d, %d, %d, %d, %d, %d\n\r", imu.accX, imu.accY, imu.accZ, imu.gyrX, imu.gyrY, imu.gyrZ, imu.magX, imu.magY, imu.magZ);
                
                sumAccX += imu.accX;
                sumAccY += imu.accY;
                sumAccZ += imu.accZ;
                
                sumGyrX += imu.gyrX;
                sumGyrY += imu.gyrY;
                sumGyrZ += imu.gyrZ;
                
                sumMagX += imu.magX;
                sumMagY += imu.magY;
                sumMagZ += imu.magZ;
                
                num++;
                
                PC.printf("Averages: %f, %f, %f, %f, %f, %f, %f, %f, %f\n", 
                    sumAccX/(double)num, sumAccY/(double)num, sumAccZ/(double)num,
                    sumGyrX/(double)num, sumGyrY/(double)num, sumGyrZ/(double)num, 
                    sumMagX/(double)num, sumMagY/(double)num, sumMagZ/(double)num
                );
                
                imu.newData = false;
            }
        }
    }
}