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 #ifdef UBLOX_WEARABLE_FRAMEWORK
00027 #include "SDCardModel.h"
00028 #else
00029 #define SEND_LOGGING_MESSAGE printf
00030 #endif
00031 
00032 GnssParser::GnssParser(void)
00033 {
00034     // Create the enable pin but set everything to disabled
00035     _gnssEnable = NULL;
00036     
00037 #ifdef TARGET_UBLOX_C030
00038     _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PushPullNoPull, 0);
00039 #else
00040     _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PullNone, 1);
00041 #endif
00042 }
00043 
00044 GnssParser::~GnssParser(void)
00045 {
00046     if (_gnssEnable != NULL) {
00047         *_gnssEnable = 0;
00048         delete _gnssEnable;
00049     }
00050 }
00051 
00052 void GnssParser::powerOff(void)
00053 {
00054     // Set the GNSS into backup mode using the command RMX-LPREQ
00055     struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/};
00056     sendUbx(0x02, 0x41, &msg, sizeof(msg));
00057 }
00058 
00059 void GnssParser::cutOffPower(void)
00060 {
00061     //Disabling PA15 to cut off power supply
00062     if (_gnssEnable != NULL)
00063         *_gnssEnable = 0;
00064     wait_ms(1);
00065 }
00066 
00067 void GnssParser::_powerOn(void)
00068 {
00069     if (_gnssEnable != NULL) {
00070        *_gnssEnable = 1;
00071     }
00072     wait_ms (1);
00073 }
00074 
00075 int GnssParser::_getMessage(Pipe<char> * pipe, char* buf, int len)
00076 {
00077     int unkn = 0;
00078     int sz = pipe->size();
00079     int fr = pipe->free();
00080     if (len > sz)
00081         len = sz;
00082     while (len > 0)
00083     {
00084         // NMEA protocol
00085         pipe->set(unkn);
00086         int nmea = _parseNmea(pipe,len);
00087         if ((nmea != NOT_FOUND) && (unkn > 0))  
00088             return UNKNOWN | pipe->get (buf,unkn);
00089         if (nmea == WAIT && fr)                       
00090             return WAIT;
00091         if (nmea > 0)                           
00092             return NMEA | pipe->get (buf,nmea);
00093         // UBX protocol
00094         
00095         pipe->set(unkn);
00096         int ubx = _parseUbx(pipe,len);
00097         if ((ubx != NOT_FOUND) && (unkn > 0))   
00098             return UNKNOWN | pipe->get (buf,unkn);
00099         if (ubx == WAIT && fr)                        
00100             return WAIT;
00101         if (ubx > 0)                            
00102             return UBX | pipe->get (buf,ubx);
00103         
00104         // UNKNOWN
00105         unkn ++;
00106         len--;
00107     }
00108     if (unkn > 0)                      
00109         return UNKNOWN | pipe->get (buf,unkn); 
00110     return WAIT;
00111 }
00112 
00113 int GnssParser::_parseNmea(Pipe<char> * pipe, int len)
00114 {
00115     int o = 0;
00116     int c = 0;
00117     char ch;
00118     if (++o > len)                      return WAIT;
00119     if ('$' != pipe->next())            return NOT_FOUND;
00120     // This needs to be extended by crc checking 
00121     for (;;)
00122     {
00123         if (++o > len)                  return WAIT;
00124         ch = pipe->next();
00125         if ('*' == ch)                  break; // crc delimiter 
00126         if (!isprint(ch))               return NOT_FOUND; 
00127         c ^= ch;
00128     }
00129     if (++o > len)                      return WAIT;
00130     ch = _toHex[(c >> 4) & 0xF]; // high nibble
00131     if (ch != pipe->next())             return NOT_FOUND;
00132     if (++o > len)                      return WAIT;
00133     ch = _toHex[(c >> 0) & 0xF]; // low nibble
00134     if (ch != pipe->next())             return NOT_FOUND;
00135     if (++o > len)                      return WAIT;
00136     if ('\r' != pipe->next())           return NOT_FOUND;
00137     if (++o > len)                      return WAIT;
00138     if ('\n' != pipe->next())           return NOT_FOUND;
00139     return o;
00140 }
00141 
00142 int GnssParser::_parseUbx(Pipe<char> * pipe, int l)
00143 {
00144     int o = 0;
00145     if (++o > l)                return WAIT;
00146     if ('\xB5' != pipe->next()) return NOT_FOUND;   
00147     if (++o > l)                return WAIT;
00148     if ('\x62' != pipe->next()) return NOT_FOUND;
00149     o += 4;
00150     if (o > l)                  return WAIT;
00151     int i,j,ca,cb;
00152     i = pipe->next(); ca  = i; cb  = ca; // cls
00153     i = pipe->next(); ca += i; cb += ca; // id
00154     i = pipe->next(); ca += i; cb += ca; // len_lsb
00155     j = pipe->next(); ca += j; cb += ca; // len_msb 
00156     j = i + (j << 8);
00157     while (j--)
00158     {
00159         if (++o > l)            return WAIT;
00160         i = pipe->next(); 
00161         ca += i; 
00162         cb += ca;
00163     }
00164     ca &= 0xFF; cb &= 0xFF;
00165     if (++o > l)                return WAIT;
00166     if (ca != pipe->next())     return NOT_FOUND;
00167     if (++o > l)                return WAIT;
00168     if (cb != pipe->next())     return NOT_FOUND;
00169     return o;
00170 }
00171 
00172 int GnssParser::send(const char* buf, int len)
00173 {
00174     return _send(buf, len);
00175 }
00176 
00177 int GnssParser::sendNmea(const char* buf, int len)
00178 {
00179     char head[1] = { '$' };
00180     char tail[5] = { '*', 0x00/*crc_high*/, 0x00/*crc_low*/, '\r', '\n' };
00181     int i;
00182     int crc = 0;
00183     for (i = 0; i < len; i ++)
00184         crc ^= *buf++;
00185     i  = _send(head, sizeof(head));
00186     i += _send(buf, len);
00187     tail[1] = _toHex[(crc > 4) & 0xF0];
00188     tail[2] = _toHex[(crc > 0) & 0x0F];
00189     i += _send(tail, sizeof(tail));
00190     return i;
00191 }
00192 
00193 int GnssParser::sendUbx(unsigned char cls, unsigned char id, const void* buf /*= NULL*/, int len /*= 0*/)
00194 {
00195     char head[6] = { 0xB5, 0x62, cls, id, (char) len, (char) (len >> 8)};
00196     char crc[2];
00197     int i;
00198     int ca = 0;
00199     int cb = 0;
00200     for (i = 2; i < 6; i ++)
00201     {
00202         ca += head[i];
00203         cb += ca;
00204     }
00205     for (i = 0; i < len; i ++)
00206     {
00207         ca += ((char*)buf)[i];
00208         cb += ca; 
00209     }
00210     i  = _send(head, sizeof(head));
00211     i += _send(buf, len);
00212     crc[0] = ca & 0xFF;
00213     crc[1] = cb & 0xFF;
00214     i += _send(crc,  sizeof(crc));
00215     return i;
00216 }
00217 
00218 const char* GnssParser::findNmeaItemPos(int ix, const char* start, const char* end)
00219 {
00220     // Find the start
00221     for (; (start < end) && (ix > 0); start ++)
00222     {
00223         if (*start == ',')
00224             ix --;
00225     }
00226     // Found and check bounds
00227     if ((ix == 0) && (start < end) && 
00228         (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n'))
00229         return start;
00230     else 
00231         return NULL;
00232 }
00233 
00234 bool GnssParser::getNmeaItem(int ix, char* buf, int len, double& val)
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 = strtod(pos, &end);
00242     // Restore the last character
00243     return (end > pos);
00244 }
00245 
00246 bool GnssParser::getNmeaItem(int ix, char* buf, int len, int& val, int base /*=10*/)
00247 {
00248     char* end = &buf[len];
00249     const char* pos = findNmeaItemPos(ix, buf, end);
00250     // Find the start
00251     if (!pos)
00252         return false;
00253     val = (int)strtol(pos, &end, base);
00254     return (end > pos);
00255 }
00256 
00257 bool GnssParser::getNmeaItem(int ix, char* buf, int len, char& val)
00258 {
00259     const char* end = &buf[len];
00260     const char* pos = findNmeaItemPos(ix, buf, end);
00261     // Find the start
00262     if (!pos)
00263         return false;
00264     // Skip leading spaces
00265     while ((pos < end) && isspace(*pos))
00266         pos++;
00267     // Check bound
00268     if ((pos < end) && 
00269         (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n'))
00270     {
00271         val = *pos;
00272         return true;
00273     }
00274     return false;
00275 }
00276 
00277 bool GnssParser::getNmeaAngle(int ix, char* buf, int len, double& val)
00278 {
00279     char ch;
00280     if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && 
00281         ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W')))
00282     {
00283         val *= 0.01;
00284         int i = (int)val;
00285         val = (val - i) / 0.6 + i;
00286         if (ch == 'S' || ch == 'W')
00287             val = -val;
00288         return true;
00289     }
00290     return false;
00291 }
00292 
00293 int GnssParser::enable_ubx() {
00294 
00295     unsigned char ubx_cfg_prt[]={0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,0x00, 0x00};
00296     int conf = RETRY;
00297     int length = 0;
00298 
00299     while(conf)
00300     {
00301         length = sendUbx(0x06, 0x00, ubx_cfg_prt, sizeof(ubx_cfg_prt));
00302         if(length >= (int)(sizeof(ubx_cfg_prt) + UBX_FRAME_SIZE))
00303         {
00304             wait(5);
00305             break;
00306         }
00307         else
00308         {
00309             conf = conf - 1;
00310         }
00311     }
00312     return (conf == 0) ? 0 : 1;
00313 }
00314 
00315 eUBX_MESSAGE GnssParser::get_ubx_message(char *buff) {
00316     eUBX_MESSAGE return_value = UNKNOWN_UBX;
00317 
00318     if(buff[SYNC_CHAR_INDEX_1] == 0xB5 && buff[SYNC_CHAR_INDEX_2] == 0x62) {
00319 
00320         switch (buff[MSG_CLASS_INDEX]) {
00321 
00322         case NAV: {
00323             switch (buff[MSG_ID_INDEX]) {
00324 
00325             case 0x07: {
00326                 return_value = UBX_NAV_PVT;
00327             }
00328             break;
00329             case 0x09: {
00330                 return_value = UBX_NAV_ODO;
00331             }
00332             break;
00333             case 0x03: {
00334                 return_value = UBX_NAV_STATUS;
00335             }
00336             break;
00337             case 0x35: {
00338                 return_value = UBX_NAV_SAT;
00339             }
00340             break;
00341             default:
00342             {
00343                 return_value = UNKNOWN_UBX;
00344             }
00345             break;
00346             }
00347         }
00348         break;
00349         case ACK: {
00350             switch (buff[MSG_ID_INDEX]) {
00351             case 0x00: {
00352                 return_value = UBX_ACK_NAK;
00353             }
00354             break;
00355             case 0x01: {
00356                 return_value = UBX_ACK_ACK;
00357             }
00358             break;
00359             default:
00360             {
00361                 return_value = UNKNOWN_UBX;
00362             }
00363             break;
00364             }
00365         }
00366         break;
00367         case LOG: {
00368             switch (buff[MSG_ID_INDEX]) {
00369             case 0x11: {
00370                 return_value = UBX_LOG_BATCH;
00371             }
00372             break;
00373             default:
00374             {
00375                 return_value = UNKNOWN_UBX;
00376             }
00377             break;
00378             }
00379         }
00380         break;
00381         default:
00382         {
00383             return_value = UNKNOWN_UBX;
00384         }
00385         break;
00386         }
00387     }
00388     return return_value;
00389 }
00390 
00391 tUBX_ACK_ACK GnssParser::decode_ubx_cfg_ack_nak_msg(char *buf) {
00392     tUBX_ACK_ACK return_decoded_msg;
00393     uint8_t index = UBX_PAYLOAD_INDEX;
00394 
00395     return_decoded_msg.msg_class = buf[index++];
00396     return_decoded_msg.msg_id = buf[index];
00397 
00398     return return_decoded_msg;
00399 }
00400 
00401 tUBX_NAV_ODO GnssParser::decode_ubx_nav_odo_msg(char *buf) {
00402     tUBX_NAV_ODO return_decoded_msg;
00403     uint8_t index = UBX_PAYLOAD_INDEX;
00404 
00405     return_decoded_msg.version = buf[index++];
00406     index +=3; // 3 bytes are reserved
00407 
00408     return_decoded_msg.itow = buf[index++];
00409     return_decoded_msg.itow |= (buf[index++] << 8);
00410     return_decoded_msg.itow |= (buf[index++] << 16);
00411     return_decoded_msg.itow |= (buf[index++] << 24);
00412 
00413     return_decoded_msg.distance = buf[index++];
00414     return_decoded_msg.distance |= (buf[index++] << 8);
00415     return_decoded_msg.distance |= (buf[index++] << 16);
00416     return_decoded_msg.distance |= (buf[index++] << 24);
00417 
00418     return_decoded_msg.totalDistance = buf[index++];
00419     return_decoded_msg.totalDistance |= (buf[index++] << 8);
00420     return_decoded_msg.totalDistance |= (buf[index++] << 16);
00421     return_decoded_msg.totalDistance |= (buf[index++] << 24);
00422 
00423     return_decoded_msg.distanceSTD = buf[index++];
00424     return_decoded_msg.distanceSTD |= (buf[index++] << 8);
00425     return_decoded_msg.distanceSTD |= (buf[index++] << 16);
00426     return_decoded_msg.distanceSTD |= (buf[index++] << 24);
00427 
00428     return return_decoded_msg;
00429 }
00430 
00431 tUBX_NAV_PVT GnssParser::decode_ubx_nav_pvt_msg(char *buf) {
00432     tUBX_NAV_PVT return_decoded_msg;
00433     uint8_t index = UBX_PAYLOAD_INDEX;
00434 
00435     return_decoded_msg.itow = buf[index++];
00436     return_decoded_msg.itow |= (buf[index++] << 8);
00437     return_decoded_msg.itow |= (buf[index++] << 16);
00438     return_decoded_msg.itow |= (buf[index++] << 24);
00439 
00440     return_decoded_msg.year = buf[index++];
00441     return_decoded_msg.year |= (buf[index++] << 8);
00442 
00443     return_decoded_msg.month = buf[index++];
00444 
00445     return_decoded_msg.day = buf[index++];
00446 
00447     // Go to Fix type
00448     index = UBX_PAYLOAD_INDEX + 20;
00449     return_decoded_msg.fixType = buf[index++];
00450     return_decoded_msg.flag1 = buf[index];
00451 
00452     // Go to lon
00453     index = UBX_PAYLOAD_INDEX + 24;
00454 
00455     return_decoded_msg.lon = buf[index++];
00456     return_decoded_msg.lon |= (buf[index++] << 8);
00457     return_decoded_msg.lon |= (buf[index++] << 16);
00458     return_decoded_msg.lon |= (buf[index++] << 24);
00459 
00460     return_decoded_msg.lat = buf[index++];
00461     return_decoded_msg.lat |= (buf[index++] << 8);
00462     return_decoded_msg.lat |= (buf[index++] << 16);
00463     return_decoded_msg.lat |= (buf[index++] << 24);
00464 
00465     return_decoded_msg.height = buf[index++];
00466     return_decoded_msg.height |= (buf[index++] << 8);
00467     return_decoded_msg.height |= (buf[index++] << 16);
00468     return_decoded_msg.height |= (buf[index++] << 24);
00469 
00470     // Go to gSpeed
00471     index = UBX_PAYLOAD_INDEX + 60;
00472     return_decoded_msg.speed = buf[index++];
00473     return_decoded_msg.speed |= (buf[index++] << 8);
00474     return_decoded_msg.speed |= (buf[index++] << 16);
00475     return_decoded_msg.speed |= (buf[index++] << 24);
00476 
00477     return return_decoded_msg;
00478 }
00479 
00480 tUBX_LOG_BATCH GnssParser::decode_ubx_log_batch_msg(char *buf) {
00481     tUBX_LOG_BATCH return_decoded_msg;
00482     uint8_t index = UBX_PAYLOAD_INDEX;
00483 
00484     // move index itow
00485     index = UBX_PAYLOAD_INDEX + 4;
00486 
00487     return_decoded_msg.itow = buf[index++];
00488     return_decoded_msg.itow |= (buf[index++] << 8);
00489     return_decoded_msg.itow |= (buf[index++] << 16);
00490     return_decoded_msg.itow |= (buf[index++] << 24);
00491 
00492     // move index lon
00493     index = UBX_PAYLOAD_INDEX + 24;
00494 
00495     return_decoded_msg.lon = buf[index++];
00496     return_decoded_msg.lon |= (buf[index++] << 8);
00497     return_decoded_msg.lon |= (buf[index++] << 16);
00498     return_decoded_msg.lon |= (buf[index++] << 24);
00499 
00500     return_decoded_msg.lat = buf[index++];
00501     return_decoded_msg.lat |= (buf[index++] << 8);
00502     return_decoded_msg.lat |= (buf[index++] << 16);
00503     return_decoded_msg.lat |= (buf[index++] << 24);
00504 
00505     return_decoded_msg.height = buf[index++];
00506     return_decoded_msg.height |= (buf[index++] << 8);
00507     return_decoded_msg.height |= (buf[index++] << 16);
00508     return_decoded_msg.height |= (buf[index++] << 24);
00509 
00510     // move index to distance
00511     index = UBX_PAYLOAD_INDEX + 84;
00512 
00513     return_decoded_msg.distance = buf[index++];
00514     return_decoded_msg.distance |= (buf[index++] << 8);
00515     return_decoded_msg.distance |= (buf[index++] << 16);
00516     return_decoded_msg.distance |= (buf[index++] << 24);
00517 
00518     return_decoded_msg.totalDistance = buf[index++];
00519     return_decoded_msg.totalDistance |= (buf[index++] << 8);
00520     return_decoded_msg.totalDistance |= (buf[index++] << 16);
00521     return_decoded_msg.totalDistance |= (buf[index++] << 24);
00522 
00523     return_decoded_msg.distanceSTD = buf[index++];
00524     return_decoded_msg.distanceSTD |= (buf[index++] << 8);
00525     return_decoded_msg.distanceSTD |= (buf[index++] << 16);
00526     return_decoded_msg.distanceSTD |= (buf[index++] << 24);
00527 
00528     return return_decoded_msg;
00529 }
00530 
00531 tUBX_NAV_STATUS GnssParser::decode_ubx_nav_status_msg(char *buf) {
00532 
00533     tUBX_NAV_STATUS return_decoded_msg;
00534     uint8_t index = UBX_PAYLOAD_INDEX;
00535 
00536     return_decoded_msg.itow = buf[index++];
00537     return_decoded_msg.itow |= (buf[index++] << 8);
00538     return_decoded_msg.itow |= (buf[index++] << 16);
00539     return_decoded_msg.itow |= (buf[index++] << 24);
00540 
00541     // move index flag
00542     return_decoded_msg.fix = buf[index++];
00543 
00544     return_decoded_msg.flags = buf[index++];
00545 
00546     // move to ttff
00547     index+=2;
00548 
00549     return_decoded_msg.ttff = buf[index++];
00550     return_decoded_msg.ttff |= (buf[index++] << 8);
00551     return_decoded_msg.ttff |= (buf[index++] << 16);
00552     return_decoded_msg.ttff |= (buf[index++] << 24);
00553 
00554     return_decoded_msg.msss = buf[index++];
00555     return_decoded_msg.msss |= (buf[index++] << 8);
00556     return_decoded_msg.msss |= (buf[index++] << 16);
00557     return_decoded_msg.msss |= (buf[index++] << 24);
00558 
00559     return return_decoded_msg;
00560 }
00561 
00562 
00563 tUBX_NAV_SAT GnssParser::decode_ubx_nav_sat_msg(char *buf, int length) {
00564     tUBX_NAV_SAT return_decoded_msg;
00565     uint8_t index = UBX_PAYLOAD_INDEX;
00566     uint8_t numberSVs = buf[index + 5];
00567 
00568     if(length == (UBX_FRAME_SIZE + 8 + (12*numberSVs))) {
00569         return_decoded_msg.status = true;
00570     }
00571     else {
00572         return_decoded_msg.status = false;
00573     }
00574 
00575     return return_decoded_msg;
00576 }
00577 
00578 int GnssParser::ubx_request_batched_data(bool sendMonFirst) {
00579     unsigned char ubx_log_retrieve_batch[]={0x00, 0x00, 0x00, 0x00};
00580 
00581     ubx_log_retrieve_batch[1] = (sendMonFirst == true) ? 0x01 : 0x00;
00582 
00583     int conf = RETRY;
00584     while(conf)
00585     {
00586 
00587         int length = sendUbx(0x21, 0x10, ubx_log_retrieve_batch, sizeof(ubx_log_retrieve_batch));
00588         if(length >= (int)(sizeof(ubx_log_retrieve_batch) + UBX_FRAME_SIZE))
00589         {
00590             wait(5);
00591             break;
00592         }
00593         else
00594         {
00595             conf = conf - 1;
00596         }
00597     }
00598     if(conf == 0)
00599     {
00600         return 1;
00601     }
00602 
00603     return 0;
00604 }
00605                 
00606 const char GnssParser::_toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' };
00607 
00608 // ----------------------------------------------------------------
00609 // Serial Implementation 
00610 // ----------------------------------------------------------------
00611 
00612 GnssSerial::GnssSerial(PinName tx /*= GNSSTXD  */, PinName rx /*= GNSSRXD */, int baudrate /*= GNSSBAUD */,
00613                        int rxSize /*= 256 */, int txSize /*= 128 */) :
00614             SerialPipe(tx, rx, baudrate, rxSize, txSize)
00615 {
00616     baud(baudrate);
00617 }
00618 
00619 GnssSerial::~GnssSerial(void)
00620 {
00621     powerOff();
00622 }
00623 
00624 bool GnssSerial::init(PinName pn)
00625 {
00626     Timer timer;
00627     int size;
00628     
00629     // Unused (kept only for compatibility with the I2C version)
00630     (void)pn;
00631     
00632     // Power up and enable the module
00633     _powerOn();
00634 
00635     // Send a byte to wakup the device again
00636     putc(0xFF);
00637     // Wait until we get some bytes
00638     size = _pipeRx.size();
00639     timer.start();
00640     while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) {
00641         /* Nothing, just wait */
00642     }
00643     timer.stop();
00644     
00645     enable_ubx();
00646 
00647     wait_ms(1000);
00648 
00649     baud(115200);
00650 
00651     // Send a byte to wakup the device again
00652     putc(0xFF);
00653     // Wait until we get some bytes
00654     size = _pipeRx.size();
00655     timer.start();
00656     while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) {
00657         /* Nothing, just wait */
00658     }
00659 
00660     return (size != _pipeRx.size());
00661 }
00662 
00663 int GnssSerial::getMessage(char* buf, int len)
00664 {
00665     return _getMessage(&_pipeRx, buf, len);   
00666 }
00667 
00668 int GnssSerial::_send(const void* buf, int len)
00669 {
00670     GET_SDCARD_INSTANCE->write(logging_file_name, (void *)buf, len);
00671 
00672     return put((const char*)buf, len, true/*=blocking*/); 
00673 }
00674 
00675 // ----------------------------------------------------------------
00676 // I2C Implementation 
00677 // ----------------------------------------------------------------
00678 
00679 GnssI2C::GnssI2C(PinName sda /*= NC */, PinName scl /*= NC */,
00680                unsigned char i2cAdr /*= (66<<1) */, int rxSize /*= 256 */) :
00681                I2C(sda,scl),
00682                _pipe(rxSize),
00683                _i2cAdr(i2cAdr)
00684 {
00685     frequency(100000);
00686 }
00687 
00688 GnssI2C::~GnssI2C(void)
00689 {
00690     powerOff();
00691 }
00692 
00693 bool GnssI2C::init(PinName pn)
00694 {
00695     // Power up and enable the module
00696     _powerOn();
00697 
00698     if (pn != NC) {
00699         DigitalOut pin(pn, 0);
00700         ::wait_us(1);
00701         pin = 1;
00702         ::wait_ms(100);
00703     }
00704     return !I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM));
00705 }
00706 
00707 int GnssI2C::getMessage(char* buf, int len)
00708 {
00709     // Fill the pipe
00710     int sz = _pipe.free();
00711     if (sz) 
00712         sz = _get(buf, sz);
00713     if (sz) 
00714         _pipe.put(buf, sz);
00715     // Now parse it
00716     return _getMessage(&_pipe, buf, len);   
00717 }
00718 
00719 int GnssI2C::send(const char* buf, int len)
00720 {
00721     int sent = 0;
00722     if (len) 
00723     {
00724         if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00725             sent = send(buf, len);
00726         stop();
00727     }
00728     return sent;
00729 }
00730 
00731 int GnssI2C::sendNmea(const char* buf, int len)
00732 { 
00733     int sent = 0;
00734     if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00735         sent = GnssParser::sendNmea(buf, len);
00736     stop();
00737     return sent;
00738 }
00739 
00740 int GnssI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len)
00741 { 
00742     int sent = 0;
00743     if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true))
00744         sent = GnssParser::sendUbx(cls, id, buf, len);
00745     I2C::stop();
00746     return sent;
00747 }
00748 
00749 int GnssI2C::_get(char* buf, int len)
00750 {
00751     int read = 0;
00752     unsigned char sz[2] = {0,0};
00753     if (!I2C::write(_i2cAdr,&REGLEN,sizeof(REGLEN),true) && 
00754         !I2C::read(_i2cAdr,(char*)sz,sizeof(sz)))
00755     {
00756         int size = 256 * (int)sz[0] + sz[1];
00757         if (size > len)
00758             size = len;
00759         if (size > 0) 
00760         {
00761             if (!I2C::write(_i2cAdr,&REGSTREAM,sizeof(REGSTREAM),true) &&
00762                 !I2C::read(_i2cAdr,buf,size)) {
00763                 read = size;
00764             }
00765         }
00766     }
00767     return read;
00768 }
00769 
00770 int GnssI2C::_send(const void* buf, int len)
00771 { 
00772     return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; 
00773 }
00774 
00775 const char GnssI2C::REGLEN    = 0xFD;
00776 const char GnssI2C::REGSTREAM = 0xFF;
00777 
00778 // End Of File