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
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
--- /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