Keith G / Mbed 2 deprecated compass_test

Initial import

Dependencies:   HMC5883L mbed

gps_utils.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 2:3a288d8c816b 1 #include "gps_utils.h"
Condo2k4 2:3a288d8c816b 2 #include "mbed.h"
Condo2k4 2:3a288d8c816b 3
Condo2k4 2:3a288d8c816b 4 double haversine(double lat1, double lon1, double lat2, double lon2) {
Condo2k4 2:3a288d8c816b 5 double sinLat = sin((lat2-lat1)*0.5);
Condo2k4 2:3a288d8c816b 6 double sinLon = sin((lon2-lon1)*0.5);
Condo2k4 2:3a288d8c816b 7 double a = sinLat*sinLat + cos(lon1)*cos(lon2)*sinLon*sinLon;
Condo2k4 2:3a288d8c816b 8 return EARTH_RADIUS_METERS * 2.0 * atan2(sqrt(a),sqrt(1.0-a));
Condo2k4 2:3a288d8c816b 9 }
Condo2k4 2:3a288d8c816b 10
Condo2k4 2:3a288d8c816b 11 double cosines(double lat1, double lon1, double lat2, double lon2) {
Condo2k4 2:3a288d8c816b 12 return acos(sin(lat1)*sin(lat2) + cos(lat1)*cos(lat2)*cos(lon2-lon1))*EARTH_RADIUS_METERS;
Condo2k4 2:3a288d8c816b 13 }
Condo2k4 2:3a288d8c816b 14
Condo2k4 2:3a288d8c816b 15 double equirectangular(double lat1, double lon1, double lat2, double lon2) {
Condo2k4 2:3a288d8c816b 16 extern Serial usb;
Condo2k4 2:3a288d8c816b 17
Condo2k4 2:3a288d8c816b 18 double x = (lon2-lon1)*cos((lat1+lat2)*0.5);
Condo2k4 2:3a288d8c816b 19 usb.printf(" equirectangular x: %0.5f\r\n", x);
Condo2k4 2:3a288d8c816b 20 double y = (lat2-lat1);
Condo2k4 2:3a288d8c816b 21 usb.printf(" equirectangular y: %0.5f\r\n", y);
Condo2k4 2:3a288d8c816b 22
Condo2k4 2:3a288d8c816b 23 return sqrt(x*x + y*y) * EARTH_RADIUS_METERS;
Condo2k4 2:3a288d8c816b 24 }
Condo2k4 2:3a288d8c816b 25
Condo2k4 2:3a288d8c816b 26
Condo2k4 2:3a288d8c816b 27 double startBearing(double lat1, double lon1, double lat2, double lon2) {
Condo2k4 2:3a288d8c816b 28 double y = sin(lon2-lon1)*cos(lat2);
Condo2k4 2:3a288d8c816b 29 double x = cos(lat1)*sin(lat2) - sin(lat1)*cos(lat2)*cos(lon2-lon1);
Condo2k4 2:3a288d8c816b 30 double b = atan2(y,x);
Condo2k4 2:3a288d8c816b 31 return b<0.0 ? (b+TWO_PI) : b;
Condo2k4 2:3a288d8c816b 32 }
Condo2k4 2:3a288d8c816b 33
Condo2k4 2:3a288d8c816b 34 double endBearing(double lat1, double lon1, double lat2, double lon2) {
Condo2k4 2:3a288d8c816b 35 double y = sin(lon1-lon2)*cos(lat1);
Condo2k4 2:3a288d8c816b 36 double x = cos(lat2)*sin(lat1) - sin(lat2)*cos(lat1)*cos(lon1-lon2);
Condo2k4 2:3a288d8c816b 37 double b = atan2(y,x);
Condo2k4 2:3a288d8c816b 38 return b<0.0 ? (b+M_PI) : (b-M_PI);
Condo2k4 2:3a288d8c816b 39 }