Diff: MTK3339.cpp

Revision:

0:bd0fe2412980

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MTK3339.cpp	Thu Nov 07 11:46:53 2013 +0000
@@ -0,0 +1,249 @@
+
+#include "mbed.h"
+#include "MTK3339.h"
+
+
+MTK3339::MTK3339(PinName tx, PinName rx) : _serial(tx, rx) {
+    _serial.baud(9600);    
+
+    _state = StateStart;
+    _sentenceMask = 0;
+    _availDataType = NmeaInvalid;
+    memset(&gga, 0, sizeof(GgaType));
+    memset(&vtg, 0, sizeof(VtgType));
+}
+
+void MTK3339::start(void (*fptr)(void), int mask) {
+    if (fptr && mask) {
+        _dataCallback.attach(fptr);
+        _sentenceMask = mask;
+        _serial.attach(this, &MTK3339::uartIrq, Serial::RxIrq);
+    }        
+}
+
+void MTK3339::stop() {
+    _dataCallback.attach(NULL);
+    _sentenceMask = 0;
+    _serial.attach(NULL);
+}
+    
+MTK3339::NmeaSentence MTK3339::getAvailableDataType() {
+    return _availDataType;
+}
+
+double MTK3339::getLatitudeAsDegrees() {
+    if(gga.fix == 0 || gga.nsIndicator == 0) return 0;
+    
+    double l = gga.latitude;
+    char ns = gga.nsIndicator;
+    
+    // convert from ddmm.mmmm to degrees only
+    // 60 minutes is 1 degree
+    
+    int deg = (int)(l / 100);
+    l = (l - deg*100.0) / 60.0;
+    l = deg + l;
+    if (ns == 'S') l = -l;
+    
+    return l;
+}
+
+double MTK3339::getLongitudeAsDegrees() {
+    if(gga.fix == 0 || gga.ewIndicator == 0) return 0;
+    
+    double l = gga.longitude;
+    char ew = gga.ewIndicator;
+    
+    // convert from ddmm.mmmm to degrees only
+    // 60 minutes is 1 degree
+    
+    int deg = (int)(l / 100);
+    l = (l - deg*100) / 60;
+    l = deg + l;
+    if (ew == 'W') l = -l;
+    
+    return l;
+}
+
+void MTK3339::parseGGA(char* data, int dataLen) {
+    //http://aprs.gids.nl/nmea/#gga
+    
+    double tm = 0;
+    
+    memset(&gga, 0, sizeof(GgaType));
+    
+    char* p = data;
+    int pos = 0;    
+    
+    p = strchr(p, ',');
+    while (p != NULL && *p != 0) {
+        p++;
+        
+        switch(pos) {
+            case 0: // time: hhmmss.sss
+                tm = strtod(p, NULL);
+                gga.hours = (int)(tm / 10000);
+                gga.minutes = ((int)tm % 10000) / 100;
+                gga.seconds = ((int)tm % 100);
+                gga.milliseconds = (int)(tm * 1000) % 1000;
+                break;
+            case 1: // latitude: ddmm.mmmm
+                gga.latitude = strtod(p, NULL);
+                break;
+            case 2: // N/S indicator (north or south)
+                if (*p == 'N' || *p == 'S') {
+                    gga.nsIndicator = *p;
+                }
+                break;
+            case 3: // longitude: dddmm.mmmm
+                gga.longitude = strtod(p, NULL);
+                break;                
+            case 4: // E/W indicator (east or west)
+                if (*p == 'E' || *p == 'W') {
+                    gga.ewIndicator = *p;
+                }
+                break;                                
+            case 5: // position indicator (1=no fix, 2=GPS fix, 3=Differential)
+                gga.fix = strtol(p, NULL, 10);
+                break;                                             
+            case 6: // num satellites
+                gga.satellites = strtol(p, NULL, 10);
+                break;                                                             
+            case 7: // hdop
+                gga.hdop = strtod(p, NULL);
+                break;                               
+            case 8: // altitude
+                gga.altitude = strtod(p, NULL);
+                break;                     
+            case 9: // units
+                // ignore units
+                break;                         
+            case 10: // geoidal separation
+                gga.geoidal = strtod(p, NULL);
+                break;                      
+        }
+        pos++;
+        
+        p = strchr(p, ',');            
+    }
+    
+}
+
+void MTK3339::parseVTG(char* data, int dataLen) {
+     
+    
+    char* p = data;
+    int pos = 0;    
+    
+    memset(&vtg, 0, sizeof(VtgType));
+    
+    p = strchr(p, ',');
+    while (p != NULL && *p != 0) {
+        p++;
+        
+        switch(pos) {
+            case 0: // course in degrees
+                vtg.course = strtod(p, NULL);
+                break;
+            case 1: // Reference (T)                
+                break;
+            case 2: // course magnetic (need customization)
+                break;
+            case 3: // reference (M)
+                break;                
+            case 4: // speed in knots
+                vtg.speedKnots = strtod(p, NULL);
+                break;                                
+            case 5: // units (N)
+                break;                                             
+            case 6: // speed in Km/h
+                vtg.speedKmHour = strtod(p, NULL);
+                break;                                                             
+            case 7: // units (K)                
+                break;                               
+            case 8: // mode
+                if (*p == 'A' || *p == 'D' || *p == 'E') {
+                    vtg.mode = *p;
+                }
+                                
+                break;                     
+                
+        }
+
+        pos++;
+        
+        p = strchr(p, ',');            
+    }
+}
+
+
+void MTK3339::parseData(char* data, int len) {
+    do {
+    
+        // verify checksum 
+        if (len < 3 || (len > 3 && data[len-3] != '*')) {
+            // invalid data
+            break;
+        }        
+        int sum = strtol(&data[len-2], NULL, 16);
+        for(int i = 1; i < len-3; i++) {
+            sum ^= data[i];
+        }
+        if (sum != 0) {
+            // invalid checksum
+            break;
+        }  
+        
+        
+        if (strncmp("$GPGGA", data, 6) == 0 && (_sentenceMask & NmeaGga) != 0) {            
+            parseGGA(data, len);
+            _availDataType = NmeaGga;
+            _dataCallback.call();
+            _availDataType = NmeaInvalid;
+        }          
+        else if (strncmp("$GPVTG", data, 6) == 0 && (_sentenceMask & NmeaVtg) != 0) {            
+            parseVTG(data, len);
+            _availDataType = NmeaVtg;
+            _dataCallback.call();
+            _availDataType = NmeaInvalid;
+        }     
+
+    
+    } while(0);
+}
+
+void MTK3339::uartIrq() {
+    char d = 0;
+    
+    while(_serial.readable()) {
+        d = _serial.getc();
+        
+        switch(_state) {
+        case StateStart:
+            if (d == '$') {                
+                _buf[0] = '$';
+                _bufPos = 1;
+                _state = StateData;
+            }
+            break;
+        case StateData:
+            if (_bufPos >= MTK3339_BUF_SZ) {
+                // error
+                _state = StateStart;
+            }
+            else if (d == '\r') {
+                
+                _buf[_bufPos] = 0;
+                
+                parseData(_buf, _bufPos);
+                
+                _state = StateStart;
+            }
+            else {
+                _buf[_bufPos++] = d;
+            }
+            
+            break;
+        }
+    }
+}