U-blox driver rework, step 1

Handle u-blox 7+8 GNSS, too.
Disabled NAV-SVINFO for now (will be rewritten as an optional feature in
a later step).
Reduced parser buffer size from 300 to 80.

2 files changed, 74 insertions, 18 deletions

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 19cf5beec..ee53e2ec4 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -219,12 +219,14 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
+#ifdef	UBX_ENABLE_NAV_SVINFO
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 		warnx("MSG CFG FAIL: NAV SVINFO");
 		return 1;
 	}
+#endif
 
 	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
 
@@ -386,7 +388,6 @@ UBX::parse_char(uint8_t b)
 		if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
 			/* compare checksum */
 			if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
-				decode_init();
 				return 1;	// message received successfully
 
 			} else {
@@ -399,7 +400,7 @@ UBX::parse_char(uint8_t b)
 			_rx_count++;
 
 		} else {
-			warnx("buffer full");
+			warnx("ubx: buffer full 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size);
 			decode_init();
 			return -1;
 		}
@@ -450,6 +451,7 @@ UBX::handle_message()
 					_gps_position->s_variance_m_s = packet->sAcc;
 					_gps_position->p_variance_m = packet->pAcc;
 					_gps_position->timestamp_variance = hrt_absolute_time();
+					_gps_position->satellites_visible = packet->numSV;
 
 					ret = 1;
 					break;
@@ -486,6 +488,7 @@ UBX::handle_message()
 					break;
 				}
 
+#ifdef UBX_ENABLE_NAV_SVINFO
 			case UBX_MESSAGE_NAV_SVINFO: {
 					//printf("GOT NAV_SVINFO\n");
 					const int length_part1 = 8;
@@ -527,7 +530,9 @@ UBX::handle_message()
 						_gps_position->satellite_azimuth[i] = 0;
 					}
 
-					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+					/* Note: _gps_position->satellites_visible is taken from NAV-SOL now. */
+					/* _gps_position->satellites_visible = satellites_used; */ // visible ~= used but we are interested in the used ones
+					/* TODO: satellites_used may be written into a new field after sat monitoring data got separated from _gps_position */
 
 					if (packet_part1->numCh > 0) {
 						_gps_position->satellite_info_available = true;
@@ -541,6 +546,7 @@ UBX::handle_message()
 					ret = 1;
 					break;
 				}
+#endif /* #ifdef UBX_ENABLE_NAV_SVINFO */
 
 			case UBX_MESSAGE_NAV_VELNED: {
 					// printf("GOT NAV_VELNED\n");
@@ -573,23 +579,34 @@ UBX::handle_message()
 				break;
 			}
 
-		case UBX_CLASS_MON: {
+		case UBX_CLASS_MON:
 			switch (_message_id) {
-			case UBX_MESSAGE_MON_HW: {
 
-					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+			case UBX_MESSAGE_MON_HW:
+				switch (_payload_size) {
 
-					_gps_position->noise_per_ms = p->noisePerMS;
-					_gps_position->jamming_indicator = p->jamInd;
+				case UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE:	/* u-blox 6 msg format */
+					_gps_position->noise_per_ms      = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->noisePerMS;
+					_gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx6_packet*) _rx_buffer)->jamInd;
+					ret = 1;
+					break;
 
+				case UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE:	/* u-blox 7+ msg format */
+					_gps_position->noise_per_ms      = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->noisePerMS;
+					_gps_position->jamming_indicator = ((struct gps_bin_mon_hw_ubx7_packet*) _rx_buffer)->jamInd;
 					ret = 1;
 					break;
+
+				default:		// unexpected payload size:
+					ret = 1;	// ignore but don't disable msg
+					break;
 				}
+				break;
 
 			default:
 				break;
 			}
-		}
+			break;
 
 		default:
 			break;
@@ -597,7 +614,7 @@ UBX::handle_message()
 
 		if (ret == 0) {
 			/* message not handled */
-			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+			warnx("ubx: message not handled: 0x%02x-0x%02x L%u", (unsigned)_message_class, (unsigned)_message_id, _payload_size);
 
 			hrt_abstime t = hrt_absolute_time();
 
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 5cf47b60b..56f73fa63 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -48,6 +48,10 @@
 
 #include "gps_helper.h"
 
+/* TODO: processing of NAV_SVINFO disabled, has to be re-written as an optional feature */
+/* TODO: make this a command line option, allocate buffer dynamically */
+#undef UBX_ENABLE_NAV_SVINFO
+
 #define UBX_SYNC1 0xB5
 #define UBX_SYNC2 0x62
 
@@ -60,10 +64,8 @@
 
 /* MessageIDs (the ones that are used) */
 #define UBX_MESSAGE_NAV_POSLLH 0x02
-//#define UBX_MESSAGE_NAV_DOP 0x04
 #define UBX_MESSAGE_NAV_SOL 0x06
 #define UBX_MESSAGE_NAV_VELNED 0x12
-//#define UBX_MESSAGE_RXM_SVSI 0x20
 #define UBX_MESSAGE_NAV_TIMEUTC 0x21
 #define UBX_MESSAGE_NAV_SVINFO 0x30
 #define UBX_MESSAGE_ACK_NAK 0x00
@@ -72,9 +74,22 @@
 #define UBX_MESSAGE_CFG_MSG 0x01
 #define UBX_MESSAGE_CFG_RATE 0x08
 #define UBX_MESSAGE_CFG_NAV5 0x24
-
 #define UBX_MESSAGE_MON_HW	0x09
 
+/* Rx msg payload sizes */
+#define UBX_NAV_POSLLH_RX_PAYLOAD_SIZE   28		/**< NAV_POSLLH  Rx msg payload size */
+#define UBX_NAV_SOL_RX_PAYLOAD_SIZE      52		/**< NAV_SOL     Rx msg payload size */
+#define UBX_NAV_VELNED_RX_PAYLOAD_SIZE   36		/**< NAV_VELNED  Rx msg payload size */
+#define UBX_NAV_TIMEUTC_RX_PAYLOAD_SIZE  20		/**< NAV_TIMEUTC Rx msg payload size */
+#define UBX_MON_HW_UBX6_RX_PAYLOAD_SIZE  68		/**< MON_HW      Rx msg payload size for u-blox 6 and below */
+#define UBX_MON_HW_UBX7_RX_PAYLOAD_SIZE  60		/**< MON_HW      Rx msg payload size for u-blox 7 and above */
+#define UBX_MAX_RX_PAYLOAD_SIZE          70		/**< arbitrary maximum for calculating parser buffer size w/o NAV_SVINFO active */
+
+/* NAV_SVINFO has variable length w/o a published max size, so limit processing to UBX_MAX_NUM_SAT */
+#define UBX_MAX_NUM_SAT                  50							/**< Practical observed max number of satellites in SVNFO msg */
+#define UBX_NAV_SVINFO_RX_PAYLOAD_SIZE   (8 + 12 * UBX_MAX_NUM_SAT)	/**< NAV_SVINFO Rx msg payload size */
+
+/* CFG class Tx msg defs */
 #define UBX_CFG_PRT_LENGTH 20
 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
 #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
@@ -87,7 +102,6 @@
 #define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
 #define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
 
-
 #define UBX_CFG_NAV5_LENGTH 36
 #define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005		/**< XXX only update dynamic model and fix mode */
 #define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
@@ -98,7 +112,6 @@
 #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_CFG_MSG_PAYLOAD_RATE1_05HZ 10
 
-#define UBX_MAX_PAYLOAD_LENGTH 500
 
 // ************
 /** the structures of the binary packets */
@@ -140,7 +153,7 @@ typedef struct {
 	uint32_t sAcc;
 	uint16_t pDOP;
 	uint8_t reserved1;
-	uint8_t numSV;
+	uint8_t numSV;					/**< Number of SVs used in Nav Solution */
 	uint32_t reserved2;
 	uint8_t ck_a;
 	uint8_t ck_b;
@@ -213,7 +226,7 @@ typedef struct {
 	uint8_t ck_b;
 } gps_bin_nav_velned_packet_t;
 
-struct gps_bin_mon_hw_packet {
+struct gps_bin_mon_hw_ubx6_packet {
 	uint32_t pinSel;
 	uint32_t pinBank;
 	uint32_t pinDir;
@@ -233,6 +246,25 @@ struct gps_bin_mon_hw_packet {
 	uint32_t pullL;
 };
 
+struct gps_bin_mon_hw_ubx7_packet {
+	uint32_t pinSel;
+	uint32_t pinBank;
+	uint32_t pinDir;
+	uint32_t pinVal;
+	uint16_t noisePerMS;
+	uint16_t agcCnt;
+	uint8_t aStatus;
+	uint8_t aPower;
+	uint8_t flags;
+	uint8_t __reserved1;
+	uint32_t usedMask;
+	uint8_t VP[17];
+	uint8_t jamInd;
+	uint16_t __reserved3;
+	uint32_t pinIrq;
+	uint32_t pulLH;
+	uint32_t pullL;
+};
 
 //typedef struct {
 //	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
@@ -343,7 +375,14 @@ typedef enum {
 //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
 #pragma pack(pop)
 
-#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
+/* calculate parser rx buffer size dependent on NAV_SVINFO enabled or not */
+/* TODO: make this a command line option, allocate buffer dynamically */
+#define RECV_BUFFER_OVERHEAD	10	// add this to the maximum Rx msg payload size to account for msg overhead */
+#ifdef UBX_ENABLE_NAV_SVINFO
+	#define RECV_BUFFER_SIZE (UBX_NAV_SVINFO_RX_PAYLOAD_SIZE + RECV_BUFFER_OVERHEAD)
+#else
+	#define RECV_BUFFER_SIZE (UBX_MAX_RX_PAYLOAD_SIZE        + RECV_BUFFER_OVERHEAD)
+#endif
 
 class UBX : public GPS_Helper
 {