A GPS serial interrupt service routine that has an on the fly nmea parser. Works with a STM32F411RE and a Adafruit GPS logger.
Dependents: Bicycl_Computer_NUCLEO-F411RE Bicycl_Computer_NUCLEO-L476RG
Fork of GPS by
nav.cpp
00001 /* 00002 File: nav.cpp 00003 Version: 0.1.0 00004 Date: Feb. 28, 2017 00005 License: GPL v2 00006 00007 Navigation class 00008 00009 **************************************************************************** 00010 This program is free software; you can redistribute it and/or modify 00011 it under the terms of the GNU General Public License as published by 00012 the Free Software Foundation; either version 2 of the License, or 00013 (at your option) any later version. 00014 00015 This program is distributed in the hope that it will be useful, 00016 but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 GNU General Public License for more details. 00019 00020 You should have received a copy of the GNU General Public License 00021 along with this program; if not, write to the Free Software 00022 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00023 **************************************************************************** 00024 */ 00025 #include <math.h> 00026 #include "nav.h" 00027 // Calculate the heading 00028 double NAV::CalculateDistance (double from_lat, double from_lon, double to_lat, double to_lon) 00029 { 00030 double R = 6371e3; 00031 double lat1 = DegreeToRadian(from_lat); 00032 double lat2 = DegreeToRadian(to_lat); 00033 double deltaLat = DegreeToRadian(to_lat - from_lat); 00034 double deltaLong = DegreeToRadian(to_lon - from_lon); 00035 00036 double a = sin(deltaLat / 2) * sin(deltaLat / 2) + cos(lat1) * cos(lat2) * sin(deltaLong / 2) * sin(deltaLong / 2); 00037 double c = 2 * atan2(sqrt(a), sqrt(1 - a)); 00038 double d = R * c; 00039 00040 return d; 00041 } 00042 00043 // Calculate bearing 00044 double NAV::CalculateBearing(double from_lat, double from_lon, double to_lat, double to_lon) 00045 { 00046 double lat1 = DegreeToRadian(from_lat); 00047 double lat2 = DegreeToRadian(to_lat); 00048 double long1 = DegreeToRadian(from_lon); 00049 double long2 = DegreeToRadian(to_lon); 00050 00051 double y = sin(long2 - long1) * cos(lat2); 00052 double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1); 00053 00054 double bearing = RadianToDegree(atan2(y, x)); 00055 00056 // Correct "wrap around" if necessary 00057 if (bearing < 0) bearing = 360.0 + bearing; 00058 if (bearing > 360.0) bearing = bearing - 360.0; 00059 00060 return bearing; 00061 } 00062 00063 /* 00064 This function takes as input the next gps coordinate that the boat is suppossed to achieve and calculates the compass 00065 heading to the waypoint 00066 Jeff Witten - 05/26/14 from Github VT-SailBOT/Navigation sailbot.c 00067 */ 00068 int NAV::direction_to_next_point (double from_lat, double from_lon, double to_lat, double to_lon) 00069 { 00070 double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat); 00071 double dist_lon = PI/180*6367449*cos(from_lat); 00072 double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat; 00073 double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon; 00074 double difference = (lat_meter/lon_meter); 00075 double degree = atan(difference); 00076 degree = fabs(degree*180/PI); 00077 int angle = 0; 00078 00079 if(to_lat >= from_lat && to_lon >= from_lon) //Quadrant I 00080 angle = 90 - degree; 00081 else if(to_lat <= from_lat && to_lon >= from_lon) //Quadrant II 00082 angle = 90 + degree; 00083 else if(to_lat <= from_lat && to_lon <= from_lon) //Quadrant III 00084 angle = 270 - degree; 00085 else if(to_lat >= from_lat && to_lon <= from_lon) //Quadrant IV 00086 angle = 270 + degree; 00087 00088 return angle; 00089 } 00090 00091 /* 00092 This function takes as input the gps coordinates of the boat and the gps coordinates of a waypoint that the boat desires to approach 00093 The function then sets a flag based on whether or not the boat is within a predetermined perimeter of the waypoint 00094 Approach: 00095 1.) Latitude and Longitude of San Francisco = 37.7844 N, 122.4167 W 00096 2.) At 40 degrees North: 1 degree latitude = 111.03 km, 1 degree longitude = 85.39 km 00097 3.) 111.03 = 85.39 * (1.30027) - used to correct approximately rectangular lat/lon grid to approximately square 00098 4.) Through unit analysis, 1 meter = 0.0000111509 degrees longitude 00099 Jeff Witten - 03/27/14 from Github VT-SailBOT/Navigation sailbot.c 00100 */ 00101 int NAV::point_proximity(double from_lat, double from_lon, double to_lat, double to_lon) 00102 { 00103 int number = 0; 00104 double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat); 00105 double dist_lon = PI/180*6367449*cos(from_lat); 00106 double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat; 00107 double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon; 00108 distance = sqrt(pow(lat_meter, 2) + pow(lon_meter,2)); 00109 00110 if (distance <= point_proximity_radius){ 00111 number = 1; 00112 } else { 00113 number = 0; 00114 } 00115 return number; 00116 } 00117 00118 /** 00119 * \brief Calculate distance between two points 00120 * This function uses an algorithm for an oblate spheroid earth model. 00121 * The algorithm is described here: 00122 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf 00123 * \return Distance in meters 00124 */ 00125 double NAV::distance_ellipsoid(double from_lat, double from_lon, double from_azimuth, double to_lat, double to_lon, double to_azimuth) 00126 { 00127 /* All variables */ 00128 double f, a, b, sqr_a, sqr_b; 00129 double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2; 00130 double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda; 00131 int remaining_steps; 00132 double sqr_u, A, B, delta_sigma; 00133 00134 if ((from_lat == to_lat) && (from_lon == to_lon)) 00135 { /* Identical points */ 00136 if ( from_azimuth != 0 ) 00137 from_azimuth = 0; 00138 if ( to_azimuth != 0 ) 00139 to_azimuth = 0; 00140 return 0; 00141 } /* Identical points */ 00142 00143 /* Earth geometry */ 00144 f = NMEA_EARTH_FLATTENING; 00145 a = NMEA_EARTH_SEMIMAJORAXIS_M; 00146 b = (1 - f) * a; 00147 sqr_a = a * a; 00148 sqr_b = b * b; 00149 00150 /* Calculation */ 00151 L = to_lon - from_lon; 00152 phi1 = from_lat; 00153 phi2 = to_lat; 00154 U1 = atan((1 - f) * tan(phi1)); 00155 U2 = atan((1 - f) * tan(phi2)); 00156 sin_U1 = sin(U1); 00157 sin_U2 = sin(U2); 00158 cos_U1 = cos(U1); 00159 cos_U2 = cos(U2); 00160 00161 /* Initialize iteration */ 00162 sigma = 0; 00163 sin_sigma = sin(sigma); 00164 cos_sigma = cos(sigma); 00165 cos_2_sigmam = 0; 00166 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; 00167 sqr_cos_alpha = 0; 00168 lambda = L; 00169 sin_lambda = sin(lambda); 00170 cos_lambda = cos(lambda); 00171 delta_lambda = lambda; 00172 remaining_steps = 20; 00173 00174 while ((delta_lambda > 1e-12) && (remaining_steps > 0)) 00175 { /* Iterate */ 00176 /* Variables */ 00177 double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev; 00178 00179 /* Calculation */ 00180 tmp1 = cos_U2 * sin_lambda; 00181 tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda; 00182 sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2); 00183 cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda; 00184 tan_sigma = sin_sigma / cos_sigma; 00185 sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma; 00186 cos_alpha = cos(asin(sin_alpha)); 00187 sqr_cos_alpha = cos_alpha * cos_alpha; 00188 cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha; 00189 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam; 00190 C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha)); 00191 lambda_prev = lambda; 00192 sigma = asin(sin_sigma); 00193 lambda = L + 00194 (1 - C) * f * sin_alpha 00195 * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam))); 00196 delta_lambda = lambda_prev - lambda; 00197 if ( delta_lambda < 0 ) delta_lambda = -delta_lambda; 00198 sin_lambda = sin(lambda); 00199 cos_lambda = cos(lambda); 00200 remaining_steps--; 00201 } /* Iterate */ 00202 00203 /* More calculation */ 00204 sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b; 00205 A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u))); 00206 B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u))); 00207 delta_sigma = B * sin_sigma * ( 00208 cos_2_sigmam + B / 4 * ( 00209 cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) - 00210 B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam) 00211 )); 00212 00213 /* Calculate result */ 00214 if ( from_azimuth != 0 ) 00215 { 00216 double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda); 00217 from_azimuth = atan(tan_alpha_1); 00218 } 00219 if ( to_azimuth != 0 ) 00220 { 00221 double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda); 00222 to_azimuth = atan(tan_alpha_2); 00223 } 00224 00225 return b * A * (sigma - delta_sigma); 00226 } 00227 00228 00229 // Convert Degrees to Radians 00230 double NAV::DegreeToRadian(double angle) 00231 { 00232 return PI * angle / 180.0; 00233 } 00234 00235 // Convert Radians to Degrees 00236 double NAV::RadianToDegree(double angle) 00237 { 00238 return angle * (180.0 / PI); 00239 }
Generated on Tue Jul 12 2022 18:50:37 by 1.7.2