File content as of revision 1:2d2270d1a5f5:

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


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

// This HAL map is for the FXOS8700Q on the K64F Freedom board.
void hal_map( MotionSensorDataCounts * acc_raw, MotionSensorDataCounts * mag_raw)
{
int16_t t;
// swap and negate accelerometer X & Y axis
t = acc_raw->x;
acc_raw->x = acc_raw->y * -1;
acc_raw->y = t * -1;
// swap magnetometer X & Y axis
t = mag_raw->x;
mag_raw->x = mag_raw->y;
mag_raw->y = t;
// negate magnetometer Z axis
mag_raw->z *= -1;
}


void compass_thread(void const *argument) {
    // 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;
}

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", axis6.q0, axis6.q1, axis6.q2, axis6.q3); 
    printf("Tcount= %d\r\n\n\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,      High Priority
     * Compass calibration, Above Normal
     * LED Update,          Normal
     * printf to console,   Below Normal
     * main(),              Normal
     */
    debugp->set_priority(osPriorityBelowNormal);
    lcdp->set_priority(osPriorityNormal);
    calibrate->set_priority(osPriorityAboveNormal);
    green = 1;
    while (true) {
        Thread::wait(osWaitForever);
        }
}