File content as of revision 0:64f7d228ad20:

#include "mbed.h"
#include "HMC5883L.h"
 
#define STOP 0  //initial
#define CAL 1   //calibration
#define RUN 2   //run
//#define M_PI 3.141592
 
Serial pc(USBTX, USBRX); 
HMC5883L compass(p9, p10);
Ticker interrupt;
 
double heading0 = 0.0;
double heading1 = 0.0;
double heading2 = 0.0;
double heading3 = 0.0;
double headingLPF = 0.0;
double initHeading;
double tgtHeading;
double preHeading = 0.0;
unsigned char mode;
 
int maxX, minX, maxY, minY;
int ofsX = 0;                   //calibration x
int ofsY = 0;                   //calibration y
int counter = 0;
 
void intrpt() {
    int16_t raw[3];
     
    compass.getXYZ(raw);
    double heading = atan2(static_cast<double>(raw[2]-ofsY), static_cast<double>(raw[0]-ofsX)); //y=raw[2]
    if(heading < 0)heading += 2*M_PI;
    if(heading > 2*M_PI)heading -= 2*M_PI;
    heading3 = heading2;
    heading2 = heading1;
    heading1 = heading0;
    heading0 = heading;
    headingLPF = (heading0 + heading1 + heading2 + heading3)/4; //low pass filter
    switch(mode) {
    case STOP:
        if(counter == 50) {
            initHeading = headingLPF;
            mode = CAL;
            maxX = raw[0];
            minX = raw[0];
            maxY = raw[2];
            minY = raw[2];
            counter = 0;
        }
        break;
    case CAL:
        if(raw[0] > maxX)maxX = raw[0];
        if(raw[0] < minX)minX = raw[0];
        if(raw[2] > maxY)maxY = raw[2];
        if(raw[2] < minY)minY = raw[2];
        if((counter > 50) 
            && (headingLPF > (initHeading-0.01)) 
            && (headingLPF < (initHeading+0.01))) {
            ofsX = (maxX + minX)/2;
            ofsY = (maxY + minY)/2;
            mode = RUN;
            counter = 0;
            pc.printf("ofs=%d,%d\r\n",ofsX,ofsY);
        }
        break;
    case RUN:
        headingLPF = headingLPF * 180.0 / M_PI;
        pc.printf("heading=%f\r\n",headingLPF);
        break;
    }
    counter++;
}
 
int main() {
    mode = STOP;
    compass.init();
    interrupt.attach(&intrpt, 0.04);
    while(1) {
     }
}