Darren Ulrich
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");
}
}
}
Diff: nav.cpp
- Revision:
- 5:c5f700c1e1af
diff -r 7240a18102a6 -r c5f700c1e1af nav.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/nav.cpp Wed Mar 01 03:38:03 2017 +0000
@@ -0,0 +1,239 @@
+ /*
+ File: nav.cpp
+ Version: 0.1.0
+ Date: Feb. 28, 2017
+ License: GPL v2
+
+ Navigation class
+
+ ****************************************************************************
+ This program is free software; you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation; either version 2 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program; if not, write to the Free Software
+ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ ****************************************************************************
+ */
+ #include <math.h>
+ #include "nav.h"
+ // Calculate the heading
+ double NAV::CalculateDistance (double from_lat, double from_lon, double to_lat, double to_lon)
+ {
+ double R = 6371e3;
+ double lat1 = DegreeToRadian(from_lat);
+ double lat2 = DegreeToRadian(to_lat);
+ double deltaLat = DegreeToRadian(to_lat - from_lat);
+ double deltaLong = DegreeToRadian(to_lon - from_lon);
+
+ double a = sin(deltaLat / 2) * sin(deltaLat / 2) + cos(lat1) * cos(lat2) * sin(deltaLong / 2) * sin(deltaLong / 2);
+ double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+ double d = R * c;
+
+ return d;
+ }
+
+ // Calculate bearing
+ double NAV::CalculateBearing(double from_lat, double from_lon, double to_lat, double to_lon)
+ {
+ double lat1 = DegreeToRadian(from_lat);
+ double lat2 = DegreeToRadian(to_lat);
+ double long1 = DegreeToRadian(from_lon);
+ double long2 = DegreeToRadian(to_lon);
+
+ double y = sin(long2 - long1) * cos(lat2);
+ double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(long2 - long1);
+
+ double bearing = RadianToDegree(atan2(y, x));
+
+ // Correct "wrap around" if necessary
+ if (bearing < 0) bearing = 360.0 + bearing;
+ if (bearing > 360.0) bearing = bearing - 360.0;
+
+ return bearing;
+ }
+
+ /*
+ This function takes as input the next gps coordinate that the boat is suppossed to achieve and calculates the compass
+ heading to the waypoint
+ Jeff Witten - 05/26/14 from Github VT-SailBOT/Navigation sailbot.c
+ */
+ int NAV::direction_to_next_point (double from_lat, double from_lon, double to_lat, double to_lon)
+ {
+ double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat);
+ double dist_lon = PI/180*6367449*cos(from_lat);
+ double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat;
+ double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon;
+ double difference = (lat_meter/lon_meter);
+ double degree = atan(difference);
+ degree = fabs(degree*180/PI);
+ int angle = 0;
+
+ if(to_lat >= from_lat && to_lon >= from_lon) //Quadrant I
+ angle = 90 - degree;
+ else if(to_lat <= from_lat && to_lon >= from_lon) //Quadrant II
+ angle = 90 + degree;
+ else if(to_lat <= from_lat && to_lon <= from_lon) //Quadrant III
+ angle = 270 - degree;
+ else if(to_lat >= from_lat && to_lon <= from_lon) //Quadrant IV
+ angle = 270 + degree;
+
+ return angle;
+ }
+
+ /*
+ This function takes as input the gps coordinates of the boat and the gps coordinates of a waypoint that the boat desires to approach
+ The function then sets a flag based on whether or not the boat is within a predetermined perimeter of the waypoint
+ Approach:
+ 1.) Latitude and Longitude of San Francisco = 37.7844 N, 122.4167 W
+ 2.) At 40 degrees North: 1 degree latitude = 111.03 km, 1 degree longitude = 85.39 km
+ 3.) 111.03 = 85.39 * (1.30027) - used to correct approximately rectangular lat/lon grid to approximately square
+ 4.) Through unit analysis, 1 meter = 0.0000111509 degrees longitude
+ Jeff Witten - 03/27/14 from Github VT-SailBOT/Navigation sailbot.c
+ */
+ int NAV::point_proximity(double from_lat, double from_lon, double to_lat, double to_lon)
+ {
+ int number = 0;
+ double dist_lat = 111132.954 - 559.822*cos(2*from_lat) + 1.1175*cos(4*from_lat);
+ double dist_lon = PI/180*6367449*cos(from_lat);
+ double lat_meter = fabs(fabs(to_lat) - fabs(from_lat))*dist_lat;
+ double lon_meter = fabs(fabs(to_lon) - fabs(from_lon))*dist_lon;
+ distance = sqrt(pow(lat_meter, 2) + pow(lon_meter,2));
+
+ if (distance <= point_proximity_radius){
+ number = 1;
+ } else {
+ number = 0;
+ }
+ return number;
+ }
+
+ /**
+ * \brief Calculate distance between two points
+ * This function uses an algorithm for an oblate spheroid earth model.
+ * The algorithm is described here:
+ * http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf
+ * \return Distance in meters
+ */
+double NAV::distance_ellipsoid(double from_lat, double from_lon, double from_azimuth, double to_lat, double to_lon, double to_azimuth)
+{
+ /* All variables */
+ double f, a, b, sqr_a, sqr_b;
+ double L, phi1, phi2, U1, U2, sin_U1, sin_U2, cos_U1, cos_U2;
+ double sigma, sin_sigma, cos_sigma, cos_2_sigmam, sqr_cos_2_sigmam, sqr_cos_alpha, lambda, sin_lambda, cos_lambda, delta_lambda;
+ int remaining_steps;
+ double sqr_u, A, B, delta_sigma;
+
+ if ((from_lat == to_lat) && (from_lon == to_lon))
+ { /* Identical points */
+ if ( from_azimuth != 0 )
+ from_azimuth = 0;
+ if ( to_azimuth != 0 )
+ to_azimuth = 0;
+ return 0;
+ } /* Identical points */
+
+ /* Earth geometry */
+ f = NMEA_EARTH_FLATTENING;
+ a = NMEA_EARTH_SEMIMAJORAXIS_M;
+ b = (1 - f) * a;
+ sqr_a = a * a;
+ sqr_b = b * b;
+
+ /* Calculation */
+ L = to_lon - from_lon;
+ phi1 = from_lat;
+ phi2 = to_lat;
+ U1 = atan((1 - f) * tan(phi1));
+ U2 = atan((1 - f) * tan(phi2));
+ sin_U1 = sin(U1);
+ sin_U2 = sin(U2);
+ cos_U1 = cos(U1);
+ cos_U2 = cos(U2);
+
+ /* Initialize iteration */
+ sigma = 0;
+ sin_sigma = sin(sigma);
+ cos_sigma = cos(sigma);
+ cos_2_sigmam = 0;
+ sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
+ sqr_cos_alpha = 0;
+ lambda = L;
+ sin_lambda = sin(lambda);
+ cos_lambda = cos(lambda);
+ delta_lambda = lambda;
+ remaining_steps = 20;
+
+ while ((delta_lambda > 1e-12) && (remaining_steps > 0))
+ { /* Iterate */
+ /* Variables */
+ double tmp1, tmp2, tan_sigma, sin_alpha, cos_alpha, C, lambda_prev;
+
+ /* Calculation */
+ tmp1 = cos_U2 * sin_lambda;
+ tmp2 = cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda;
+ sin_sigma = sqrt(tmp1 * tmp1 + tmp2 * tmp2);
+ cos_sigma = sin_U1 * sin_U2 + cos_U1 * cos_U2 * cos_lambda;
+ tan_sigma = sin_sigma / cos_sigma;
+ sin_alpha = cos_U1 * cos_U2 * sin_lambda / sin_sigma;
+ cos_alpha = cos(asin(sin_alpha));
+ sqr_cos_alpha = cos_alpha * cos_alpha;
+ cos_2_sigmam = cos_sigma - 2 * sin_U1 * sin_U2 / sqr_cos_alpha;
+ sqr_cos_2_sigmam = cos_2_sigmam * cos_2_sigmam;
+ C = f / 16 * sqr_cos_alpha * (4 + f * (4 - 3 * sqr_cos_alpha));
+ lambda_prev = lambda;
+ sigma = asin(sin_sigma);
+ lambda = L +
+ (1 - C) * f * sin_alpha
+ * (sigma + C * sin_sigma * (cos_2_sigmam + C * cos_sigma * (-1 + 2 * sqr_cos_2_sigmam)));
+ delta_lambda = lambda_prev - lambda;
+ if ( delta_lambda < 0 ) delta_lambda = -delta_lambda;
+ sin_lambda = sin(lambda);
+ cos_lambda = cos(lambda);
+ remaining_steps--;
+ } /* Iterate */
+
+ /* More calculation */
+ sqr_u = sqr_cos_alpha * (sqr_a - sqr_b) / sqr_b;
+ A = 1 + sqr_u / 16384 * (4096 + sqr_u * (-768 + sqr_u * (320 - 175 * sqr_u)));
+ B = sqr_u / 1024 * (256 + sqr_u * (-128 + sqr_u * (74 - 47 * sqr_u)));
+ delta_sigma = B * sin_sigma * (
+ cos_2_sigmam + B / 4 * (
+ cos_sigma * (-1 + 2 * sqr_cos_2_sigmam) -
+ B / 6 * cos_2_sigmam * (-3 + 4 * sin_sigma * sin_sigma) * (-3 + 4 * sqr_cos_2_sigmam)
+ ));
+
+ /* Calculate result */
+ if ( from_azimuth != 0 )
+ {
+ double tan_alpha_1 = cos_U2 * sin_lambda / (cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lambda);
+ from_azimuth = atan(tan_alpha_1);
+ }
+ if ( to_azimuth != 0 )
+ {
+ double tan_alpha_2 = cos_U1 * sin_lambda / (-sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lambda);
+ to_azimuth = atan(tan_alpha_2);
+ }
+
+ return b * A * (sigma - delta_sigma);
+}
+
+
+ // Convert Degrees to Radians
+ double NAV::DegreeToRadian(double angle)
+ {
+ return PI * angle / 180.0;
+ }
+
+ // Convert Radians to Degrees
+ double NAV::RadianToDegree(double angle)
+ {
+ return angle * (180.0 / PI);
+ }
\ No newline at end of file