This class provides an API to communicate with a u-blox GNSS chip. The files here were originally part of the C027_Support library (https://developer.mbed.org/teams/ublox/code/C027_Support/ at revision 138:dafbbf31bf76) but have been separated out, primarily for use on the u-blox C030 board where the cellular interace portion of the C027_Support library will instead be provided through the new mbed Cellular API.
Dependents: example-ublox-at-cellular-interface-ext example-low-power-sleep example-C030-out-of-box-demo example-C030-out-of-box-demo ... more
Diff: gnss.cpp
- Revision:
- 33:75163fa7e453
- Parent:
- 30:de9fcdbc4d06
- Child:
- 34:d6a65d4c902a
diff -r 36bf0ff17131 -r 75163fa7e453 gnss.cpp --- a/gnss.cpp Wed Apr 24 17:55:48 2019 +0500 +++ b/gnss.cpp Wed May 29 16:45:00 2019 +0500 @@ -33,7 +33,7 @@ { // Create the enable pin but set everything to disabled _gnssEnable = NULL; - + #ifdef TARGET_UBLOX_C030 _gnssEnable = new DigitalInOut(GNSSEN, PIN_OUTPUT, PushPullNoPull, 0); #else @@ -52,13 +52,16 @@ void GnssParser::powerOff(void) { // Set the GNSS into backup mode using the command RMX-LPREQ - struct { unsigned long dur; unsigned long flags; } msg = {0/*endless*/,0/*backup*/}; + struct { + unsigned long dur; + unsigned long flags; + } msg = {0/*endless*/,0/*backup*/}; sendUbx(0x02, 0x41, &msg, sizeof(msg)); } void GnssParser::cutOffPower(void) { - //Disabling PA15 to cut off power supply + //Disabling PA15 to cut off power supply if (_gnssEnable != NULL) *_gnssEnable = 0; wait_ms(1); @@ -67,7 +70,7 @@ void GnssParser::_powerOn(void) { if (_gnssEnable != NULL) { - *_gnssEnable = 1; + *_gnssEnable = 1; } wait_ms (1); } @@ -84,29 +87,29 @@ // NMEA protocol pipe->set(unkn); int nmea = _parseNmea(pipe,len); - if ((nmea != NOT_FOUND) && (unkn > 0)) + if ((nmea != NOT_FOUND) && (unkn > 0)) return UNKNOWN | pipe->get(buf,unkn); - if (nmea == WAIT && fr) + if (nmea == WAIT && fr) return WAIT; - if (nmea > 0) + if (nmea > 0) return NMEA | pipe->get(buf,nmea); // UBX protocol - + pipe->set(unkn); int ubx = _parseUbx(pipe,len); - if ((ubx != NOT_FOUND) && (unkn > 0)) + if ((ubx != NOT_FOUND) && (unkn > 0)) return UNKNOWN | pipe->get(buf,unkn); - if (ubx == WAIT && fr) + if (ubx == WAIT && fr) return WAIT; - if (ubx > 0) + if (ubx > 0) return UBX | pipe->get(buf,ubx); - + // UNKNOWN unkn ++; len--; } - if (unkn > 0) - return UNKNOWN | pipe->get(buf,unkn); + if (unkn > 0) + return UNKNOWN | pipe->get(buf,unkn); return WAIT; } @@ -117,13 +120,13 @@ char ch; if (++o > len) return WAIT; if ('$' != pipe->next()) return NOT_FOUND; - // This needs to be extended by crc checking + // This needs to be extended by crc checking for (;;) { if (++o > len) return WAIT; ch = pipe->next(); - if ('*' == ch) break; // crc delimiter - if (!isprint(ch)) return NOT_FOUND; + if ('*' == ch) break; // crc delimiter + if (!isprint(ch)) return NOT_FOUND; c ^= ch; } if (++o > len) return WAIT; @@ -143,25 +146,34 @@ { int o = 0; if (++o > l) return WAIT; - if ('\xB5' != pipe->next()) return NOT_FOUND; + if ('\xB5' != pipe->next()) return NOT_FOUND; if (++o > l) return WAIT; if ('\x62' != pipe->next()) return NOT_FOUND; o += 4; if (o > l) return WAIT; int i,j,ca,cb; - i = pipe->next(); ca = i; cb = ca; // cls - i = pipe->next(); ca += i; cb += ca; // id - i = pipe->next(); ca += i; cb += ca; // len_lsb - j = pipe->next(); ca += j; cb += ca; // len_msb + i = pipe->next(); + ca = i; + cb = ca; // cls + i = pipe->next(); + ca += i; + cb += ca; // id + i = pipe->next(); + ca += i; + cb += ca; // len_lsb + j = pipe->next(); + ca += j; + cb += ca; // len_msb j = i + (j << 8); while (j--) { if (++o > l) return WAIT; - i = pipe->next(); - ca += i; + i = pipe->next(); + ca += i; cb += ca; } - ca &= 0xFF; cb &= 0xFF; + ca &= 0xFF; + cb &= 0xFF; if (++o > l) return WAIT; if (ca != pipe->next()) return NOT_FOUND; if (++o > l) return WAIT; @@ -205,7 +217,7 @@ for (i = 0; i < len; i ++) { ca += ((char*)buf)[i]; - cb += ca; + cb += ca; } i = _send(head, sizeof(head)); i += _send(buf, len); @@ -224,10 +236,10 @@ ix --; } // Found and check bounds - if ((ix == 0) && (start < end) && - (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n')) + if ((ix == 0) && (start < end) && + (*start != ',') && (*start != '*') && (*start != '\r') && (*start != '\n')) return start; - else + else return NULL; } @@ -265,8 +277,8 @@ while ((pos < end) && isspace(*pos)) pos++; // Check bound - if ((pos < end) && - (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n')) + if ((pos < end) && + (*pos != ',') && (*pos != '*') && (*pos != '\r') && (*pos != '\n')) { val = *pos; return true; @@ -277,8 +289,8 @@ bool GnssParser::getNmeaAngle(int ix, char* buf, int len, double& val) { char ch; - if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && - ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W'))) + if (getNmeaItem(ix,buf,len,val) && getNmeaItem(ix+1,buf,len,ch) && + ((ch == 'S') || (ch == 'N') || (ch == 'E') || (ch == 'W'))) { val *= 0.01; int i = (int)val; @@ -292,326 +304,326 @@ int GnssParser::enable_ubx() { - 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}; - int conf = RETRY; - int length = 0; + 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}; + int conf = RETRY; + int length = 0; - while(conf) - { - length = sendUbx(0x06, 0x00, ubx_cfg_prt, sizeof(ubx_cfg_prt)); - if(length >= (int)(sizeof(ubx_cfg_prt) + UBX_FRAME_SIZE)) - { - wait(5); - break; - } - else - { - conf = conf - 1; - } - } - return (conf == 0) ? 0 : 1; + while(conf) + { + length = sendUbx(0x06, 0x00, ubx_cfg_prt, sizeof(ubx_cfg_prt)); + if(length >= (int)(sizeof(ubx_cfg_prt) + UBX_FRAME_SIZE)) + { + wait(5); + break; + } + else + { + conf = conf - 1; + } + } + return (conf == 0) ? 0 : 1; } eUBX_MESSAGE GnssParser::get_ubx_message(char *buff) { - eUBX_MESSAGE return_value = UNKNOWN_UBX; + eUBX_MESSAGE return_value = UNKNOWN_UBX; - if(buff[SYNC_CHAR_INDEX_1] == 0xB5 && buff[SYNC_CHAR_INDEX_2] == 0x62) { + if(buff[SYNC_CHAR_INDEX_1] == 0xB5 && buff[SYNC_CHAR_INDEX_2] == 0x62) { - switch (buff[MSG_CLASS_INDEX]) { + switch (buff[MSG_CLASS_INDEX]) { - case NAV: { - switch (buff[MSG_ID_INDEX]) { + case NAV: { + switch (buff[MSG_ID_INDEX]) { - case 0x07: { - return_value = UBX_NAV_PVT; - } - break; - case 0x09: { - return_value = UBX_NAV_ODO; - } - break; - case 0x03: { - return_value = UBX_NAV_STATUS; - } - break; - case 0x35: { - return_value = UBX_NAV_SAT; - } - break; - default: - { - return_value = UNKNOWN_UBX; - } - break; - } - } - break; - case ACK: { - switch (buff[MSG_ID_INDEX]) { - case 0x00: { - return_value = UBX_ACK_NAK; - } - break; - case 0x01: { - return_value = UBX_ACK_ACK; - } - break; - default: - { - return_value = UNKNOWN_UBX; - } - break; - } - } - break; - case LOG: { - switch (buff[MSG_ID_INDEX]) { - case 0x11: { - return_value = UBX_LOG_BATCH; - } - break; - default: - { - return_value = UNKNOWN_UBX; - } - break; - } - } - break; - default: - { - return_value = UNKNOWN_UBX; - } - break; - } - } - return return_value; + case 0x07: { + return_value = UBX_NAV_PVT; + } + break; + case 0x09: { + return_value = UBX_NAV_ODO; + } + break; + case 0x03: { + return_value = UBX_NAV_STATUS; + } + break; + case 0x35: { + return_value = UBX_NAV_SAT; + } + break; + default: + { + return_value = UNKNOWN_UBX; + } + break; + } + } + break; + case ACK: { + switch (buff[MSG_ID_INDEX]) { + case 0x00: { + return_value = UBX_ACK_NAK; + } + break; + case 0x01: { + return_value = UBX_ACK_ACK; + } + break; + default: + { + return_value = UNKNOWN_UBX; + } + break; + } + } + break; + case LOG: { + switch (buff[MSG_ID_INDEX]) { + case 0x11: { + return_value = UBX_LOG_BATCH; + } + break; + default: + { + return_value = UNKNOWN_UBX; + } + break; + } + } + break; + default: + { + return_value = UNKNOWN_UBX; + } + break; + } + } + return return_value; } tUBX_ACK_ACK GnssParser::decode_ubx_cfg_ack_nak_msg(char *buf) { - tUBX_ACK_ACK return_decoded_msg; - uint8_t index = UBX_PAYLOAD_INDEX; + tUBX_ACK_ACK return_decoded_msg; + uint8_t index = UBX_PAYLOAD_INDEX; - return_decoded_msg.msg_class = buf[index++]; - return_decoded_msg.msg_id = buf[index]; + return_decoded_msg.msg_class = buf[index++]; + return_decoded_msg.msg_id = buf[index]; - return return_decoded_msg; + return return_decoded_msg; } tUBX_NAV_ODO GnssParser::decode_ubx_nav_odo_msg(char *buf) { - tUBX_NAV_ODO return_decoded_msg; - uint8_t index = UBX_PAYLOAD_INDEX; + tUBX_NAV_ODO return_decoded_msg; + uint8_t index = UBX_PAYLOAD_INDEX; - return_decoded_msg.version = buf[index++]; - index +=3; // 3 bytes are reserved + return_decoded_msg.version = buf[index++]; + index +=3; // 3 bytes are reserved - return_decoded_msg.itow = buf[index++]; - return_decoded_msg.itow |= (buf[index++] << 8); - return_decoded_msg.itow |= (buf[index++] << 16); - return_decoded_msg.itow |= (buf[index++] << 24); + return_decoded_msg.itow = buf[index++]; + return_decoded_msg.itow |= (buf[index++] << 8); + return_decoded_msg.itow |= (buf[index++] << 16); + return_decoded_msg.itow |= (buf[index++] << 24); - return_decoded_msg.distance = buf[index++]; - return_decoded_msg.distance |= (buf[index++] << 8); - return_decoded_msg.distance |= (buf[index++] << 16); - return_decoded_msg.distance |= (buf[index++] << 24); + return_decoded_msg.distance = buf[index++]; + return_decoded_msg.distance |= (buf[index++] << 8); + return_decoded_msg.distance |= (buf[index++] << 16); + return_decoded_msg.distance |= (buf[index++] << 24); - return_decoded_msg.totalDistance = buf[index++]; - return_decoded_msg.totalDistance |= (buf[index++] << 8); - return_decoded_msg.totalDistance |= (buf[index++] << 16); - return_decoded_msg.totalDistance |= (buf[index++] << 24); + return_decoded_msg.totalDistance = buf[index++]; + return_decoded_msg.totalDistance |= (buf[index++] << 8); + return_decoded_msg.totalDistance |= (buf[index++] << 16); + return_decoded_msg.totalDistance |= (buf[index++] << 24); - return_decoded_msg.distanceSTD = buf[index++]; - return_decoded_msg.distanceSTD |= (buf[index++] << 8); - return_decoded_msg.distanceSTD |= (buf[index++] << 16); - return_decoded_msg.distanceSTD |= (buf[index++] << 24); + return_decoded_msg.distanceSTD = buf[index++]; + return_decoded_msg.distanceSTD |= (buf[index++] << 8); + return_decoded_msg.distanceSTD |= (buf[index++] << 16); + return_decoded_msg.distanceSTD |= (buf[index++] << 24); - return return_decoded_msg; + return return_decoded_msg; } tUBX_NAV_PVT GnssParser::decode_ubx_nav_pvt_msg(char *buf) { - tUBX_NAV_PVT return_decoded_msg; - uint8_t index = UBX_PAYLOAD_INDEX; + tUBX_NAV_PVT return_decoded_msg; + uint8_t index = UBX_PAYLOAD_INDEX; - return_decoded_msg.itow = buf[index++]; - return_decoded_msg.itow |= (buf[index++] << 8); - return_decoded_msg.itow |= (buf[index++] << 16); - return_decoded_msg.itow |= (buf[index++] << 24); + return_decoded_msg.itow = buf[index++]; + return_decoded_msg.itow |= (buf[index++] << 8); + return_decoded_msg.itow |= (buf[index++] << 16); + return_decoded_msg.itow |= (buf[index++] << 24); - return_decoded_msg.year = buf[index++]; - return_decoded_msg.year |= (buf[index++] << 8); + return_decoded_msg.year = buf[index++]; + return_decoded_msg.year |= (buf[index++] << 8); - return_decoded_msg.month = buf[index++]; + return_decoded_msg.month = buf[index++]; - return_decoded_msg.day = buf[index++]; + return_decoded_msg.day = buf[index++]; - // Go to Fix type - index = UBX_PAYLOAD_INDEX + 20; + // Go to Fix type + index = UBX_PAYLOAD_INDEX + 20; return_decoded_msg.fixType = buf[index++]; return_decoded_msg.flag1 = buf[index]; - // Go to lon - index = UBX_PAYLOAD_INDEX + 24; + // Go to lon + index = UBX_PAYLOAD_INDEX + 24; - return_decoded_msg.lon = buf[index++]; - return_decoded_msg.lon |= (buf[index++] << 8); - return_decoded_msg.lon |= (buf[index++] << 16); - return_decoded_msg.lon |= (buf[index++] << 24); + return_decoded_msg.lon = buf[index++]; + return_decoded_msg.lon |= (buf[index++] << 8); + return_decoded_msg.lon |= (buf[index++] << 16); + return_decoded_msg.lon |= (buf[index++] << 24); - return_decoded_msg.lat = buf[index++]; - return_decoded_msg.lat |= (buf[index++] << 8); - return_decoded_msg.lat |= (buf[index++] << 16); - return_decoded_msg.lat |= (buf[index++] << 24); + return_decoded_msg.lat = buf[index++]; + return_decoded_msg.lat |= (buf[index++] << 8); + return_decoded_msg.lat |= (buf[index++] << 16); + return_decoded_msg.lat |= (buf[index++] << 24); - return_decoded_msg.height = buf[index++]; - return_decoded_msg.height |= (buf[index++] << 8); - return_decoded_msg.height |= (buf[index++] << 16); - return_decoded_msg.height |= (buf[index++] << 24); + return_decoded_msg.height = buf[index++]; + return_decoded_msg.height |= (buf[index++] << 8); + return_decoded_msg.height |= (buf[index++] << 16); + return_decoded_msg.height |= (buf[index++] << 24); - // Go to gSpeed - index = UBX_PAYLOAD_INDEX + 60; - return_decoded_msg.speed = buf[index++]; - return_decoded_msg.speed |= (buf[index++] << 8); - return_decoded_msg.speed |= (buf[index++] << 16); - return_decoded_msg.speed |= (buf[index++] << 24); + // Go to gSpeed + index = UBX_PAYLOAD_INDEX + 60; + return_decoded_msg.speed = buf[index++]; + return_decoded_msg.speed |= (buf[index++] << 8); + return_decoded_msg.speed |= (buf[index++] << 16); + return_decoded_msg.speed |= (buf[index++] << 24); - return return_decoded_msg; + return return_decoded_msg; } tUBX_LOG_BATCH GnssParser::decode_ubx_log_batch_msg(char *buf) { - tUBX_LOG_BATCH return_decoded_msg; - uint8_t index = UBX_PAYLOAD_INDEX; + tUBX_LOG_BATCH return_decoded_msg; + uint8_t index = UBX_PAYLOAD_INDEX; - // move index itow - index = UBX_PAYLOAD_INDEX + 4; + // move index itow + index = UBX_PAYLOAD_INDEX + 4; - return_decoded_msg.itow = buf[index++]; - return_decoded_msg.itow |= (buf[index++] << 8); - return_decoded_msg.itow |= (buf[index++] << 16); - return_decoded_msg.itow |= (buf[index++] << 24); + return_decoded_msg.itow = buf[index++]; + return_decoded_msg.itow |= (buf[index++] << 8); + return_decoded_msg.itow |= (buf[index++] << 16); + return_decoded_msg.itow |= (buf[index++] << 24); - // move index lon - index = UBX_PAYLOAD_INDEX + 24; + // move index lon + index = UBX_PAYLOAD_INDEX + 24; - return_decoded_msg.lon = buf[index++]; - return_decoded_msg.lon |= (buf[index++] << 8); - return_decoded_msg.lon |= (buf[index++] << 16); - return_decoded_msg.lon |= (buf[index++] << 24); + return_decoded_msg.lon = buf[index++]; + return_decoded_msg.lon |= (buf[index++] << 8); + return_decoded_msg.lon |= (buf[index++] << 16); + return_decoded_msg.lon |= (buf[index++] << 24); - return_decoded_msg.lat = buf[index++]; - return_decoded_msg.lat |= (buf[index++] << 8); - return_decoded_msg.lat |= (buf[index++] << 16); - return_decoded_msg.lat |= (buf[index++] << 24); + return_decoded_msg.lat = buf[index++]; + return_decoded_msg.lat |= (buf[index++] << 8); + return_decoded_msg.lat |= (buf[index++] << 16); + return_decoded_msg.lat |= (buf[index++] << 24); - return_decoded_msg.height = buf[index++]; - return_decoded_msg.height |= (buf[index++] << 8); - return_decoded_msg.height |= (buf[index++] << 16); - return_decoded_msg.height |= (buf[index++] << 24); + return_decoded_msg.height = buf[index++]; + return_decoded_msg.height |= (buf[index++] << 8); + return_decoded_msg.height |= (buf[index++] << 16); + return_decoded_msg.height |= (buf[index++] << 24); - // move index to distance - index = UBX_PAYLOAD_INDEX + 84; + // move index to distance + index = UBX_PAYLOAD_INDEX + 84; - return_decoded_msg.distance = buf[index++]; - return_decoded_msg.distance |= (buf[index++] << 8); - return_decoded_msg.distance |= (buf[index++] << 16); - return_decoded_msg.distance |= (buf[index++] << 24); + return_decoded_msg.distance = buf[index++]; + return_decoded_msg.distance |= (buf[index++] << 8); + return_decoded_msg.distance |= (buf[index++] << 16); + return_decoded_msg.distance |= (buf[index++] << 24); - return_decoded_msg.totalDistance = buf[index++]; - return_decoded_msg.totalDistance |= (buf[index++] << 8); - return_decoded_msg.totalDistance |= (buf[index++] << 16); - return_decoded_msg.totalDistance |= (buf[index++] << 24); + return_decoded_msg.totalDistance = buf[index++]; + return_decoded_msg.totalDistance |= (buf[index++] << 8); + return_decoded_msg.totalDistance |= (buf[index++] << 16); + return_decoded_msg.totalDistance |= (buf[index++] << 24); - return_decoded_msg.distanceSTD = buf[index++]; - return_decoded_msg.distanceSTD |= (buf[index++] << 8); - return_decoded_msg.distanceSTD |= (buf[index++] << 16); - return_decoded_msg.distanceSTD |= (buf[index++] << 24); + return_decoded_msg.distanceSTD = buf[index++]; + return_decoded_msg.distanceSTD |= (buf[index++] << 8); + return_decoded_msg.distanceSTD |= (buf[index++] << 16); + return_decoded_msg.distanceSTD |= (buf[index++] << 24); - return return_decoded_msg; + return return_decoded_msg; } tUBX_NAV_STATUS GnssParser::decode_ubx_nav_status_msg(char *buf) { - tUBX_NAV_STATUS return_decoded_msg; - uint8_t index = UBX_PAYLOAD_INDEX; + tUBX_NAV_STATUS return_decoded_msg; + uint8_t index = UBX_PAYLOAD_INDEX; - return_decoded_msg.itow = buf[index++]; - return_decoded_msg.itow |= (buf[index++] << 8); - return_decoded_msg.itow |= (buf[index++] << 16); - return_decoded_msg.itow |= (buf[index++] << 24); + return_decoded_msg.itow = buf[index++]; + return_decoded_msg.itow |= (buf[index++] << 8); + return_decoded_msg.itow |= (buf[index++] << 16); + return_decoded_msg.itow |= (buf[index++] << 24); - // move index flag - return_decoded_msg.fix = buf[index++]; + // move index flag + return_decoded_msg.fix = buf[index++]; - return_decoded_msg.flags = buf[index++]; + return_decoded_msg.flags = buf[index++]; - // move to ttff - index+=2; + // move to ttff + index+=2; - return_decoded_msg.ttff = buf[index++]; - return_decoded_msg.ttff |= (buf[index++] << 8); - return_decoded_msg.ttff |= (buf[index++] << 16); - return_decoded_msg.ttff |= (buf[index++] << 24); + return_decoded_msg.ttff = buf[index++]; + return_decoded_msg.ttff |= (buf[index++] << 8); + return_decoded_msg.ttff |= (buf[index++] << 16); + return_decoded_msg.ttff |= (buf[index++] << 24); - return_decoded_msg.msss = buf[index++]; - return_decoded_msg.msss |= (buf[index++] << 8); - return_decoded_msg.msss |= (buf[index++] << 16); - return_decoded_msg.msss |= (buf[index++] << 24); + return_decoded_msg.msss = buf[index++]; + return_decoded_msg.msss |= (buf[index++] << 8); + return_decoded_msg.msss |= (buf[index++] << 16); + return_decoded_msg.msss |= (buf[index++] << 24); - return return_decoded_msg; + return return_decoded_msg; } tUBX_NAV_SAT GnssParser::decode_ubx_nav_sat_msg(char *buf, int length) { - tUBX_NAV_SAT return_decoded_msg; - uint8_t index = UBX_PAYLOAD_INDEX; - uint8_t numberSVs = buf[index + 5]; + tUBX_NAV_SAT return_decoded_msg; + uint8_t index = UBX_PAYLOAD_INDEX; + uint8_t numberSVs = buf[index + 5]; - if(length == (UBX_FRAME_SIZE + 8 + (12*numberSVs))) { - return_decoded_msg.status = true; - } - else { - return_decoded_msg.status = false; - } + if(length == (UBX_FRAME_SIZE + 8 + (12*numberSVs))) { + return_decoded_msg.status = true; + } + else { + return_decoded_msg.status = false; + } - return return_decoded_msg; + return return_decoded_msg; } int GnssParser::ubx_request_batched_data(bool sendMonFirst) { - unsigned char ubx_log_retrieve_batch[]={0x00, 0x00, 0x00, 0x00}; + unsigned char ubx_log_retrieve_batch[]= {0x00, 0x00, 0x00, 0x00}; - ubx_log_retrieve_batch[1] = (sendMonFirst == true) ? 0x01 : 0x00; + ubx_log_retrieve_batch[1] = (sendMonFirst == true) ? 0x01 : 0x00; - int conf = RETRY; - while(conf) - { + int conf = RETRY; + while(conf) + { - int length = sendUbx(0x21, 0x10, ubx_log_retrieve_batch, sizeof(ubx_log_retrieve_batch)); - if(length >= (int)(sizeof(ubx_log_retrieve_batch) + UBX_FRAME_SIZE)) - { - wait(5); - break; - } - else - { - conf = conf - 1; - } - } - if(conf == 0) - { - return 1; - } + int length = sendUbx(0x21, 0x10, ubx_log_retrieve_batch, sizeof(ubx_log_retrieve_batch)); + if(length >= (int)(sizeof(ubx_log_retrieve_batch) + UBX_FRAME_SIZE)) + { + wait(1); + break; + } + else + { + conf = conf - 1; + } + } + if(conf == 0) + { + return 1; + } - return 0; + return 0; } - + const char GnssParser::_toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' }; // ---------------------------------------------------------------- -// Serial Implementation +// Serial Implementation // ---------------------------------------------------------------- GnssSerial::GnssSerial(PinName tx /*= GNSSTXD */, PinName rx /*= GNSSRXD */, int baudrate /*= GNSSBAUD */, int rxSize /*= 256 */, int txSize /*= 128 */) : - SerialPipe(tx, rx, baudrate, rxSize, txSize) + SerialPipe(tx, rx, baudrate, rxSize, txSize) { baud(baudrate); } @@ -625,10 +637,10 @@ { Timer timer; int size; - + // Unused (kept only for compatibility with the I2C version) (void)pn; - + // Power up and enable the module _powerOn(); @@ -641,7 +653,7 @@ /* Nothing, just wait */ } timer.stop(); - + enable_ubx(); wait_ms(1000); @@ -649,38 +661,38 @@ baud(115200); // Send a byte to wakup the device again - putc(0xFF); - // Wait until we get some bytes - size = _pipeRx.size(); - timer.start(); - while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) { - /* Nothing, just wait */ - } + putc(0xFF); + // Wait until we get some bytes + size = _pipeRx.size(); + timer.start(); + while ((timer.read_ms() < 1000) && (size == _pipeRx.size())) { + /* Nothing, just wait */ + } return (size != _pipeRx.size()); } int GnssSerial::getMessage(char* buf, int len) { - return _getMessage(&_pipeRx, buf, len); + return _getMessage(&_pipeRx, buf, len); } int GnssSerial::_send(const void* buf, int len) { - GET_SDCARD_INSTANCE->write(logging_file_name, (void *)buf, len); + GET_SDCARD_INSTANCE->write(logging_file_name, (void *)buf, len); - return put((const char*)buf, len, true/*=blocking*/); + return put((const char*)buf, len, true/*=blocking*/); } // ---------------------------------------------------------------- -// I2C Implementation +// I2C Implementation // ---------------------------------------------------------------- GnssI2C::GnssI2C(PinName sda /*= NC */, PinName scl /*= NC */, - unsigned char i2cAdr /*= (66<<1) */, int rxSize /*= 256 */) : - I2C(sda,scl), - _pipe(rxSize), - _i2cAdr(i2cAdr) + unsigned char i2cAdr /*= (66<<1) */, int rxSize /*= 256 */) : + I2C(sda,scl), + _pipe(rxSize), + _i2cAdr(i2cAdr) { frequency(100000); } @@ -708,18 +720,18 @@ { // Fill the pipe int sz = _pipe.free(); - if (sz) + if (sz) sz = _get(buf, sz); - if (sz) + if (sz) _pipe.put(buf, sz); // Now parse it - return _getMessage(&_pipe, buf, len); + return _getMessage(&_pipe, buf, len); } int GnssI2C::send(const char* buf, int len) { int sent = 0; - if (len) + if (len) { if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) sent = send(buf, len); @@ -729,7 +741,7 @@ } int GnssI2C::sendNmea(const char* buf, int len) -{ +{ int sent = 0; if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) sent = GnssParser::sendNmea(buf, len); @@ -738,7 +750,7 @@ } int GnssI2C::sendUbx(unsigned char cls, unsigned char id, const void* buf, int len) -{ +{ int sent = 0; if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true)) sent = GnssParser::sendUbx(cls, id, buf, len); @@ -750,16 +762,16 @@ { int read = 0; unsigned char sz[2] = {0,0}; - if (!I2C::write(_i2cAdr,®LEN,sizeof(REGLEN),true) && - !I2C::read(_i2cAdr,(char*)sz,sizeof(sz))) + if (!I2C::write(_i2cAdr,®LEN,sizeof(REGLEN),true) && + !I2C::read(_i2cAdr,(char*)sz,sizeof(sz))) { int size = 256 * (int)sz[0] + sz[1]; if (size > len) size = len; - if (size > 0) + if (size > 0) { if (!I2C::write(_i2cAdr,®STREAM,sizeof(REGSTREAM),true) && - !I2C::read(_i2cAdr,buf,size)) { + !I2C::read(_i2cAdr,buf,size)) { read = size; } } @@ -768,8 +780,8 @@ } int GnssI2C::_send(const void* buf, int len) -{ - return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; +{ + return !I2C::write(_i2cAdr,(const char*)buf,len,true) ? len : 0; } const char GnssI2C::REGLEN = 0xFD;