gnss
Diff: gnss.cpp
- Revision:
- 8:720841961804
- Parent:
- 6:56eda66d585b
- Child:
- 9:cff83b9f5093
--- a/gnss.cpp Wed Jun 14 20:44:42 2017 +0100 +++ b/gnss.cpp Wed May 16 10:58:16 2018 +0500 @@ -275,6 +275,267 @@ } return false; } + +int GnssParser::enable_ubx() { + + unsigned char ubx_cfg_prt[]={0x01, 0x00, 0x00, 0x00, 0xD0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x03, 0x00, 0x03, 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; + } + } + + unsigned char enable_ubx_nav_pvt[]={0x01, 0x07, 0x02}; + conf = RETRY; + while(conf) + { + + length = sendUbx(0x06, 0x01, enable_ubx_nav_pvt, sizeof(enable_ubx_nav_pvt)); + if(length >= (int)(sizeof(enable_ubx_nav_pvt) + UBX_FRAME_SIZE)) + { + wait(5); + break; + } + else + { + conf = conf - 1; + } + } + if(conf == 0) + { + return 1; + } + + unsigned char ubx_cfg_cfg[] = { 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03}; + conf = RETRY; + while(conf) + { + length = sendUbx(0x06, 0x09, ubx_cfg_cfg, sizeof(ubx_cfg_cfg)); + if(length >= (int)(sizeof(ubx_cfg_cfg) + UBX_FRAME_SIZE)) + { + wait(5); + break; + } + else + { + conf = conf - 1; + } + } + if(conf == 0) + { + return 1; + } + + return 0; +} + +eUBX_MESSAGE GnssParser::get_ubx_message(char *buff) { + eUBX_MESSAGE return_value = UNKNOWN_UBX; + + if(buff[SYNC_CHAR_INDEX_1] == 0xB5 && buff[SYNC_CHAR_INDEX_2] == 0x62) { + + switch (buff[MSG_CLASS_INDEX]) { + + case NAV: { + switch (buff[MSG_ID_INDEX]) { + + case 0x07: { + return_value = UBX_NAV_PVT; + } + break; + case 0x09: { + return_value = UBX_NAV_ODO; + } + 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_CFG_ACK GnssParser::decode_ubx_cfg_ack_nak_msg(char *buf) { + tUBX_CFG_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 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; + + 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.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.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; +} + +tUBX_NAV_PVT GnssParser::decode_ubx_nav_pvt_msg(char *buf) { + 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.year = buf[index++]; + return_decoded_msg.year |= (buf[index++] << 8); + + return_decoded_msg.month = buf[index++]; + + return_decoded_msg.day = buf[index++]; + + // 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.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 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; + + // 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); + + // 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.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); + + // 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.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 return_decoded_msg; +} const char GnssParser::_toHex[] = { '0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F' }; @@ -315,6 +576,8 @@ } timer.stop(); + enable_ubx(); + return (size != _pipeRx.size()); }