Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Estimation/GeoPosition/GeoPosition.cpp@25:bb5356402687, 2018-11-30 (annotated)

Committer:
shimniok
Date:
Fri Nov 30 16:11:53 2018 +0000
Revision:
25:bb5356402687
Parent:
0:a6a169de725f 
Initial publish of revised version.

Who changed what in which revision?

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