Wajahat Abbas
gnss
Diff: gnss.cpp
- Revision:
- 33:75163fa7e453
- Parent:
- 30:de9fcdbc4d06
- Child:
- 34:d6a65d4c902a
--- 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;