MTK3339.cpp

00001 // Original author: Embedded Artists
00002 // Revised by T. Bronez, 2015-12-16
00003 
00004 #include "mbed.h"
00005 #include "MTK3339.h"
00006 #define min(a,b) a>b?a:b 
00007 
00008 /*
00009 // For DEBUG, can flash LED2 on arrival of RMC message
00010 DigitalOut led2(LED2);
00011 Timeout timeout2;
00012 void led2Off() {
00013     led2 = 1;
00014 }
00015 void flashLED2() {
00016     led2 = 0;
00017     timeout2.attach(&led2Off, 0.1);
00018 }
00019 */
00020 
00021 MTK3339::MTK3339(PinName tx, PinName rx) : _serial(tx, rx) {
00022     _serial.baud(9600);    
00023     _state = StateStart;
00024     _sentenceMask = 0;
00025     _availDataType = NMEA_NONE;
00026     memset(&rmc, 0, sizeof(RmcType));
00027 
00028     memset(&ggaMsg, 0, MSG_BUF_SZ+1);
00029     memset(&gsaMsg, 0, MSG_BUF_SZ+1);
00030     memset(&gsvMsg, 0, MSG_BUF_SZ+1);
00031     memset(&rmcMsg, 0, MSG_BUF_SZ+1);
00032     memset(&vtgMsg, 0, MSG_BUF_SZ+1);
00033 }
00034 
00035 void MTK3339::start(void (*fptr)(void), int mask) {
00036     if (fptr && mask) {
00037         _dataCallback.attach(fptr);
00038         _sentenceMask = mask;
00039         _serial.attach(this, &MTK3339::uartIrq, Serial::RxIrq);
00040     }
00041 
00042     // DEBUG: Query antenna status    
00043     //_serial.printf("$PGTOP,11,3*6F\r\n");
00044     //_serial.printf("$PGCMD,33,1*6C\r\n");
00045 }
00046 
00047 void MTK3339::stop() {
00048     _dataCallback.attach(NULL);
00049     _sentenceMask = 0;
00050     _serial.attach(NULL);
00051 }
00052     
00053 MTK3339::NmeaSentence MTK3339::getAvailableDataType() {
00054     return _availDataType;
00055 }
00056 
00057 void MTK3339::decodeRMC () {
00058     // See http://aprs.gids.nl/nmea/#rmc for RMC sentence definition
00059     const double sixtieth = 1.0/60.0;
00060     int n = 0;      // scratch int for conversions
00061     double d = 0;   // scratch double for conversions
00062     char* q = NULL; // scratch pointer for conversions
00063 
00064     memset(&rmc, 0, sizeof(RmcType));
00065     char* p = rmcMsg;
00066 
00067     /** 
00068      * Find and decode field 0, UTC time, as hhmmss.sss
00069      */
00070     p = strchr(p, ',');                 // First comma
00071     if (p == NULL || *p == 0) return;   // Unexpected
00072     p++;                                // Start of field
00073 
00074     q = p;
00075     // Decode time hours
00076     n  = ((*q++) - 48)*10;  // tens
00077     n += ((*q++) - 48);     // ones
00078     rmc.gps_tm.tm_hour = n;
00079     // Decode time minutes
00080     n  = ((*q++) - 48)*10;  // tens
00081     n += ((*q++) - 48);     // ones
00082     rmc.gps_tm.tm_min = n;
00083     // Decode time seconds
00084     n  = ((*q++) - 48)*10;  // tens
00085     n += ((*q++) - 48);     // ones
00086     rmc.gps_tm.tm_sec = n;
00087     // Decode time milliseconds
00088     q++;                    // decimal point
00089     n  = ((*q++) - 48)*100; // hundreds of msec = tenths of second
00090     n += ((*q++) - 48)*10;  // tens of msec = hundredths of second
00091     n += ((*q++) - 48);     // ones of msec = thousandths of second
00092     rmc.msec = n;
00093 
00094     /** 
00095      * Find and decode field 1, GPS status, as single
00096      * character 'A' for valid or 'V' for invalid
00097      */
00098     p = strchr(p, ',');                 // Next comma
00099     if (p == NULL || *p == 0) return;   // Unexpected
00100     p++;                                // Start of field
00101 
00102     if (*p == 'A' || *p == 'V')
00103         rmc.status = *p;
00104     else
00105         rmc.status = ' ';
00106 
00107     /**
00108      * Find and decode field 2, latitude, formatted as ddmm.mmmm
00109      */
00110     p = strchr(p, ',');                 // Next comma
00111     if (p == NULL || *p == 0) return;   // Unexpected
00112     p++;                                // Start of field
00113 
00114     q = p;
00115     d  = ((*q++) - 48)*10;      // tens
00116     d += ((*q++) - 48);         // ones
00117     rmc.lat_dd = d;             // whole degrees
00118     
00119     d  = ((*q++) - 48)*10;      // tens
00120     d += ((*q++) - 48);         // ones
00121     q++;                        // decimal point
00122     d += ((*q++) - 48)*0.1000;  // tenths
00123     d += ((*q++) - 48)*0.0100;  // hundredths
00124     d += ((*q++) - 48)*0.0010;  // thousandths
00125     d += ((*q++) - 48)*0.0001;  // ten-thousandths
00126     if (*q == ',')
00127         rmc.lat_dd += sixtieth*d;   // decimal degrees
00128     else
00129         rmc.lat_dd = NAN;           // Unexpected
00130         
00131     /**
00132      * Find and decode field 3, latitude sense, as single
00133      * character 'N' for north or 'S' for south
00134      */
00135     p = strchr(p, ',');                 // Next comma
00136     if (p == NULL || *p == 0) return;   // Unexpected
00137     p++;                                // Start of field
00138 
00139     if (*p == 'S') rmc.lat_dd = -rmc.lat_dd;
00140 
00141     /**
00142      * Find and decode field 4, longitude, formatted as dddmm.mmmm
00143      */
00144     p = strchr(p, ',');                 // Next comma
00145     if (p == NULL || *p == 0) return;   // Unexpected
00146     p++;                                // Start of field
00147 
00148     q = p;
00149     d  = ((*q++) - 48)*100;     // hundreds
00150     d += ((*q++) - 48)*10;      // tens
00151     d += ((*q++) - 48);         // ones
00152     rmc.lon_dd = d;             // whole degrees
00153     
00154     d  = ((*q++) - 48)*10;      // tens
00155     d += ((*q++) - 48);         // ones
00156     q++;                        // decimal point
00157     d += ((*q++) - 48)*0.1000;  // tenths
00158     d += ((*q++) - 48)*0.0100;  // hundredths
00159     d += ((*q++) - 48)*0.0010;  // thousandths
00160     d += ((*q++) - 48)*0.0001;  // ten-thousandths
00161     if (*q == ',')
00162         rmc.lon_dd += sixtieth*d;   // decimal degrees
00163     else
00164         rmc.lon_dd = NAN;           // Unexpected
00165 
00166     /**
00167      * Find and decode field 5, longitude sense, as single
00168      * character 'E' for east or 'W' for west
00169      */
00170     p = strchr(p, ',');                 // Next comma
00171     if (p == NULL || *p == 0) return;   // Unexpected
00172     p++;                                // Start of field
00173 
00174     if (*p == 'W') rmc.lon_dd = -rmc.lon_dd;
00175     
00176     /**
00177      * Find and skip field 6, speed in knots
00178      */
00179     p = strchr(p, ',');                 // Next comma
00180     if (p == NULL || *p == 0) return;   // Unexpected
00181     p++;                                // Start of field
00182 
00183     /**
00184      * Find and skip field 7, course in degrees
00185      */
00186     p = strchr(p, ',');                 // Next comma
00187     if (p == NULL || *p == 0) return;   // Unexpected
00188     p++;                                // Start of field
00189 
00190     /**
00191      * Find and decode field 8, UTC date, formatted as ddmmyy
00192      * where 1<=dd<=31, 1<=mm<=12, and yy is offset from 2000
00193      */
00194     p = strchr(p, ',');                 // Next comma
00195     if (p == NULL || *p == 0) return;   // Unexpected
00196     p++;                                // Start of field
00197 
00198     q = p;
00199     // Decode date day
00200     n  = ((*q++) - 48)*10;  // tens
00201     n += ((*q++) - 48);     // ones
00202     rmc.gps_tm.tm_mday = n;
00203     // Decode date month
00204     n  = ((*q++) - 48)*10;      // tens
00205     n += ((*q++) - 48);         // ones
00206     rmc.gps_tm.tm_mon = n-1;  // tm_mon runs 0-11, not 1-12
00207     // Decode date year
00208     n  = ((*q++) - 48)*10;          // tens
00209     n += ((*q++) - 48);             // ones
00210     rmc.gps_tm.tm_year = n+100;   // tm_year is offset from year 1900, not 2000
00211 
00212     /**
00213      * Find and skip field 9, magnetic variation
00214      * (not implemented in GPS firmware)
00215      */
00216     p = strchr(p, ',');                 // Next comma
00217     if (p == NULL || *p == 0) return;   // Unexpected
00218     p++;                                // Start of field
00219 
00220     /**
00221      * Find and skip field 10, magnetic variation direction
00222      * (not implemented in GPS firmware)
00223      */
00224     p = strchr(p, ',');                 // Next comma
00225     if (p == NULL || *p == 0) return;   // Unexpected
00226     p++;                                // Start of field
00227 }
00228 
00229 void MTK3339::saveGGA(char* data, int dataLen) {
00230     // See http://aprs.gids.nl/nmea/#gga for GGA sentence definition
00231     memcpy (ggaMsg, data, min(dataLen, MSG_BUF_SZ));
00232 }
00233 
00234 void MTK3339::saveGSA(char* data, int dataLen) {
00235     // See http://aprs.gids.nl/nmea/#gsa for GSA sentence definition
00236     memcpy (gsaMsg, data, min(dataLen, MSG_BUF_SZ));
00237 }
00238 
00239 void MTK3339::saveGSV(char* data, int dataLen) {
00240     // See http://aprs.gids.nl/nmea/#gsv for GSV sentence definition
00241     memcpy (gsvMsg, data, min(dataLen, MSG_BUF_SZ));
00242 }
00243 
00244 void MTK3339::saveRMC(char* data, int dataLen) {
00245     // See http://aprs.gids.nl/nmea/#rmc for RMC sentence definition
00246     memcpy (rmcMsg, data, min(dataLen, MSG_BUF_SZ));
00247 }
00248 
00249 void MTK3339::saveVTG(char* data, int dataLen) {
00250     // See http://aprs.gids.nl/nmea/#vtg for VTG sentence definition
00251     memcpy (vtgMsg, data, min(dataLen, MSG_BUF_SZ));
00252 }
00253 
00254 void MTK3339::saveData(char* data, int len) {
00255     do {
00256         // Verify checksum 
00257         if (len < 3 || (len > 3 && data[len-3] != '*')) {
00258             // invalid data
00259             break;
00260         }        
00261         int sum = strtol(&data[len-2], NULL, 16);
00262         for(int i = 1; i < len-3; i++) {
00263             sum ^= data[i];
00264         }
00265         if (sum != 0) {
00266             // invalid checksum
00267             break;
00268         }
00269 
00270         //printf("msg: %s\n", data); // DEBUG
00271         
00272         // Save appropriate sentence for later decoding   
00273         if (strncmp("$GPRMC", data, 6) == 0 && (_sentenceMask & NMEA_RMC) != 0) {
00274             //flashLED2();    // for DEBUG
00275             saveRMC(data, len);
00276             _availDataType = NMEA_RMC;
00277             _dataCallback.call();
00278             _availDataType = NMEA_NONE;
00279         }
00280         else if (strncmp("$GPGGA", data, 6) == 0 && (_sentenceMask & NMEA_GGA) != 0) {
00281             saveGGA(data, len);
00282             _availDataType = NMEA_GGA;
00283             _dataCallback.call();
00284             _availDataType = NMEA_NONE;
00285         }          
00286         else if (strncmp("$GPGSA", data, 6) == 0 && (_sentenceMask & NMEA_GSA) != 0) {
00287             saveGSA(data, len);
00288             _availDataType = NMEA_GSA;
00289             _dataCallback.call();
00290             _availDataType = NMEA_NONE;
00291         }          
00292         else if (strncmp("$GPGSV", data, 6) == 0 && (_sentenceMask & NMEA_GSV) != 0) {
00293             saveGSV(data, len);
00294             _availDataType = NMEA_GSV;
00295             _dataCallback.call();
00296             _availDataType = NMEA_NONE;
00297         }          
00298         else if (strncmp("$GPVTG", data, 6) == 0 && (_sentenceMask & NMEA_VTG) != 0) {
00299             saveVTG(data, len);
00300             _availDataType = NMEA_VTG;
00301             _dataCallback.call();
00302             _availDataType = NMEA_NONE;
00303         }
00304     } while(0);
00305 }
00306 
00307 void MTK3339::uartIrq() {
00308     char d = 0;
00309     while(_serial.readable()) {
00310         d = _serial.getc();
00311         switch(_state) {
00312         case StateStart:
00313             if (d == '$') {                
00314                 _buf[0] = '$';
00315                 _bufPos = 1;
00316                 _state = StateData;
00317             }
00318             break;
00319         case StateData:
00320             if (_bufPos >= MTK3339_BUF_SZ) {
00321                 // error
00322                 _state = StateStart;
00323             }
00324             else if (d == '\r') {
00325                 _buf[_bufPos] = 0;
00326                 saveData(_buf, _bufPos);
00327                 _state = StateStart;
00328             }
00329             else {
00330                 _buf[_bufPos++] = d;
00331             }
00332             break;
00333         }
00334     }
00335 }
00336 
00337 /*
00338  From http://forum.trenz-electronic.de/index.php?topic=273.0
00339  (untested), other MTK3339 commands and responses:
00340  
00341  To disable antenna status/advisor messages,
00342     $PGCMD,33,0*6D<CR><LF>
00343     
00344  MTK3339 should acknowledge with
00345     $PGACK,33,0*6E<CR><LF>
00346 
00347  To enable antenna status/advisor messages,
00348     $PGCMD,33,1*6C<CR><LF>
00349     
00350  MTK3339 should acknowledge with
00351     $PGACK,33,1*6F<CR><LF>
00352 
00353  To query antenna status,
00354     $PGTOP,11,3*6F<CR><LF>
00355     
00356  MTK3339 should respond with one of the following:
00357     $PGTOP,11,1*6D  active antenna shorted
00358     $PGTOP,11,2*6E  using internal antenna
00359     $PGTOP,11,3*6F  using active antenna
00360 */