Revision 3:af291e8853b1, committed 2016-03-24

Comitter:

Condo2k4

Date:

Thu Mar 24 16:40:18 2016 +0000

Parent:

2:3a288d8c816b

Commit message:

Compass Calibration

Changed in this revision

--- a/main.cpp	Wed Mar 23 19:22:07 2016 +0000
+++ b/main.cpp	Thu Mar 24 16:40:18 2016 +0000
@@ -41,6 +41,79 @@
     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();
@@ -53,22 +126,22 @@
 //    }
 //}
 
-double degToRad(double deg) {
-    return deg*M_PI/180.0;
-}
+//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));
+//}
 
-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));
+//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
@@ -78,4 +151,4 @@
 //    gpsTest(3,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
 //    
 //    gpsTest(4,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
-}
\ No newline at end of file
+//}
\ No newline at end of file