File content as of revision 2:d8b182fbe018:

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

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('6'); //Tell the IMU to start sending binary data, if it isn't already
}

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) {
            //p_pc->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*/) {
            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 = 512-(~(*i));
    else *i = 512+*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);


    //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');
    while (true){
        if (imu.readable) {
            //PC.printf("fun");
            imu.getData();
            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);
        }
    }
}