Revision 8:720841961804, committed 2018-05-16

Comitter:

fahim.alavi@u-blox.com

Date:

Wed May 16 10:58:16 2018 +0500

Parent:

6:56eda66d585b

Child:

9:cff83b9f5093

Commit message:

Decoder methods added for UBX-NAV-PVT, UBX-LOG-BATCH, UBX-ACK-ACK/NAK & UBX-NAV-ODO

Changed in this revision

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

--- a/gnss.h	Wed Jun 14 20:44:42 2017 +0100
+++ b/gnss.h	Wed May 16 10:58:16 2018 +0500
@@ -39,6 +39,56 @@
 # define GNSSBAUD GPSBAUD
 #endif
 
+#define UBX_FRAME_SIZE 8
+#define RETRY 5
+#define SYNC_CHAR_INDEX_1 0
+#define SYNC_CHAR_INDEX_2 1
+#define MSG_CLASS_INDEX 2
+#define MSG_ID_INDEX 3
+#define UBX_LENGTH_INDEX 4
+#define UBX_PAYLOAD_INDEX 6
+
+enum eUBX_MSG_CLASS {NAV = 0x01, ACK = 0x05, LOG = 0x21};
+
+enum eUBX_MESSAGE  {UBX_LOG_BATCH, UBX_ACK_ACK, UBX_ACK_NAK, UBX_NAV_ODO, UBX_NAV_PVT, UNKNOWN_UBX};
+
+typedef struct UBX_CFG_ACK {
+	uint8_t msg_class;
+	uint8_t msg_id;
+
+}tUBX_CFG_ACK;
+
+typedef struct UBX_NAV_ODO {
+	uint8_t version;
+	uint8_t reserved[3];
+	uint32_t itow;
+	uint32_t distance;
+	uint32_t totalDistance;
+	uint32_t distanceSTD;
+}tUBX_NAV_ODO;
+
+typedef struct UBX_NAV_PVT {
+	uint32_t itow;
+	uint16_t year;
+	uint8_t month;
+	uint8_t day;
+	int32_t lon; // scaling 1e-7
+	int32_t lat; // scaling 1e-7
+	int32_t height;
+
+}tUBX_NAV_PVT;
+
+typedef struct UBX_LOG_BATCH {
+	uint32_t itow;
+	int32_t lon; // scaling 1e-7
+	int32_t lat; // scaling 1e-7
+	int32_t height;
+	uint32_t distance;
+	uint32_t totalDistance;
+	uint32_t distanceSTD;
+
+}tUBX_LOG_BATCH;
+
 /** Basic GNSS parser class.
 */
 class GnssParser
@@ -153,7 +203,43 @@
      * @return true if successful, false otherwise.
      */
     static bool getNmeaAngle(int ix, char* buf, int len, double& val);
+
+    /** Enable UBX messages.
+	 * @param none
+	 * @return 0 if successful, false otherwise.
+	 */
+    int enable_ubx();
     
+    /** GET Message type of receiver UBX message
+     * @param buff the UXB message
+     * @return eUBX_MESSAGE
+     */
+    eUBX_MESSAGE get_ubx_message(char *);
+
+    /** Method to parse contents of UBX CFG-ACK/NAK and return messageid amd class for which ACK is received
+     * @param buff the UXB message
+     * @return tUBX_CFG_ACK
+     */
+    tUBX_CFG_ACK decode_ubx_cfg_ack_nak_msg(char *);
+
+    /** Method to parse contents of UBX_NAV_ODO and return decoded msg
+     * @param buff the UXB message
+     * @return tUBX_NAV_ODO
+     */
+    tUBX_NAV_ODO decode_ubx_nav_odo_msg(char *);
+
+    /** Method to parse contents of UBX_NAV_PVT and return decoded msg
+	 * @param buff the UXB message
+	 * @return tUBX_NAV_PVT
+	 */
+    tUBX_NAV_PVT decode_ubx_nav_pvt_msg(char *);
+
+    /** Method to parse contents of UBX_LOG_BATCH and return decoded msg
+	 * @param buff the UXB message
+	 * @return tUBX_LOG_BATCH
+	 */
+    tUBX_LOG_BATCH decode_ubx_log_batch_msg(char *);
+
 protected:
     /** Power on the GNSS module.
     */