File content as of revision 7:9aa830f5811e:

#include "mbed.h"
#include <ctype.h>
#include "GPS.h"

int GPSParser::_getMessage(Pipe<char>* pipe, char* buf, int len)
{
    int unkn = 0;
    int sz = pipe->size();
    if (len > sz)
        len = sz;
    while (len > 0)
    {
        // NMEA protocol
        int nmea = _parseNmea(pipe,len);
        if ((nmea != NOT_FOUND) && (unkn > 0))  return unkn;
        if (nmea == WAIT)                       return WAIT;
        if (nmea > 0)                           return NMEA | pipe->get(buf,nmea);
        // UBX protocol
        int ubx = _parseUbx(pipe,len);
        if ((ubx != NOT_FOUND) && (unkn > 0))   return unkn;
        if (ubx == WAIT)                        return WAIT;
        if (ubx > 0)                            return UBX | pipe->get(buf,ubx);
        // UNKNOWN
        *buf++ = pipe->getc();
        unkn ++;
        len--;
    }
    if (unkn != NOT_FOUND)                      return unkn; 
    return WAIT;
}

int GPSParser::_parseNmea(Pipe<char>* pipe, int len)
{
    int o = 0;
    int c = 0;
    char ch;
    pipe->start();
    if (++o > len)                      return WAIT;
    if ('$' != pipe->next())            return NOT_FOUND;
    // this needs to be extended by crc checking 
    for (;;)
    {
        if (++o > len)                  return WAIT;
        ch = pipe->next();
        if ('*' == ch)                  break; // crc delimiter 
        if (!isprint(ch))               return NOT_FOUND; 
        c ^= ch;
    }
    if (++o > len)                      return WAIT;
    ch = toHex[(c >> 4) & 0xF]; // high nibble
    if (ch != pipe->next())             return NOT_FOUND;
    if (++o > len)                      return WAIT;
    ch = toHex[(c >> 0) & 0xF]; // low nibble
    if (ch != pipe->next())             return NOT_FOUND;
    if (++o > len)                      return WAIT;
    if ('\r' != pipe->next())           return NOT_FOUND;
    if (++o > len)                      return WAIT;
    if ('\n' != pipe->next())           return NOT_FOUND;
    return o;
}

int GPSParser::_parseUbx(Pipe<char>* pipe, int l)
{
    int o = 0;
    pipe->start();
    if (++o > l)                return WAIT;
    if ('\xB5' != pipe->next()) return NOT_FOUND;   
    if (++o > l)                return WAIT;
    if ('\x62' != pipe->next()) return NOT_FOUND;
    o += 4;
    if (o > l)                  return WAIT;
    int i,j,ca,cb;
    i = pipe->next(); ca  = i; cb  = ca; // cls
    i = pipe->next(); ca += i; cb += ca; // id
    i = pipe->next(); ca += i; cb += ca; // len_lsb
    j = pipe->next(); ca += j; cb += ca; // len_msb 
    j = i + (j << 8);
    while (j--)
    {
        if (++o > l)            return WAIT;
        i = pipe->next(); 
        ca += i; 
        cb += ca;
    }
    ca &= 0xFF; cb &= 0xFF;
    if (++o > l)                return WAIT;
    if (ca != pipe->next())     return NOT_FOUND;
    if (++o > l)                return WAIT;
    if (cb != pipe->next())     return NOT_FOUND;
    return o;
}

int GPSParser::send(const char* buf, int len)
{
    return _send(buf, len);
}

int GPSParser::sendNmea(const char* buf, int len)
{
    char head[1] = { '$' };
    char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
    int i;
    int crc = 0;
    for (i = 0; i < len; i ++)
        crc ^= *buf++;
    i  = _send(head, sizeof(head));
    i += _send(buf, len);
    tail[1] = toHex[(crc > 4) & 0xF0];
    tail[2] = toHex[(crc > 0) & 0x0F];
    i += _send(tail, sizeof(tail));
    return i;
}

int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
{
    char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 };
    char crc[2];
    int i;
    int ca = 0;
    int cb = 0;
    for (i = 2; i < 6; i ++)
    {
        ca += head[i];
        cb += ca;
    }
    for (i = 0; i < len; i ++)
    {
        ca += ((char*)buf)[i];
        cb += ca;
    }
    i  = _send(head, sizeof(head));
    i += _send(buf, len);
    crc[0] = ca & 0xFF;
    crc[1] = cb & 0xFF;
    i += _send(crc,  sizeof(crc));
    return i;
}

const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end)
{
    // find the start
    for (; (start < end) && (ix > 0); start ++)
    {
        if (*start == ',')
            ix --;
    }
    // found and check bounds
    if ((ix == 0) && (start < end) && 
        (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
        return start;
    else 
        return NULL;
}

bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val)
{
    char* end = &buf[len];
    const char* pos = findNmeaItemPos(ix, buf, end);
    // find the start
    if (!pos)
        return false;
    val = strtod(pos, &end);
    // restore the last character
    return (end > pos);
}

bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
{
    char* end = &buf[len];
    const char* pos = findNmeaItemPos(ix, buf, end);
    // find the start
    if (!pos)
        return false;
    val = (int)strtol(pos, &end, base);
    return (end > pos);
}

bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val)
{
    const char* end = &buf[len];
    const char* pos = findNmeaItemPos(ix, buf, end);
    // find the start
    if (!pos)
        return false;
    // skip leading spaces
    while ((pos < end) && isspace(*pos))
        pos++;
    // check bound
    if ((pos < end) && 
        (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
    {
        val = *pos;
        return true;
    }
    return false;
}

const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };

// ----------------------------------------------------------------
// Serial Implementation 
// ----------------------------------------------------------------

GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/) : 
            Serial(tx, rx, "gps"), _pipe(256)
{
    attach(this, &GPSSerial::serialRxIrq, RxIrq);
    baud(baudrate);
}

GPSSerial::~GPSSerial(void)
{
    attach(NULL, RxIrq);
}

void GPSSerial::serialRxIrq(void)
{
    while (serial_readable(&_serial))
        _pipe.putc(serial_getc(&_serial));
}

int GPSSerial::getMessage(char* buf, int len)
{
    return _getMessage(&_pipe, buf, len);   
}

char GPSSerial::next(void)
{ 
    return _pipe.next(); 
}

int GPSSerial::_send(const void* buf, int len)   
{ 
    for (int i = 0; i < len; i ++)
        putc(((char*)buf)[i]); 
    return len; 
}

// ----------------------------------------------------------------
// I2C Implementation 
// ----------------------------------------------------------------

GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/) : 
            I2C(sda,scl),
            _pipe(256)
{
    found = false;
}

bool GPSI2C::detect(void)
{
    if (!found)
    {
        int w = I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM));
        if (w == 0)
            found = true;
    }
    return found;
}

int GPSI2C::getMessage(char* buf, int len)
{
    // fill the pipe
    int sz = _pipe.free();
    if (sz) 
        sz = _get(buf, sz);
    if (sz) 
        _pipe.put(buf, sz);
    // now parse it
    return _getMessage(&_pipe, buf, len);   
}

int GPSI2C::send(const char* buf, int len)
{
    int sent = 0;
    if (len) 
    {
        if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
            sent = _send(buf, len);
        found = len == sent;
        stop();
    }
    return sent;
}

int GPSI2C::sendNmea(const char* buf, int len)
{ 
    int sent = 0;
    if (len) 
    {
        if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
            sent = GPSParser::sendNmea(buf, len);
        found = len == sent;
        stop();
    }
    return sent;
}

int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
{ 
    int sent = 0;
    if (len) 
    {
        if (!I2C::write(GPSADR,&REGSTREAM,sizeof(REGSTREAM),true))
            sent = GPSParser::sendUbx(cls, id, buf, len);
        found = (len == sent);
        stop();
    }
    return sent;
}

int GPSI2C::_get(char* buf, int len)
{
    int read = 0;
    unsigned char sz[2];
    if (!I2C::write(GPSADR,&REGLEN,sizeof(REGLEN),true) && 
        !I2C::read(GPSADR,(char*)sz,sizeof(sz),true))
    {
        int size = 256 * (int)sz[0] + sz[1];
        if (size > len)
            size = len;
        if (size > 0)
            read = !I2C::read(GPSADR,buf,size, true) ? size : 0;
        stop();
        found = read = size;
    }
    else 
        found = false;
    I2C::stop();
    return read;
}

char GPSI2C::next(void)                       
{ 
    return _pipe.next(); 
}

int GPSI2C::_send(const void* buf, int len)
{ 
    return !I2C::write(GPSADR,(const char*)buf,len,true) ? len : 0; 
}

const char GPSI2C::REGLEN    = 0xFD;
const char GPSI2C::REGSTREAM = 0xFF;