GPS.cpp

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