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  


  
}