Diff: gnss.cpp

Revision:

7:720841961804

Parent:

6:56eda66d585b

Child:

8: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());
 }