GPS.cpp

00001 #include "mbed.h"
00002 #include <ctype.h>
00003 #include "GPS.h"
00004 #ifdef TARGET_UBLOX_C027
00005  #include "C027_api.h"
00006 #endif
00007 
00008 void GPSParser::powerOff(void)
00009 {
00010     // set the gps into backup mode using the command RMX-LPREQ
00011     //struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/};
00012     // updated for M8
00013     struct { unsigned long dur; unsigned long flags; } msg = {0, 2};
00014     sendUbx(0x02, 0x41, &msg, sizeof(msg));
00015     
00016 }
00017 
00018 int GPSParser::_getMessage(Pipe<char> * pipe, char* buf, int len)
00019 {
00020     int unkn = 0;
00021     int sz = pipe->size();
00022     int fr = pipe->free();
00023     if (len > sz)
00024         len = sz;
00025     while (len > 0)
00026     {
00027         // NMEA protocol
00028         pipe->set(unkn);
00029         int nmea = _parseNmea(pipe,len);
00030         if ((nmea != NOT_FOUND) && (unkn > 0))  
00031             return UNKNOWN | pipe->get (buf,unkn);
00032         if (nmea == WAIT && fr)                       
00033             return WAIT;
00034         if (nmea > 0)                           
00035             return NMEA | pipe->get (buf,nmea);
00036         // UBX protocol
00037         
00038         pipe->set(unkn);
00039         int ubx = _parseUbx(pipe,len);
00040         if ((ubx != NOT_FOUND) && (unkn > 0))   
00041             return UNKNOWN | pipe->get (buf,unkn);
00042         if (ubx == WAIT && fr)                        
00043             return WAIT;
00044         if (ubx > 0)                            
00045             return UBX | pipe->get (buf,ubx);
00046         
00047         // UNKNOWN
00048         unkn ++;
00049         len--;
00050     }
00051     if (unkn > 0)                      
00052         return UNKNOWN | pipe->get (buf,unkn); 
00053     return WAIT;
00054 }
00055 
00056 int GPSParser::_parseNmea(Pipe<char> * pipe, int len)
00057 {
00058     int o = 0;
00059     int c = 0;
00060     char ch;
00061     if (++o > len)                      return WAIT;
00062     if ('$' != pipe->next())            return NOT_FOUND;
00063     // this needs to be extended by crc checking 
00064     for (;;)
00065     {
00066         if (++o > len)                  return WAIT;
00067         ch = pipe->next();
00068         if ('*' == ch)                  break; // crc delimiter 
00069         if (!isprint(ch))               return NOT_FOUND; 
00070         c ^= ch;
00071     }
00072     if (++o > len)                      return WAIT;
00073     ch = toHex[(c >> 4) & 0xF]; // high nibble
00074     if (ch != pipe->next())             return NOT_FOUND;
00075     if (++o > len)                      return WAIT;
00076     ch = toHex[(c >> 0) & 0xF]; // low nibble
00077     if (ch != pipe->next())             return NOT_FOUND;
00078     if (++o > len)                      return WAIT;
00079     if ('\r' != pipe->next())           return NOT_FOUND;
00080     if (++o > len)                      return WAIT;
00081     if ('\n' != pipe->next())           return NOT_FOUND;
00082     return o;
00083 }
00084 
00085 int GPSParser::_parseUbx(Pipe<char> * pipe, int l)
00086 {
00087     int o = 0;
00088     if (++o > l)                return WAIT;
00089     if ('\xB5' != pipe->next()) return NOT_FOUND;   
00090     if (++o > l)                return WAIT;
00091     if ('\x62' != pipe->next()) return NOT_FOUND;
00092     o += 4;
00093     if (o > l)                  return WAIT;
00094     int i,j,ca,cb;
00095     i = pipe->next(); ca  = i; cb  = ca; // cls
00096     i = pipe->next(); ca += i; cb += ca; // id
00097     i = pipe->next(); ca += i; cb += ca; // len_lsb
00098     j = pipe->next(); ca += j; cb += ca; // len_msb 
00099     j = i + (j << 8);
00100     while (j--)
00101     {
00102         if (++o > l)            return WAIT;
00103         i = pipe->next(); 
00104         ca += i; 
00105         cb += ca;
00106     }
00107     ca &= 0xFF; cb &= 0xFF;
00108     if (++o > l)                return WAIT;
00109     if (ca != pipe->next())     return NOT_FOUND;
00110     if (++o > l)                return WAIT;
00111     if (cb != pipe->next())     return NOT_FOUND;
00112     return o;
00113 }
00114 
00115 int GPSParser::send(const char* buf, int len)
00116 {
00117     return _send(buf, len);
00118 }
00119 
00120 int GPSParser::sendNmea(const char* buf, int len)
00121 {
00122     char head[1] = { '$' };
00123     char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
00124     int i;
00125     int crc = 0;
00126     for (i = 0; i < len; i ++)
00127         crc ^= *buf++;
00128     i  = _send(head, sizeof(head));
00129     i += _send(buf, len);
00130     tail[1] = toHex[(crc > 4) & 0xF0];
00131     tail[2] = toHex[(crc > 0) & 0x0F];
00132     i += _send(tail, sizeof(tail));
00133     return i;
00134 }
00135 
00136 int GPSParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/)
00137 {
00138     char head[6] = { 0xB5, 0x62, cls, id, len >> 0, len >> 8 };
00139     char crc[2];
00140     int i;
00141     int ca = 0;
00142     int cb = 0;
00143     for (i = 2; i < 6; i ++)
00144     {
00145         ca += head[i];
00146         cb += ca;
00147     }
00148     for (i = 0; i < len; i ++)
00149     {
00150         ca += ((char*)buf)[i];
00151         cb += ca; 
00152     }
00153     i  = _send(head, sizeof(head));
00154     i += _send(buf, len);
00155     crc[0] = ca & 0xFF;
00156     crc[1] = cb & 0xFF;
00157     i += _send(crc,  sizeof(crc));
00158     return i;
00159 }
00160 
00161 const char* GPSParser::findNmeaItemPos(int ix, const char* start, const char* end)
00162 {
00163     // find the start
00164     for (; (start < end) && (ix > 0); start ++)
00165     {
00166         if (*start == ',')
00167             ix --;
00168     }
00169     // found and check bounds
00170     if ((ix == 0) && (start < end) && 
00171         (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
00172         return start;
00173     else 
00174         return NULL;
00175 }
00176 
00177 bool GPSParser::getNmeaItem(int ix, char* buf, int len, double& val)
00178 {
00179     char* end = &buf[len];
00180     const char* pos = findNmeaItemPos(ix, buf, end);
00181     // find the start
00182     if (!pos)
00183         return false;
00184     val = strtod(pos, &end);
00185     // restore the last character
00186     return (end > pos);
00187 }
00188 
00189 bool GPSParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
00190 {
00191     char* end = &buf[len];
00192     const char* pos = findNmeaItemPos(ix, buf, end);
00193     // find the start
00194     if (!pos)
00195         return false;
00196     val = (int)strtol(pos, &end, base);
00197     return (end > pos);
00198 }
00199 
00200 bool GPSParser::getNmeaItem(int ix, char* buf, int len, char& val)
00201 {
00202     const char* end = &buf[len];
00203     const char* pos = findNmeaItemPos(ix, buf, end);
00204     // find the start
00205     if (!pos)
00206         return false;
00207     // skip leading spaces
00208     while ((pos < end) && isspace(*pos))
00209         pos++;
00210     // check bound
00211     if ((pos < end) && 
00212         (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
00213     {
00214         val = *pos;
00215         return true;
00216     }
00217     return false;
00218 }
00219 
00220 bool GPSParser::getNmeaAngle(int ix, char* buf, int len, double& val)
00221 {
00222     char ch;
00223     if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && 
00224         ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
00225     {
00226         val *= 0.01;
00227         int i = (int)val;
00228         val = (val - i) / 0.6 + i;
00229         if (ch == 'S' || ch == 'W')
00230             val = -val;
00231         return true;
00232     }
00233     return false;
00234 }
00235                 
00236 const char GPSParser::toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
00237 
00238 // ----------------------------------------------------------------
00239 // Serial Implementation 
00240 // ----------------------------------------------------------------
00241 
00242 GPSSerial::GPSSerial(PinName tx /*= GPSTXD*/, PinName rx /*= GPSRXD*/, int baudrate /*= GPSBAUD*/,
00243             int rxSize /*= 256*/, int txSize /*= 128*/) : 
00244             SerialPipe(tx, rx, rxSize, txSize)
00245 {
00246     baud(baudrate);
00247 #ifdef TARGET_UBLOX_C027
00248     _onboard = (tx == GPSTXD) || (rx == GPSRXD);
00249     if (_onboard)
00250         c027_gps_powerOn(); 
00251 #endif
00252 }
00253 
00254 GPSSerial::~GPSSerial(void)
00255 {
00256     powerOff();
00257 #ifdef TARGET_UBLOX_C027
00258     if (_onboard)
00259          c027_gps_powerOff();
00260 #endif
00261 }
00262 
00263 bool GPSSerial::init(PinName pn)
00264 {
00265     // send a byte to wakup the device again
00266     putc(0xFF);
00267     // wait until we get some bytes
00268     int size = _pipeRx.size();
00269     Timer timer;
00270     timer.start();
00271     while ((100 > timer.read_ms()) && (size == _pipeRx.size()))
00272         /* nothing / just wait */;
00273     return (size != _pipeRx.size());
00274 }
00275 
00276 int GPSSerial::getMessage(char* buf, int len)
00277 {
00278     return _getMessage(&_pipeRx, buf, len);   
00279 }
00280 
00281 int GPSSerial::_send(const void* buf, int len)   
00282 { 
00283     return put((const char*)buf, len, true/*=blocking*/); 
00284 }
00285 
00286 // ----------------------------------------------------------------
00287 // I2C Implementation 
00288 // ----------------------------------------------------------------
00289 
00290 GPSI2C::GPSI2C(PinName sda /*= GPSSDA*/, PinName scl /*= GPSSCL*/,
00291             unsigned char i2cAdr /*=GPSADR*/, int rxSize /*= 256*/) : 
00292             I2C(sda,scl),
00293             _pipe(rxSize),
00294             _i2cAdr(i2cAdr)
00295 {
00296     frequency(100000);
00297 #ifdef TARGET_UBLOX_C027
00298     _onboard = (sda == GPSSDA) && (scl == GPSSCL);
00299     if (_onboard)
00300         c027_gps_powerOn(); 
00301 #endif
00302 }
00303 
00304 GPSI2C::~GPSI2C(void)
00305 {
00306     powerOff();
00307 #ifdef TARGET_UBLOX_C027
00308     if (_onboard)
00309          c027_gps_powerOff();
00310 #endif
00311 }
00312 
00313 bool GPSI2C::init(PinName pn)
00314 {
00315     if (pn != NC) {
00316         DigitalOut pin(pn, 0);
00317         ::wait_us(1);
00318         pin = 1;
00319         ::wait_ms(100);
00320     }
00321     return !I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM));
00322 }
00323 
00324 int GPSI2C::getMessage(char* buf, int len)
00325 {
00326     // fill the pipe
00327     int sz = _pipe.free();
00328     if (sz) 
00329         sz = _get(buf, sz);
00330     if (sz) 
00331         _pipe.put(buf, sz);
00332     // now parse it
00333     return _getMessage(&_pipe, buf, len);   
00334 }
00335 
00336 int GPSI2C::send(const char* buf, int len)
00337 {
00338     int sent = 0;
00339     if (len) 
00340     {
00341         if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00342             sent = send(buf, len);
00343         stop();
00344     }
00345     return sent;
00346 }
00347 
00348 int GPSI2C::sendNmea(const char* buf, int len)
00349 { 
00350     int sent = 0;
00351     if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00352         sent = GPSParser::sendNmea(buf, len);
00353     stop();
00354     return sent;
00355 }
00356 
00357 int GPSI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
00358 { 
00359     int sent = 0;
00360     if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00361         sent = GPSParser::sendUbx(cls, id, buf, len);
00362     I2C::stop();
00363     return sent;
00364 }
00365 
00366 int GPSI2C::_get(char* buf, int len)
00367 {
00368     int read = 0;
00369     unsigned char sz[2] = {0,0};
00370     if (!I2C::write(_i2cAdr,&REGLEN,sizeof(REGLEN),true) && 
00371         !I2C::read(_i2cAdr,(char*)sz,sizeof(sz)))
00372     {
00373         int size = 256 * (int)sz[0] + sz[1];
00374         if (size > len)
00375             size = len;
00376         if (size > 0) 
00377         {
00378             if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true) &&
00379                 !I2C::read(_i2cAdr,buf,size)) {
00380                 read = size;
00381             }
00382         }
00383     }
00384     return read;
00385 }
00386 
00387 int GPSI2C::_send(const void* buf, int len)
00388 { 
00389     return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; 
00390 }
00391 
00392 const char GPSI2C::REGLEN    = 0xFD;
00393 const char GPSI2C::REGSTREAM = 0xFF;