ubx: send update only if got POSLLH & VELNED & TIMEUTC

2 files changed, 100 insertions, 87 deletions

diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index cbfb61d00..c143eeb0c 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
 	_gps_position(gps_position),
 	_configured(false),
 	_waiting_for_ack(false),
+	_got_posllh(false),
+	_got_velned(false),
+	_got_timeutc(false),
 	_disable_cmd_last(0)
 {
 	decode_init();
@@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
-//	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;
-//	}
-//
-//	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
-//
-//	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-//		warnx("MSG CFG FAIL: MON HW");
-//		return 1;
-//	}
+	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;
+	}
+
+	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("MSG CFG FAIL: MON HW");
+		return 1;
+	}
 
 	_configured = true;
 	return 0;
@@ -275,9 +278,10 @@ UBX::receive(unsigned timeout)
 	bool handled = false;
 
 	while (true) {
+		bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
 
 		/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
-		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
@@ -286,7 +290,10 @@ UBX::receive(unsigned timeout)
 
 		} else if (ret == 0) {
 			/* return success after short delay after receiving a packet or timeout after long delay */
-			if (handled) {
+			if (ready_to_return) {
+				_got_posllh = false;
+				_got_velned = false;
+				_got_timeutc = false;
 				return 1;
 
 			} else {
@@ -438,6 +445,7 @@ UBX::handle_message()
 
 					_rate_count_lat_lon++;
 
+					_got_posllh = true;
 					ret = 1;
 					break;
 				}
@@ -482,65 +490,66 @@ UBX::handle_message()
 					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 					_gps_position->timestamp_time = hrt_absolute_time();
 
+					_got_timeutc = true;
 					ret = 1;
 					break;
 				}
 
-//			case UBX_MESSAGE_NAV_SVINFO: {
-//					//printf("GOT NAV_SVINFO\n");
-//					const int length_part1 = 8;
-//					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
-//					const int length_part2 = 12;
-//					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-//
-//					uint8_t satellites_used = 0;
-//					int i;
-//
-//					//printf("Number of Channels: %d\n", packet_part1->numCh);
-//					for (i = 0; i < packet_part1->numCh; i++) {
-//						/* set pointer to sattelite_i information */
-//						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
-//
-//						/* write satellite information to global storage */
-//						uint8_t sv_used = packet_part2->flags & 0x01;
-//
-//						if (sv_used) {
-//							/* count SVs used for NAV */
-//							satellites_used++;
-//						}
-//
-//						/* record info for all channels, whether or not the SV is used for NAV */
-//						_gps_position->satellite_used[i] = sv_used;
-//						_gps_position->satellite_snr[i] = packet_part2->cno;
-//						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-//						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-//						_gps_position->satellite_prn[i] = packet_part2->svid;
-//						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
-//					}
-//
-//					for (i = packet_part1->numCh; i < 20; i++) {
-//						/* unused channels have to be set to zero for e.g. MAVLink */
-//						_gps_position->satellite_prn[i] = 0;
-//						_gps_position->satellite_used[i] = 0;
-//						_gps_position->satellite_snr[i] = 0;
-//						_gps_position->satellite_elevation[i] = 0;
-//						_gps_position->satellite_azimuth[i] = 0;
-//					}
-//
-//					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
-//
-//					if (packet_part1->numCh > 0) {
-//						_gps_position->satellite_info_available = true;
-//
-//					} else {
-//						_gps_position->satellite_info_available = false;
-//					}
-//
-//					_gps_position->timestamp_satellites = hrt_absolute_time();
-//
-//					ret = 1;
-//					break;
-//				}
+			case UBX_MESSAGE_NAV_SVINFO: {
+					//printf("GOT NAV_SVINFO\n");
+					const int length_part1 = 8;
+					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
+					const int length_part2 = 12;
+					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+
+					uint8_t satellites_used = 0;
+					int i;
+
+					//printf("Number of Channels: %d\n", packet_part1->numCh);
+					for (i = 0; i < packet_part1->numCh; i++) {
+						/* set pointer to sattelite_i information */
+						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
+
+						/* write satellite information to global storage */
+						uint8_t sv_used = packet_part2->flags & 0x01;
+
+						if (sv_used) {
+							/* count SVs used for NAV */
+							satellites_used++;
+						}
+
+						/* record info for all channels, whether or not the SV is used for NAV */
+						_gps_position->satellite_used[i] = sv_used;
+						_gps_position->satellite_snr[i] = packet_part2->cno;
+						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+						_gps_position->satellite_prn[i] = packet_part2->svid;
+						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
+					}
+
+					for (i = packet_part1->numCh; i < 20; i++) {
+						/* unused channels have to be set to zero for e.g. MAVLink */
+						_gps_position->satellite_prn[i] = 0;
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
+					}
+
+					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+
+					if (packet_part1->numCh > 0) {
+						_gps_position->satellite_info_available = true;
+
+					} else {
+						_gps_position->satellite_info_available = false;
+					}
+
+					_gps_position->timestamp_satellites = hrt_absolute_time();
+
+					ret = 1;
+					break;
+				}
 
 			case UBX_MESSAGE_NAV_VELNED: {
 					// printf("GOT NAV_VELNED\n");
@@ -557,6 +566,7 @@ UBX::handle_message()
 
 					_rate_count_vel++;
 
+					_got_velned = true;
 					ret = 1;
 					break;
 				}
@@ -573,23 +583,23 @@ UBX::handle_message()
 				break;
 			}
 
-//		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;
-//
-//					_gps_position->noise_per_ms = p->noisePerMS;
-//					_gps_position->jamming_indicator = p->jamInd;
-//
-//					ret = 1;
-//					break;
-//				}
-//
-//			default:
-//				break;
-//			}
-//		}
+		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;
+
+					_gps_position->noise_per_ms = p->noisePerMS;
+					_gps_position->jamming_indicator = p->jamInd;
+
+					ret = 1;
+					break;
+				}
+
+			default:
+				break;
+			}
+		}
 
 		default:
 			break;
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 5cf47b60b..43d688893 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -397,6 +397,9 @@ private:
 	struct vehicle_gps_position_s *_gps_position;
 	bool			_configured;
 	bool			_waiting_for_ack;
+	bool			_got_posllh;
+	bool			_got_velned;
+	bool			_got_timeutc;
 	uint8_t			_message_class_needed;
 	uint8_t			_message_id_needed;
 	ubx_decode_state_t	_decode_state;