gnss.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2017 u-blox
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 /**
00018  * @file gnss.cpp
00019  * This file defines a class that communicates with a u-blox GNSS chip.
00020  */
00021 
00022 #include "mbed.h"
00023 #include "ctype.h"
00024 #include "gnss.h"
00025 
00026 GnssParser::GnssParser(void)
00027 {
00028     // Create the enable pin but set everything to disabled
00029     _gnssEnable = NULL;
00030     
00031 #if GNSSEN != NC
00032 #  ifdef TARGET_UBLOX_C030
00033     _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PushPullNoPull, 0);
00034 #  else
00035     _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PullNone, 1);
00036 #  endif
00037 #endif
00038 }
00039 
00040 GnssParser::~GnssParser(void)
00041 {
00042     if (_gnssEnable != NULL) {
00043         *_gnssEnable = 0;
00044         delete _gnssEnable;
00045     }
00046 }
00047 
00048 void GnssParser::powerOff(void)
00049 {
00050     // Set the GNSS into backup mode using the command RMX-LPREQ
00051     struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/};
00052     sendUbx(0x02, 0x41, &msg, sizeof(msg));
00053 }
00054 
00055 void GnssParser::_powerOn(void)
00056 {
00057     if (_gnssEnable != NULL) {
00058        *_gnssEnable = 1;
00059     }
00060     wait_ms (1);
00061 }
00062 
00063 int GnssParser::_getMessage(Pipe<char> * pipe, char* buf, int len)
00064 {
00065     int unkn = 0;
00066     int sz = pipe->size();
00067     int fr = pipe->free();
00068     if (len > sz)
00069         len = sz;
00070     while (len > 0)
00071     {
00072         // NMEA protocol
00073         pipe->set(unkn);
00074         int nmea = _parseNmea(pipe,len);
00075         if ((nmea != NOT_FOUND) && (unkn > 0))  
00076             return UNKNOWN | pipe->get (buf,unkn);
00077         if (nmea == WAIT && fr)                       
00078             return WAIT;
00079         if (nmea > 0)                           
00080             return NMEA | pipe->get (buf,nmea);
00081         // UBX protocol
00082         
00083         pipe->set(unkn);
00084         int ubx = _parseUbx(pipe,len);
00085         if ((ubx != NOT_FOUND) && (unkn > 0))   
00086             return UNKNOWN | pipe->get (buf,unkn);
00087         if (ubx == WAIT && fr)                        
00088             return WAIT;
00089         if (ubx > 0)                            
00090             return UBX | pipe->get (buf,ubx);
00091         
00092         // UNKNOWN
00093         unkn ++;
00094         len--;
00095     }
00096     if (unkn > 0)                      
00097         return UNKNOWN | pipe->get (buf,unkn); 
00098     return WAIT;
00099 }
00100 
00101 int GnssParser::_parseNmea(Pipe<char> * pipe, int len)
00102 {
00103     int o = 0;
00104     int c = 0;
00105     char ch;
00106     if (++o > len)                      return WAIT;
00107     if ('$' != pipe->next())            return NOT_FOUND;
00108     // This needs to be extended by crc checking 
00109     for (;;)
00110     {
00111         if (++o > len)                  return WAIT;
00112         ch = pipe->next();
00113         if ('*' == ch)                  break; // crc delimiter 
00114         if (!isprint(ch))               return NOT_FOUND; 
00115         c ^= ch;
00116     }
00117     if (++o > len)                      return WAIT;
00118     ch = _toHex[(c >> 4) & 0xF]; // high nibble
00119     if (ch != pipe->next())             return NOT_FOUND;
00120     if (++o > len)                      return WAIT;
00121     ch = _toHex[(c >> 0) & 0xF]; // low nibble
00122     if (ch != pipe->next())             return NOT_FOUND;
00123     if (++o > len)                      return WAIT;
00124     if ('\r' != pipe->next())           return NOT_FOUND;
00125     if (++o > len)                      return WAIT;
00126     if ('\n' != pipe->next())           return NOT_FOUND;
00127     return o;
00128 }
00129 
00130 int GnssParser::_parseUbx(Pipe<char> * pipe, int l)
00131 {
00132     int o = 0;
00133     if (++o > l)                return WAIT;
00134     if ('\xB5' != pipe->next()) return NOT_FOUND;   
00135     if (++o > l)                return WAIT;
00136     if ('\x62' != pipe->next()) return NOT_FOUND;
00137     o += 4;
00138     if (o > l)                  return WAIT;
00139     int i,j,ca,cb;
00140     i = pipe->next(); ca  = i; cb  = ca; // cls
00141     i = pipe->next(); ca += i; cb += ca; // id
00142     i = pipe->next(); ca += i; cb += ca; // len_lsb
00143     j = pipe->next(); ca += j; cb += ca; // len_msb 
00144     j = i + (j << 8);
00145     while (j--)
00146     {
00147         if (++o > l)            return WAIT;
00148         i = pipe->next(); 
00149         ca += i; 
00150         cb += ca;
00151     }
00152     ca &= 0xFF; cb &= 0xFF;
00153     if (++o > l)                return WAIT;
00154     if (ca != pipe->next())     return NOT_FOUND;
00155     if (++o > l)                return WAIT;
00156     if (cb != pipe->next())     return NOT_FOUND;
00157     return o;
00158 }
00159 
00160 int GnssParser::send(const char* buf, int len)
00161 {
00162     return _send(buf, len);
00163 }
00164 
00165 int GnssParser::sendNmea(const char* buf, int len)
00166 {
00167     char head[1] = { '$' };
00168     char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
00169     int i;
00170     int crc = 0;
00171     for (i = 0; i < len; i ++)
00172         crc ^= *buf++;
00173     i  = _send(head, sizeof(head));
00174     i += _send(buf, len);
00175     tail[1] = _toHex[(crc > 4) & 0xF0];
00176     tail[2] = _toHex[(crc > 0) & 0x0F];
00177     i += _send(tail, sizeof(tail));
00178     return i;
00179 }
00180 
00181 int GnssParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/)
00182 {
00183     char head[6] = { 0xB5, 0x62, cls, id, (char) len, (char) (len >> 8)};
00184     char crc[2];
00185     int i;
00186     int ca = 0;
00187     int cb = 0;
00188     for (i = 2; i < 6; i ++)
00189     {
00190         ca += head[i];
00191         cb += ca;
00192     }
00193     for (i = 0; i < len; i ++)
00194     {
00195         ca += ((char*)buf)[i];
00196         cb += ca; 
00197     }
00198     i  = _send(head, sizeof(head));
00199     i += _send(buf, len);
00200     crc[0] = ca & 0xFF;
00201     crc[1] = cb & 0xFF;
00202     i += _send(crc,  sizeof(crc));
00203     return i;
00204 }
00205 
00206 const char* GnssParser::findNmeaItemPos(int ix, const char* start, const char* end)
00207 {
00208     // Find the start
00209     for (; (start < end) && (ix > 0); start ++)
00210     {
00211         if (*start == ',')
00212             ix --;
00213     }
00214     // Found and check bounds
00215     if ((ix == 0) && (start < end) && 
00216         (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
00217         return start;
00218     else 
00219         return NULL;
00220 }
00221 
00222 bool GnssParser::getNmeaItem(int ix, char* buf, int len, double& val)
00223 {
00224     char* end = &buf[len];
00225     const char* pos = findNmeaItemPos(ix, buf, end);
00226     // Find the start
00227     if (!pos)
00228         return false;
00229     val = strtod(pos, &end);
00230     // Restore the last character
00231     return (end > pos);
00232 }
00233 
00234 bool GnssParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
00235 {
00236     char* end = &buf[len];
00237     const char* pos = findNmeaItemPos(ix, buf, end);
00238     // Find the start
00239     if (!pos)
00240         return false;
00241     val = (int)strtol(pos, &end, base);
00242     return (end > pos);
00243 }
00244 
00245 bool GnssParser::getNmeaItem(int ix, char* buf, int len, char& val)
00246 {
00247     const char* end = &buf[len];
00248     const char* pos = findNmeaItemPos(ix, buf, end);
00249     // Find the start
00250     if (!pos)
00251         return false;
00252     // Skip leading spaces
00253     while ((pos < end) && isspace(*pos))
00254         pos++;
00255     // Check bound
00256     if ((pos < end) && 
00257         (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
00258     {
00259         val = *pos;
00260         return true;
00261     }
00262     return false;
00263 }
00264 
00265 bool GnssParser::getNmeaAngle(int ix, char* buf, int len, double& val)
00266 {
00267     char ch;
00268     if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && 
00269         ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
00270     {
00271         val *= 0.01;
00272         int i = (int)val;
00273         val = (val - i) / 0.6 + i;
00274         if (ch == 'S' || ch == 'W')
00275             val = -val;
00276         return true;
00277     }
00278     return false;
00279 }
00280                 
00281 const char GnssParser::_toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
00282 
00283 // ----------------------------------------------------------------
00284 // Serial Implementation 
00285 // ----------------------------------------------------------------
00286 
00287 GnssSerial::GnssSerial(PinName tx /*= GNSSTXD  */, PinName rx /*= GNSSRXD */, int baudrate /*= GNSSBAUD */,
00288                        int rxSize /*= 256 */, int txSize /*= 128 */) :
00289             SerialPipe(tx, rx, baudrate, rxSize, txSize)
00290 {
00291     baud(baudrate);
00292 }
00293 
00294 GnssSerial::~GnssSerial(void)
00295 {
00296     powerOff();
00297 }
00298 
00299 bool GnssSerial::init(PinName pn)
00300 {
00301     Timer timer;
00302     int size;
00303     
00304     // Unused (kept only for compatibility with the I2C version)
00305     (void)pn;
00306     
00307     // Power up and enable the module
00308     _powerOn();
00309 
00310     // Send a byte to wakup the device again
00311     putc(0xFF);
00312     // Wait until we get some bytes
00313     size = _pipeRx.size();
00314     timer.start();
00315     while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) {
00316         /* Nothing, just wait */
00317     }
00318     timer.stop();
00319     
00320     return (size != _pipeRx.size());
00321 }
00322 
00323 int GnssSerial::getMessage(char* buf, int len)
00324 {
00325     return _getMessage(&_pipeRx, buf, len);   
00326 }
00327 
00328 int GnssSerial::_send(const void* buf, int len)
00329 { 
00330     return put((const char*)buf, len, true/*=blocking*/); 
00331 }
00332 
00333 // ----------------------------------------------------------------
00334 // I2C Implementation 
00335 // ----------------------------------------------------------------
00336 
00337 GnssI2C::GnssI2C(PinName sda /*= NC */, PinName scl /*= NC */,
00338                unsigned char i2cAdr /*= (66<<1) */, int rxSize /*= 256 */) :
00339                I2C(sda,scl),
00340                _pipe(rxSize),
00341                _i2cAdr(i2cAdr)
00342 {
00343     frequency(100000);
00344 }
00345 
00346 GnssI2C::~GnssI2C(void)
00347 {
00348     powerOff();
00349 }
00350 
00351 bool GnssI2C::init(PinName pn)
00352 {
00353     // Power up and enable the module
00354     _powerOn();
00355 
00356     if (pn != NC) {
00357         DigitalOut pin(pn, 0);
00358         ::wait_us(1);
00359         pin = 1;
00360         ::wait_ms(100);
00361     }
00362     return !I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM));
00363 }
00364 
00365 int GnssI2C::getMessage(char* buf, int len)
00366 {
00367     // Fill the pipe
00368     int sz = _pipe.free();
00369     if (sz) 
00370         sz = _get(buf, sz);
00371     if (sz) 
00372         _pipe.put(buf, sz);
00373     // Now parse it
00374     return _getMessage(&_pipe, buf, len);   
00375 }
00376 
00377 int GnssI2C::send(const char* buf, int len)
00378 {
00379     int sent = 0;
00380     if (len) 
00381     {
00382         if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00383             sent = send(buf, len);
00384         stop();
00385     }
00386     return sent;
00387 }
00388 
00389 int GnssI2C::sendNmea(const char* buf, int len)
00390 { 
00391     int sent = 0;
00392     if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00393         sent = GnssParser::sendNmea(buf, len);
00394     stop();
00395     return sent;
00396 }
00397 
00398 int GnssI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
00399 { 
00400     int sent = 0;
00401     if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00402         sent = GnssParser::sendUbx(cls, id, buf, len);
00403     I2C::stop();
00404     return sent;
00405 }
00406 
00407 int GnssI2C::_get(char* buf, int len)
00408 {
00409     int read = 0;
00410     unsigned char sz[2] = {0,0};
00411     if (!I2C::write(_i2cAdr,&REGLEN,sizeof(REGLEN),true) && 
00412         !I2C::read(_i2cAdr,(char*)sz,sizeof(sz)))
00413     {
00414         int size = 256 * (int)sz[0] + sz[1];
00415         if (size > len)
00416             size = len;
00417         if (size > 0) 
00418         {
00419             if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true) &&
00420                 !I2C::read(_i2cAdr,buf,size)) {
00421                 read = size;
00422             }
00423         }
00424     }
00425     return read;
00426 }
00427 
00428 int GnssI2C::_send(const void* buf, int len)
00429 { 
00430     return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; 
00431 }
00432 
00433 const char GnssI2C::REGLEN    = 0xFD;
00434 const char GnssI2C::REGSTREAM = 0xFF;
00435 
00436 // End Of File