local copy of gps code
Fork of GPS by
GPS.cpp
- Committer:
- ftagius
- Date:
- 2015-06-16
- Revision:
- 1:d2ea48d59900
- Parent:
- 0:15611c7938a3
File content as of revision 1:d2ea48d59900:
/* mbed EM-406 GPS Module Library * Copyright (c) 2008-2010, sford * * Permission is hereby granted, free of charge, to any person obtaining a copy * of this software and associated documentation files (the "Software"), to deal * in the Software without restriction, including without limitation the rights * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * copies of the Software, and to permit persons to whom the Software is * furnished to do so, subject to the following conditions: * * The above copyright notice and this permission notice shall be included in * all copies or substantial portions of the Software. * * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * THE SOFTWARE. */ #include "GPS.h" GPS::GPS(PinName tx, PinName rx) : _gps(tx, rx) { _gps.baud(9600); l_longitude = 0.0; l_latitude = 0.0; r_longitude = 0.0; r_latitude = 0.0; distance = -1; lock_valid = false; sentences = 0; sentence_index=0; satelites=0; sat_data0id=0; sat_data0elv=0; sat_data0azimuth=0; sat_data0sig=0; sat_data1id=0; sat_data1elv=0; sat_data1azimuth=0; sat_data1sig=0; sat_data2id=0; sat_data2elv=0; sat_data2azimuth=0; sat_data2sig=0; sat_data3id=0; sat_data3elv=0; sat_data3azimuth=0; sat_data3sig=0; } void GPS::flushSerialBuffer(void) { char char1 = 0; while (_gps.readable()) { char1 = _gps.getc(); } return; } int GPS::sample() { float time; char ns, ew; int lock; int rc = 0; int line_parsed = 0; //printf("in gpsd sample \r\n"); while(1) { getline(); // Check if it is a GPGGA msg (matches both locked and non-locked msg) // printf("$%s\r\n",msg); if(sscanf(msg, "GPGGA,%f,%f,%c,%f,%c,%d", &time, &l_latitude, &ns, &l_longitude, &ew, &lock) >= 1) { if(!lock) { l_longitude = 0.0; l_latitude = 0.0; lock_valid = false; printf("lock valid is false\r\n"); return 1; } else { //printf("%s\r\n", msg); if(ns == 'S') { l_latitude *= -1.0; } if(ew == 'W') { l_longitude *= -1.0; } float degrees = trunc(l_latitude / 100.0f); float minutes = l_latitude - (degrees * 100.0f); l_latitude = degrees + minutes / 60.0f; degrees = trunc(l_longitude / 100.0f); //degrees = trunc(longitude / 100.0f * 0.01f); //printf("my lon=%f deg=%f\r\n",longitude, degrees); minutes = l_longitude - (degrees * 100.0f); l_longitude = degrees + minutes / 60.0f; lock_valid = true; //printf("local: lat=%f lon=%f\r\n",l_latitude, l_longitude); //printf("gpgga rcvd\r\n"); rc = 1; } } // Check if it is a GPRMC msg else if (sscanf(msg, "GPRMC,%f,%c,%f,%c,%f,%f,%d", &utc_time, &ns, &nmea_longitude, &ew, &speed_k, &course_d, &date) >= 1) { line_parsed = RMC; } // GLL - Geographic Position-Lat/Lon else if (sscanf(msg, "GPGLL,%f,%c,%f,%c,%f,%c", &l_latitude, &ns, &l_longitude, &ew, &utc_time, &gll_status) >= 1) { if(ns == 'S') { l_latitude *= -1.0; } if(ew == 'W') { l_longitude *= -1.0; } float degrees = trunc(l_latitude / 100.0f); //printf("my lat=%f deg=%f\r\n",latitude, degrees); float minutes = l_latitude - (degrees * 100.0f); l_latitude = degrees + minutes / 60.0f; degrees = trunc(l_longitude / 100.0f); //degrees = trunc(longitude / 100.0f * 0.01f); //printf("my lon=%f deg=%f\r\n",longitude, degrees); minutes = l_longitude - (degrees * 100.0f); l_longitude = degrees + minutes / 60.0f; lock_valid = true; line_parsed = GLL; //printf("gpgll rcvd\r\n"); rc = 1; } // VTG-Course Over Ground and Ground Speed 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) { line_parsed = VTG; } else if (sscanf(msg, "GPGSV,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d*", &sentences, &sentence_index, &satelites, &sat_data0id, &sat_data0elv, &sat_data0azimuth, &sat_data0sig, &sat_data1id, &sat_data1elv, &sat_data1azimuth, &sat_data1sig, &sat_data2id, &sat_data2elv, &sat_data2azimuth, &sat_data2sig, &sat_data3id, &sat_data3elv, &sat_data3azimuth, &sat_data3sig) >= 1 ) { int avg = (sat_data0sig+sat_data1sig+sat_data2sig)/3; //printf("gps snr_01=%d snr_02=%d, snr_03=%d, avg=%d\r\n", sat_data0sig, sat_data1sig, sat_data3sig, avg ); rc = 1; } else rc = 0; if (rc == 1) { return rc; } } return rc; } float GPS::trunc(float v) { if(v < 0.0) { v*= -1.0; v = floor(v); v*=-1.0; } else { v = floor(v); } return v; } void GPS::getline() { //printf("in getline\r\n"); while(_gps.getc() != '$'); // wait for the start of a line for(int i=0; i<1022; i++) { msg[i] = _gps.getc(); if(msg[i] == '\r') { msg[i] = 0; //printf("return from getline\r\n"); return; } } //printf("overflowed message limit\r\n"); error("Overflowed message limit"); } void GPS::sendCommand(char *str) { _gps.printf("%s",str); } void GPS::setBaud57600(void) { _gps.baud(9600); wait_ms(100); #if 0 _gps.printf("$PMTK251,57600*2C\r\n"); // set baud to 57600 wait(1); _gps.baud(57600); wait(1); #endif // getline(); // printf("msg2=%s\r\n", msg); //printf("gpsbaud is 57600\r\n"); //_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"); // //_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");// _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) //_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 //_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 // _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 //_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. #if 1 //_gps.printf("$MTK220,1000*1F\r\n"); // set 1 hz update rate wait_ms(100); _gps.printf("$PMTK220,200*2C\r\n"); // set 5 hz nmea echoe rate wait_ms(100); _gps.printf("$PMTK300,200,0,0,0,0*2F\r\n"); // set 5 hz position update rate wait_ms(100); //_gps.printf("$PMTK220,100*2\r\n"); // set 10 hz update rate #endif }