File content as of revision 2:770f77c92bab:

#include "mbed.h"
#include "FXOS8700Q.h"
#include "eCompass_Lib.h"
#include "rtos.h"
#include "C12832.h"

// Using Arduino pin notation
C12832 lcd(D11, D13, D12, D7, D10); // App shield LCD
FXOS8700Q combo1( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
Serial pc(USBTX, USBRX);
DigitalOut gpo(D0);
DigitalOut red(LED_RED);
DigitalOut led(D5);
DigitalOut green(LED_GREEN);
DigitalOut blue(LED_BLUE);
eCompass compass;

extern axis6_t axis6;
extern uint32_t seconds;
extern uint32_t compass_type; // optional, NED compass is default
extern int32_t tcount;
extern uint8_t cdebug;
int  l = 0;


void hal_map( int16_t * acc, int16_t * mag)
{
int16_t t;
// swap and negate X & Y axis
t = acc[0];
acc[0] = acc[1] * -1;
acc[1] = t * -1;
// swap mag X & Y axis
t = mag[0];
mag[0] = mag[1];
mag[1] = t;
// negate mag Z axis
mag[2] *= -1;
}

//
// Print data values for debug
//
void debug_print(void)
{
    // Some useful printf statements for debug
    printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
    printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
    printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
    printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\r\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3); 
}

void doRGB(void)
{
    int16_t h;
    h = axis6.yaw;
    if((h >= 358) || (h <= 2)) {
        red = 1;
        green = 0;
        blue = 1;
        return;
        }
    if((h >= 353) || (h <= 7)) {
        red = 1;
        green = 0;
        blue = 0;
        return;
        }
    if((h >=  85) && (h <=  95)) {
        red = 0;
        green = 1;
        blue = 1;
        return;
        }
    if((h >= 265) && (h <= 275)) {
        red = 0;
        green = 1;
        blue = 1;
        return;
        }
    if((h >= 175) && (h <= 185)) {
        red = 0;
        green = 1;
        blue = 0;
        return;
        }
    red = 1;
    green = 1;
    blue = 1;
}
    

void compass_thread(void const *argument) {
    static int16_t acc_raw[3], mag_raw[3];
    // get raw data from the sensors
    combo1.AccXYZraw( acc_raw);
    combo1.MagXYZraw( mag_raw);
    if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
    if(!l)      led = 0;
        else    led = 1;
    doRGB();
    if(l++ >= 50) { // take car of business once a second
        seconds++;
        debug_print();
        compass.calibrate(); // re-calibrate the eCompass every second
        l = 0;
        }
    tcount++;
}
         
int main() {

uint8_t d[2];
int16_t acc_raw[3], mag_raw[3];
d[0] = 0;
red = 1;
green = 1;
blue = 1;
//cdebug = 1;  // uncomment to disable compass
lcd.cls();
lcd.locate(0,1);
lcd.printf("K64F eCompass");
printf("\r\n\n\n\n\n\n\n");
combo1.enable();
combo1.readRegs( FXOS8700Q_STATUS, d, 1);
printf("Status= %X\r\n", d[0]);
combo1.readRegs( FXOS8700Q_WHOAMI, d, 1);
printf("Who AM I= %X\r\n", d[0]);
combo1.readRegs( FXOS8700Q_XYZ_DATA_CFG, d, 1);
printf("XYZ Cfg= %X\r\n", d[0]);
combo1.readRegs( FXOS8700Q_CTRL_REG1, d, 1);
printf("CTRL REG1= %X\r\n", d[0]);
combo1.readRegs( FXOS8700Q_M_CTRL_REG1, d, 1);
printf("MAG CTRL REG1= %X\r\n", d[0]);
combo1.readRegs( FXOS8700Q_M_CTRL_REG2, d, 1);
printf("Mag CTRL REG2= %X\r\n\r\n", d[0]);

combo1.AccXYZraw( acc_raw);
combo1.MagXYZraw( mag_raw);

RtosTimer compass_timer(compass_thread, osTimerPeriodic);
compass_timer.start(20); // Run the Compass every 20ms
while(1) {
    lcd.locate(0,11);
    lcd.printf("Seconds: %04d Heading: %03d", seconds, axis6.yaw);
    lcd.locate(0,22);
    lcd.printf("Roll   : %03d  Pitch: %02d", axis6.roll, axis6.pitch);
    wait(0.1);
    }
}