CaryCoders / GPS

local copy of gps code

Fork of GPS by Simon Ford

GPS.cpp@1:d2ea48d59900, 2015-06-16 (annotated)

Committer:
ftagius
Date:
Tue Jun 16 11:52:07 2015 +0000
Revision:
1:d2ea48d59900
Parent:
0:15611c7938a3 
local updates

Who changed what in which revision?

UserRevisionLine numberNew 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
simon 0:15611c7938a3 25 GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) {
ftagius 1:d2ea48d59900 26 _gps.baud(9600);
ftagius 1:d2ea48d59900 27 l_longitude = 0.0;
ftagius 1:d2ea48d59900 28 l_latitude = 0.0;
ftagius 1:d2ea48d59900 29 r_longitude = 0.0;
ftagius 1:d2ea48d59900 30 r_latitude = 0.0;
ftagius 1:d2ea48d59900 31 distance = -1;
ftagius 1:d2ea48d59900 32 lock_valid = false;
ftagius 1:d2ea48d59900 33 sentences = 0;
ftagius 1:d2ea48d59900 34 sentence_index=0;
ftagius 1:d2ea48d59900 35 satelites=0;
ftagius 1:d2ea48d59900 36 sat_data0id=0;
ftagius 1:d2ea48d59900 37 sat_data0elv=0;
ftagius 1:d2ea48d59900 38 sat_data0azimuth=0;
ftagius 1:d2ea48d59900 39 sat_data0sig=0;
ftagius 1:d2ea48d59900 40 sat_data1id=0;
ftagius 1:d2ea48d59900 41 sat_data1elv=0;
ftagius 1:d2ea48d59900 42 sat_data1azimuth=0;
ftagius 1:d2ea48d59900 43 sat_data1sig=0;
ftagius 1:d2ea48d59900 44 sat_data2id=0;
ftagius 1:d2ea48d59900 45 sat_data2elv=0;
ftagius 1:d2ea48d59900 46 sat_data2azimuth=0;
ftagius 1:d2ea48d59900 47 sat_data2sig=0;
ftagius 1:d2ea48d59900 48 sat_data3id=0;
ftagius 1:d2ea48d59900 49 sat_data3elv=0;
ftagius 1:d2ea48d59900 50 sat_data3azimuth=0;
ftagius 1:d2ea48d59900 51 sat_data3sig=0;
simon 0:15611c7938a3 52 }
simon 0:15611c7938a3 53
ftagius 1:d2ea48d59900 54 void GPS::flushSerialBuffer(void) { char char1 = 0; while (_gps.readable()) { char1 = _gps.getc(); } return; }
ftagius 1:d2ea48d59900 55
simon 0:15611c7938a3 56 int GPS::sample() {
simon 0:15611c7938a3 57 float time;
simon 0:15611c7938a3 58 char ns, ew;
simon 0:15611c7938a3 59 int lock;
ftagius 1:d2ea48d59900 60 int rc = 0;
ftagius 1:d2ea48d59900 61 int line_parsed = 0;
ftagius 1:d2ea48d59900 62 //printf("in gpsd sample \r\n");
simon 0:15611c7938a3 63 while(1) {
ftagius 1:d2ea48d59900 64
simon 0:15611c7938a3 65 getline();
ftagius 1:d2ea48d59900 66
simon 0:15611c7938a3 67 // Check if it is a GPGGA msg (matches both locked and non-locked msg)
ftagius 1:d2ea48d59900 68 // printf("$%s\r\n",msg);
ftagius 1:d2ea48d59900 69 if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d", &time, &l_latitude, &ns, &l_longitude, &ew, &lock) >= 1) {
simon 0:15611c7938a3 70 if(!lock) {
ftagius 1:d2ea48d59900 71 l_longitude = 0.0;
ftagius 1:d2ea48d59900 72 l_latitude = 0.0;
ftagius 1:d2ea48d59900 73 lock_valid = false;
ftagius 1:d2ea48d59900 74 printf("lock valid is false\r\n");
ftagius 1:d2ea48d59900 75 return 1;
simon 0:15611c7938a3 76 } else {
ftagius 1:d2ea48d59900 77 //printf("%s\r\n", msg);
ftagius 1:d2ea48d59900 78 if(ns == 'S') { l_latitude *= -1.0; }
ftagius 1:d2ea48d59900 79 if(ew == 'W') { l_longitude *= -1.0; }
ftagius 1:d2ea48d59900 80 float degrees = trunc(l_latitude / 100.0f);
ftagius 1:d2ea48d59900 81
ftagius 1:d2ea48d59900 82
ftagius 1:d2ea48d59900 83 float minutes = l_latitude - (degrees * 100.0f);
ftagius 1:d2ea48d59900 84 l_latitude = degrees + minutes / 60.0f;
ftagius 1:d2ea48d59900 85 degrees = trunc(l_longitude / 100.0f);
ftagius 1:d2ea48d59900 86 //degrees = trunc(longitude / 100.0f * 0.01f);
ftagius 1:d2ea48d59900 87 //printf("my lon=%f deg=%f\r\n",longitude, degrees);
ftagius 1:d2ea48d59900 88 minutes = l_longitude - (degrees * 100.0f);
ftagius 1:d2ea48d59900 89 l_longitude = degrees + minutes / 60.0f;
ftagius 1:d2ea48d59900 90 lock_valid = true;
ftagius 1:d2ea48d59900 91 //printf("local: lat=%f lon=%f\r\n",l_latitude, l_longitude);
ftagius 1:d2ea48d59900 92 //printf("gpgga rcvd\r\n");
ftagius 1:d2ea48d59900 93
ftagius 1:d2ea48d59900 94 rc = 1;
simon 0:15611c7938a3 95 }
simon 0:15611c7938a3 96 }
ftagius 1:d2ea48d59900 97 // Check if it is a GPRMC msg
ftagius 1:d2ea48d59900 98 else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%f,%d", &utc_time, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) {
ftagius 1:d2ea48d59900 99 line_parsed = RMC;
ftagius 1:d2ea48d59900 100
ftagius 1:d2ea48d59900 101 }
ftagius 1:d2ea48d59900 102 // GLL - Geographic Position-Lat/Lon
ftagius 1:d2ea48d59900 103 else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &l_latitude, &ns, &l_longitude, &ew, &utc_time, &gll_status) >= 1) {
ftagius 1:d2ea48d59900 104 if(ns == 'S') { l_latitude *= -1.0; }
ftagius 1:d2ea48d59900 105 if(ew == 'W') { l_longitude *= -1.0; }
ftagius 1:d2ea48d59900 106 float degrees = trunc(l_latitude / 100.0f);
ftagius 1:d2ea48d59900 107 //printf("my lat=%f deg=%f\r\n",latitude, degrees);
ftagius 1:d2ea48d59900 108
ftagius 1:d2ea48d59900 109 float minutes = l_latitude - (degrees * 100.0f);
ftagius 1:d2ea48d59900 110 l_latitude = degrees + minutes / 60.0f;
ftagius 1:d2ea48d59900 111 degrees = trunc(l_longitude / 100.0f);
ftagius 1:d2ea48d59900 112 //degrees = trunc(longitude / 100.0f * 0.01f);
ftagius 1:d2ea48d59900 113 //printf("my lon=%f deg=%f\r\n",longitude, degrees);
ftagius 1:d2ea48d59900 114 minutes = l_longitude - (degrees * 100.0f);
ftagius 1:d2ea48d59900 115 l_longitude = degrees + minutes / 60.0f;
ftagius 1:d2ea48d59900 116 lock_valid = true;
ftagius 1:d2ea48d59900 117 line_parsed = GLL;
ftagius 1:d2ea48d59900 118 //printf("gpgll rcvd\r\n");
ftagius 1:d2ea48d59900 119
ftagius 1:d2ea48d59900 120 rc = 1;
ftagius 1:d2ea48d59900 121
ftagius 1:d2ea48d59900 122 }
ftagius 1:d2ea48d59900 123 // VTG-Course Over Ground and Ground Speed
ftagius 1:d2ea48d59900 124 else if (sscanf(msg, "GPVTG,%f,%c,%f,%c,%f,%c,%f,%c", &course_t, &course_t_unit, &course_m, &course_m_unit, &speed_k, &speed_k_unit, &speed_km, &speed_km_unit) >= 1) {
ftagius 1:d2ea48d59900 125 line_parsed = VTG;
ftagius 1:d2ea48d59900 126
ftagius 1:d2ea48d59900 127 }
ftagius 1:d2ea48d59900 128 else if (sscanf(msg, "GPGSV,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d*",
ftagius 1:d2ea48d59900 129 &sentences, &sentence_index, &satelites,
ftagius 1:d2ea48d59900 130 &sat_data0id, &sat_data0elv, &sat_data0azimuth, &sat_data0sig,
ftagius 1:d2ea48d59900 131 &sat_data1id, &sat_data1elv, &sat_data1azimuth, &sat_data1sig,
ftagius 1:d2ea48d59900 132 &sat_data2id, &sat_data2elv, &sat_data2azimuth, &sat_data2sig,
ftagius 1:d2ea48d59900 133 &sat_data3id, &sat_data3elv, &sat_data3azimuth, &sat_data3sig) >= 1 )
ftagius 1:d2ea48d59900 134 {
ftagius 1:d2ea48d59900 135 int avg = (sat_data0sig+sat_data1sig+sat_data2sig)/3;
ftagius 1:d2ea48d59900 136 //printf("gps snr_01=%d snr_02=%d, snr_03=%d, avg=%d\r\n", sat_data0sig, sat_data1sig, sat_data3sig, avg );
ftagius 1:d2ea48d59900 137 rc = 1;
ftagius 1:d2ea48d59900 138 }
ftagius 1:d2ea48d59900 139 else
ftagius 1:d2ea48d59900 140 rc = 0;
ftagius 1:d2ea48d59900 141 if (rc == 1)
ftagius 1:d2ea48d59900 142 {
ftagius 1:d2ea48d59900 143
ftagius 1:d2ea48d59900 144 return rc;
ftagius 1:d2ea48d59900 145 }
simon 0:15611c7938a3 146 }
ftagius 1:d2ea48d59900 147
ftagius 1:d2ea48d59900 148 return rc;
simon 0:15611c7938a3 149 }
simon 0:15611c7938a3 150
simon 0:15611c7938a3 151 float GPS::trunc(float v) {
simon 0:15611c7938a3 152 if(v < 0.0) {
simon 0:15611c7938a3 153 v*= -1.0;
simon 0:15611c7938a3 154 v = floor(v);
simon 0:15611c7938a3 155 v*=-1.0;
simon 0:15611c7938a3 156 } else {
simon 0:15611c7938a3 157 v = floor(v);
simon 0:15611c7938a3 158 }
simon 0:15611c7938a3 159 return v;
simon 0:15611c7938a3 160 }
simon 0:15611c7938a3 161
simon 0:15611c7938a3 162 void GPS::getline() {
ftagius 1:d2ea48d59900 163 //printf("in getline\r\n");
simon 0:15611c7938a3 164 while(_gps.getc() != '$'); // wait for the start of a line
ftagius 1:d2ea48d59900 165 for(int i=0; i<1022; i++) {
simon 0:15611c7938a3 166 msg[i] = _gps.getc();
simon 0:15611c7938a3 167 if(msg[i] == '\r') {
simon 0:15611c7938a3 168 msg[i] = 0;
ftagius 1:d2ea48d59900 169 //printf("return from getline\r\n");
simon 0:15611c7938a3 170 return;
simon 0:15611c7938a3 171 }
simon 0:15611c7938a3 172 }
ftagius 1:d2ea48d59900 173 //printf("overflowed message limit\r\n");
simon 0:15611c7938a3 174 error("Overflowed message limit");
simon 0:15611c7938a3 175 }
ftagius 1:d2ea48d59900 176
ftagius 1:d2ea48d59900 177
ftagius 1:d2ea48d59900 178 void GPS::sendCommand(char *str) {
ftagius 1:d2ea48d59900 179 _gps.printf("%s",str);
ftagius 1:d2ea48d59900 180 }
ftagius 1:d2ea48d59900 181
ftagius 1:d2ea48d59900 182 void GPS::setBaud57600(void) {
ftagius 1:d2ea48d59900 183
ftagius 1:d2ea48d59900 184 _gps.baud(9600);
ftagius 1:d2ea48d59900 185 wait_ms(100);
ftagius 1:d2ea48d59900 186 #if 0
ftagius 1:d2ea48d59900 187 _gps.printf("$PMTK251,57600*2C\r\n"); // set baud to 57600
ftagius 1:d2ea48d59900 188 wait(1);
ftagius 1:d2ea48d59900 189 _gps.baud(57600);
ftagius 1:d2ea48d59900 190 wait(1);
ftagius 1:d2ea48d59900 191 #endif
ftagius 1:d2ea48d59900 192 // getline();
ftagius 1:d2ea48d59900 193 // printf("msg2=%s\r\n", msg);
ftagius 1:d2ea48d59900 194
ftagius 1:d2ea48d59900 195 //printf("gpsbaud is 57600\r\n");
ftagius 1:d2ea48d59900 196 //_gps.printf("$PMTK314,0,0,1,1,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2D\r\n"); //
ftagius 1:d2ea48d59900 197 //_gps.printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");//
ftagius 1:d2ea48d59900 198
ftagius 1:d2ea48d59900 199
ftagius 1:d2ea48d59900 200 _gps.printf("$PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");// (output GGA only)
ftagius 1:d2ea48d59900 201 //_gps.printf("$PMTK314,0,0,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // output GGA and GSV
ftagius 1:d2ea48d59900 202 //_gps.printf("$PMTK314,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0*29\r\n");// output GSV only
ftagius 1:d2ea48d59900 203 // _gps.printf("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"); // output all data
ftagius 1:d2ea48d59900 204 //_gps.printf("$PMTK314,0,0,1,1,5,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2D\r\n"); // VTG, GGA and GSA. GSA is only sent once every five transmissions.
ftagius 1:d2ea48d59900 205 #if 1
ftagius 1:d2ea48d59900 206 //_gps.printf("$MTK220,1000*1F\r\n"); // set 1 hz update rate
ftagius 1:d2ea48d59900 207 wait_ms(100);
ftagius 1:d2ea48d59900 208 _gps.printf("$PMTK220,200*2C\r\n"); // set 5 hz nmea echoe rate
ftagius 1:d2ea48d59900 209 wait_ms(100);
ftagius 1:d2ea48d59900 210 _gps.printf("$PMTK300,200,0,0,0,0*2F\r\n"); // set 5 hz position update rate
ftagius 1:d2ea48d59900 211 wait_ms(100);
ftagius 1:d2ea48d59900 212 //_gps.printf("$PMTK220,100*2\r\n"); // set 10 hz update rate
ftagius 1:d2ea48d59900 213 #endif
ftagius 1:d2ea48d59900 214
ftagius 1:d2ea48d59900 215
ftagius 1:d2ea48d59900 216
ftagius 1:d2ea48d59900 217 }
ftagius 1:d2ea48d59900 218
ftagius 1:d2ea48d59900 219