GPSParser.cpp

00001 #include "GPSParser.h"
00002 
00003 GPSParser::GPSParser (PinName tx, PinName rx, int baud, int num_sentence, MODSERIAL &pc, PwmOut &indicator)
00004 :
00005  GLL (0),
00006  RMC (0),
00007  VTG (1),
00008  GGA (1),
00009  GSA (0),
00010  GSV (0),
00011  GRS (0),
00012  GST (0),
00013  MALM (0),
00014  MEPH (0),
00015  MDGP (0),
00016  MDBG (0),
00017  ZDA (0),
00018  MCHN (0),
00019  num_sentences (num_sentence),
00020  baudrate (baud),
00021  ctr (0),
00022  gps (tx, rx),
00023  pc (pc),
00024  indicator (indicator)
00025 {
00026     pc.printf ("Initializing GPS...\r\n");
00027     gps.baud (baudrate);
00028     
00029     // BEGIN CONFIGURATION OF GPS use http://www.hhhh.org/wiml/proj/nmeaxor.html for checksum calculations...
00030     gps.printf ("$PMTK313,1*2E\r\n"); //enable SBAS satellite searching
00031     gps.printf ("$PMTK301,2*2E\r\n"); //set DGPS mode to WAAS
00032     gps.printf ("$PMTK314,%d,%d,%d,%d,%d,%d,%d,%d,0,0,0,0,0,%d,%d,%d,%d,%d,%d*28\r\n", GLL, RMC, VTG, GGA, GSA, GSV, GRS, GST, MALM, MEPH, MDGP, MDBG, ZDA, MCHN); //enable NMEA output sentences and frequencies
00033     // END CONFIGURATION OF GPS
00034     wait (0.1);
00035     gps.rxBufferFlush ();
00036     gps.attach (this, &GPSParser::ready_buffer);
00037 }
00038 
00039 void GPSParser::process_readings ()
00040 {
00041     char buff [num_sentences][128];
00042     //gps.attach (this, &GPSParserCpp::ready_buffer);
00043     
00044     for (int i = 0; i < num_sentences; i++)
00045     {
00046         for (int k = 0; k < 128; k++)
00047         {
00048             buff [i][k] = gps.getc();
00049             if (buff [i][k] == ',' && buff [i][k - 1] == ',') // if the current and last character read were both ',' then assign the next buff position ',' and the current buff position a '0' and increment k by one.
00050             {
00051                 buff [i][k + 1] = buff [i][k];
00052                 buff [i][k] = '0';
00053                 k++;
00054             }
00055             if (buff [i][k] == '*' && buff [i][k - 1] == ',') // if the current character is '*' and last character read were both ',' then assign the next buff position '*' and the current buff position a '0' and increment k by one.
00056             {
00057                 buff [i][k + 1] = buff [i][k];
00058                 buff [i][k] = '0';
00059                 k++;
00060             }
00061             //pc.putc (buff [i][k]);
00062             if (buff [i][k] == '\n')
00063             {
00064                 k++;
00065                 buff [i][k] = '\0';
00066                 break;
00067             }
00068         }
00069     }
00070     
00071     for (int i = 0; i < num_sentences; i++)
00072     {
00073         if (!sscanf (buff [i], "$GPGGA,%2d%2d%2d.%3d,%f,%c,%f,%c,%d,%d,%f,%f,M,%f,M,%f,%X*%hX\r\n", &readings.hours, &readings.minutes, &readings.seconds, &readings.m_seconds, &readings.latitude, &readings.ns, &readings.longitude, &readings.ew, &readings.fix, &readings.sats_used, &readings.HDOP, &readings.altitude, &readings.geoidal_sep, &readings.AODC, &readings.DGPS_ID, &readings.checksum_gga))
00074             sscanf (buff [i + 1], "$GPGGA,%2d%2d%2d.%3d,%f,%c,%f,%c,%d,%d,%f,%f,M,%f,M,%f,%X*%hX\r\n", &readings.hours, &readings.minutes, &readings.seconds, &readings.m_seconds, &readings.latitude, &readings.ns, &readings.longitude, &readings.ew, &readings.fix, &readings.sats_used, &readings.HDOP, &readings.altitude, &readings.geoidal_sep, &readings.AODC, &readings.DGPS_ID, &readings.checksum_gga);
00075         
00076         if(!sscanf (buff [i], "$GPVTG,%f,T,%f,M,%f,N,%f,K,%c*%hX\r\n", &readings.heading, &readings.mag_heading, &readings.speed_knots, &readings.speed_kph, &readings.mode, &readings.checksum_vtg))
00077             sscanf (buff [i + 1], "$GPVTG,%f,T,%f,M,%f,N,%f,K,%c*%hX\r\n", &readings.heading, &readings.mag_heading, &readings.speed_knots, &readings.speed_kph, &readings.mode, &readings.checksum_vtg);
00078             
00079         pc.printf ("%s", buff [i]);
00080     }
00081     if (readings.ns == 'S')
00082         readings.latitude *= -1.0f;
00083         
00084     if (readings.ew == 'W')
00085         readings.longitude *= -1.0f;
00086     
00087     readings.lat_deg = (int)(readings.latitude / 100);
00088     readings.lat_min = readings.latitude - (readings.lat_deg * 100.0f);
00089     readings.deg_latitude = readings.lat_deg + readings.lat_min / 60.0f;
00090     readings.long_deg = (int)(readings.longitude /100);
00091     readings.long_min = readings.longitude - (readings.long_deg * 100.0f);
00092     readings.deg_longitude = readings.long_deg + readings.long_min / 60.0f;
00093     
00094     gps.rxBufferFlush ();
00095 }
00096 
00097 void GPSParser::ready_buffer(MODSERIAL_IRQ_INFO *q)
00098 {
00099     indicator = 1;
00100     MODSERIAL *serial = q->serial;
00101 
00102     if (serial->rxGetLastChar() == '\n')
00103         ctr++;
00104 
00105     if (ctr == num_sentences)
00106     {
00107         process_readings ();
00108         ctr = 0;
00109     }
00110     indicator = 0;
00111 }