File content as of revision 0:4e1d43dc608f:

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

#define MMA8451_I2C_ADDRESS (0x1d<<1)


MAG3110  mag( PTE25, PTE24);
MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
I2C      i2c( PTE25, PTE24);
DigitalOut led(LED_RED);
Serial pc(USBTX, USBRX);
SLCD slcd;

axis6_t axis6;
unsigned int seconds;
unsigned int compass_type;
int tcount;

// This routing move and negates data as needed the
// properly align the sensor axis for our desired compass (NED)
//
void hal_map(int16_t * acc, int16_t * mag)
{
    int16_t t;
    // swap accelerometer x & y
    t = acc[1];
    acc[1] = acc[0];
    acc[0] = t;
    // negate accelerometer x & y
    acc[0] *= -1;
    acc[1] *= -1;
    // negate magnetometer y
    mag[1] *= -1;
    
}
    

void manage_axis6(int16_t * acc_raw, int16_t * mag_raw)
{
    axis6.timestamp = tcount;
    hal_map(acc_raw, mag_raw);
    //
    // raw data
    axis6.acc_x = acc_raw[0];
    axis6.acc_y = acc_raw[1];
    axis6.acc_z = acc_raw[2];
    axis6.mag_x = mag_raw[0];
    axis6.mag_y = mag_raw[1];
    axis6.mag_z = mag_raw[2];
    //
    // raw data converted to floating ouing
    axis6.fax = (float) acc_raw[0];
    axis6.fay = (float) acc_raw[1];
    axis6.faz = (float) acc_raw[2];
    axis6.fmx = (float) mag_raw[0];
    axis6.fmy = (float) mag_raw[1];
    axis6.fmz = (float) mag_raw[2];
    //
    // Accelerometer data converted to Gs
    axis6.fGax = ((float) acc_raw[0]) / 4096.0;
    axis6.fGay = ((float) acc_raw[1]) / 4096.0;
    axis6.fGaz = ((float) acc_raw[2]) / 4096.0;
    //
    // Magnetometer data converted to microteslas
    axis6.fUTmx = ((float) mag_raw[0]) / 10.0;
    axis6.fUTmy = ((float) mag_raw[1]) / 10.0;
    axis6.fUTmz = ((float) mag_raw[2]) / 10.0; 
    }

//
// This is the 50Hz thread where the magic happens
//
void compass_thread(void const *argument) {
    static int16_t acc_raw[3], mag_raw[3];
    static int l = 0;
    while (true) {
        // Signal flags that are reported as event are automatically cleared.
        Thread::signal_wait(0x1);
        // get raw data from the sensors
        mag.ReadXYZraw(mag_raw);
        acc.getAccXYZraw(acc_raw);
        
        manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass
        if(tcount) run_eCompass(); // calculate the eCompass
        slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
        if(l++ >= 50) { // take car of business once a second
            seconds++;
            calibrate_eCompass(); // re-calibrate the eCompass every second
            l = 0;
            /* Some usful 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);
            */
            led = !led;
            }
        tcount++;
    }
}
                                   
int main() {
    Thread thread(compass_thread);
    float mag_data[3];
    
    slcd.clear();
    tcount = 0;

    init_eCompass();
    compass_type = NED_COMPASS;
    seconds = 0;
    // make sure our sensors are talking to us
    printf("\r\n%X\r\n", acc.getWhoAmI());
    printf("%X\r\n", mag.readReg(MAG_WHO_AM_I));
    mag.ReadXYZ(mag_data); // flush the magnetmeter
    while (1) {
        Thread::wait(20); // run out thread every 20ms for a 50Hz rate
        thread.signal_set(0x1);    
        }
}