Keith G / Mbed 2 deprecated compass_test

Initial import

Dependencies:   HMC5883L mbed

main.cpp@2:3a288d8c816b, 2016-03-23 (annotated)

Committer:
Condo2k4
Date:
Wed Mar 23 19:22:07 2016 +0000
Revision:
2:3a288d8c816b
Parent:
0:b3e9ce4cc500
Child:
3:af291e8853b1
added gps utils

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 2:3a288d8c816b 44 //int main()
Condo2k4 2:3a288d8c816b 45 //{
Condo2k4 2:3a288d8c816b 46 // compass.init();
Condo2k4 2:3a288d8c816b 47 // for(;;) {
Condo2k4 2:3a288d8c816b 48 //
Condo2k4 2:3a288d8c816b 49 // wait_ms(500);
Condo2k4 2:3a288d8c816b 50 //
Condo2k4 2:3a288d8c816b 51 // double h = smoothedHeading();
Condo2k4 2:3a288d8c816b 52 // usb.printf("%.2f\r\n",h);
Condo2k4 2:3a288d8c816b 53 // }
Condo2k4 2:3a288d8c816b 54 //}
Condo2k4 2:3a288d8c816b 55
Condo2k4 2:3a288d8c816b 56 double degToRad(double deg) {
Condo2k4 2:3a288d8c816b 57 return deg*M_PI/180.0;
Condo2k4 0:b3e9ce4cc500 58 }
Condo2k4 0:b3e9ce4cc500 59
Condo2k4 2:3a288d8c816b 60 void gpsTest(int test, double lat1, double lon1, double lat2, double lon2) {
Condo2k4 2:3a288d8c816b 61 usb.printf("Test %d\r\n\r\nDistances:\r\n", test);
Condo2k4 2:3a288d8c816b 62 usb.printf(" Haversine: %.3fm\r\n", haversine(lat1, lon1, lat2, lon2));
Condo2k4 2:3a288d8c816b 63 usb.printf(" Cosines: %.3fm\r\n", cosines(lat1, lon1, lat2, lon2));
Condo2k4 2:3a288d8c816b 64 usb.printf(" Equirectangular: %.3fm\r\n", equirectangular(lat1, lon1, lat2, lon2));
Condo2k4 2:3a288d8c816b 65 usb.printf("\nBearings:\r\n Start: %.3f rad\r\n", startBearing(lat1, lon1, lat2, lon2));
Condo2k4 2:3a288d8c816b 66 usb.printf(" End: %.3f rad\r\n\r\n", endBearing(lat1, lon1, lat2, lon2));
Condo2k4 2:3a288d8c816b 67 }
Condo2k4 2:3a288d8c816b 68
Condo2k4 2:3a288d8c816b 69 int main() {
Condo2k4 2:3a288d8c816b 70
Condo2k4 2:3a288d8c816b 71 gpsTest(1,degToRad(51.302310),degToRad(1.138862),degToRad(51.303853),degToRad(1.147445));
Condo2k4 2:3a288d8c816b 72 //Actual Distance = 968.9km
Condo2k4 2:3a288d8c816b 73 //Initial Bearing = 0.1592
Condo2k4 2:3a288d8c816b 74 //Final Bearing = 0.1968
Condo2k4 2:3a288d8c816b 75
Condo2k4 2:3a288d8c816b 76 // gpsTest(2,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
Condo2k4 2:3a288d8c816b 77 //
Condo2k4 2:3a288d8c816b 78 // gpsTest(3,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
Condo2k4 2:3a288d8c816b 79 //
Condo2k4 2:3a288d8c816b 80 // gpsTest(4,degToRad(0),degToRad(0),degToRad(0),degToRad(0));
Condo2k4 0:b3e9ce4cc500 81 }