File content as of revision 3:af291e8853b1:

#include "mbed.h"

#include "gps_utils.h"

//Test for correctness when in a room that does produce it's own magnetic field...
//Magnetic Declination for Canterbury = WEST 0deg 16min
#define DECLINATION_ANGLE ((-16.0*M_PI)/(60.0*180.0))
#include "HMC5883L.h"

Serial usb(USBTX,USBRX);

HMC5883L compass(I2C_SDA, I2C_SCL);
#define SMOOTHING 0.75

double smoothedHeading() {
    static int historic = compass.getHeadingXYDeg();
    
    double h = compass.getHeadingXYDeg();
    
    if(h==historic) return h;
    
    if( (h<180.0)==(historic<180.0) ) {
        
        historic = historic*SMOOTHING + h*(1.0-SMOOTHING);
        
    } else {
        if(h<180) {
            
            historic = historic*SMOOTHING + (h+360.0)*(1.0-SMOOTHING);
            
        } else {
            
            historic = (historic+360.0)*SMOOTHING + h*(1.0-SMOOTHING);
            
        }
        if(historic>=360.0) {
            historic-=360.0;
        }
    }
    
    return historic;
}

#define COMPASS_CALC 200

double xs[COMPASS_CALC];
double ys[COMPASS_CALC];
int compassIndex = 0;
bool completed = false;

void tick()
{
    int16_t raw_data[3];
    compass.getXYZ(raw_data);
    
    xs[compassIndex] = static_cast<double>(raw_data[0]);
    ys[compassIndex] = static_cast<double>(raw_data[2]);
    
    compassIndex++;
    if(compassIndex==COMPASS_CALC) {
        completed = true;
        compassIndex = 0;
    }
}

Ticker ticker;

int main() {
    ticker.attach_us(&tick,50000);
    
    while(1) {
        wait(0.5f);
        if(completed) {
            double avgX=0.0, avgY = 0.0;
            
            for(int i=0; i<COMPASS_CALC; i++) {
                avgX+=xs[i]; avgY+=ys[i];
            }
            avgX/=COMPASS_CALC; avgY/=COMPASS_CALC;
            
            double Suu=0.0, Suv=0.0, Svv=0.0, Suuu=0.0, Suvv=0.0, Suuv=0.0, Svvv=0.0;
            
            for(int i=0; i<COMPASS_CALC; i++) {
                double u = xs[i]-avgX;
                double v = ys[i]-avgY;
                Suu+=u*u; Suv+=u*v; Svv+=v*v;
                Suuu+=u*u*u; Suvv+=u*v*v;
                Suuv+=u*u*v; Svvv+=v*v*v;
            }
            
            double a = (Suuu+Suvv)*0.5;
            double b = (Svvv+Suuv)*0.5;
            double d = Suv*Suv-Suu*Svv;
            
            double uc = (b*Suv-a*Svv)/d;
            double vc = (a*Suv-b*Suu)/d;
            
            double R = sqrt(uc*uc + vc*vc + (Suu+Svv)/COMPASS_CALC);
            double cx = uc+avgX-R;
            double cy = vc+avgY-R;
            
            usb.printf("X offset: %.3f\r\n", -cx);
            usb.printf("Y offset: %.3f\r\n", -cy);
            usb.printf("Radius: %.3f\r\n", R);
            
            int16_t raw_data[3];
            compass.getXYZ(raw_data);
            //The  HMC5883L gives X Z Y order
            double heading = atan2(static_cast<double>(raw_data[2])-cy, static_cast<double>(raw_data[0])-cx);
            
            usb.printf("Heading: %.3f\r\n", heading);
            
        }
    }
}

//int main()
//{
//    compass.init();
//    for(;;) {
//        
//        wait_ms(500);
//        
//        double h = smoothedHeading();
//        usb.printf("%.2f\r\n",h);
//    }
//}

//double degToRad(double deg) {
//    return deg*M_PI/180.0;
//}
//
//void gpsTest(int test, double lat1, double lon1, double lat2, double lon2) {
//    usb.printf("Test %d\r\n\r\nDistances:\r\n", test);
//    usb.printf("  Haversine: %.3fm\r\n", haversine(lat1, lon1, lat2, lon2));
//    usb.printf("  Cosines: %.3fm\r\n", cosines(lat1, lon1, lat2, lon2));
//    usb.printf("  Equirectangular: %.3fm\r\n", equirectangular(lat1, lon1, lat2, lon2));
//    usb.printf("\nBearings:\r\n  Start: %.3f rad\r\n", startBearing(lat1, lon1, lat2, lon2));
//    usb.printf("  End: %.3f rad\r\n\r\n", endBearing(lat1, lon1, lat2, lon2));
//}

//int main() {
//    
//    gpsTest(1,degToRad(51.302310),degToRad(1.138862),degToRad(51.303853),degToRad(1.147445));
    //Actual Distance = 968.9km
    //Initial Bearing = 0.1592
    //Final Bearing = 0.1968
    
//    gpsTest(2,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
//    
//    gpsTest(3,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
//    
//    gpsTest(4,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
//}