Changed parse interface, differentiation between config needed and position updated, working but might be solved more elegant

5 files changed, 81 insertions, 69 deletions

diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h
index 1ae27ed2f..dfde115ef 100644
--- a/apps/drivers/drv_gps.h
+++ b/apps/drivers/drv_gps.h
@@ -44,6 +44,8 @@
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
+#define GPS_DEFAULT_UART_PORT "/dev/ttyS3"
+
 #define GPS_DEVICE_PATH	"/dev/gps"
 
 typedef enum {
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
index 5905db6b8..28dc949d4 100644
--- a/apps/drivers/gps/gps.cpp
+++ b/apps/drivers/gps/gps.cpp
@@ -278,7 +278,7 @@ GPS::config()
 	int length = 0;
 	uint8_t	send_buffer[SEND_BUFFER_LENGTH];
 
-	_Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH);
+	_Helper->configure(send_buffer, length, SEND_BUFFER_LENGTH, _baudrate_changed, _baudrate);
 
 	/* The config message is sent sent at the old baudrate */
 	if (length > 0) {
@@ -339,6 +339,9 @@ GPS::task_main()
 	uint64_t last_rate_measurement = hrt_absolute_time();
 	unsigned last_rate_count = 0;
 
+	bool pos_updated;
+	bool config_needed_res;
+
 	/* loop handling received serial bytes and also configuring in between */
 	while (!_task_should_exit) {
 
@@ -391,17 +394,17 @@ GPS::task_main()
 		lock();
 
 
-
-
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
 		} else if (ret == 0) {
-			config();
+			/* no response from the GPS */
 			if (_config_needed == false) {
 				_config_needed = true;
-				warnx("lost GPS module");
+				warnx("GPS module timeout");
+				_Helper->reset();
 			}
+			config();
 		} else if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
 			if (fds[0].revents & POLLIN) {
@@ -416,39 +419,43 @@ GPS::task_main()
 
 				/* pass received bytes to the packet decoder */
 				int j;
-				int ret_parse = 0;
 				for (j = 0; j < count; j++) {
-					ret_parse += _Helper->parse(buf[j], &_report);
-				}
+					pos_updated = false;
+					config_needed_res = _config_needed;
+					_Helper->parse(buf[j], &_report, config_needed_res, pos_updated);
+
+					if (pos_updated) {
+						/* opportunistic publishing - else invalid data would end up on the bus */
+						if (_report_pub > 0) {
+							orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+						} else {
+							_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+						}
+						last_rate_count++;
+
+						/* measure update rate every 5 seconds */
+						if (hrt_absolute_time() - last_rate_measurement > 5000000) {
+							_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
+							last_rate_measurement = hrt_absolute_time();
+							last_rate_count = 0;
+						}
 
-				if (ret_parse < 0) {
-					/* This means something went wrong in the parser, let's reconfigure */
-					if (!_config_needed) {
-						_config_needed = true;
 					}
-					config();
-				} else if (ret_parse > 0) {
-					/* Looks like we got a valid position update, stop configuring and publish it */
-					if (_config_needed) {
+					if (config_needed_res == true && _config_needed == false) {
+						/* the parser told us that an error happened and reconfiguration is needed */
+						_config_needed = true;
+						warnx("GPS module lost");
+						_Helper->reset();
+						config();
+					} else if (config_needed_res == true && _config_needed == true) {
+						/* we are still configuring */
+						config();
+					} else if (config_needed_res == false && _config_needed == true) {
+						/* the parser is happy, stop configuring */
+						warnx("GPS module found");
 						_config_needed = false;
 					}
-
-					/* opportunistic publishing - else invalid data would end up on the bus */
-					if (_report_pub > 0) {
-						orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
-					} else {
-						_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
-					}
-					last_rate_count++;
-
-					/* measure update rate every 5 seconds */
-					if (hrt_absolute_time() - last_rate_measurement > 5000000) {
-						_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
-						last_rate_measurement = hrt_absolute_time();
-						last_rate_count = 0;
-					}
 				}
-				/* else if ret_parse == 0: just keep parsing */
 			}
 		}
 	}
@@ -662,7 +669,7 @@ gps_main(int argc, char *argv[])
 {
 
 	/* set to default */
-	char* device_name = "/dev/ttyS3";
+	char* device_name = GPS_DEFAULT_UART_PORT;
 
 	/*
 	 * Start/load the driver.
diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h
index 8a6b0148b..176b7eba8 100644
--- a/apps/drivers/gps/gps_helper.h
+++ b/apps/drivers/gps/gps_helper.h
@@ -40,9 +40,9 @@
 class GPS_Helper
 {
 public:
-	virtual void			reset() = 0;
-	virtual void			configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0;
-	virtual int 			parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0;
+	virtual void			reset(void) = 0;
+	virtual void			configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate) = 0;
+	virtual void 			parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0;
 };
 
 #endif /* GPS_HELPER_H */
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index a6f181a73..440ec74c5 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -72,7 +72,7 @@ UBX::reset()
 }
 
 void
-UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length)
+UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate)
 {
 	/* make sure the buffer, where the message is written to, is long enough */
 	assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length);
@@ -84,9 +84,10 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate,
 	if (!_waiting_for_ack) {
 		_waiting_for_ack = true;
 		if (_config_state == UBX_CONFIG_STATE_CONFIGURED) {
-			config_needed = false;
-			length = 0;
-			_config_state = UBX_CONFIG_STATE_PRT; /* set the state for next time */
+			/* This should never happen, the parser should set the flag,
+			 * if it should be reconfigured, reset() should be called first.
+			 */
+			warnx("ubx: already configured");
 			_waiting_for_ack = false;
 			return;
 		} else if (_config_state == UBX_CONFIG_STATE_PRT) {
@@ -246,12 +247,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate,
 	}
 }
 
-int
-UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
+void
+UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated)
 {
-	/* if no error happens and no new report is ready yet, ret will stay 0 */
-	int ret = 0;
-
 	switch (_decode_state) {
 		/* First, look for sync1 */
 		case UBX_DECODE_UNINIT:
@@ -382,7 +380,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 				default: //should not happen because we set the class
 					warnx("UBX Error, we set a class that we don't know");
 					decodeInit();
-					ret = -1;
+					config_needed = true;
 					break;
 				}
 				break;
@@ -417,7 +415,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 									gps_position->counter_pos_valid++;
 									gps_position->counter++;
 
+									/* Add timestamp to finish the report */
+									gps_position->timestamp = hrt_absolute_time();
+
 									_new_nav_posllh = true;
+									/* set flag to trigger publishing of new position */
+									pos_updated = true;
 
 								} else {
 									warnx("NAV_POSLLH: checksum invalid");
@@ -436,11 +439,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
 
 									gps_position->fix_type = packet->gpsFix;
-
-									gps_position->counter++;
 									gps_position->s_variance = packet->sAcc;
 									gps_position->p_variance = packet->pAcc;
 
+									gps_position->counter++;
+									gps_position->timestamp = hrt_absolute_time();
+
+
 									_new_nav_sol = true;
 
 								} else {
@@ -463,6 +468,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 									gps_position->epv =  packet->vDOP;
 
 									gps_position->counter++;
+									gps_position->timestamp = hrt_absolute_time();
 
 									_new_nav_dop = true;
 
@@ -496,6 +502,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 									gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 
 									gps_position->counter++;
+									gps_position->timestamp = hrt_absolute_time();
 
 									_new_nav_timeutc = true;
 
@@ -587,6 +594,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 
 									gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones
 									gps_position->counter++;
+									gps_position->timestamp = hrt_absolute_time();
 
 									// as this message arrives only with 1Hz and is not essential, we don't take it into account for the report
 
@@ -614,6 +622,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 									gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
 
 									gps_position->counter++;
+									gps_position->timestamp = hrt_absolute_time();
 
 									_new_nav_velned = true;
 
@@ -647,8 +656,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 //
 //								// Reset state machine to decode next packet
 //								decodeInit();
-//								return ret;
-//
 //								break;
 //							}
 
@@ -701,6 +708,8 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 									case UBX_CONFIG_STATE_MSG_NAV_VELNED:
 										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
 											_config_state = UBX_CONFIG_STATE_CONFIGURED;
+											/* set the flag to tell the driver that configuration was successful */
+											config_needed = false;
 										break;
 //									case UBX_CONFIG_STATE_MSG_RXM_SVSI:
 //										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
@@ -715,7 +724,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 
 							// Reset state machine to decode next packet
 							decodeInit();
-
 							break;
 						}
 
@@ -727,16 +735,14 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 							if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
 
 								warnx("UBX: Received: Not Acknowledged");
-								ret = 1;
-
+								/* configuration obviously not successful */
+								config_needed = true;
 							} else {
 								warnx("ACK_NAK: checksum invalid\n");
 							}
 
 							// Reset state machine to decode next packet
 							decodeInit();
-							return ret;
-
 							break;
 						}
 
@@ -752,7 +758,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 					} else {
 						warnx("buffer full, restarting");
 						decodeInit();
-						ret = -1;
+						config_needed = true;
 					}
 				break;
 		default:
@@ -762,21 +768,18 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position)
 	/* return 1 when position updates and the remaining packets updated at least once */
 	if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) {
 
-		/* Add timestamp to finish the report */
-		gps_position->timestamp = hrt_absolute_time();
-		/* Reset the flags */
+		/* we have received everything, this most probably means that the configuration is fine */
+		config_needed = false;
 
-		/* update on every position change, accept minor delay on other measurements */
+		/* Reset the flags */
 		_new_nav_posllh = false;
-		// _new_nav_timeutc = false;
-		// _new_nav_dop = false;
-		// _new_nav_sol = false;
-		// _new_nav_velned = false;
+		_new_nav_timeutc = false;
+		_new_nav_dop = false;
+		_new_nav_sol = false;
+		_new_nav_velned = false;
 
-		ret = 1;
 	}
-
-	return ret;
+	return;
 }
 
 void
diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h
index 0cac10f0a..d3c6c6d4c 100644
--- a/apps/drivers/gps/ubx.h
+++ b/apps/drivers/gps/ubx.h
@@ -341,8 +341,8 @@ public:
 	UBX();
 	~UBX();
 	void				reset(void);
-	void				configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length);
-	int 				parse(uint8_t, struct vehicle_gps_position_s*);
+	void				configure(uint8_t *buffer, int &length, const unsigned max_length, bool &baudrate_changed, unsigned &baudrate);
+	void 				parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);
 
 private:
 	/**