Diff: main.cpp

Revision:

0:64f7d228ad20

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Aug 07 10:29:15 2015 +0000
@@ -0,0 +1,81 @@
+#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) {
+     }
+}
\ No newline at end of file