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