File content as of revision 4:c9861b0d3219:

// Original author: Embedded Artists
// Revised by T. Bronez, 2015-12-16

#include "mbed.h"
#include "MTK3339.h"
#define min(a,b) a>b?a:b 

/*
// For DEBUG, can flash LED2 on arrival of RMC message
DigitalOut led2(LED2);
Timeout timeout2;
void led2Off() {
    led2 = 1;
}
void flashLED2() {
    led2 = 0;
    timeout2.attach(&led2Off, 0.1);
}
*/

MTK3339::MTK3339(PinName tx, PinName rx) : _serial(tx, rx) {
    _serial.baud(9600);    
    _state = StateStart;
    _sentenceMask = 0;
    _availDataType = NMEA_NONE;
    memset(&rmc, 0, sizeof(RmcType));

    memset(&ggaMsg, 0, MSG_BUF_SZ+1);
    memset(&gsaMsg, 0, MSG_BUF_SZ+1);
    memset(&gsvMsg, 0, MSG_BUF_SZ+1);
    memset(&rmcMsg, 0, MSG_BUF_SZ+1);
    memset(&vtgMsg, 0, MSG_BUF_SZ+1);
}

void MTK3339::start(void (*fptr)(void), int mask) {
    if (fptr && mask) {
        _dataCallback.attach(fptr);
        _sentenceMask = mask;
        _serial.attach(this, &MTK3339::uartIrq, Serial::RxIrq);
    }

    // DEBUG: Query antenna status    
    //_serial.printf("$PGTOP,11,3*6F\r\n");
    //_serial.printf("$PGCMD,33,1*6C\r\n");
}

void MTK3339::stop() {
    _dataCallback.attach(NULL);
    _sentenceMask = 0;
    _serial.attach(NULL);
}
    
MTK3339::NmeaSentence MTK3339::getAvailableDataType() {
    return _availDataType;
}

void MTK3339::decodeRMC() {
    // See http://aprs.gids.nl/nmea/#rmc for RMC sentence definition
    const double sixtieth = 1.0/60.0;
    int n = 0;      // scratch int for conversions
    double d = 0;   // scratch double for conversions
    char* q = NULL; // scratch pointer for conversions

    memset(&rmc, 0, sizeof(RmcType));
    char* p = rmcMsg;

    /** 
     * Find and decode field 0, UTC time, as hhmmss.sss
     */
    p = strchr(p, ',');                 // First comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    q = p;
    // Decode time hours
    n  = ((*q++) - 48)*10;  // tens
    n += ((*q++) - 48);     // ones
    rmc.gps_tm.tm_hour = n;
    // Decode time minutes
    n  = ((*q++) - 48)*10;  // tens
    n += ((*q++) - 48);     // ones
    rmc.gps_tm.tm_min = n;
    // Decode time seconds
    n  = ((*q++) - 48)*10;  // tens
    n += ((*q++) - 48);     // ones
    rmc.gps_tm.tm_sec = n;
    // Decode time milliseconds
    q++;                    // decimal point
    n  = ((*q++) - 48)*100; // hundreds of msec = tenths of second
    n += ((*q++) - 48)*10;  // tens of msec = hundredths of second
    n += ((*q++) - 48);     // ones of msec = thousandths of second
    rmc.msec = n;

    /** 
     * Find and decode field 1, GPS status, as single
     * character 'A' for valid or 'V' for invalid
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    if (*p == 'A' || *p == 'V')
        rmc.status = *p;
    else
        rmc.status = ' ';

    /**
     * Find and decode field 2, latitude, formatted as ddmm.mmmm
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    q = p;
    d  = ((*q++) - 48)*10;      // tens
    d += ((*q++) - 48);         // ones
    rmc.lat_dd = d;             // whole degrees
    
    d  = ((*q++) - 48)*10;      // tens
    d += ((*q++) - 48);         // ones
    q++;                        // decimal point
    d += ((*q++) - 48)*0.1000;  // tenths
    d += ((*q++) - 48)*0.0100;  // hundredths
    d += ((*q++) - 48)*0.0010;  // thousandths
    d += ((*q++) - 48)*0.0001;  // ten-thousandths
    if (*q == ',')
        rmc.lat_dd += sixtieth*d;   // decimal degrees
    else
        rmc.lat_dd = NAN;           // Unexpected
        
    /**
     * Find and decode field 3, latitude sense, as single
     * character 'N' for north or 'S' for south
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    if (*p == 'S') rmc.lat_dd = -rmc.lat_dd;

    /**
     * Find and decode field 4, longitude, formatted as dddmm.mmmm
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    q = p;
    d  = ((*q++) - 48)*100;     // hundreds
    d += ((*q++) - 48)*10;      // tens
    d += ((*q++) - 48);         // ones
    rmc.lon_dd = d;             // whole degrees
    
    d  = ((*q++) - 48)*10;      // tens
    d += ((*q++) - 48);         // ones
    q++;                        // decimal point
    d += ((*q++) - 48)*0.1000;  // tenths
    d += ((*q++) - 48)*0.0100;  // hundredths
    d += ((*q++) - 48)*0.0010;  // thousandths
    d += ((*q++) - 48)*0.0001;  // ten-thousandths
    if (*q == ',')
        rmc.lon_dd += sixtieth*d;   // decimal degrees
    else
        rmc.lon_dd = NAN;           // Unexpected

    /**
     * Find and decode field 5, longitude sense, as single
     * character 'E' for east or 'W' for west
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    if (*p == 'W') rmc.lon_dd = -rmc.lon_dd;
    
    /**
     * Find and skip field 6, speed in knots
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    /**
     * Find and skip field 7, course in degrees
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    /**
     * Find and decode field 8, UTC date, formatted as ddmmyy
     * where 1<=dd<=31, 1<=mm<=12, and yy is offset from 2000
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    q = p;
    // Decode date day
    n  = ((*q++) - 48)*10;  // tens
    n += ((*q++) - 48);     // ones
    rmc.gps_tm.tm_mday = n;
    // Decode date month
    n  = ((*q++) - 48)*10;      // tens
    n += ((*q++) - 48);         // ones
    rmc.gps_tm.tm_mon = n-1;  // tm_mon runs 0-11, not 1-12
    // Decode date year
    n  = ((*q++) - 48)*10;          // tens
    n += ((*q++) - 48);             // ones
    rmc.gps_tm.tm_year = n+100;   // tm_year is offset from year 1900, not 2000

    /**
     * Find and skip field 9, magnetic variation
     * (not implemented in GPS firmware)
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field

    /**
     * Find and skip field 10, magnetic variation direction
     * (not implemented in GPS firmware)
     */
    p = strchr(p, ',');                 // Next comma
    if (p == NULL || *p == 0) return;   // Unexpected
    p++;                                // Start of field
}

void MTK3339::saveGGA(char* data, int dataLen) {
    // See http://aprs.gids.nl/nmea/#gga for GGA sentence definition
    memcpy (ggaMsg, data, min(dataLen, MSG_BUF_SZ));
}

void MTK3339::saveGSA(char* data, int dataLen) {
    // See http://aprs.gids.nl/nmea/#gsa for GSA sentence definition
    memcpy (gsaMsg, data, min(dataLen, MSG_BUF_SZ));
}

void MTK3339::saveGSV(char* data, int dataLen) {
    // See http://aprs.gids.nl/nmea/#gsv for GSV sentence definition
    memcpy (gsvMsg, data, min(dataLen, MSG_BUF_SZ));
}

void MTK3339::saveRMC(char* data, int dataLen) {
    // See http://aprs.gids.nl/nmea/#rmc for RMC sentence definition
    memcpy (rmcMsg, data, min(dataLen, MSG_BUF_SZ));
}

void MTK3339::saveVTG(char* data, int dataLen) {
    // See http://aprs.gids.nl/nmea/#vtg for VTG sentence definition
    memcpy (vtgMsg, data, min(dataLen, MSG_BUF_SZ));
}

void MTK3339::saveData(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;
        }

        //printf("msg: %s\n", data); // DEBUG
        
        // Save appropriate sentence for later decoding   
        if (strncmp("$GPRMC", data, 6) == 0 && (_sentenceMask & NMEA_RMC) != 0) {
            //flashLED2();    // for DEBUG
            saveRMC(data, len);
            _availDataType = NMEA_RMC;
            _dataCallback.call();
            _availDataType = NMEA_NONE;
        }
        else if (strncmp("$GPGGA", data, 6) == 0 && (_sentenceMask & NMEA_GGA) != 0) {
            saveGGA(data, len);
            _availDataType = NMEA_GGA;
            _dataCallback.call();
            _availDataType = NMEA_NONE;
        }          
        else if (strncmp("$GPGSA", data, 6) == 0 && (_sentenceMask & NMEA_GSA) != 0) {
            saveGSA(data, len);
            _availDataType = NMEA_GSA;
            _dataCallback.call();
            _availDataType = NMEA_NONE;
        }          
        else if (strncmp("$GPGSV", data, 6) == 0 && (_sentenceMask & NMEA_GSV) != 0) {
            saveGSV(data, len);
            _availDataType = NMEA_GSV;
            _dataCallback.call();
            _availDataType = NMEA_NONE;
        }          
        else if (strncmp("$GPVTG", data, 6) == 0 && (_sentenceMask & NMEA_VTG) != 0) {
            saveVTG(data, len);
            _availDataType = NMEA_VTG;
            _dataCallback.call();
            _availDataType = NMEA_NONE;
        }
    } 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;
                saveData(_buf, _bufPos);
                _state = StateStart;
            }
            else {
                _buf[_bufPos++] = d;
            }
            break;
        }
    }
}

/*
 From http://forum.trenz-electronic.de/index.php?topic=273.0
 (untested), other MTK3339 commands and responses:
 
 To disable antenna status/advisor messages,
    $PGCMD,33,0*6D<CR><LF>
    
 MTK3339 should acknowledge with
    $PGACK,33,0*6E<CR><LF>

 To enable antenna status/advisor messages,
    $PGCMD,33,1*6C<CR><LF>
    
 MTK3339 should acknowledge with
    $PGACK,33,1*6F<CR><LF>

 To query antenna status,
    $PGTOP,11,3*6F<CR><LF>
    
 MTK3339 should respond with one of the following:
    $PGTOP,11,1*6D  active antenna shorted
    $PGTOP,11,2*6E  using internal antenna
    $PGTOP,11,3*6F  using active antenna
*/