Request and display MON-VER message at startup

2 files changed, 84 insertions, 10 deletions

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 56e7a8bd7..be19ce488 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -93,6 +93,8 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct sate
 	_ack_waiting_msg(0)
 {
 	decode_init();
+	memset(_ubx_sw_version, 0, sizeof(_ubx_sw_version));
+	memset(_ubx_hw_version, 0, sizeof(_ubx_hw_version));
 }
 
 UBX::~UBX()
@@ -221,6 +223,9 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
+	/* request module version information by sending an empty MON-VER message */
+	send_message(UBX_MSG_MON_VER, nullptr, 0);
+
 	_configured = true;
 	return 0;
 }
@@ -385,10 +390,16 @@ UBX::parse_char(const uint8_t b)
 	case UBX_DECODE_PAYLOAD:
 		UBX_TRACE_PARSER(".");
 		add_byte_to_checksum(b);
-		if (_rx_msg != UBX_MSG_NAV_SVINFO) {
-			ret = payload_rx_add(b);	// add a payload byte
-		} else {
-			ret = payload_rx_add_svinfo(b);	// add a svinfo payload byte
+		switch (_rx_msg) {
+		case UBX_MSG_NAV_SVINFO:
+			ret = payload_rx_add_nav_svinfo(b);	// add a NAV-SVINFO payload byte
+			break;
+		case UBX_MSG_MON_VER:
+			ret = payload_rx_add_mon_ver(b);	// add a MON-VER payload byte
+			break;
+		default:
+			ret = payload_rx_add(b);		// add a payload byte
+			break;
 		}
 		if (ret < 0) {
 			// payload not handled, discard message
@@ -477,6 +488,9 @@ UBX::payload_rx_init()
 			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
 		break;
 
+	case UBX_MSG_MON_VER:
+		break;		// unconditionally handle this message
+
 	case UBX_MSG_MON_HW:
 		if (   (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t))	/* u-blox 6 msg format */
 		    && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t)))	/* u-blox 7+ msg format */
@@ -559,10 +573,10 @@ UBX::payload_rx_add(const uint8_t b)
 }
 
 /**
- * Add svinfo payload rx byte
+ * Add NAV-SVINFO payload rx byte
  */
 int	// -1 = error, 0 = ok, 1 = payload completed
-UBX::payload_rx_add_svinfo(const uint8_t b)
+UBX::payload_rx_add_nav_svinfo(const uint8_t b)
 {
 	int ret = 0;
 
@@ -607,6 +621,41 @@ UBX::payload_rx_add_svinfo(const uint8_t b)
 }
 
 /**
+ * Add MON-VER payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_mon_ver(const uint8_t b)
+{
+	int ret = 0;
+
+	if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+		// Fill Part 1 buffer
+		_buf.raw[_rx_payload_index] = b;
+	} else {
+		if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+			// Part 1 complete: decode Part 1 buffer
+			strncpy(_ubx_sw_version, (char*)_buf.payload_rx_mon_ver_part1.swVersion, sizeof(_ubx_sw_version));
+			strncpy(_ubx_hw_version, (char*)_buf.payload_rx_mon_ver_part1.hwVersion, sizeof(_ubx_hw_version));
+			UBX_WARN("VER sw  %30s", _ubx_sw_version);
+			UBX_WARN("VER hw  %10s", _ubx_hw_version);
+		}
+		// fill Part 2 buffer
+		unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
+		_buf.raw[buf_index] = b;
+		if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
+			// Part 2 complete: decode Part 2 buffer
+			UBX_WARN("VER ext %30s", _buf.payload_rx_mon_ver_part2.extension);
+		}
+	}
+
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
+
+	return ret;
+}
+
+/**
  * Finish payload rx
  */
 int	// 0 = no message handled, 1 = message handled, 2 = sat info message handled
@@ -712,6 +761,12 @@ UBX::payload_rx_done(void)
 		ret = 1;
 		break;
 
+	case UBX_MSG_MON_VER:
+		UBX_TRACE_RXMSG("Rx MON-VER\n");
+
+		ret = 1;
+		break;
+
 	case UBX_MSG_MON_HW:
 		UBX_TRACE_RXMSG("Rx MON-HW\n");
 
@@ -813,10 +868,12 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len
 
 	// Calculate checksum
 	calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum);  // skip 2 sync bytes
-	calc_checksum(payload, length, &checksum);
+	if (payload != nullptr)
+		calc_checksum(payload, length, &checksum);
 
 	// Send message
 	write(_fd, (const void *)&header, sizeof(header));
-	write(_fd, (const void *)payload, length);
+	if (payload != nullptr)
+		write(_fd, (const void *)payload, length);
 	write(_fd, (const void *)&checksum, sizeof(checksum));
 }
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 8fd8d4da6..c8c89dfad 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -72,6 +72,7 @@
 #define UBX_ID_CFG_MSG		0x01
 #define UBX_ID_CFG_RATE		0x08
 #define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_MON_VER		0x04
 #define UBX_ID_MON_HW		0x09
 
 /* Message Classes & IDs */
@@ -87,6 +88,7 @@
 #define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
 #define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
 #define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
+#define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
 
 /* TX CFG-PRT message contents */
 #define UBX_TX_CFG_PRT_PORTID		0x01		/**< UART1 */
@@ -249,6 +251,17 @@ typedef struct {
 	uint32_t	pullL;
 } ubx_payload_rx_mon_hw_ubx7_t;
 
+/* Rx MON-VER Part 1 */
+typedef struct {
+	uint8_t		swVersion[30];
+	uint8_t		hwVersion[10];
+} ubx_payload_rx_mon_ver_part1_t;
+
+/* Rx MON-VER Part 2 (repeated) */
+typedef struct {
+	uint8_t		extension[30];
+} ubx_payload_rx_mon_ver_part2_t;
+
 /* Rx ACK-ACK */
 typedef	union {
 	uint16_t	msg;
@@ -333,6 +346,8 @@ typedef union {
 	ubx_payload_rx_nav_velned_t		payload_rx_nav_velned;
 	ubx_payload_rx_mon_hw_ubx6_t		payload_rx_mon_hw_ubx6;
 	ubx_payload_rx_mon_hw_ubx7_t		payload_rx_mon_hw_ubx7;
+	ubx_payload_rx_mon_ver_part1_t		payload_rx_mon_ver_part1;
+	ubx_payload_rx_mon_ver_part2_t		payload_rx_mon_ver_part2;
 	ubx_payload_rx_ack_ack_t		payload_rx_ack_ack;
 	ubx_payload_rx_ack_nak_t		payload_rx_ack_nak;
 	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
@@ -366,7 +381,6 @@ typedef enum {
 	UBX_RXMSG_ERROR_LENGTH
 } ubx_rxmsg_state_t;
 
-
 /* ACK state */
 typedef enum {
 	UBX_ACK_IDLE = 0,
@@ -400,7 +414,8 @@ private:
 	 * Add payload rx byte
 	 */
 	int			payload_rx_add(const uint8_t b);
-	int			payload_rx_add_svinfo(const uint8_t b);
+	int			payload_rx_add_nav_svinfo(const uint8_t b);
+	int			payload_rx_add_mon_ver(const uint8_t b);
 
 	/**
 	 * Finish payload rx
@@ -455,6 +470,8 @@ private:
 	hrt_abstime		_disable_cmd_last;
 	uint16_t		_ack_waiting_msg;
 	ubx_buf_t		_buf;
+	char			_ubx_sw_version[30];
+	char			_ubx_hw_version[10];
 };
 
 #endif /* UBX_H_ */