Basic GPS message parser
Fork of GPS by
GPS.cpp@3:ac28455d8e5a, 2017-08-22 (annotated)
- Committer:
- SomeRandomBloke
- Date:
- Tue Aug 22 15:23:05 2017 +0000
- Revision:
- 3:ac28455d8e5a
- Parent:
- 2:8c97b7918a94
Added Altitude
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
simon | 0:15611c7938a3 | 1 | /* mbed EM-406 GPS Module Library |
simon | 0:15611c7938a3 | 2 | * Copyright (c) 2008-2010, sford |
simon | 0:15611c7938a3 | 3 | * |
simon | 0:15611c7938a3 | 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
simon | 0:15611c7938a3 | 5 | * of this software and associated documentation files (the "Software"), to deal |
simon | 0:15611c7938a3 | 6 | * in the Software without restriction, including without limitation the rights |
simon | 0:15611c7938a3 | 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
simon | 0:15611c7938a3 | 8 | * copies of the Software, and to permit persons to whom the Software is |
simon | 0:15611c7938a3 | 9 | * furnished to do so, subject to the following conditions: |
simon | 0:15611c7938a3 | 10 | * |
simon | 0:15611c7938a3 | 11 | * The above copyright notice and this permission notice shall be included in |
simon | 0:15611c7938a3 | 12 | * all copies or substantial portions of the Software. |
simon | 0:15611c7938a3 | 13 | * |
simon | 0:15611c7938a3 | 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
simon | 0:15611c7938a3 | 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
simon | 0:15611c7938a3 | 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
simon | 0:15611c7938a3 | 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
simon | 0:15611c7938a3 | 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
simon | 0:15611c7938a3 | 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
simon | 0:15611c7938a3 | 20 | * THE SOFTWARE. |
simon | 0:15611c7938a3 | 21 | */ |
simon | 0:15611c7938a3 | 22 | |
simon | 0:15611c7938a3 | 23 | #include "GPS.h" |
simon | 0:15611c7938a3 | 24 | |
SomeRandomBloke | 2:8c97b7918a94 | 25 | GPS::GPS(PinName tx, PinName rx, int baudrate) : _gps(tx, rx) { |
SomeRandomBloke | 2:8c97b7918a94 | 26 | _gps.baud(baudrate); |
simon | 0:15611c7938a3 | 27 | longitude = 0.0; |
SomeRandomBloke | 3:ac28455d8e5a | 28 | latitude = 0.0; |
SomeRandomBloke | 3:ac28455d8e5a | 29 | altitude = 0.0; |
simon | 0:15611c7938a3 | 30 | } |
simon | 0:15611c7938a3 | 31 | |
simon | 0:15611c7938a3 | 32 | int GPS::sample() { |
SomeRandomBloke | 1:1f849dbf3b8b | 33 | //float time; |
simon | 0:15611c7938a3 | 34 | char ns, ew; |
simon | 0:15611c7938a3 | 35 | int lock; |
SomeRandomBloke | 3:ac28455d8e5a | 36 | int satsInUse; |
SomeRandomBloke | 3:ac28455d8e5a | 37 | float hdop; |
simon | 0:15611c7938a3 | 38 | |
simon | 0:15611c7938a3 | 39 | while(1) { |
simon | 0:15611c7938a3 | 40 | getline(); |
simon | 0:15611c7938a3 | 41 | |
simon | 0:15611c7938a3 | 42 | // Check if it is a GPGGA msg (matches both locked and non-locked msg) |
SomeRandomBloke | 1:1f849dbf3b8b | 43 | // GPGGA,212700.000,5124.4324,N,00115.7464,W,1,8,1.30,113.7,M,47.5,M,,*47 |
SomeRandomBloke | 1:1f849dbf3b8b | 44 | // Position -1.929107, 51.407211 |
SomeRandomBloke | 1:1f849dbf3b8b | 45 | // 51.4136298,-1.2670097 |
SomeRandomBloke | 1:1f849dbf3b8b | 46 | //Position -1.262323, 51.407383 |
SomeRandomBloke | 3:ac28455d8e5a | 47 | if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d,%d,%f,%f", &time, &latitude, &ns, &longitude, &ew, &lock, &satsInUse, &hdop, &altitude) >= 1) { |
simon | 0:15611c7938a3 | 48 | if(!lock) { |
simon | 0:15611c7938a3 | 49 | longitude = 0.0; |
SomeRandomBloke | 3:ac28455d8e5a | 50 | latitude = 0.0; |
SomeRandomBloke | 3:ac28455d8e5a | 51 | altitude = 0; |
simon | 0:15611c7938a3 | 52 | return 0; |
simon | 0:15611c7938a3 | 53 | } else { |
SomeRandomBloke | 1:1f849dbf3b8b | 54 | //printf("%s\n\r",msg); |
simon | 0:15611c7938a3 | 55 | if(ns == 'S') { latitude *= -1.0; } |
simon | 0:15611c7938a3 | 56 | if(ew == 'W') { longitude *= -1.0; } |
simon | 0:15611c7938a3 | 57 | float degrees = trunc(latitude / 100.0f); |
simon | 0:15611c7938a3 | 58 | float minutes = latitude - (degrees * 100.0f); |
SomeRandomBloke | 1:1f849dbf3b8b | 59 | //printf("LAT degrees %f, minutes %f\n\r",degrees,minutes); |
SomeRandomBloke | 1:1f849dbf3b8b | 60 | latitude = degrees + (minutes / 60.0f); |
SomeRandomBloke | 1:1f849dbf3b8b | 61 | |
SomeRandomBloke | 1:1f849dbf3b8b | 62 | degrees = trunc(longitude / 100.0f); // * 0.01f); |
simon | 0:15611c7938a3 | 63 | minutes = longitude - (degrees * 100.0f); |
SomeRandomBloke | 1:1f849dbf3b8b | 64 | //printf("LON degrees %f, minutes %f\n\r",degrees,minutes); |
SomeRandomBloke | 1:1f849dbf3b8b | 65 | longitude = degrees + (minutes / 60.0f); |
simon | 0:15611c7938a3 | 66 | return 1; |
simon | 0:15611c7938a3 | 67 | } |
simon | 0:15611c7938a3 | 68 | } |
simon | 0:15611c7938a3 | 69 | } |
simon | 0:15611c7938a3 | 70 | } |
simon | 0:15611c7938a3 | 71 | |
simon | 0:15611c7938a3 | 72 | float GPS::trunc(float v) { |
SomeRandomBloke | 2:8c97b7918a94 | 73 | if(v < 0.0f) { |
SomeRandomBloke | 2:8c97b7918a94 | 74 | v*= -1.0f; |
simon | 0:15611c7938a3 | 75 | v = floor(v); |
SomeRandomBloke | 2:8c97b7918a94 | 76 | v*=-1.0f; |
simon | 0:15611c7938a3 | 77 | } else { |
simon | 0:15611c7938a3 | 78 | v = floor(v); |
simon | 0:15611c7938a3 | 79 | } |
simon | 0:15611c7938a3 | 80 | return v; |
simon | 0:15611c7938a3 | 81 | } |
simon | 0:15611c7938a3 | 82 | |
simon | 0:15611c7938a3 | 83 | void GPS::getline() { |
simon | 0:15611c7938a3 | 84 | while(_gps.getc() != '$'); // wait for the start of a line |
SomeRandomBloke | 2:8c97b7918a94 | 85 | for(int i=0; i<255; i++) { |
simon | 0:15611c7938a3 | 86 | msg[i] = _gps.getc(); |
SomeRandomBloke | 2:8c97b7918a94 | 87 | // printf("%c",msg[i]); |
simon | 0:15611c7938a3 | 88 | if(msg[i] == '\r') { |
simon | 0:15611c7938a3 | 89 | msg[i] = 0; |
simon | 0:15611c7938a3 | 90 | return; |
simon | 0:15611c7938a3 | 91 | } |
simon | 0:15611c7938a3 | 92 | } |
SomeRandomBloke | 2:8c97b7918a94 | 93 | // error("Overflowed message limit"); |
SomeRandomBloke | 1:1f849dbf3b8b | 94 | // printf("Overflow"); |
simon | 0:15611c7938a3 | 95 | } |