Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Estimation/GeoPosition/GeoPosition.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:826c6171fc1b 1 #include "GeoPosition.h"
shimniok 0:826c6171fc1b 2 #include <math.h>
shimniok 0:826c6171fc1b 3
shimniok 0:826c6171fc1b 4 // Earth's mean radius in meters
shimniok 0:826c6171fc1b 5 #define EARTHRADIUS 6371000.0
shimniok 0:826c6171fc1b 6 // TODO: 2 altitude
shimniok 0:826c6171fc1b 7
shimniok 0:826c6171fc1b 8 GeoPosition::GeoPosition(): _R(EARTHRADIUS), _latitude(0.0), _longitude(0.0)
shimniok 0:826c6171fc1b 9 {
shimniok 0:826c6171fc1b 10 }
shimniok 0:826c6171fc1b 11
shimniok 0:826c6171fc1b 12 GeoPosition::GeoPosition(double latitude, double longitude): _R(EARTHRADIUS), _latitude(latitude), _longitude(longitude)
shimniok 0:826c6171fc1b 13 {
shimniok 0:826c6171fc1b 14 }
shimniok 0:826c6171fc1b 15
shimniok 0:826c6171fc1b 16 /*
shimniok 0:826c6171fc1b 17 double GeoPosition::easting()
shimniok 0:826c6171fc1b 18 {
shimniok 0:826c6171fc1b 19 }
shimniok 0:826c6171fc1b 20
shimniok 0:826c6171fc1b 21 double GeoPosition::northing()
shimniok 0:826c6171fc1b 22 {
shimniok 0:826c6171fc1b 23 }
shimniok 0:826c6171fc1b 24 */
shimniok 0:826c6171fc1b 25
shimniok 0:826c6171fc1b 26 double GeoPosition::latitude()
shimniok 0:826c6171fc1b 27 {
shimniok 0:826c6171fc1b 28 return _latitude;
shimniok 0:826c6171fc1b 29 }
shimniok 0:826c6171fc1b 30
shimniok 0:826c6171fc1b 31 double GeoPosition::longitude()
shimniok 0:826c6171fc1b 32 {
shimniok 0:826c6171fc1b 33 return _longitude;
shimniok 0:826c6171fc1b 34 }
shimniok 0:826c6171fc1b 35
shimniok 0:826c6171fc1b 36 void GeoPosition::set(double latitude, double longitude)
shimniok 0:826c6171fc1b 37 {
shimniok 0:826c6171fc1b 38 _latitude = latitude;
shimniok 0:826c6171fc1b 39 _longitude = longitude;
shimniok 0:826c6171fc1b 40 }
shimniok 0:826c6171fc1b 41
shimniok 0:826c6171fc1b 42 void GeoPosition::set(GeoPosition pos)
shimniok 0:826c6171fc1b 43 {
shimniok 0:826c6171fc1b 44 _latitude = pos.latitude();
shimniok 0:826c6171fc1b 45 _longitude = pos.longitude();
shimniok 0:826c6171fc1b 46 }
shimniok 0:826c6171fc1b 47
shimniok 0:826c6171fc1b 48 /*
shimniok 0:826c6171fc1b 49 void GeoPosition::set(UTM coord)
shimniok 0:826c6171fc1b 50 {
shimniok 0:826c6171fc1b 51 }
shimniok 0:826c6171fc1b 52 */
shimniok 0:826c6171fc1b 53
shimniok 0:826c6171fc1b 54 void GeoPosition::move(float course, float distance)
shimniok 0:826c6171fc1b 55 {
shimniok 0:826c6171fc1b 56 double d = distance / _R;
shimniok 0:826c6171fc1b 57 double c = radians(course);
shimniok 0:826c6171fc1b 58 double rlat1 = radians(_latitude);
shimniok 0:826c6171fc1b 59 double rlon1 = radians(_longitude);
shimniok 0:826c6171fc1b 60
shimniok 0:826c6171fc1b 61 // Precompute to improve performance
shimniok 0:826c6171fc1b 62 double cd = cos(d);
shimniok 0:826c6171fc1b 63 double sd = sin(d);
shimniok 0:826c6171fc1b 64 double srlat1 = sin(rlat1);
shimniok 0:826c6171fc1b 65 double crlat1 = cos(rlat1);
shimniok 0:826c6171fc1b 66
shimniok 0:826c6171fc1b 67 double rlat2 = asin(srlat1*cd + crlat1*sd*cos(c));
shimniok 0:826c6171fc1b 68 double rlon2 = rlon1 + atan2(sin(c)*sd*crlat1, cd-srlat1*sin(rlat2));
shimniok 0:826c6171fc1b 69
shimniok 0:826c6171fc1b 70 _latitude = degrees(rlat2);
shimniok 0:826c6171fc1b 71 _longitude = degrees(rlon2);
shimniok 0:826c6171fc1b 72
shimniok 0:826c6171fc1b 73 // bring back within the range -180 to +180
shimniok 0:826c6171fc1b 74 while (_longitude < -180.0) _longitude += 360.0;
shimniok 0:826c6171fc1b 75 while (_longitude > 180.0) _longitude -= 360.0;
shimniok 0:826c6171fc1b 76 }
shimniok 0:826c6171fc1b 77
shimniok 0:826c6171fc1b 78 /*
shimniok 0:826c6171fc1b 79 void GeoPosition::move(Direction toWhere)
shimniok 0:826c6171fc1b 80 {
shimniok 0:826c6171fc1b 81 }
shimniok 0:826c6171fc1b 82
shimniok 0:826c6171fc1b 83 Direction GeoPosition::direction(GeoPosition startingPoint)
shimniok 0:826c6171fc1b 84 {
shimniok 0:826c6171fc1b 85 }
shimniok 0:826c6171fc1b 86 */
shimniok 0:826c6171fc1b 87
shimniok 0:826c6171fc1b 88 float GeoPosition::bearing(GeoPosition from)
shimniok 0:826c6171fc1b 89 {
shimniok 0:826c6171fc1b 90 return bearingFrom(from);
shimniok 0:826c6171fc1b 91 }
shimniok 0:826c6171fc1b 92
shimniok 0:826c6171fc1b 93 float GeoPosition::bearingTo(GeoPosition to)
shimniok 0:826c6171fc1b 94 {
shimniok 0:826c6171fc1b 95 double lat1 = radians(_latitude);
shimniok 0:826c6171fc1b 96 double lon1 = radians(_longitude);
shimniok 0:826c6171fc1b 97 double lat2 = radians(to.latitude());
shimniok 0:826c6171fc1b 98 double lon2 = radians(to.longitude());
shimniok 0:826c6171fc1b 99 double dLon = lon2 - lon1;
shimniok 0:826c6171fc1b 100
shimniok 0:826c6171fc1b 101 double y = sin(dLon) * cos(lat2);
shimniok 0:826c6171fc1b 102 double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
shimniok 0:826c6171fc1b 103
shimniok 0:826c6171fc1b 104 return degrees(atan2(y, x));
shimniok 0:826c6171fc1b 105 }
shimniok 0:826c6171fc1b 106
shimniok 0:826c6171fc1b 107 /*
shimniok 0:826c6171fc1b 108 JavaScript:
shimniok 0:826c6171fc1b 109 var y = Math.sin(dLon) * Math.cos(lat2);
shimniok 0:826c6171fc1b 110 var x = Math.cos(lat1)*Math.sin(lat2) -
shimniok 0:826c6171fc1b 111 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon);
shimniok 0:826c6171fc1b 112 var brng = Math.atan2(y, x).toDeg();
shimniok 0:826c6171fc1b 113 */
shimniok 0:826c6171fc1b 114 float GeoPosition::bearingFrom(GeoPosition from)
shimniok 0:826c6171fc1b 115 {
shimniok 0:826c6171fc1b 116 double lat1 = radians(from.latitude());
shimniok 0:826c6171fc1b 117 double lon1 = radians(from.longitude());
shimniok 0:826c6171fc1b 118 double lat2 = radians(_latitude);
shimniok 0:826c6171fc1b 119 double lon2 = radians(_longitude);
shimniok 0:826c6171fc1b 120 double dLon = lon2 - lon1;
shimniok 0:826c6171fc1b 121
shimniok 0:826c6171fc1b 122 double y = sin(dLon) * cos(lat2);
shimniok 0:826c6171fc1b 123 double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
shimniok 0:826c6171fc1b 124
shimniok 0:826c6171fc1b 125 return degrees(atan2(y, x));
shimniok 0:826c6171fc1b 126 }
shimniok 0:826c6171fc1b 127
shimniok 0:826c6171fc1b 128 /*
shimniok 0:826c6171fc1b 129 float GeoPosition::bearing(LatLong startingPoint)
shimniok 0:826c6171fc1b 130 {
shimniok 0:826c6171fc1b 131 }
shimniok 0:826c6171fc1b 132
shimniok 0:826c6171fc1b 133 float GeoPosition::bearing(UTM startingPoint)
shimniok 0:826c6171fc1b 134 {
shimniok 0:826c6171fc1b 135 }
shimniok 0:826c6171fc1b 136 */
shimniok 0:826c6171fc1b 137
shimniok 0:826c6171fc1b 138 float GeoPosition::distance(GeoPosition from)
shimniok 0:826c6171fc1b 139 {
shimniok 0:826c6171fc1b 140 return distanceFrom(from);
shimniok 0:826c6171fc1b 141 }
shimniok 0:826c6171fc1b 142
shimniok 0:826c6171fc1b 143 float GeoPosition::distanceTo(GeoPosition to)
shimniok 0:826c6171fc1b 144 {
shimniok 0:826c6171fc1b 145 double lat1 = radians(_latitude);
shimniok 0:826c6171fc1b 146 double lon1 = radians(_longitude);
shimniok 0:826c6171fc1b 147 double lat2 = radians(to.latitude());
shimniok 0:826c6171fc1b 148 double lon2 = radians(to.longitude());
shimniok 0:826c6171fc1b 149 double dLat = lat2 - lat1;
shimniok 0:826c6171fc1b 150 double dLon = lon2 - lon1;
shimniok 0:826c6171fc1b 151
shimniok 0:826c6171fc1b 152 double a = sin(dLat/2.0) * sin(dLat/2.0) +
shimniok 0:826c6171fc1b 153 cos(lat1) * cos(lat2) *
shimniok 0:826c6171fc1b 154 sin(dLon/2.0) * sin(dLon/2.0);
shimniok 0:826c6171fc1b 155 double c = 2.0 * atan2(sqrt(a), sqrt(1-a));
shimniok 0:826c6171fc1b 156
shimniok 0:826c6171fc1b 157 return _R * c;
shimniok 0:826c6171fc1b 158 }
shimniok 0:826c6171fc1b 159
shimniok 0:826c6171fc1b 160 float GeoPosition::distanceFrom(GeoPosition from)
shimniok 0:826c6171fc1b 161 {
shimniok 0:826c6171fc1b 162 double lat1 = radians(from.latitude());
shimniok 0:826c6171fc1b 163 double lon1 = radians(from.longitude());
shimniok 0:826c6171fc1b 164 double lat2 = radians(_latitude);
shimniok 0:826c6171fc1b 165 double lon2 = radians(_longitude);
shimniok 0:826c6171fc1b 166 double dLat = lat2 - lat1;
shimniok 0:826c6171fc1b 167 double dLon = lon2 - lon1;
shimniok 0:826c6171fc1b 168
shimniok 0:826c6171fc1b 169 double a = sin(dLat/2.0) * sin(dLat/2.0) +
shimniok 0:826c6171fc1b 170 cos(lat1) * cos(lat2) *
shimniok 0:826c6171fc1b 171 sin(dLon/2.0) * sin(dLon/2.0);
shimniok 0:826c6171fc1b 172 double c = 2.0 * atan2(sqrt(a), sqrt(1-a));
shimniok 0:826c6171fc1b 173
shimniok 0:826c6171fc1b 174 return _R * c;
shimniok 0:826c6171fc1b 175 }
shimniok 0:826c6171fc1b 176
shimniok 0:826c6171fc1b 177 /*
shimniok 0:826c6171fc1b 178 float GeoPosition::distance(LatLong startingPoint)
shimniok 0:826c6171fc1b 179 {
shimniok 0:826c6171fc1b 180 }
shimniok 0:826c6171fc1b 181
shimniok 0:826c6171fc1b 182 float GeoPosition::distance(UTM startingPoint)
shimniok 0:826c6171fc1b 183 {
shimniok 0:826c6171fc1b 184 }
shimniok 0:826c6171fc1b 185 */
shimniok 0:826c6171fc1b 186
shimniok 0:826c6171fc1b 187 void GeoPosition::setTimestamp(int time) {
shimniok 0:826c6171fc1b 188 _time = time;
shimniok 0:826c6171fc1b 189 }
shimniok 0:826c6171fc1b 190
shimniok 0:826c6171fc1b 191 int GeoPosition::getTimestamp(void) {
shimniok 0:826c6171fc1b 192 return _time;
shimniok 0:826c6171fc1b 193 }
shimniok 0:826c6171fc1b 194
shimniok 0:826c6171fc1b 195
shimniok 0:826c6171fc1b 196