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