File content as of revision 12:12369ee344ab:

#include "mbed.h"
#include "FXOS8700Q.h"
#include "eCompass_Lib.h"
#include "rtos.h"
//#include "MotionSensorDtypes.h"


FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);

DigitalOut gpo(D0);
DigitalOut led(LED_RED);
eCompass compass;

//void calibrate_thread(void const *argument);
//void print_thread(void const *argument);
//void compass_thread(void const *argument);



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;
volatile int sflag = 0;

MotionSensorDataCounts mag_raw;
MotionSensorDataCounts acc_raw;

void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
{
int16_t t;
// swap and negate X & Y axis
t = acc_raw->x;
acc_raw->x = acc_raw->y * -1;
acc_raw->y = t * -1;
// swap mag X & Y axis
t = mag_raw->x;
mag_raw->x = mag_raw->y;
mag_raw->y = t;
// negate mag Z axis
mag_raw->z *= -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 compass_thread(void const *argument) {

    // get raw data from the sensors
    acc.getAxis( acc_raw);
    mag.getAxis( mag_raw);
    if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
    if(l++ >= 50) { // take car of business once a second
        seconds++;
        sflag = 1;
        compass.calibrate();
        debug_print();
        l = 0;
        led = !led;
        }
    tcount++;
}
 
/*  
void calibrate_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x1);
        compass.calibrate(); // re-calibrate the eCompass every second
    }
}
 

  
void print_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x1);
        debug_print(); // re-calibrate the eCompass every second
    }
}
 */            

int compass_main() {


RtosTimer compass_timer(compass_thread, osTimerPeriodic);

//cdebug = 1;  // uncomment to disable compass
printf("\r\n\n\n\n\n\n\n");
printf("Who AM I= %X\r\n", acc.whoAmI());
acc.enable();


acc.getAxis( acc_raw);
mag.getAxis( mag_raw);

compass_timer.start(20); // Run the Compass every 20ms
    while(1) {
        Thread::wait(osWaitForever);
    }
}