christian b / GPS

Mbed library to handle GPS data reception and parsing

Dependents:   GPS_U-blox_NEO-6M_Code

Features

  • All positionning parameters are contained into a global data structure.
  • Automatic nema string parsing and data structure update.
    • GSA,GGA,VTG and RMC
  • Convert latitude and longitude to decimal value.
  • Converts latittude,longitude and altitude to ECEF coordinates.

Planed developement

  • Test library for RTOS use.
  • Complete the nema parsing decoders (couple of parameters are not parsed yet and not present in the data structure).
  • Add conversion tool to get ENU coordinates.

utils/GPSUtils.hpp@3:20f8faf2ad18, 2016-02-15 (annotated)

Committer:
chris215
Date:
Mon Feb 15 00:42:32 2016 +0000
Revision:
3:20f8faf2ad18
Parent:
2:72ac4d7044a7 
Code refactor. Replace usage of strtok_r() with a custom version to fix parsing issues when the token is empty.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chris215 0:0c1aa5906cef 1 #ifndef _GPSUTILS_H_
chris215 0:0c1aa5906cef 2 #define _GPSUTILS_H_
chris215 0:0c1aa5906cef 3
chris215 0:0c1aa5906cef 4 #include "mbedGPSDefs.h"
chris215 0:0c1aa5906cef 5 #include "GPGGAdecoder.hpp"
chris215 0:0c1aa5906cef 6 #include "GPVTGdecoder.hpp"
chris215 0:0c1aa5906cef 7 #include "GPGSAdecoder.hpp"
chris215 0:0c1aa5906cef 8 #include "GPRMCdecoder.hpp"
chris215 0:0c1aa5906cef 9 #include "WGS84.h"
chris215 0:0c1aa5906cef 10
chris215 0:0c1aa5906cef 11 #define PI 3.141592653589793238462643383279502884
chris215 0:0c1aa5906cef 12 #define RADIANS_TO_DEGREES(radians) ((radians) * (180.0 / PI))
chris215 0:0c1aa5906cef 13 #define DEGREES_TO_RADIANS ( PI / 180.0)
chris215 0:0c1aa5906cef 14 #define WGS84_A 6378137.000
chris215 0:0c1aa5906cef 15 //298.257223563
chris215 0:0c1aa5906cef 16 #define WGS84_E 0.081819190842621
chris215 0:0c1aa5906cef 17
chris215 0:0c1aa5906cef 18 /*
chris215 0:0c1aa5906cef 19 Source of information about nema sentences :
chris215 0:0c1aa5906cef 20 http://www.gpsinformation.org/dale/nmea.htm
chris215 0:0c1aa5906cef 21 */
chris215 2:72ac4d7044a7 22 int CheckForHeader(char* DataToCheck, char* RefData,int datasize);
chris215 2:72ac4d7044a7 23 int IdentifyGPSMessage(message& GPSMessageToIdentify)
chris215 2:72ac4d7044a7 24 {
chris215 2:72ac4d7044a7 25 if(CheckForHeader(GPSMessageToIdentify.data,GPS_CMD_GPGGA,6))
chris215 2:72ac4d7044a7 26 {
chris215 2:72ac4d7044a7 27 return GPS_GPGGAR;
chris215 2:72ac4d7044a7 28 }
chris215 2:72ac4d7044a7 29 else if(CheckForHeader(GPSMessageToIdentify.data,GPS_CMD_GPVTG,6))
chris215 2:72ac4d7044a7 30 {
chris215 2:72ac4d7044a7 31 return GPS_GPVTGR;
chris215 2:72ac4d7044a7 32 }
chris215 2:72ac4d7044a7 33 else if(CheckForHeader(GPSMessageToIdentify.data,GPS_CMD_GPGSA,6))
chris215 2:72ac4d7044a7 34 {
chris215 2:72ac4d7044a7 35 return GPS_GPGSAR;
chris215 2:72ac4d7044a7 36 }
chris215 2:72ac4d7044a7 37 else if(CheckForHeader(GPSMessageToIdentify.data,GPS_CMD_GPRMC,6))
chris215 2:72ac4d7044a7 38 {
chris215 2:72ac4d7044a7 39 return GPS_GPRMCR;
chris215 2:72ac4d7044a7 40 }
chris215 2:72ac4d7044a7 41 return 0;
chris215 2:72ac4d7044a7 42 }
chris215 2:72ac4d7044a7 43
chris215 2:72ac4d7044a7 44 int CheckForHeader(char* DataToCheck, char* RefData,int datasize)
chris215 2:72ac4d7044a7 45 {
chris215 2:72ac4d7044a7 46 int NumberOfMatchingElements = memcmp(DataToCheck,RefData,datasize);
chris215 2:72ac4d7044a7 47
chris215 2:72ac4d7044a7 48 if(NumberOfMatchingElements==0)
chris215 2:72ac4d7044a7 49 return 1;
chris215 2:72ac4d7044a7 50 return 0;
chris215 2:72ac4d7044a7 51 }
chris215 2:72ac4d7044a7 52
chris215 0:0c1aa5906cef 53 void DecodeMessage(message& msg,GPSInfo& data)
chris215 0:0c1aa5906cef 54 {
chris215 0:0c1aa5906cef 55
chris215 0:0c1aa5906cef 56 if(strstr(msg.data,GPS_CMD_GPGGA)){
chris215 0:0c1aa5906cef 57 DecodeGPGGA(msg.data,data);
chris215 0:0c1aa5906cef 58 }
chris215 0:0c1aa5906cef 59
chris215 0:0c1aa5906cef 60 if(strstr(msg.data,GPS_CMD_GPVTG)){
chris215 0:0c1aa5906cef 61 DecodeGPVTG(msg.data,data);
chris215 0:0c1aa5906cef 62 }
chris215 0:0c1aa5906cef 63
chris215 0:0c1aa5906cef 64 if(strstr(msg.data,GPS_CMD_GPGSA)){
chris215 0:0c1aa5906cef 65 DecodeGPGSA(msg.data,data);
chris215 0:0c1aa5906cef 66 }
chris215 0:0c1aa5906cef 67 if(strstr(msg.data,GPS_CMD_GPRMC)){
chris215 0:0c1aa5906cef 68 DecodeGPRMC(msg.data,data);
chris215 0:0c1aa5906cef 69 }
chris215 0:0c1aa5906cef 70 }
chris215 0:0c1aa5906cef 71
chris215 0:0c1aa5906cef 72 ECEFPoint GeoDesicToECEF(geodPoint p)
chris215 0:0c1aa5906cef 73 {
chris215 0:0c1aa5906cef 74 ECEFPoint newECEFpoint;
chris215 0:0c1aa5906cef 75 double lat = 0.0;
chris215 0:0c1aa5906cef 76 double lon = 0.0;
chris215 0:0c1aa5906cef 77 lat = p.latitude;
chris215 0:0c1aa5906cef 78 lon = p.longitude;
chris215 0:0c1aa5906cef 79
chris215 0:0c1aa5906cef 80 double clat = cos(lat * DEGREES_TO_RADIANS);
chris215 0:0c1aa5906cef 81 double slat = sin(lat * DEGREES_TO_RADIANS);
chris215 0:0c1aa5906cef 82 double clon = cos(lon * DEGREES_TO_RADIANS);
chris215 0:0c1aa5906cef 83 double slon = sin(lon * DEGREES_TO_RADIANS);
chris215 0:0c1aa5906cef 84
chris215 0:0c1aa5906cef 85 double N = WGS84_A / sqrt(1.0 - WGS84_E * WGS84_E * slat * slat);
chris215 0:0c1aa5906cef 86
chris215 0:0c1aa5906cef 87 newECEFpoint.x = (N + p.altitude) * clat * clon;
chris215 0:0c1aa5906cef 88 newECEFpoint.y = (N + p.altitude) * clat * slon;
chris215 0:0c1aa5906cef 89 newECEFpoint.z = (N * (1.0 - WGS84_E * WGS84_E) + p.altitude) * slat;
chris215 0:0c1aa5906cef 90
chris215 0:0c1aa5906cef 91 return newECEFpoint;
chris215 0:0c1aa5906cef 92 }
chris215 0:0c1aa5906cef 93
chris215 0:0c1aa5906cef 94 ECEFDistance DistanceBetweenTwoECEFPoints(ECEFPoint p1, ECEFPoint p2)
chris215 0:0c1aa5906cef 95 {
chris215 0:0c1aa5906cef 96 ECEFDistance newDistance;
chris215 0:0c1aa5906cef 97 newDistance.dx = p1.x-p2.x;
chris215 0:0c1aa5906cef 98 newDistance.dy = p1.y-p2.y;
chris215 0:0c1aa5906cef 99 newDistance.dz = p1.z-p2.z;
chris215 0:0c1aa5906cef 100 newDistance.d = sqrt((newDistance.dx*newDistance.dx) + (newDistance.dy*newDistance.dy) + (newDistance.dz*newDistance.dz));
chris215 0:0c1aa5906cef 101 return newDistance;
chris215 0:0c1aa5906cef 102 }
chris215 0:0c1aa5906cef 103
chris215 0:0c1aa5906cef 104 float nmea_to_dec(double nema_coor, char nsew) {
chris215 0:0c1aa5906cef 105 int degree = (int)(nema_coor/100);
chris215 0:0c1aa5906cef 106 double minutes = nema_coor - degree*100;
chris215 0:0c1aa5906cef 107 double dec_deg = minutes / 60;
chris215 0:0c1aa5906cef 108 double decimal = degree + dec_deg;
chris215 0:0c1aa5906cef 109 if (nsew == 'S' || nsew == 'W') { // return negative
chris215 0:0c1aa5906cef 110 decimal *= -1;
chris215 0:0c1aa5906cef 111 }
chris215 0:0c1aa5906cef 112 return decimal;
chris215 0:0c1aa5906cef 113 }
chris215 1:fade122a76a8 114
chris215 1:fade122a76a8 115 // Coverts ECEF to ENU coordinates centered at given lat, lon
chris215 1:fade122a76a8 116 void ecefToEnu(double lat, double lon, double x, double y, double z, double xr, double yr, double zr, double *e, double *n, double *u)
chris215 1:fade122a76a8 117 {
chris215 1:fade122a76a8 118 double clat = cos(lat * DEGREES_TO_RADIANS);
chris215 1:fade122a76a8 119 double slat = sin(lat * DEGREES_TO_RADIANS);
chris215 1:fade122a76a8 120 double clon = cos(lon * DEGREES_TO_RADIANS);
chris215 1:fade122a76a8 121 double slon = sin(lon * DEGREES_TO_RADIANS);
chris215 1:fade122a76a8 122 double dx = x - xr;
chris215 1:fade122a76a8 123 double dy = y - yr;
chris215 1:fade122a76a8 124 double dz = z - zr;
chris215 1:fade122a76a8 125
chris215 1:fade122a76a8 126 *e = -slon*dx + clon*dy;
chris215 1:fade122a76a8 127 *n = -slat*clon*dx - slat*slon*dy + clat*dz;
chris215 1:fade122a76a8 128 *u = clat*clon*dx + clat*slon*dy + slat*dz;
chris215 1:fade122a76a8 129 }
chris215 3:20f8faf2ad18 130
chris215 3:20f8faf2ad18 131
chris215 0:0c1aa5906cef 132 #endif