Darren Ulrich / GPSISR

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 Simon Ford

main.cpp

#include "mbed.h"
#include "GPSISR.h"

#define PIN_RX_GPS      PA_12 //GPS Shield RX pin
#define PIN_TX_GPS      PA_11 //GPS Shield TX pin
Serial pc(USBTX, USBRX);

// Set up serial interrupe service handler for gps characters.
GPS MyGPS(PIN_TX_GPS,PIN_RX_GPS, 9600);
int main()
{
    while (1) {
	if (MyGPS.dataready()) {
					MyGPS.read();
					pc.printf("NMEA has valid data");
					pc.printf("Sats : %d \n", MyGPS.buffer.satellites);
					pc.printf("%d-%d-%d\n", MyGPS.buffer.month, MyGPS.buffer.day, MyGPS.buffer.year);
					pc.printf("%d:%d:%d\n", MyGPS.buffer.hours, MyGPS.buffer.minutes, MyGPS.buffer.seconds);
	}
	else {
                pc.printf("NMEA has no valid data");
	}   
   }  
}

nav.cpp@6:f0c13bb7d266, 2017-03-01 (annotated)

Committer:
trevieze
Date:
Wed Mar 01 15:36:05 2017 +0000
Revision:
6:f0c13bb7d266
Parent:
5:c5f700c1e1af 
Math error in GPS speed calculation from Knots to Kilometer. Should be ; 1Kn = 1.85Km

Who changed what in which revision?

UserRevisionLine numberNew contents of line
trevieze 5:c5f700c1e1af 1 /*
trevieze 5:c5f700c1e1af 2 File: nav.cpp
trevieze 5:c5f700c1e1af 3 Version: 0.1.0
trevieze 5:c5f700c1e1af 4 Date: Feb. 28, 2017
trevieze 5:c5f700c1e1af 5 License: GPL v2
trevieze 5:c5f700c1e1af 6
trevieze 5:c5f700c1e1af 7 Navigation class
trevieze 5:c5f700c1e1af 8
trevieze 5:c5f700c1e1af 9 ****************************************************************************
trevieze 5:c5f700c1e1af 10 This program is free software; you can redistribute it and/or modify
trevieze 5:c5f700c1e1af 11 it under the terms of the GNU General Public License as published by
trevieze 5:c5f700c1e1af 12 the Free Software Foundation; either version 2 of the License, or
trevieze 5:c5f700c1e1af 13 (at your option) any later version.
trevieze 5:c5f700c1e1af 14
trevieze 5:c5f700c1e1af 15 This program is distributed in the hope that it will be useful,
trevieze 5:c5f700c1e1af 16 but WITHOUT ANY WARRANTY; without even the implied warranty of
trevieze 5:c5f700c1e1af 17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
trevieze 5:c5f700c1e1af 18 GNU General Public License for more details.
trevieze 5:c5f700c1e1af 19
trevieze 5:c5f700c1e1af 20 You should have received a copy of the GNU General Public License
trevieze 5:c5f700c1e1af 21 along with this program; if not, write to the Free Software
trevieze 5:c5f700c1e1af 22 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
trevieze 5:c5f700c1e1af 23 ****************************************************************************
trevieze 5:c5f700c1e1af 24 */
trevieze 5:c5f700c1e1af 25 #include <math.h>
trevieze 5:c5f700c1e1af 26 #include "nav.h"
trevieze 5:c5f700c1e1af 27 // Calculate the heading
trevieze 5:c5f700c1e1af 28 double NAV::CalculateDistance (double from_lat, double from_lon, double to_lat, double to_lon)
trevieze 5:c5f700c1e1af 29 {
trevieze 5:c5f700c1e1af 30 double R = 6371e3;
trevieze 5:c5f700c1e1af 31 double lat1 = DegreeToRadian(from_lat);
trevieze 5:c5f700c1e1af 32 double lat2 = DegreeToRadian(to_lat);
trevieze 5:c5f700c1e1af 33 double deltaLat = DegreeToRadian(to_lat - from_lat);
trevieze 5:c5f700c1e1af 34 double deltaLong = DegreeToRadian(to_lon - from_lon);
trevieze 5:c5f700c1e1af 35
trevieze 5:c5f700c1e1af 36 double a = sin(deltaLat / 2) * sin(deltaLat / 2) + cos(lat1) * cos(lat2) * sin(deltaLong / 2) * sin(deltaLong / 2);
trevieze 5:c5f700c1e1af 37 double c = 2 * atan2(sqrt(a), sqrt(1 - a));
trevieze 5:c5f700c1e1af 38 double d = R * c;
trevieze 5:c5f700c1e1af 39
trevieze 5:c5f700c1e1af 40 return d;
trevieze 5:c5f700c1e1af 41 }
trevieze 5:c5f700c1e1af 42
trevieze 5:c5f700c1e1af 43 // Calculate bearing
trevieze 5:c5f700c1e1af 44 double NAV::CalculateBearing(double from_lat, double from_lon, double to_lat, double to_lon)
trevieze 5:c5f700c1e1af 45 {
trevieze 5:c5f700c1e1af 46 double lat1 = DegreeToRadian(from_lat);
trevieze 5:c5f700c1e1af 47 double lat2 = DegreeToRadian(to_lat);
trevieze 5:c5f700c1e1af 48 double long1 = DegreeToRadian(from_lon);
trevieze 5:c5f700c1e1af 49 double long2 = DegreeToRadian(to_lon);
trevieze 5:c5f700c1e1af 50
trevieze 5:c5f700c1e1af 51 double y = sin(long2 - long1) * cos(lat2);
trevieze 5:c5f700c1e1af 52 double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1);
trevieze 5:c5f700c1e1af 53
trevieze 5:c5f700c1e1af 54 double bearing = RadianToDegree(atan2(y, x));
trevieze 5:c5f700c1e1af 55
trevieze 5:c5f700c1e1af 56 // Correct "wrap around" if necessary
trevieze 5:c5f700c1e1af 57 if (bearing < 0) bearing = 360.0 + bearing;
trevieze 5:c5f700c1e1af 58 if (bearing > 360.0) bearing = bearing - 360.0;
trevieze 5:c5f700c1e1af 59
trevieze 5:c5f700c1e1af 60 return bearing;
trevieze 5:c5f700c1e1af 61 }
trevieze 5:c5f700c1e1af 62
trevieze 5:c5f700c1e1af 63 /*
trevieze 5:c5f700c1e1af 64 This function takes as input the next gps coordinate that the boat is suppossed to achieve and calculates the compass
trevieze 5:c5f700c1e1af 65 heading to the waypoint
trevieze 5:c5f700c1e1af 66 Jeff Witten - 05/26/14 from Github VT-SailBOT/Navigation sailbot.c
trevieze 5:c5f700c1e1af 67 */
trevieze 5:c5f700c1e1af 68 int NAV::direction_to_next_point (double from_lat, double from_lon, double to_lat, double to_lon)
trevieze 5:c5f700c1e1af 69 {
trevieze 5:c5f700c1e1af 70 double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat);
trevieze 5:c5f700c1e1af 71 double dist_lon = PI/180*6367449*cos(from_lat);
trevieze 5:c5f700c1e1af 72 double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat;
trevieze 5:c5f700c1e1af 73 double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon;
trevieze 5:c5f700c1e1af 74 double difference = (lat_meter/lon_meter);
trevieze 5:c5f700c1e1af 75 double degree = atan(difference);
trevieze 5:c5f700c1e1af 76 degree = fabs(degree*180/PI);
trevieze 5:c5f700c1e1af 77 int angle = 0;
trevieze 5:c5f700c1e1af 78
trevieze 5:c5f700c1e1af 79 if(to_lat >= from_lat && to_lon >= from_lon) //Quadrant I
trevieze 5:c5f700c1e1af 80 angle = 90 - degree;
trevieze 5:c5f700c1e1af 81 else if(to_lat <= from_lat && to_lon >= from_lon) //Quadrant II
trevieze 5:c5f700c1e1af 82 angle = 90 + degree;
trevieze 5:c5f700c1e1af 83 else if(to_lat <= from_lat && to_lon <= from_lon) //Quadrant III
trevieze 5:c5f700c1e1af 84 angle = 270 - degree;
trevieze 5:c5f700c1e1af 85 else if(to_lat >= from_lat && to_lon <= from_lon) //Quadrant IV
trevieze 5:c5f700c1e1af 86 angle = 270 + degree;
trevieze 5:c5f700c1e1af 87
trevieze 5:c5f700c1e1af 88 return angle;
trevieze 5:c5f700c1e1af 89 }
trevieze 5:c5f700c1e1af 90
trevieze 5:c5f700c1e1af 91 /*
trevieze 5:c5f700c1e1af 92 This function takes as input the gps coordinates of the boat and the gps coordinates of a waypoint that the boat desires to approach
trevieze 5:c5f700c1e1af 93 The function then sets a flag based on whether or not the boat is within a predetermined perimeter of the waypoint
trevieze 5:c5f700c1e1af 94 Approach:
trevieze 5:c5f700c1e1af 95 1.) Latitude and Longitude of San Francisco = 37.7844 N, 122.4167 W
trevieze 5:c5f700c1e1af 96 2.) At 40 degrees North: 1 degree latitude = 111.03 km, 1 degree longitude = 85.39 km
trevieze 5:c5f700c1e1af 97 3.) 111.03 = 85.39 * (1.30027) - used to correct approximately rectangular lat/lon grid to approximately square
trevieze 5:c5f700c1e1af 98 4.) Through unit analysis, 1 meter = 0.0000111509 degrees longitude
trevieze 5:c5f700c1e1af 99 Jeff Witten - 03/27/14 from Github VT-SailBOT/Navigation sailbot.c
trevieze 5:c5f700c1e1af 100 */
trevieze 5:c5f700c1e1af 101 int NAV::point_proximity(double from_lat, double from_lon, double to_lat, double to_lon)
trevieze 5:c5f700c1e1af 102 {
trevieze 5:c5f700c1e1af 103 int number = 0;
trevieze 5:c5f700c1e1af 104 double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat);
trevieze 5:c5f700c1e1af 105 double dist_lon = PI/180*6367449*cos(from_lat);
trevieze 5:c5f700c1e1af 106 double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat;
trevieze 5:c5f700c1e1af 107 double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon;
trevieze 5:c5f700c1e1af 108 distance = sqrt(pow(lat_meter, 2) + pow(lon_meter,2));
trevieze 5:c5f700c1e1af 109
trevieze 5:c5f700c1e1af 110 if (distance <= point_proximity_radius){
trevieze 5:c5f700c1e1af 111 number = 1;
trevieze 5:c5f700c1e1af 112 } else {
trevieze 5:c5f700c1e1af 113 number = 0;
trevieze 5:c5f700c1e1af 114 }
trevieze 5:c5f700c1e1af 115 return number;
trevieze 5:c5f700c1e1af 116 }
trevieze 5:c5f700c1e1af 117
trevieze 5:c5f700c1e1af 118 /**
trevieze 5:c5f700c1e1af 119 * \brief Calculate distance between two points
trevieze 5:c5f700c1e1af 120 * This function uses an algorithm for an oblate spheroid earth model.
trevieze 5:c5f700c1e1af 121 * The algorithm is described here:
trevieze 5:c5f700c1e1af 122 * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
trevieze 5:c5f700c1e1af 123 * \return Distance in meters
trevieze 5:c5f700c1e1af 124 */
trevieze 5:c5f700c1e1af 125 double NAV::distance_ellipsoid(double from_lat, double from_lon, double from_azimuth, double to_lat, double to_lon, double to_azimuth)
trevieze 5:c5f700c1e1af 126 {
trevieze 5:c5f700c1e1af 127 /* All variables */
trevieze 5:c5f700c1e1af 128 double f, a, b, sqr_a, sqr_b;
trevieze 5:c5f700c1e1af 129 double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
trevieze 5:c5f700c1e1af 130 double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
trevieze 5:c5f700c1e1af 131 int remaining_steps;
trevieze 5:c5f700c1e1af 132 double sqr_u, A, B, delta_sigma;
trevieze 5:c5f700c1e1af 133
trevieze 5:c5f700c1e1af 134 if ((from_lat == to_lat) && (from_lon == to_lon))
trevieze 5:c5f700c1e1af 135 { /* Identical points */
trevieze 5:c5f700c1e1af 136 if ( from_azimuth != 0 )
trevieze 5:c5f700c1e1af 137 from_azimuth = 0;
trevieze 5:c5f700c1e1af 138 if ( to_azimuth != 0 )
trevieze 5:c5f700c1e1af 139 to_azimuth = 0;
trevieze 5:c5f700c1e1af 140 return 0;
trevieze 5:c5f700c1e1af 141 } /* Identical points */
trevieze 5:c5f700c1e1af 142
trevieze 5:c5f700c1e1af 143 /* Earth geometry */
trevieze 5:c5f700c1e1af 144 f = NMEA_EARTH_FLATTENING;
trevieze 5:c5f700c1e1af 145 a = NMEA_EARTH_SEMIMAJORAXIS_M;
trevieze 5:c5f700c1e1af 146 b = (1 - f) * a;
trevieze 5:c5f700c1e1af 147 sqr_a = a * a;
trevieze 5:c5f700c1e1af 148 sqr_b = b * b;
trevieze 5:c5f700c1e1af 149
trevieze 5:c5f700c1e1af 150 /* Calculation */
trevieze 5:c5f700c1e1af 151 L = to_lon - from_lon;
trevieze 5:c5f700c1e1af 152 phi1 = from_lat;
trevieze 5:c5f700c1e1af 153 phi2 = to_lat;
trevieze 5:c5f700c1e1af 154 U1 = atan((1 - f) * tan(phi1));
trevieze 5:c5f700c1e1af 155 U2 = atan((1 - f) * tan(phi2));
trevieze 5:c5f700c1e1af 156 sin_U1 = sin(U1);
trevieze 5:c5f700c1e1af 157 sin_U2 = sin(U2);
trevieze 5:c5f700c1e1af 158 cos_U1 = cos(U1);
trevieze 5:c5f700c1e1af 159 cos_U2 = cos(U2);
trevieze 5:c5f700c1e1af 160
trevieze 5:c5f700c1e1af 161 /* Initialize iteration */
trevieze 5:c5f700c1e1af 162 sigma = 0;
trevieze 5:c5f700c1e1af 163 sin_sigma = sin(sigma);
trevieze 5:c5f700c1e1af 164 cos_sigma = cos(sigma);
trevieze 5:c5f700c1e1af 165 cos_2_sigmam = 0;
trevieze 5:c5f700c1e1af 166 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
trevieze 5:c5f700c1e1af 167 sqr_cos_alpha = 0;
trevieze 5:c5f700c1e1af 168 lambda = L;
trevieze 5:c5f700c1e1af 169 sin_lambda = sin(lambda);
trevieze 5:c5f700c1e1af 170 cos_lambda = cos(lambda);
trevieze 5:c5f700c1e1af 171 delta_lambda = lambda;
trevieze 5:c5f700c1e1af 172 remaining_steps = 20;
trevieze 5:c5f700c1e1af 173
trevieze 5:c5f700c1e1af 174 while ((delta_lambda > 1e-12) && (remaining_steps > 0))
trevieze 5:c5f700c1e1af 175 { /* Iterate */
trevieze 5:c5f700c1e1af 176 /* Variables */
trevieze 5:c5f700c1e1af 177 double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev;
trevieze 5:c5f700c1e1af 178
trevieze 5:c5f700c1e1af 179 /* Calculation */
trevieze 5:c5f700c1e1af 180 tmp1 = cos_U2 * sin_lambda;
trevieze 5:c5f700c1e1af 181 tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
trevieze 5:c5f700c1e1af 182 sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
trevieze 5:c5f700c1e1af 183 cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
trevieze 5:c5f700c1e1af 184 tan_sigma = sin_sigma / cos_sigma;
trevieze 5:c5f700c1e1af 185 sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
trevieze 5:c5f700c1e1af 186 cos_alpha = cos(asin(sin_alpha));
trevieze 5:c5f700c1e1af 187 sqr_cos_alpha = cos_alpha * cos_alpha;
trevieze 5:c5f700c1e1af 188 cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
trevieze 5:c5f700c1e1af 189 sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
trevieze 5:c5f700c1e1af 190 C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
trevieze 5:c5f700c1e1af 191 lambda_prev = lambda;
trevieze 5:c5f700c1e1af 192 sigma = asin(sin_sigma);
trevieze 5:c5f700c1e1af 193 lambda = L +
trevieze 5:c5f700c1e1af 194 (1 - C) * f * sin_alpha
trevieze 5:c5f700c1e1af 195 * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
trevieze 5:c5f700c1e1af 196 delta_lambda = lambda_prev - lambda;
trevieze 5:c5f700c1e1af 197 if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
trevieze 5:c5f700c1e1af 198 sin_lambda = sin(lambda);
trevieze 5:c5f700c1e1af 199 cos_lambda = cos(lambda);
trevieze 5:c5f700c1e1af 200 remaining_steps--;
trevieze 5:c5f700c1e1af 201 } /* Iterate */
trevieze 5:c5f700c1e1af 202
trevieze 5:c5f700c1e1af 203 /* More calculation */
trevieze 5:c5f700c1e1af 204 sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
trevieze 5:c5f700c1e1af 205 A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
trevieze 5:c5f700c1e1af 206 B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
trevieze 5:c5f700c1e1af 207 delta_sigma = B * sin_sigma * (
trevieze 5:c5f700c1e1af 208 cos_2_sigmam + B / 4 * (
trevieze 5:c5f700c1e1af 209 cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
trevieze 5:c5f700c1e1af 210 B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
trevieze 5:c5f700c1e1af 211 ));
trevieze 5:c5f700c1e1af 212
trevieze 5:c5f700c1e1af 213 /* Calculate result */
trevieze 5:c5f700c1e1af 214 if ( from_azimuth != 0 )
trevieze 5:c5f700c1e1af 215 {
trevieze 5:c5f700c1e1af 216 double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
trevieze 5:c5f700c1e1af 217 from_azimuth = atan(tan_alpha_1);
trevieze 5:c5f700c1e1af 218 }
trevieze 5:c5f700c1e1af 219 if ( to_azimuth != 0 )
trevieze 5:c5f700c1e1af 220 {
trevieze 5:c5f700c1e1af 221 double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
trevieze 5:c5f700c1e1af 222 to_azimuth = atan(tan_alpha_2);
trevieze 5:c5f700c1e1af 223 }
trevieze 5:c5f700c1e1af 224
trevieze 5:c5f700c1e1af 225 return b * A * (sigma - delta_sigma);
trevieze 5:c5f700c1e1af 226 }
trevieze 5:c5f700c1e1af 227
trevieze 5:c5f700c1e1af 228
trevieze 5:c5f700c1e1af 229 // Convert Degrees to Radians
trevieze 5:c5f700c1e1af 230 double NAV::DegreeToRadian(double angle)
trevieze 5:c5f700c1e1af 231 {
trevieze 5:c5f700c1e1af 232 return PI * angle / 180.0;
trevieze 5:c5f700c1e1af 233 }
trevieze 5:c5f700c1e1af 234
trevieze 5:c5f700c1e1af 235 // Convert Radians to Degrees
trevieze 5:c5f700c1e1af 236 double NAV::RadianToDegree(double angle)
trevieze 5:c5f700c1e1af 237 {
trevieze 5:c5f700c1e1af 238 return angle * (180.0 / PI);
trevieze 5:c5f700c1e1af 239 }