Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.
Dependencies: Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo
GeoPosition.cpp
00001 #include "GeoPosition.h" 00002 #include <math.h> 00003 00004 // Earth's mean radius in meters 00005 #define EARTHRADIUS 6371000.0 00006 // TODO: 2 altitude 00007 00008 GeoPosition::GeoPosition(): _R(EARTHRADIUS), _latitude(0.0), _longitude(0.0) 00009 { 00010 } 00011 00012 GeoPosition::GeoPosition(double latitude, double longitude): _R(EARTHRADIUS), _latitude(latitude), _longitude(longitude) 00013 { 00014 } 00015 00016 /* 00017 double GeoPosition::easting() 00018 { 00019 } 00020 00021 double GeoPosition::northing() 00022 { 00023 } 00024 */ 00025 00026 double GeoPosition::latitude() 00027 { 00028 return _latitude; 00029 } 00030 00031 double GeoPosition::longitude() 00032 { 00033 return _longitude; 00034 } 00035 00036 void GeoPosition::set(double latitude, double longitude) 00037 { 00038 _latitude = latitude; 00039 _longitude = longitude; 00040 } 00041 00042 void GeoPosition::set(GeoPosition pos) 00043 { 00044 _latitude = pos.latitude(); 00045 _longitude = pos.longitude(); 00046 } 00047 00048 /* 00049 void GeoPosition::set(UTM coord) 00050 { 00051 } 00052 */ 00053 00054 void GeoPosition::move(float course, float distance) 00055 { 00056 double d = distance / _R; 00057 double c = radians(course); 00058 double rlat1 = radians(_latitude); 00059 double rlon1 = radians(_longitude); 00060 00061 // Precompute to improve performance 00062 double cd = cos(d); 00063 double sd = sin(d); 00064 double srlat1 = sin(rlat1); 00065 double crlat1 = cos(rlat1); 00066 00067 double rlat2 = asin(srlat1*cd + crlat1*sd*cos(c)); 00068 double rlon2 = rlon1 + atan2(sin(c)*sd*crlat1, cd-srlat1*sin(rlat2)); 00069 00070 _latitude = degrees(rlat2); 00071 _longitude = degrees(rlon2); 00072 00073 // bring back within the range -180 to +180 00074 while (_longitude < -180.0) _longitude += 360.0; 00075 while (_longitude > 180.0) _longitude -= 360.0; 00076 } 00077 00078 /* 00079 void GeoPosition::move(Direction toWhere) 00080 { 00081 } 00082 00083 Direction GeoPosition::direction(GeoPosition startingPoint) 00084 { 00085 } 00086 */ 00087 00088 float GeoPosition::bearing(GeoPosition from) 00089 { 00090 return bearingFrom(from); 00091 } 00092 00093 float GeoPosition::bearingTo(GeoPosition to) 00094 { 00095 double lat1 = radians(_latitude); 00096 double lon1 = radians(_longitude); 00097 double lat2 = radians(to.latitude()); 00098 double lon2 = radians(to.longitude()); 00099 double dLon = lon2 - lon1; 00100 00101 double y = sin(dLon) * cos(lat2); 00102 double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon); 00103 00104 return degrees(atan2(y, x)); 00105 } 00106 00107 /* 00108 JavaScript: 00109 var y = Math.sin(dLon) * Math.cos(lat2); 00110 var x = Math.cos(lat1)*Math.sin(lat2) - 00111 Math.sin(lat1)*Math.cos(lat2)*Math.cos(dLon); 00112 var brng = Math.atan2(y, x).toDeg(); 00113 */ 00114 float GeoPosition::bearingFrom(GeoPosition from) 00115 { 00116 double lat1 = radians(from.latitude()); 00117 double lon1 = radians(from.longitude()); 00118 double lat2 = radians(_latitude); 00119 double lon2 = radians(_longitude); 00120 double dLon = lon2 - lon1; 00121 00122 double y = sin(dLon) * cos(lat2); 00123 double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon); 00124 00125 return degrees(atan2(y, x)); 00126 } 00127 00128 /* 00129 float GeoPosition::bearing(LatLong startingPoint) 00130 { 00131 } 00132 00133 float GeoPosition::bearing(UTM startingPoint) 00134 { 00135 } 00136 */ 00137 00138 float GeoPosition::distance(GeoPosition from) 00139 { 00140 return distanceFrom(from); 00141 } 00142 00143 float GeoPosition::distanceTo(GeoPosition to) 00144 { 00145 double lat1 = radians(_latitude); 00146 double lon1 = radians(_longitude); 00147 double lat2 = radians(to.latitude()); 00148 double lon2 = radians(to.longitude()); 00149 double dLat = lat2 - lat1; 00150 double dLon = lon2 - lon1; 00151 00152 double a = sin(dLat/2.0) * sin(dLat/2.0) + 00153 cos(lat1) * cos(lat2) * 00154 sin(dLon/2.0) * sin(dLon/2.0); 00155 double c = 2.0 * atan2(sqrt(a), sqrt(1-a)); 00156 00157 return _R * c; 00158 } 00159 00160 float GeoPosition::distanceFrom(GeoPosition from) 00161 { 00162 double lat1 = radians(from.latitude()); 00163 double lon1 = radians(from.longitude()); 00164 double lat2 = radians(_latitude); 00165 double lon2 = radians(_longitude); 00166 double dLat = lat2 - lat1; 00167 double dLon = lon2 - lon1; 00168 00169 double a = sin(dLat/2.0) * sin(dLat/2.0) + 00170 cos(lat1) * cos(lat2) * 00171 sin(dLon/2.0) * sin(dLon/2.0); 00172 double c = 2.0 * atan2(sqrt(a), sqrt(1-a)); 00173 00174 return _R * c; 00175 } 00176 00177 /* 00178 float GeoPosition::distance(LatLong startingPoint) 00179 { 00180 } 00181 00182 float GeoPosition::distance(UTM startingPoint) 00183 { 00184 } 00185 */ 00186 00187 void GeoPosition::setTimestamp(int time) { 00188 _time = time; 00189 } 00190 00191 int GeoPosition::getTimestamp(void) { 00192 return _time; 00193 } 00194 00195 00196
Generated on Tue Jul 12 2022 14:09:25 by 1.7.2