MTK3339.cpp

00001 
00002 #include "mbed.h"
00003 #include "MTK3339.h"
00004 
00005 
00006 MTK3339::MTK3339(PinName tx, PinName rx) : _serial(tx, rx) {
00007     _serial.baud(9600);    
00008 
00009     _state = StateStart;
00010     _sentenceMask = 0;
00011     _availDataType = NmeaInvalid;
00012     memset(&gga, 0, sizeof(GgaType));
00013     memset(&vtg, 0, sizeof(VtgType));
00014 }
00015 
00016 void MTK3339::start(void (*fptr)(void), int mask) {
00017     if (fptr && mask) {
00018         _dataCallback.attach(fptr);
00019         _sentenceMask = mask;
00020         _serial.attach(this, &MTK3339::uartIrq, Serial::RxIrq);
00021     }        
00022 }
00023 
00024 void MTK3339::stop() {
00025     _dataCallback.attach(NULL);
00026     _sentenceMask = 0;
00027     _serial.attach(NULL);
00028 }
00029     
00030 MTK3339::NmeaSentence MTK3339::getAvailableDataType() {
00031     return _availDataType;
00032 }
00033 
00034 double MTK3339::getLatitudeAsDegrees() {
00035     if(gga.fix == 0 || gga.nsIndicator == 0) return 0;
00036     
00037     double l = gga.latitude;
00038     char ns = gga.nsIndicator;
00039     
00040     // convert from ddmm.mmmm to degrees only
00041     // 60 minutes is 1 degree
00042     
00043     int deg = (int)(l / 100);
00044     l = (l - deg*100.0) / 60.0;
00045     l = deg + l;
00046     if (ns == 'S') l = -l;
00047     
00048     return l;
00049 }
00050 
00051 double MTK3339::getLongitudeAsDegrees() {
00052     if(gga.fix == 0 || gga.ewIndicator == 0) return 0;
00053     
00054     double l = gga.longitude;
00055     char ew = gga.ewIndicator;
00056     
00057     // convert from ddmm.mmmm to degrees only
00058     // 60 minutes is 1 degree
00059     
00060     int deg = (int)(l / 100);
00061     l = (l - deg*100) / 60;
00062     l = deg + l;
00063     if (ew == 'W') l = -l;
00064     
00065     return l;
00066 }
00067 
00068 void MTK3339::parseGGA(char* data, int dataLen) {
00069     //http://aprs.gids.nl/nmea/#gga
00070     
00071     double tm = 0;
00072     
00073     memset(&gga, 0, sizeof(GgaType));
00074     
00075     char* p = data;
00076     int pos = 0;    
00077     
00078     p = strchr(p, ',');
00079     while (p != NULL && *p != 0) {
00080         p++;
00081         
00082         switch(pos) {
00083             case 0: // time: hhmmss.sss
00084                 tm = strtod(p, NULL);
00085                 gga.hours = (int)(tm / 10000);
00086                 gga.minutes = ((int)tm % 10000) / 100;
00087                 gga.seconds = ((int)tm % 100);
00088                 gga.milliseconds = (int)(tm * 1000) % 1000;
00089                 break;
00090             case 1: // latitude: ddmm.mmmm
00091                 gga.latitude = strtod(p, NULL);
00092                 break;
00093             case 2: // N/S indicator (north or south)
00094                 if (*p == 'N' || *p == 'S') {
00095                     gga.nsIndicator = *p;
00096                 }
00097                 break;
00098             case 3: // longitude: dddmm.mmmm
00099                 gga.longitude = strtod(p, NULL);
00100                 break;                
00101             case 4: // E/W indicator (east or west)
00102                 if (*p == 'E' || *p == 'W') {
00103                     gga.ewIndicator = *p;
00104                 }
00105                 break;                                
00106             case 5: // position indicator (1=no fix, 2=GPS fix, 3=Differential)
00107                 gga.fix = strtol(p, NULL, 10);
00108                 break;                                             
00109             case 6: // num satellites
00110                 gga.satellites = strtol(p, NULL, 10);
00111                 break;                                                             
00112             case 7: // hdop
00113                 gga.hdop = strtod(p, NULL);
00114                 break;                               
00115             case 8: // altitude
00116                 gga.altitude = strtod(p, NULL);
00117                 break;                     
00118             case 9: // units
00119                 // ignore units
00120                 break;                         
00121             case 10: // geoidal separation
00122                 gga.geoidal = strtod(p, NULL);
00123                 break;                      
00124         }
00125         pos++;
00126         
00127         p = strchr(p, ',');            
00128     }
00129     
00130 }
00131 
00132 void MTK3339::parseVTG(char* data, int dataLen) {
00133      
00134     
00135     char* p = data;
00136     int pos = 0;    
00137     
00138     memset(&vtg, 0, sizeof(VtgType));
00139     
00140     p = strchr(p, ',');
00141     while (p != NULL && *p != 0) {
00142         p++;
00143         
00144         switch(pos) {
00145             case 0: // course in degrees
00146                 vtg.course = strtod(p, NULL);
00147                 break;
00148             case 1: // Reference (T)                
00149                 break;
00150             case 2: // course magnetic (need customization)
00151                 break;
00152             case 3: // reference (M)
00153                 break;                
00154             case 4: // speed in knots
00155                 vtg.speedKnots = strtod(p, NULL);
00156                 break;                                
00157             case 5: // units (N)
00158                 break;                                             
00159             case 6: // speed in Km/h
00160                 vtg.speedKmHour = strtod(p, NULL);
00161                 break;                                                             
00162             case 7: // units (K)                
00163                 break;                               
00164             case 8: // mode
00165                 if (*p == 'A' || *p == 'D' || *p == 'E') {
00166                     vtg.mode = *p;
00167                 }
00168                                 
00169                 break;                     
00170                 
00171         }
00172 
00173         pos++;
00174         
00175         p = strchr(p, ',');            
00176     }
00177 }
00178 
00179 
00180 void MTK3339::parseData(char* data, int len) {
00181     do {
00182     
00183         // verify checksum 
00184         if (len < 3 || (len > 3 && data[len-3] != '*')) {
00185             // invalid data
00186             break;
00187         }        
00188         int sum = strtol(&data[len-2], NULL, 16);
00189         for(int i = 1; i < len-3; i++) {
00190             sum ^= data[i];
00191         }
00192         if (sum != 0) {
00193             // invalid checksum
00194             break;
00195         }  
00196         
00197         
00198         if (strncmp("$GPGGA", data, 6) == 0 && (_sentenceMask & NmeaGga) != 0) {            
00199             parseGGA(data, len);
00200             _availDataType = NmeaGga;
00201             _dataCallback.call();
00202             _availDataType = NmeaInvalid;
00203         }          
00204         else if (strncmp("$GPVTG", data, 6) == 0 && (_sentenceMask & NmeaVtg) != 0) {            
00205             parseVTG(data, len);
00206             _availDataType = NmeaVtg;
00207             _dataCallback.call();
00208             _availDataType = NmeaInvalid;
00209         }     
00210 
00211     
00212     } while(0);
00213 }
00214 
00215 void MTK3339::uartIrq() {
00216     char d = 0;
00217     
00218     while(_serial.readable()) {
00219         d = _serial.getc();
00220         
00221         switch(_state) {
00222         case StateStart:
00223             if (d == '$') {                
00224                 _buf[0] = '$';
00225                 _bufPos = 1;
00226                 _state = StateData;
00227             }
00228             break;
00229         case StateData:
00230             if (_bufPos >= MTK3339_BUF_SZ) {
00231                 // error
00232                 _state = StateStart;
00233             }
00234             else if (d == '\r') {
00235                 
00236                 _buf[_bufPos] = 0;
00237                 
00238                 parseData(_buf, _bufPos);
00239                 
00240                 _state = StateStart;
00241             }
00242             else {
00243                 _buf[_bufPos++] = d;
00244             }
00245             
00246             break;
00247         }
00248     }
00249 }