File content as of revision 3:cc1415081d36:

#include "mbed.h"
#include "rtos.h"
#include "FreeIMU.h"

Serial pc(USBTX, USBRX); // tx, rx

typedef struct {
    float q[4];
} imu_t;

FreeIMU imu;
Mail<imu_t, 8> imu_queue;
int i = 0;

void getIMUdata(void const *n)
{
    i++;
    if (i%4==0) {
        float *q = (float *)imu_queue.alloc();
        imu.getQ(q);
        imu_queue.put((imu_t*)q);
    } else {
        imu.getQ(NULL);
    }
}

// thanks to Francesco Ferrara and the Simplo project for the following code!
void writeVar(void * val, uint8_t type_bytes)
{
    char * addr=(char *)(val);
    for(uint8_t i=0; i<type_bytes; i++) {
        //Serial1.write(addr[i]);
        pc.putc(addr[i]);
    }
}

void writeArr(void * varr, uint8_t arr_length, uint8_t type_bytes)
{
    char * arr = (char*) varr;
    for(uint8_t i=0; i<arr_length; i++) {
        writeVar(&arr[i * type_bytes], type_bytes);
    }
}

void serialFloatPrint(float f)
{
    char * b = (char *) &f;
    for(int i=0; i<4; i++) {

        char b1 = (b[i] >> 4) & 0x0f;
        char b2 = (b[i] & 0x0f);

        char c1 = (b1 < 10) ? ('0' + b1) : 'A' + b1 - 10;
        char c2 = (b2 < 10) ? ('0' + b2) : 'A' + b2 - 10;

        pc.putc(c1);
        pc.putc(c2);
    }
}

void serialPrintFloatArr(float * arr, int length)
{
    for(int i=0; i<length; i++) {
        serialFloatPrint(arr[i]);
        pc.putc(',');
    }
}

//if the Processing program cannot accept serial inputs, uncomment this to send quaternion information automatically
//#define AUTO_OUTPUT

int main()
{
    pc.baud(115200);
    imu.init(true);
    pc.baud(115200);
    RtosTimer IMUTimer(getIMUdata, osTimerPeriodic, (void *)0);
    IMUTimer.start(5);

    while(true) {
        int16_t raw_values[9];

        Thread::wait(5);
        char cmd = 'q';
#ifndef AUTO_OUTPUT
        cmd = pc.getc();
#endif

        if(cmd=='v') {
            char str[256];
            sprintf(str, "FreeIMU library by %s, FREQ:%s, LIB_VERSION: %s, IMU: %s", FREEIMU_DEVELOPER, 0x10000000, FREEIMU_LIB_VERSION, 0);
            pc.printf("%s", str);
            pc.printf("\r\n");
        }

        if (cmd=='q') {
            uint8_t count = 32;
#ifndef AUTO_OUTPUT
            count = pc.getc();
#endif
            for(uint8_t i=0; i<count; i++) {
                osEvent evt = imu_queue.get();
                if (evt.status == osEventMail) {
                    imu_t *obj = (imu_t*)evt.value.p;
                    serialPrintFloatArr(obj->q, 4);
                    imu_queue.free(obj);
                    pc.printf("\r\n");
                }
                Thread::yield();
            }
        }
        
        if(cmd=='o') {
            uint8_t count = pc.getc();
            float raw_valuesf[9];
            for(uint8_t i=0; i<count; i++) {

                imu.getValues(raw_valuesf);

                pc.printf("%f, %f, %f", raw_valuesf[3], raw_valuesf[4], raw_valuesf[5]);

                pc.printf("\r\n");
                Thread::wait(4);
            }
        }

        if(cmd=='b') {
            uint8_t count = pc.getc();
            for(uint8_t i=0; i<count; i++) {

                imu.accgyro->getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);

                writeArr(raw_values, 6, sizeof(int16_t)); // writes accelerometer and gyro values

                imu.magn->getValues(&raw_values[0], &raw_values[1], &raw_values[2]);
                writeArr(raw_values, 3, sizeof(int16_t));

                pc.printf("\r\n");
                Thread::wait(4);
            }
        }
    }

}