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