Keith G / Mbed 2 deprecated compass_test

Initial import

Dependencies:   HMC5883L mbed

main.cpp@3:af291e8853b1, 2016-03-24 (annotated)

Committer:
Condo2k4
Date:
Thu Mar 24 16:40:18 2016 +0000
Revision:
3:af291e8853b1
Parent:
2:3a288d8c816b 
Compass Calibration

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Condo2k4 0:b3e9ce4cc500 1 #include "mbed.h"
Condo2k4 0:b3e9ce4cc500 2
Condo2k4 2:3a288d8c816b 3 #include "gps_utils.h"
Condo2k4 2:3a288d8c816b 4
Condo2k4 0:b3e9ce4cc500 5 //Test for correctness when in a room that does produce it's own magnetic field...
Condo2k4 0:b3e9ce4cc500 6 //Magnetic Declination for Canterbury = WEST 0deg 16min
Condo2k4 0:b3e9ce4cc500 7 #define DECLINATION_ANGLE ((-16.0*M_PI)/(60.0*180.0))
Condo2k4 0:b3e9ce4cc500 8 #include "HMC5883L.h"
Condo2k4 0:b3e9ce4cc500 9
Condo2k4 0:b3e9ce4cc500 10 Serial usb(USBTX,USBRX);
Condo2k4 0:b3e9ce4cc500 11
Condo2k4 0:b3e9ce4cc500 12 HMC5883L compass(I2C_SDA, I2C_SCL);
Condo2k4 0:b3e9ce4cc500 13 #define SMOOTHING 0.75
Condo2k4 0:b3e9ce4cc500 14
Condo2k4 0:b3e9ce4cc500 15 double smoothedHeading() {
Condo2k4 0:b3e9ce4cc500 16 static int historic = compass.getHeadingXYDeg();
Condo2k4 0:b3e9ce4cc500 17
Condo2k4 0:b3e9ce4cc500 18 double h = compass.getHeadingXYDeg();
Condo2k4 0:b3e9ce4cc500 19
Condo2k4 0:b3e9ce4cc500 20 if(h==historic) return h;
Condo2k4 0:b3e9ce4cc500 21
Condo2k4 0:b3e9ce4cc500 22 if( (h<180.0)==(historic<180.0) ) {
Condo2k4 0:b3e9ce4cc500 23
Condo2k4 0:b3e9ce4cc500 24 historic = historic*SMOOTHING + h*(1.0-SMOOTHING);
Condo2k4 0:b3e9ce4cc500 25
Condo2k4 0:b3e9ce4cc500 26 } else {
Condo2k4 0:b3e9ce4cc500 27 if(h<180) {
Condo2k4 0:b3e9ce4cc500 28
Condo2k4 0:b3e9ce4cc500 29 historic = historic*SMOOTHING + (h+360.0)*(1.0-SMOOTHING);
Condo2k4 0:b3e9ce4cc500 30
Condo2k4 0:b3e9ce4cc500 31 } else {
Condo2k4 0:b3e9ce4cc500 32
Condo2k4 0:b3e9ce4cc500 33 historic = (historic+360.0)*SMOOTHING + h*(1.0-SMOOTHING);
Condo2k4 0:b3e9ce4cc500 34
Condo2k4 0:b3e9ce4cc500 35 }
Condo2k4 0:b3e9ce4cc500 36 if(historic>=360.0) {
Condo2k4 0:b3e9ce4cc500 37 historic-=360.0;
Condo2k4 0:b3e9ce4cc500 38 }
Condo2k4 0:b3e9ce4cc500 39 }
Condo2k4 0:b3e9ce4cc500 40
Condo2k4 0:b3e9ce4cc500 41 return historic;
Condo2k4 2:3a288d8c816b 42 }
Condo2k4 2:3a288d8c816b 43
Condo2k4 3:af291e8853b1 44 #define COMPASS_CALC 200
Condo2k4 3:af291e8853b1 45
Condo2k4 3:af291e8853b1 46 double xs[COMPASS_CALC];
Condo2k4 3:af291e8853b1 47 double ys[COMPASS_CALC];
Condo2k4 3:af291e8853b1 48 int compassIndex = 0;
Condo2k4 3:af291e8853b1 49 bool completed = false;
Condo2k4 3:af291e8853b1 50
Condo2k4 3:af291e8853b1 51 void tick()
Condo2k4 3:af291e8853b1 52 {
Condo2k4 3:af291e8853b1 53 int16_t raw_data[3];
Condo2k4 3:af291e8853b1 54 compass.getXYZ(raw_data);
Condo2k4 3:af291e8853b1 55
Condo2k4 3:af291e8853b1 56 xs[compassIndex] = static_cast<double>(raw_data[0]);
Condo2k4 3:af291e8853b1 57 ys[compassIndex] = static_cast<double>(raw_data[2]);
Condo2k4 3:af291e8853b1 58
Condo2k4 3:af291e8853b1 59 compassIndex++;
Condo2k4 3:af291e8853b1 60 if(compassIndex==COMPASS_CALC) {
Condo2k4 3:af291e8853b1 61 completed = true;
Condo2k4 3:af291e8853b1 62 compassIndex = 0;
Condo2k4 3:af291e8853b1 63 }
Condo2k4 3:af291e8853b1 64 }
Condo2k4 3:af291e8853b1 65
Condo2k4 3:af291e8853b1 66 Ticker ticker;
Condo2k4 3:af291e8853b1 67
Condo2k4 3:af291e8853b1 68 int main() {
Condo2k4 3:af291e8853b1 69 ticker.attach_us(&tick,50000);
Condo2k4 3:af291e8853b1 70
Condo2k4 3:af291e8853b1 71 while(1) {
Condo2k4 3:af291e8853b1 72 wait(0.5f);
Condo2k4 3:af291e8853b1 73 if(completed) {
Condo2k4 3:af291e8853b1 74 double avgX=0.0, avgY = 0.0;
Condo2k4 3:af291e8853b1 75
Condo2k4 3:af291e8853b1 76 for(int i=0; i<COMPASS_CALC; i++) {
Condo2k4 3:af291e8853b1 77 avgX+=xs[i]; avgY+=ys[i];
Condo2k4 3:af291e8853b1 78 }
Condo2k4 3:af291e8853b1 79 avgX/=COMPASS_CALC; avgY/=COMPASS_CALC;
Condo2k4 3:af291e8853b1 80
Condo2k4 3:af291e8853b1 81 double Suu=0.0, Suv=0.0, Svv=0.0, Suuu=0.0, Suvv=0.0, Suuv=0.0, Svvv=0.0;
Condo2k4 3:af291e8853b1 82
Condo2k4 3:af291e8853b1 83 for(int i=0; i<COMPASS_CALC; i++) {
Condo2k4 3:af291e8853b1 84 double u = xs[i]-avgX;
Condo2k4 3:af291e8853b1 85 double v = ys[i]-avgY;
Condo2k4 3:af291e8853b1 86 Suu+=u*u; Suv+=u*v; Svv+=v*v;
Condo2k4 3:af291e8853b1 87 Suuu+=u*u*u; Suvv+=u*v*v;
Condo2k4 3:af291e8853b1 88 Suuv+=u*u*v; Svvv+=v*v*v;
Condo2k4 3:af291e8853b1 89 }
Condo2k4 3:af291e8853b1 90
Condo2k4 3:af291e8853b1 91 double a = (Suuu+Suvv)*0.5;
Condo2k4 3:af291e8853b1 92 double b = (Svvv+Suuv)*0.5;
Condo2k4 3:af291e8853b1 93 double d = Suv*Suv-Suu*Svv;
Condo2k4 3:af291e8853b1 94
Condo2k4 3:af291e8853b1 95 double uc = (b*Suv-a*Svv)/d;
Condo2k4 3:af291e8853b1 96 double vc = (a*Suv-b*Suu)/d;
Condo2k4 3:af291e8853b1 97
Condo2k4 3:af291e8853b1 98 double R = sqrt(uc*uc + vc*vc + (Suu+Svv)/COMPASS_CALC);
Condo2k4 3:af291e8853b1 99 double cx = uc+avgX-R;
Condo2k4 3:af291e8853b1 100 double cy = vc+avgY-R;
Condo2k4 3:af291e8853b1 101
Condo2k4 3:af291e8853b1 102 usb.printf("X offset: %.3f\r\n", -cx);
Condo2k4 3:af291e8853b1 103 usb.printf("Y offset: %.3f\r\n", -cy);
Condo2k4 3:af291e8853b1 104 usb.printf("Radius: %.3f\r\n", R);
Condo2k4 3:af291e8853b1 105
Condo2k4 3:af291e8853b1 106 int16_t raw_data[3];
Condo2k4 3:af291e8853b1 107 compass.getXYZ(raw_data);
Condo2k4 3:af291e8853b1 108 //The HMC5883L gives X Z Y order
Condo2k4 3:af291e8853b1 109 double heading = atan2(static_cast<double>(raw_data[2])-cy, static_cast<double>(raw_data[0])-cx);
Condo2k4 3:af291e8853b1 110
Condo2k4 3:af291e8853b1 111 usb.printf("Heading: %.3f\r\n", heading);
Condo2k4 3:af291e8853b1 112
Condo2k4 3:af291e8853b1 113 }
Condo2k4 3:af291e8853b1 114 }
Condo2k4 3:af291e8853b1 115 }
Condo2k4 3:af291e8853b1 116
Condo2k4 2:3a288d8c816b 117 //int main()
Condo2k4 2:3a288d8c816b 118 //{
Condo2k4 2:3a288d8c816b 119 // compass.init();
Condo2k4 2:3a288d8c816b 120 // for(;;) {
Condo2k4 2:3a288d8c816b 121 //
Condo2k4 2:3a288d8c816b 122 // wait_ms(500);
Condo2k4 2:3a288d8c816b 123 //
Condo2k4 2:3a288d8c816b 124 // double h = smoothedHeading();
Condo2k4 2:3a288d8c816b 125 // usb.printf("%.2f\r\n",h);
Condo2k4 2:3a288d8c816b 126 // }
Condo2k4 2:3a288d8c816b 127 //}
Condo2k4 2:3a288d8c816b 128
Condo2k4 3:af291e8853b1 129 //double degToRad(double deg) {
Condo2k4 3:af291e8853b1 130 // return deg*M_PI/180.0;
Condo2k4 3:af291e8853b1 131 //}
Condo2k4 3:af291e8853b1 132 //
Condo2k4 3:af291e8853b1 133 //void gpsTest(int test, double lat1, double lon1, double lat2, double lon2) {
Condo2k4 3:af291e8853b1 134 // usb.printf("Test %d\r\n\r\nDistances:\r\n", test);
Condo2k4 3:af291e8853b1 135 // usb.printf(" Haversine: %.3fm\r\n", haversine(lat1, lon1, lat2, lon2));
Condo2k4 3:af291e8853b1 136 // usb.printf(" Cosines: %.3fm\r\n", cosines(lat1, lon1, lat2, lon2));
Condo2k4 3:af291e8853b1 137 // usb.printf(" Equirectangular: %.3fm\r\n", equirectangular(lat1, lon1, lat2, lon2));
Condo2k4 3:af291e8853b1 138 // usb.printf("\nBearings:\r\n Start: %.3f rad\r\n", startBearing(lat1, lon1, lat2, lon2));
Condo2k4 3:af291e8853b1 139 // usb.printf(" End: %.3f rad\r\n\r\n", endBearing(lat1, lon1, lat2, lon2));
Condo2k4 3:af291e8853b1 140 //}
Condo2k4 0:b3e9ce4cc500 141
Condo2k4 3:af291e8853b1 142 //int main() {
Condo2k4 3:af291e8853b1 143 //
Condo2k4 3:af291e8853b1 144 // gpsTest(1,degToRad(51.302310),degToRad(1.138862),degToRad(51.303853),degToRad(1.147445));
Condo2k4 2:3a288d8c816b 145 //Actual Distance = 968.9km
Condo2k4 2:3a288d8c816b 146 //Initial Bearing = 0.1592
Condo2k4 2:3a288d8c816b 147 //Final Bearing = 0.1968
Condo2k4 2:3a288d8c816b 148
Condo2k4 2:3a288d8c816b 149 // gpsTest(2,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
Condo2k4 2:3a288d8c816b 150 //
Condo2k4 2:3a288d8c816b 151 // gpsTest(3,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
Condo2k4 2:3a288d8c816b 152 //
Condo2k4 2:3a288d8c816b 153 // gpsTest(4,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
Condo2k4 3:af291e8853b1 154 //}