File content as of revision 0:2126a25ea273:

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

/* 
 *
 * Changes which must be made to default rtos settings:
 * Default scheduler stack size changed to 2048 for target K64F (RTX_Conf_CM.h, line 66)
 * Default stack size changed to 1024 words for ARMCC Compiler (cmsis_os.h, line 121)
 * The Timer Thread must run at Real Time priority (RTX_Conf_CM.c, line 182)
 * 
 */

extern Thread osTimerThread;
FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
FXOS8700Q_mag mag( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
eCompass compass;
DigitalOut red(D5); // RED LED on expansion shield
DigitalOut green(D9); // GREEN LED on expansion shield
DigitalOut blue(LED3); // BLUE LED on K64F Freedom board
C12832 lcd(D11, D13, D12, D7, D10); // App shield LCD


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;


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

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;
}


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

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= %d Y= %d Z= %d    ", acc_raw.x, acc_raw.y, acc_raw.z);
    //printf("Mag: X= %d Y= %d Z= %d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
    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\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); 
    printf("Tcount= %d\r\n", tcount);
}

void calibrate_thread(void const *argument) {
    while(true) {
        Thread::signal_wait(0x2); 
        blue = 0;
        compass.calibrate();
        blue = 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) {
int h, m, s;
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x4);  
        red = 0;  
        h = seconds / 3600;
        m = (seconds % 3600) / 60;
        s = seconds % 60;
        lcd.locate(0,1);
        lcd.printf("K64F eCompass\nRunTime %02d:%02d:%02d\nRoll: %03d Pitch: %02d Yaw: %03d ", h, m, s, axis6.roll, axis6.pitch, axis6.yaw);
        red = 1;
    }
}


int main (void) {
    red = 1;
    green = 1;
    blue = 1;
    Thread t1(print_thread);  
    Thread t2(calibrate_thread);
    Thread t3(lcd_thread);
    debugp = &t1;
    calibrate = &t2;
    lcdp = &t3;   
    acc.enable();
    mag.enable();
    lcd.cls();
    lcd.locate(0,1);
    lcd.printf("K64F eCompass");  
    acc.getAxis( acc_raw);
    mag.getAxis( mag_raw);
    // The Timer Thread runs at Real Time priority, see RTX_Conf_CM.c, line 182
    RtosTimer compass_timer(compass_thread, osTimerPeriodic);  
    compass_timer.start(20);
    /*
     * Thread Priorities
     * Compass THread,      Real Time (Highest)
     * Compass calibration, High Priority
     * LED Update,          Above Normal
     * Pprintf to console,  Normal
     * main(),              Normal
     */
    debugp->set_priority(osPriorityNormal);
    lcdp->set_priority(osPriorityAboveNormal);
    calibrate->set_priority(osPriorityHigh);
    green = 1;
    while (true) {
        Thread::wait(osWaitForever);
        }
}