File content as of revision 4:ba1dbfb683fb:

#include "mbed.h"
#include "eCompass_Lib.h"
#include "MAG3110.h"
#include "MMA8451Q.h"
#include "rtos.h"
#include "SLCD.h"

#define MMA8451_I2C_ADDRESS (0x1d<<1)


eCompass compass;
MAG3110  mag( PTE25, PTE24);
MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
DigitalOut red(LED_RED);
DigitalOut green(LED_GREEN);
Serial pc(USBTX, USBRX);
SLCD slcd;

extern axis6_t axis6;
extern uint32_t seconds;
extern uint32_t compass_type;
extern int32_t tcount;
extern uint8_t cdebug; 


MotionSensorDataCounts mag_raw;
MotionSensorDataCounts acc_raw;


Thread *(debugp);
Thread *(calibrate);
Thread *(lcdp);

//
// Print data values for debug
//
void debug_print(void)
{
    int h, m, s;
    h = seconds / 3600;
    m = (seconds % 3600) / 60;
    s = seconds % 60;
    // Some useful printf statements for debug
    printf("Runtime= %d:%02d:%02d\r\n", h, m, s);
    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", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
    printf("Quaternion: Q0= %1.4f Q1= %1.4f Q2= %1.4f Q3= %1.4f\r\n\n", axis6.q0, axis6.q1, axis6.q2, axis6.q3); 
}
// HAL Map for KL46 Freedom board MMA8451Q & MAG3110 sensors
//
// This routing move and negates data as needed the
// properly align the sensor axis for our desired compass (NED)
// For more information see Freescale appication note AN4696
//
void hal_map( MotionSensorDataCounts * acc_data, MotionSensorDataCounts * mag_data)
{
    int16_t t;
    // swap and negate accelerometer x & y
    t = acc_data->y;
    acc_data->y = acc_data->x * -1;
    acc_data->x = t * -1;

    // negate magnetometer y
    mag_data->y *= -1;
    
}

//
// This is the 50Hz thread where the magic happens
//
int  l = 0;

void compass_thread(void const *argument) {
        // get raw data from the sensors
        mag.getAxis(mag_raw);
        acc.getAxis(acc_raw);
        if(tcount) compass.run( acc_raw, mag_raw); // calculate the eCompass
        if(!(l % 10)) lcdp->signal_set(0x04);
        if(l++ >= 50) { // take car of business once a second
            seconds++;
            calibrate->signal_set(0x2);
            debugp->signal_set(0x01);
            l = 0;
            green = !green;
            }
        tcount++;
}


void calibrate_thread(void const *argument) {
    while(true) {
        Thread::signal_wait(0x2); 
        red = 0;
        compass.calibrate();
        red = 1;
        }
}

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


void lcd_thread(void const *argument) {
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x4);  
        slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
    }
}
                                 
int main() {

    slcd.clear();
    tcount = 0;
    red = 1;
    green = 1;
    //cdebug = 1; // Set to 1 to disable eCompass in order to observe raw mag data.
    compass_type = NED_COMPASS;
    seconds = 0;
    Thread t1(print_thread);  
    Thread t2(calibrate_thread);
    Thread t3(lcd_thread);
    debugp = &t1;
    calibrate = &t2;
    lcdp = &t3;  
    mag.enable();
    acc.enable();
    // Say hello to our sensors
    // Native KL46-FRDM sensors
    printf("\r\nMMA8451Q ID= %X\r\n", acc.whoAmI());
    printf("MAG3110 ID= %X\r\n", mag.whoAmI());
    mag.getAxis(mag_raw); // flush the magnetmeter
    RtosTimer compass_timer(compass_thread, osTimerPeriodic);
    
    /*
     * Thread Priorities
     * Compass Thread,      High Priority
     * Compass calibration, Above Normal
     * LED Update,          Normal
     * printf to console,   Below Normal
     * main(),              Normal
     */

    debugp->set_priority(osPriorityBelowNormal);
    lcdp->set_priority(osPriorityLow);
    calibrate->set_priority(osPriorityAboveNormal);
    compass_timer.start(20); // Run the Compass every 20ms
    
    Thread::wait(osWaitForever);   
}