File content as of revision 16:995c9920f14e:

#include "compass_sensor.h"

char addr = 0x1E<<1; //address of the compass
char rawBytes[6]; //raw data extracted from the read of the compass sensor
short data[6]; //rawBytes is the entered into data
short magComponent[3]; //x, z, y
double heading; //the "degree" variable
double sum; //used for calculating the average

void compass_config(void) //configures the sensor
{  
    i2c_port.start(); //starts the compass
    i2c_port.write(addr); //sets the address
    i2c_port.write(0x02); //writes the bytes
    i2c_port.write(0x00); //writes the bytes
    i2c_port.stop(); //stops
}

double calculation(void)
{
                i2c_port.read(addr,rawBytes,6); //reads bytes into rawBytes, there are 6 array 
                for (int i = 0; i<6; i++) //read rawBytes array into data array
                {
                    data[i] = rawBytes[i];
                }
                magComponent[0] = data[0]<<8 | data[1]; //x-component
                magComponent[2] = data[2]<<8 | data[3]; //z-component
                magComponent[1] = data[4]<<8 | data[5]; //y-component
        
            heading = atan2((double)magComponent[1], (double)magComponent[0]); //equation for getting the degrees in radians
            heading = heading*(180.0/3.1416); //radians to degrees
            heading = (heading + 14.85); //adjust for Spokane
            heading = heading-(0.13); //declination in years
            return heading;
}

float compass_n()
{        
                wait(0.02);
                i2c_port.start(); //starts the compass
                i2c_port.write(addr); //compass is given its address again
                i2c_port.write(0x03); //told to given 3 bytes
                i2c_port.stop();
            for (int m = 0; m<5; m++) //average of 5
            {
                heading = calculation(); //function that calculates the degrees from N
                sum += heading; //sum of the heading(s)
            }
            heading = sum/5; //the average of the heading(s) collected
            if (heading > 180){
                heading = heading - 360; //if greater than 180, then subtract 360 since it was too large
                }
            if (heading < -180){
                heading = heading +360;  //if less than -180, then add 360 since it was too small
                }
            return heading;
}