File content as of revision 7:b275c76de4ba:

#include "mbed.h"
#include "compass.h"

I2C *compass;
const int compass_addr = 0x1E << 1;
int compass_avg_num = 1;
const char config_single_measure_mode[2] = {0x02, 0x01};
const char compass_config_data[1] = {0x03};

void compass_init(int num){
    compass = &connection;
    compass->write(compass_addr, config_single_measure_mode, 2); //set single measure mode
}


float get_heading(float declination){
    char rawBytes[6];
    short data[6];
    short magComponents[3]; //x, z, y
    
    compass->write(compass_addr, config_single_measure_mode, 2); //set single measure mode
    compass->write(compass_addr, compass_config_data, 1); //choose where to read from
    compass->read(compass_addr, rawBytes, 6); //read in 6 bytes
        
    //process the data
    //move to data
    for (int i = 0; i < 6; i++){
        data[i] = rawBytes[i];
    }
    //do bitshift things
    //move to magcomps (x, z, y)
    for (int i = 0; i < 3; i++){
        magComponents[i] = data[2*i] << 8 | data[2*i + 1];    
    }
    return calc_heading(magComponents, declination);
}

float calc_heading(short mags[3], float declination){
    double heading;
    heading = atan2((double)mags[2],(double)mags[0]); 
    heading = heading * 180 / 3.141593; //convert to radians
    heading += declination; //adjust for mag north vs true north
    if ( heading > 180) //keep the heading between -180 and 180 degrees
        heading -=360;    
    #ifdef DEBUG_MODE
    pc.printf("Heading: %f\r\n", heading);
    #endif
    return heading;
}

float get_avg_heading(float declination, int n){
    if (n <= 0)
        return 0;
    double total_heading = 0;
    for (int i = 0; i < n; i++){
        total_heading += get_heading(declination); //sum up the total of n readings
    }
    return total_heading / n; //divide by n to get the average
}