Diff: C027_Support/GPS.cpp

Revision:

3:002503ab7199

Parent:

2:bc413454a83e

--- a/C027_Support/GPS.cpp	Mon Jul 28 15:45:16 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,354 +0,0 @@
-#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();
-    int fr = pipe->free();
-    if (len > sz)
-        len = sz;
-    while (len > 0)
-    {
-        // NMEA protocol
-        pipe->set(unkn);
-        int nmea = _parseNmea(pipe,len);
-        if ((nmea != NOT_FOUND) && (unkn > 0))  
-            return UNKNOWN | pipe->get(buf,unkn);
-        if (nmea == WAIT && fr)                       
-            return WAIT;
-        if (nmea > 0)                           
-            return NMEA | pipe->get(buf,nmea);
-        // UBX protocol
-        
-        pipe->set(unkn);
-        int ubx = _parseUbx(pipe,len);
-        if ((ubx != NOT_FOUND) && (unkn > 0))   
-            return UNKNOWN | pipe->get(buf,unkn);
-        if (ubx == WAIT && fr)                        
-            return WAIT;
-        if (ubx > 0)                            
-            return UBX | pipe->get(buf,ubx);
-        
-        // UNKNOWN
-        unkn ++;
-        len--;
-    }
-    if (unkn > 0)                      
-        return UNKNOWN | pipe->get(buf,unkn); 
-    return WAIT;
-}
-
-int GPSParser::_parseNmea(Pipe<char>* pipe, int len)
-{
-    int o = 0;
-    int c = 0;
-    char ch;
-    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;
-    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);
-}
-
-void GPSParser::powerOff(void)
-{
-    // set the gps into backup mode using the command RMX-LPREQ
-    struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/};
-    sendUbx(0x02, 0x41, &msg, sizeof(msg));
-}
-
-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 /*= NULL*/, int len /*= 0*/)
-{
-    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;
-}
-
-bool GPSParser::getNmeaAngle(int ix, char* buf, int len, double& val)
-{
-    char ch;
-    if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && 
-        ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
-    {
-        val *= 0.01;
-        int i = (int)val;
-        val = (val - i) / 0.6 + i;
-        if (ch == 'S' || ch == 'W')
-            val = -val;
-        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*/,
-            int rxSize /*= 256*/, int txSize /*= 128*/) : 
-            SerialPipe(tx, rx, rxSize, txSize)
-{
-    baud(baudrate);
-}
-
-int GPSSerial::getMessage(char* buf, int len)
-{
-    return _getMessage(&_pipeRx, buf, len);   
-}
-
-int GPSSerial::_send(const void* buf, int len)   
-{ 
-    return put((const char*)buf, len, true/*=blocking*/); 
-}
-
-// ----------------------------------------------------------------
-// I2C Implementation 
-// ----------------------------------------------------------------
-
-GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/,
-            unsigned char i2cAdr /*=GPSADR*/, int rxSize /*= 256*/) : 
-            I2C(sda,scl),
-            _pipe(rxSize),
-            _i2cAdr(i2cAdr)
-{
-    frequency(100000);
-    found = false;
-}
-
-bool GPSI2C::detect(void)
-{
-    if (!found)
-    {
-        int w = I2C::write(_i2cAdr,&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(_i2cAdr,&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 (!I2C::write(_i2cAdr,&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 (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
-        sent = GPSParser::sendUbx(cls, id, buf, len);
-    found = (len == sent);
-    I2C::stop();
-    return sent;
-}
-
-int GPSI2C::_get(char* buf, int len)
-{
-    int read = 0;
-    unsigned char sz[2] = {0,0};
-    if (!I2C::write(_i2cAdr,&REGLEN,sizeof(REGLEN),true) && 
-        !I2C::read(_i2cAdr,(char*)sz,sizeof(sz)))
-    {
-        int size = 256 * (int)sz[0] + sz[1];
-        if (size > len)
-            size = len;
-        if (size > 0) 
-        {
-            if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true) &&
-                !I2C::read(_i2cAdr,buf,size)) {
-                read = size;
-            }
-            else 
-                found = false;
-        }
-    }
-    else 
-        found = false;
-    return read;
-}
-
-int GPSI2C::_send(const void* buf, int len)
-{ 
-    return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; 
-}
-
-const char GPSI2C::REGLEN    = 0xFD;
-const char GPSI2C::REGSTREAM = 0xFF;