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@5:8a73e34b3978, 2016-02-16 (annotated)
- Committer:
- chris215
- Date:
- Tue Feb 16 02:57:35 2016 +0000
- Revision:
- 5:8a73e34b3978
- Parent:
- 3:20f8faf2ad18
Function renaming. Adding gps update calls.
Who changed what in which revision?
User | Revision | Line number | New 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 |