Another rewrite: most of the polling, reading and writing is now inside the GPS classes

8 files changed, 564 insertions, 548 deletions

diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h
index 67248efcd..1dda8ef0b 100644
--- a/apps/drivers/drv_gps.h
+++ b/apps/drivers/drv_gps.h
@@ -49,7 +49,8 @@
 #define GPS_DEVICE_PATH	"/dev/gps"
 
 typedef enum {
-	GPS_DRIVER_MODE_UBX = 0,
+	GPS_DRIVER_MODE_NONE = 0,
+	GPS_DRIVER_MODE_UBX,
 	GPS_DRIVER_MODE_MTK,
 	GPS_DRIVER_MODE_NMEA,
 } gps_driver_mode_t;
diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
index 76b5f4533..6d7cfcc68 100644
--- a/apps/drivers/gps/gps.cpp
+++ b/apps/drivers/gps/gps.cpp
@@ -36,10 +36,7 @@
  * Driver for the GPS on a serial port
  */
 
-#include <nuttx/config.h>
-
-#include <drivers/device/i2c.h>
-
+#include <nuttx/clock.h>
 #include <sys/types.h>
 #include <stdint.h>
 #include <stdio.h>
@@ -54,30 +51,24 @@
 #include <math.h>
 #include <unistd.h>
 #include <fcntl.h>
-
+#include <nuttx/config.h>
 #include <nuttx/arch.h>
-#include <nuttx/clock.h>
-
 #include <arch/board/board.h>
-
 #include <drivers/drv_hrt.h>
-
+#include <drivers/device/i2c.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
-
 #include <drivers/drv_gps.h>
-
+#include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
-//#include "ubx.h"
+#include "ubx.h"
 #include "mtk.h"
 
-#define SEND_BUFFER_LENGTH 100
-#define TIMEOUT 1000000 //1s
 
-#define NUMBER_OF_TRIES 5
-#define CONFIG_TIMEOUT 2000000
+#define TIMEOUT_5HZ 400
+#define RATE_MEASUREMENT_PERIOD 5000000
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) :
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
-	_mode(GPS_DRIVER_MODE_MTK),
+	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
 	_report_pub(-1),
 	_rate(0.0f)
@@ -279,12 +270,11 @@ GPS::task_main()
 		}
 
 		switch (_mode) {
-//			case GPS_DRIVER_MODE_UBX:
-//				_Helper = new UBX();
-//				break;
+			case GPS_DRIVER_MODE_UBX:
+				_Helper = new UBX(_serial_fd, &_report);
+				break;
 			case GPS_DRIVER_MODE_MTK:
-				printf("try new mtk\n");
-				_Helper = new MTK();
+				_Helper = new MTK(_serial_fd, &_report);
 				break;
 			case GPS_DRIVER_MODE_NMEA:
 				//_Helper = new NMEA(); //TODO: add NMEA
@@ -292,11 +282,11 @@ GPS::task_main()
 			default:
 				break;
 		}
-		// XXX unlock/lock makes sense?
 		unlock();
-		if (_Helper->configure(_serial_fd, _baudrate) == 0) {
-
-			while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) {
+		if (_Helper->configure(_baudrate) == 0) {
+			unlock();
+			while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+//				lock();
 				/* 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);
@@ -307,7 +297,7 @@ GPS::task_main()
 				last_rate_count++;
 
 				/* measure update rate every 5 seconds */
-				if (hrt_absolute_time() - last_rate_measurement > 5000000) {
+				if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
 					_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
 					last_rate_measurement = hrt_absolute_time();
 					last_rate_count = 0;
@@ -323,8 +313,26 @@ GPS::task_main()
 				_healthy = false;
 				_rate = 0.0f;
 			}
+
+			lock();
 		}
 		lock();
+
+		/* select next mode */
+		switch (_mode) {
+			case GPS_DRIVER_MODE_UBX:
+				_mode = GPS_DRIVER_MODE_MTK;
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_mode = GPS_DRIVER_MODE_UBX;
+				break;
+		//				case GPS_DRIVER_MODE_NMEA:
+		//					_mode = GPS_DRIVER_MODE_UBX;
+		//					break;
+			default:
+				break;
+		}
+
 	}
 	debug("exiting");
 
diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp
index 2caa82af1..9c1fad569 100644
--- a/apps/drivers/gps/gps_helper.cpp
+++ b/apps/drivers/gps/gps_helper.cpp
@@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
 
 	case 115200: speed = B115200; break;
 
+	warnx("try baudrate: %d\n", speed);
+
 	default:
 		warnx("ERROR: Unsupported baudrate: %d\n", baud);
 		return -EINVAL;
diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h
index 537ca819c..f3d3bc40b 100644
--- a/apps/drivers/gps/gps_helper.h
+++ b/apps/drivers/gps/gps_helper.h
@@ -44,8 +44,8 @@
 class GPS_Helper
 {
 public:
-	virtual int				configure(const int &fd, unsigned &baud) = 0;
-	virtual int 			receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0;
+	virtual int				configure(unsigned &baud) = 0;
+	virtual int 			receive(unsigned timeout) = 0;
 	int 					set_baudrate(const int &fd, unsigned baud);
 };
 
diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp
index 4ccbbfbe4..4762bd503 100644
--- a/apps/drivers/gps/mtk.cpp
+++ b/apps/drivers/gps/mtk.cpp
@@ -47,7 +47,9 @@
 #include "mtk.h"
 
 
-MTK::MTK() :
+MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
 _mtk_revision(0)
 {
 	decode_init();
@@ -58,40 +60,40 @@ MTK::~MTK()
 }
 
 int
-MTK::configure(const int &fd, unsigned &baudrate)
+MTK::configure(unsigned &baudrate)
 {
 	/* set baudrate first */
-	if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0)
+	if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
 		return -1;
 
 	baudrate = MTK_BAUDRATE;
 
 	/* Write config messages, don't wait for an answer */
-	if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
+	if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
 	usleep(10000);
 
-	if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
+	if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
 	usleep(10000);
 
-	if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) {
+	if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
 	usleep(10000);
 
-	if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) {
+	if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
 	usleep(10000);
 
-	if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
 		warnx("mtk: config write failed");
 		return -1;
 	}
@@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate)
 }
 
 int
-MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
+MTK::receive(unsigned timeout)
 {
 	/* poll descriptor */
 	pollfd fds[1];
-	fds[0].fd = fd;
+	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
 	uint8_t buf[32];
 	gps_mtk_packet_t packet;
 
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
 	int j = 0;
 	ssize_t count = 0;
 
@@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
 			/* pass received bytes to the packet decoder */
 			while (j < count) {
 				if (parse_char(buf[j], packet) > 0) {
-					handle_message(packet, gps_position);
+					handle_message(packet);
 					return 1;
 				}
+				/* in case we keep trying but only get crap from GPS */
+				if (time_started + timeout*1000 < hrt_absolute_time() ) {
+					return -1;
+				}
 				j++;
 			}
 			/* everything is read */
@@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
 		}
 
 		/* then poll for new data */
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ);
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
@@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
 				 * won't block even on a blocking device.  If more bytes are
 				 * available, we'll go back to poll() again...
 				 */
-				count = ::read(fd, buf, sizeof(buf));
+				count = ::read(_fd, buf, sizeof(buf));
 			}
 		}
 	}
@@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 }
 
 void
-MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position)
+MTK::handle_message(gps_mtk_packet_t &packet)
 {
 	if (_mtk_revision == 16) {
-		gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
-		gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
+		_gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
+		_gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
 	} else if (_mtk_revision == 19) {
-		gps_position.lat = packet.latitude; // both degrees*1e7
-		gps_position.lon = packet.longitude; // both degrees*1e7
+		_gps_position->lat = packet.latitude; // both degrees*1e7
+		_gps_position->lon = packet.longitude; // both degrees*1e7
 	} else {
 		warnx("mtk: unknown revision");
-		gps_position.lat = 0;
-		gps_position.lon = 0;
+		_gps_position->lat = 0;
+		_gps_position->lon = 0;
 	}
-	gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
-	gps_position.fix_type = packet.fix_type;
-	gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
-	gps_position.epv_m = 0.0; //unknown in mtk custom mode
-	gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
-	gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
-	gps_position.satellites_visible = packet.satellites;
+	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
+	_gps_position->fix_type = packet.fix_type;
+	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
+	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
+	_gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s
+	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
+	_gps_position->satellites_visible = packet.satellites;
 
 	/* convert time and date information to unix timestamp */
 	struct tm timeinfo; //TODO: test this conversion
@@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps
 	timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
 	time_t epoch = mktime(&timeinfo);
 
-	gps_position.time_gps_usec = epoch * 1e6; //TODO: test this
-	gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3;
-	gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time();
+	_gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
+	_gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
+	_gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
 
 	return;
 }
diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h
index 39063beab..d4e390b01 100644
--- a/apps/drivers/gps/mtk.h
+++ b/apps/drivers/gps/mtk.h
@@ -85,10 +85,10 @@ typedef struct {
 class MTK : public GPS_Helper
 {
 public:
-	MTK();
+	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
 	~MTK();
-	int					receive(const int &fd, struct vehicle_gps_position_s &gps_position);
-	int					configure(const int &fd, unsigned &baudrate);
+	int					receive(unsigned timeout);
+	int					configure(unsigned &baudrate);
 
 private:
 	/**
@@ -99,7 +99,7 @@ private:
 	/**
 	 * Handle the package once it has arrived
 	 */
-	void				handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position);
+	void				handle_message(gps_mtk_packet_t &packet);
 
 	/**
 	 * Reset the parse state machine for a fresh start
@@ -111,6 +111,8 @@ private:
 	 */
 	void				add_byte_to_checksum(uint8_t);
 
+	int					_fd;
+	struct vehicle_gps_position_s *_gps_position;
 	mtk_decode_state_t	_decode_state;
 	uint8_t				_mtk_revision;
 	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index 8e2396564..74cbc5aaf 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -37,6 +37,7 @@
 
 #include <unistd.h>
 #include <stdio.h>
+#include <poll.h>
 #include <math.h>
 #include <string.h>
 #include <assert.h>
@@ -47,172 +48,251 @@
 
 #include "ubx.h"
 
+#define UBX_CONFIG_TIMEOUT 100
 
-UBX::UBX() :
-_config_state(UBX_CONFIG_STATE_PRT),
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
 _waiting_for_ack(false)
 {
-	reset();
+	decode_init();
 }
 
 UBX::~UBX()
 {
 }
 
-void
-UBX::reset()
+int
+UBX::configure(unsigned &baudrate)
 {
-	decodeInit();
-	_config_state = UBX_CONFIG_STATE_PRT;
-	_waiting_for_ack = false;
-}
+	_waiting_for_ack = true;
+
+	/* try different baudrates */
+	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+
+	for (int baud_i = 0; baud_i < 5; baud_i++) {
+		baudrate = baudrates_to_try[baud_i];
+		set_baudrate(_fd, baudrate);
+
+		/* Send a CFG-PRT message to set the UBX protocol for in and out
+		 * and leave the baudrate as it is, we just want an ACK-ACK from this
+		 */
+		type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
+		/* Set everything else of the packet to 0, otherwise the module wont accept it */
+		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_PRT;
+
+		/* Define the package contents, don't change the baudrate */
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= baudrate;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
 
-void
-UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
-{
-	/* Only send a new config message when we got the ACK of the last one,
-	 * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command
-	 * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs.
-	 */
-	if (!_waiting_for_ack) {
-		_waiting_for_ack = true;
-		if (_config_state == UBX_CONFIG_STATE_CONFIGURED) {
-			/* 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) {
-
-			/* Send a CFG-PRT message to set the UBX protocol for in and out
-			 * and leave the baudrate as it is, we just want an ACK-ACK from this
-			 */
-			type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
-			/* Set everything else of the packet to 0, otherwise the module wont accept it */
-			memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
-
-			/* Define the package contents, don't change the baudrate */
-			cfg_prt_packet.clsID		= UBX_CLASS_CFG;
-			cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
-			cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
-			cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
-			cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
-			cfg_prt_packet.baudRate		= baudrate;
-			cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
-			cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
-
-			sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-
-		} else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) {
-
-			/* Send a CFG-PRT message again, this time change the baudrate */
-			type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
-			memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
-
-			cfg_prt_packet.clsID		= UBX_CLASS_CFG;
-			cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
-			cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
-			cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
-			cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
-			cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
-			cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
-			cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
-
-			sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-
-			/* If the new baudrate will be different from the current one, we should report that back to the driver */
-			if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
-				baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE;
-				baudrate_changed = true;
-				/* Don't wait for an ACK, we're switching baudrate and we might never get,
-				 * after that, start fresh */
-				reset();
-			}
+		/* Send a CFG-PRT message again, this time change the baudrate */
+
+		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
+		cfg_prt_packet.msgID		= UBX_MESSAGE_CFG_PRT;
+		cfg_prt_packet.length		= UBX_CFG_PRT_LENGTH;
+		cfg_prt_packet.portID		= UBX_CFG_PRT_PAYLOAD_PORTID;
+		cfg_prt_packet.mode			= UBX_CFG_PRT_PAYLOAD_MODE;
+		cfg_prt_packet.baudRate		= UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
+		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
+			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		}
 
-		} else if (_config_state == UBX_CONFIG_STATE_RATE) {
+		/* no ack is ecpected here, keep going configuring */
 
-			/* send a CFT-RATE message to define update rate */
-			type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
-			memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+		/* send a CFT-RATE message to define update rate */
+		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
+		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
 
-			cfg_rate_packet.clsID		= UBX_CLASS_CFG;
-			cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
-			cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
-			cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
-			cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
-			cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_RATE;
 
-			sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
+		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
+		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
+		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
+		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASRATE;
+		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
+		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
 
-		} else if (_config_state == UBX_CONFIG_STATE_NAV5) {
-			/* send a NAV5 message to set the options for the internal filter */
-			type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
-			memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
+		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
 
-			cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
-			cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
-			cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
-			cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
-			cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
-			cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+		/* send a NAV5 message to set the options for the internal filter */
+		type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
+		memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
 
-			sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
 
-		} else {
-			/* Catch the remaining config states here, they all need the same packet type */
+		cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
+		cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
+		cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
+		cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
+		cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
+		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
 
-			type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
-			memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
+		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
 
-			cfg_msg_packet.clsID        = UBX_CLASS_CFG;
-			cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
-			cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
-			/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
-			cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
+		type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
+		memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
 
-			switch (_config_state) {
-				case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
-					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
-					break;
-				case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
-					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
-					break;
-//				case UBX_CONFIG_STATE_MSG_NAV_DOP:
-//					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-//					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
-//					break;
-				case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
-					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
-					/* For satelites info 1Hz is enough */
-					cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
-					break;
-				case UBX_CONFIG_STATE_MSG_NAV_SOL:
-					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
-					break;
-				case UBX_CONFIG_STATE_MSG_NAV_VELNED:
-					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
-					break;
-//				case UBX_CONFIG_STATE_MSG_RXM_SVSI:
-//					cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
-//					cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
-//					break;
-				default:
-					break;
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_MSG;
+
+		cfg_msg_packet.clsID        = UBX_CLASS_CFG;
+		cfg_msg_packet.msgID        = UBX_MESSAGE_CFG_MSG;
+		cfg_msg_packet.length       = UBX_CFG_MSG_LENGTH;
+		/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
+		cfg_msg_packet.rate[1]      = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
+		/* For satelites info 1Hz is enough */
+		cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+//		cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
+
+//		cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
+//		cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
+
+		_waiting_for_ack = false;
+		return 0;
+	}
+	return -1;
+}
+
+int
+UBX::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = _fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+
+	/* timeout additional to poll */
+	uint64_t time_started = hrt_absolute_time();
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* pass received bytes to the packet decoder */
+		while (j < count) {
+			if (parse_char(buf[j]) > 0) {
+				/* return to configure during configuration or to the gps driver during normal work
+				 * if a packet has arrived */
+				 if (handle_message() > 0)
+					return 1;
+			}
+			/* in case we keep trying but only get crap from GPS */
+			if (time_started + timeout*1000 < hrt_absolute_time() ) {
+				return -1;
 			}
+			j++;
+		}
 
-			sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
+		/* everything is read */
+		j = count = 0;
+
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+
+		if (ret < 0) {
+			/* something went wrong when polling */
+			return -1;
+
+		} else if (ret == 0) {
+			/* Timeout */
+			return -1;
+
+		} else if (ret > 0) {
+			/* if we have new data from GPS, go handle it */
+			if (fds[0].revents & POLLIN) {
+				/*
+				 * We are here because poll says there is some data, so this
+				 * won't block even on a blocking device.  If more bytes are
+				 * available, we'll go back to poll() again...
+				 */
+				count = ::read(_fd, buf, sizeof(buf));
+			}
 		}
 	}
 }
 
-void
-UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated)
+int
+UBX::parse_char(uint8_t b)
 {
 	switch (_decode_state) {
 		/* First, look for sync1 */
@@ -227,13 +307,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 				_decode_state = UBX_DECODE_GOT_SYNC2;
 			} else {
 				/* Second start symbol was wrong, reset state machine */
-				decodeInit();
+				decode_init();
 			}
 			break;
 		/* Now look for class */
 		case UBX_DECODE_GOT_SYNC2:
 			/* everything except sync1 and sync2 needs to be added to the checksum */
-			addByteToChecksum(b);
+			add_byte_to_checksum(b);
 			/* check for known class */
 			switch (b) {
 				case UBX_CLASS_ACK:
@@ -256,12 +336,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 					_message_class = CFG;
 					break;
 				default: //unknown class: reset state machine
-					decodeInit();
+					decode_init();
 					break;
 			}
 			break;
 		case UBX_DECODE_GOT_CLASS:
-			addByteToChecksum(b);
+			add_byte_to_checksum(b);
 			switch (_message_class) {
 				case NAV:
 					switch (b) {
@@ -296,7 +376,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 						break;
 
 					default: //unknown class: reset state machine, should not happen
-						decodeInit();
+						decode_init();
 						break;
 					}
 					break;
@@ -308,7 +388,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 //						break;
 //
 //					default: //unknown class: reset state machine, should not happen
-//						decodeInit();
+//						decode_init();
 //						break;
 //					}
 //					break;
@@ -321,7 +401,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 						break;
 
 					default: //unknown class: reset state machine, should not happen
-						decodeInit();
+						decode_init();
 						break;
 					}
 					break;
@@ -337,384 +417,282 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 						_message_id = ACK_NAK;
 						break;
 					default: //unknown class: reset state machine, should not happen
-						decodeInit();
+						decode_init();
 						break;
 					}
 					break;
 				default: //should not happen because we set the class
 					warnx("UBX Error, we set a class that we don't know");
-					decodeInit();
-					config_needed = true;
+					decode_init();
+//					config_needed = true;
 					break;
 				}
 				break;
 			case UBX_DECODE_GOT_MESSAGEID:
-				addByteToChecksum(b);
+				add_byte_to_checksum(b);
 				_payload_size = b; //this is the first length byte
 				_decode_state = UBX_DECODE_GOT_LENGTH1;
 				break;
 			case UBX_DECODE_GOT_LENGTH1:
-				addByteToChecksum(b);
+				add_byte_to_checksum(b);
 				_payload_size += b << 8; // here comes the second byte of length
 				_decode_state = UBX_DECODE_GOT_LENGTH2;
 				break;
 			case UBX_DECODE_GOT_LENGTH2:
 				/* Add to checksum if not yet at checksum byte */
 				if (_rx_count < _payload_size)
-					addByteToChecksum(b);
+					add_byte_to_checksum(b);
 					_rx_buffer[_rx_count] = b;
 					/* once the payload has arrived, we can process the information */
 					if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
-						switch (_message_id) { //this enum is unique for all ids --> no need to check the class
-							case NAV_POSLLH: {
-//								printf("GOT NAV_POSLLH MESSAGE\n");
-								gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
 
-								//Check if checksum is valid and the store the gps information
-								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-									gps_position->lat = packet->lat;
-									gps_position->lon = packet->lon;
-									gps_position->alt = packet->height_msl;
-
-									gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
-									gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+						/* compare checksum */
+						if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
+							return 1;
+						} else {
+							decode_init();
+							return -1;
+							warnx("ubx: Checksum wrong");
+						}
 
-									/* Add timestamp to finish the report */
-									gps_position->timestamp_position = hrt_absolute_time();
+						return 1;
+					} else if (_rx_count < RECV_BUFFER_SIZE) {
+						_rx_count++;
+					} else {
+						warnx("ubx: buffer full");
+						decode_init();
+						return -1;
+					}
+				break;
+		default:
+			break;
+	}
+	return 0; //XXX ?
+}
 
-									/* set flag to trigger publishing of new position */
-									pos_updated = true;
 
-								} else {
-									warnx("NAV_POSLLH: checksum invalid");
-								}
+int
+UBX::handle_message()
+{
+	int ret = 0;
 
-								// Reset state machine to decode next packet
-								decodeInit();
-								break;
-							}
+	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
+		case NAV_POSLLH: {
+//			printf("GOT NAV_POSLLH MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
 
-							case NAV_SOL: {
-//								printf("GOT NAV_SOL MESSAGE\n");
-								gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+				_gps_position->lat = packet->lat;
+				_gps_position->lon = packet->lon;
+				_gps_position->alt = packet->height_msl;
 
-								//Check if checksum is valid and the store the gps information
-								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
+				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
+				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
 
-									gps_position->fix_type = packet->gpsFix;
-									gps_position->s_variance_m_s = packet->sAcc;
-									gps_position->p_variance_m = packet->pAcc;
+				/* Add timestamp to finish the report */
+				_gps_position->timestamp_position = hrt_absolute_time();
+				/* only return 1 when new position is available */
+				ret = 1;
+			}
+			break;
+		}
 
-									gps_position->timestamp_variance = hrt_absolute_time();
+		case NAV_SOL: {
+//			printf("GOT NAV_SOL MESSAGE\n");
+			if (!_waiting_for_ack) {
+				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
 
-								} else {
-									warnx("NAV_SOL: checksum invalid");
-								}
+				_gps_position->fix_type = packet->gpsFix;
+				_gps_position->s_variance_m_s = packet->sAcc;
+				_gps_position->p_variance_m = packet->pAcc;
 
-								// Reset state machine to decode next packet
-								decodeInit();
-								break;
-							}
+				_gps_position->timestamp_variance = hrt_absolute_time();
+			}
+			break;
+		}
 
-//							case NAV_DOP: {
-////								printf("GOT NAV_DOP MESSAGE\n");
-//								gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
-//
-//								//Check if checksum is valid and the store the gps information
-//								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-//
-//									gps_position->eph_m =  packet->hDOP;
-//									gps_position->epv =  packet->vDOP;
-//
-//									gps_position->timestamp_posdilution = hrt_absolute_time();
-//
-//									_new_nav_dop = true;
+//		case NAV_DOP: {
+////		printf("GOT NAV_DOP MESSAGE\n");
+//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
 //
-//								} else {
-//									warnx("NAV_DOP: checksum invalid");
-//								}
+//			_gps_position->eph_m =  packet->hDOP;
+//			_gps_position->epv =  packet->vDOP;
 //
-//								// Reset state machine to decode next packet
-//								decodeInit();
-//								break;
-//							}
-
-							case NAV_TIMEUTC: {
-//								printf("GOT NAV_TIMEUTC MESSAGE\n");
-								gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
-
-								//Check if checksum is valid and the store the gps information
-								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-									//convert to unix timestamp
-									struct tm timeinfo;
-									timeinfo.tm_year = packet->year - 1900;
-									timeinfo.tm_mon = packet->month - 1;
-									timeinfo.tm_mday = packet->day;
-									timeinfo.tm_hour = packet->hour;
-									timeinfo.tm_min = packet->min;
-									timeinfo.tm_sec = packet->sec;
-
-									time_t epoch = mktime(&timeinfo);
-
-									gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-									gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
-
-									gps_position->timestamp_time = hrt_absolute_time();
-
-								} else {
-									warnx("NAV_TIMEUTC: checksum invalid");
-								}
-
-								// Reset state machine to decode next packet
-								decodeInit();
-								break;
-							}
-
-							case NAV_SVINFO: {
-//								printf("GOT NAV_SVINFO MESSAGE\n");
-
-								//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
-								const int length_part1 = 8;
-								char _rx_buffer_part1[length_part1];
-								memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-								gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
-
-								//read checksum
-								const int length_part3 = 2;
-								char _rx_buffer_part3[length_part3];
-								memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
-								gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3;
-
-								//Check if checksum is valid and then store the gps information
-								if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) {
-									//definitions needed to read numCh elements from the buffer:
-									const int length_part2 = 12;
-									gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-									char _rx_buffer_part2[length_part2]; //for temporal storage
-
-									uint8_t satellites_used = 0;
-									int i;
-
-									for (i = 0; i < packet_part1->numCh; i++) { //for each channel
-
-										/* Get satellite information from the buffer */
-										memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
-										packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
-
-
-										/* Write satellite information in the global storage */
-										gps_position->satellite_prn[i] = packet_part2->svid;
-
-										//if satellite information is healthy store the data
-										uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
-
-										if (!unhealthy) {
-											if ((packet_part2->flags) & 1) { //flags is a bitfield
-												gps_position->satellite_used[i] = 1;
-												satellites_used++;
-
-											} else {
-												gps_position->satellite_used[i] = 0;
-											}
-
-											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);
-
-										} else {
-											gps_position->satellite_used[i] = 0;
-											gps_position->satellite_snr[i] = 0;
-											gps_position->satellite_elevation[i] = 0;
-											gps_position->satellite_azimuth[i] = 0;
-										}
-
-									}
-
-									for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
-										/* 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
-
-									/* set timestamp if any sat info is available */
-									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();
-
-								} else {
-									warnx("NAV_SVINFO: checksum invalid");
-								}
-
-								// Reset state machine to decode next packet
-								decodeInit();
-								break;
-							}
-
-							case NAV_VELNED: {
-//								printf("GOT NAV_VELNED MESSAGE\n");
-								gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
-
-								//Check if checksum is valid and the store the gps information
-								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-
-									gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
-									gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
-									gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
-									gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
-									gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
-									gps_position->vel_ned_valid = true;
-									gps_position->timestamp_velocity = hrt_absolute_time();
-
-								} else {
-									warnx("NAV_VELNED: checksum invalid");
-								}
-
-								// Reset state machine to decode next packet
-								decodeInit();
-								break;
-							}
-
-//							case RXM_SVSI: {
-//								printf("GOT RXM_SVSI MESSAGE\n");
-//								const int length_part1 = 7;
-//								char _rx_buffer_part1[length_part1];
-//								memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-//								gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
+//			_gps_position->timestamp_posdilution = hrt_absolute_time();
 //
-//								//Check if checksum is valid and the store the gps information
-//								if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
+//			_new_nav_dop = true;
 //
-//									gps_position->satellites_visible = packet->numVis;
-//									gps_position->counter++;
-//									_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
-//									ret = 1;
-//
-//								} else {
-//									warnx("RXM_SVSI: checksum invalid\n");
-//								}
-//
-//								// Reset state machine to decode next packet
-//								decodeInit();
-//								break;
-//							}
-
-						case ACK_ACK: {
-//							printf("GOT ACK_ACK\n");
-							gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
-
-							//Check if checksum is valid
-							if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-
-								_waiting_for_ack = false;
-
-								switch (_config_state) {
-									case UBX_CONFIG_STATE_PRT:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
-											_config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE;
-										break;
-									case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
-											_config_state = UBX_CONFIG_STATE_RATE;
-										break;
-									case UBX_CONFIG_STATE_RATE:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE)
-											_config_state = UBX_CONFIG_STATE_NAV5;
-										break;
-									case UBX_CONFIG_STATE_NAV5:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
-											_config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH;
-										break;
-									case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
-											_config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC;
-										break;
-									case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
-											_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
-										break;
-//									case UBX_CONFIG_STATE_MSG_NAV_DOP:
-//										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
-//											_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
-//										break;
-									case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
-											_config_state = UBX_CONFIG_STATE_MSG_NAV_SOL;
-										break;
-									case UBX_CONFIG_STATE_MSG_NAV_SOL:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
-											_config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED;
-										break;
-									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)
-//											_config_state = UBX_CONFIG_STATE_CONFIGURED;
-//										break;
-									default:
-										break;
-								}
-							} else {
-								warnx("ACK_ACK: checksum invalid");
-							}
-
-							// Reset state machine to decode next packet
-							decodeInit();
-							break;
-						}
+//			break;
+//		}
 
-						case ACK_NAK: {
-//							printf("GOT ACK_NAK\n");
-							gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer;
+		case NAV_TIMEUTC: {
+//			printf("GOT NAV_TIMEUTC MESSAGE\n");
 
-							//Check if checksum is valid
-							if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
+			if (!_waiting_for_ack) {
+				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
 
-								warnx("UBX: Received: Not Acknowledged");
-								/* configuration obviously not successful */
-								config_needed = true;
-							} else {
-								warnx("ACK_NAK: checksum invalid\n");
-							}
+				//convert to unix timestamp
+				struct tm timeinfo;
+				timeinfo.tm_year = packet->year - 1900;
+				timeinfo.tm_mon = packet->month - 1;
+				timeinfo.tm_mday = packet->day;
+				timeinfo.tm_hour = packet->hour;
+				timeinfo.tm_min = packet->min;
+				timeinfo.tm_sec = packet->sec;
 
-							// Reset state machine to decode next packet
-							decodeInit();
-							break;
-						}
+				time_t epoch = mktime(&timeinfo);
 
-						default: //we don't know the message
-							warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
-							decodeInit();
+				_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+
+				_gps_position->timestamp_time = hrt_absolute_time();
+			}
+			break;
+		}
 
-							break;
+		case NAV_SVINFO: {
+//			printf("GOT NAV_SVINFO MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
+				const int length_part1 = 8;
+				char _rx_buffer_part1[length_part1];
+				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
+
+				//read checksum
+				const int length_part3 = 2;
+				char _rx_buffer_part3[length_part3];
+				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
+
+				//definitions needed to read numCh elements from the buffer:
+				const int length_part2 = 12;
+				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
+				char _rx_buffer_part2[length_part2]; //for temporal storage
+
+				uint8_t satellites_used = 0;
+				int i;
+
+				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
+
+					/* Get satellite information from the buffer */
+					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
+					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
+
+
+					/* Write satellite information in the global storage */
+					_gps_position->satellite_prn[i] = packet_part2->svid;
+
+					//if satellite information is healthy store the data
+					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
+
+					if (!unhealthy) {
+						if ((packet_part2->flags) & 1) { //flags is a bitfield
+							_gps_position->satellite_used[i] = 1;
+							satellites_used++;
+
+						} else {
+							_gps_position->satellite_used[i] = 0;
 						}
-					} // end if _rx_count high enough
-					else if (_rx_count < RECV_BUFFER_SIZE) {
-						_rx_count++;
+
+						_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);
+
 					} else {
-						warnx("buffer full, restarting");
-						decodeInit();
-						config_needed = true;
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
 					}
-				break;
-		default:
+
+				}
+
+				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
+					/* 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
+
+				/* set timestamp if any sat info is available */
+				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();
+			}
+
 			break;
-	}
-	return;
+		}
+
+		case NAV_VELNED: {
+//			printf("GOT NAV_VELNED MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+
+				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
+				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
+				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->vel_ned_valid = true;
+				_gps_position->timestamp_velocity = hrt_absolute_time();
+			}
+
+			break;
+		}
+
+//		case RXM_SVSI: {
+//				printf("GOT RXM_SVSI MESSAGE\n");
+//				const int length_part1 = 7;
+//				char _rx_buffer_part1[length_part1];
+//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
+//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
+//
+//				_gps_position->satellites_visible = packet->numVis;
+//				_gps_position->counter++;
+//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
+//
+//			break;
+//		}
+		case ACK_ACK: {
+//			printf("GOT ACK_ACK\n");
+			gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
+
+			if (_waiting_for_ack) {
+				if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
+					ret = 1;
+				}
+			}
+		}
+		break;
+
+		case ACK_NAK: {
+//			printf("GOT ACK_NAK\n");
+			warnx("UBX: Received: Not Acknowledged");
+			/* configuration obviously not successful */
+			ret = -1;
+			break;
+		}
+
+		default: //we don't know the message
+			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
+			ret = -1;
+			break;
+		}
+	// end if _rx_count high enough
+	decode_init();
+	return ret; //XXX?
 }
 
 void
-UBX::decodeInit(void)
+UBX::decode_init(void)
 {
 	_rx_ck_a = 0;
 	_rx_ck_b = 0;
@@ -726,14 +704,14 @@ UBX::decodeInit(void)
 }
 
 void
-UBX::addByteToChecksum(uint8_t b)
+UBX::add_byte_to_checksum(uint8_t b)
 {
 	_rx_ck_a = _rx_ck_a + b;
 	_rx_ck_b = _rx_ck_b + _rx_ck_a;
 }
 
 void
-UBX::addChecksumToMessage(uint8_t* message, const unsigned length)
+UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
 {
 	uint8_t ck_a = 0;
 	uint8_t ck_b = 0;
@@ -749,12 +727,12 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length)
 }
 
 void
-UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length)
+UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
 {
 	ssize_t ret = 0;
 
 	/* Calculate the checksum now */
-	addChecksumToMessage(packet, length);
+	add_checksum_to_message(packet, length);
 
 	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
 
diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h
index c420e83b9..e3dd1c7ea 100644
--- a/apps/drivers/gps/ubx.h
+++ b/apps/drivers/gps/ubx.h
@@ -86,6 +86,8 @@
 #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 
+#define UBX_MAX_PAYLOAD_LENGTH 500
+
 // ************
 /** the structures of the binary packets */
 #pragma pack(push, 1)
@@ -337,35 +339,49 @@ typedef enum {
 class UBX : public GPS_Helper
 {
 public:
-	UBX();
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
 	~UBX();
-	void				reset(void);
-	void				configure(const int &fd, bool &baudrate_changed, unsigned &baudrate);
-	void 				parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);
+	int					receive(unsigned timeout);
+	int					configure(unsigned &baudrate);
 
 private:
+
+	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	int					handle_message(void);
+
 	/**
 	 * Reset the parse state machine for a fresh start
 	 */
-	void				decodeInit(void);
+	void				decode_init(void);
 
 	/**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void				addByteToChecksum(uint8_t);
+	void				add_byte_to_checksum(uint8_t);
 
 	/**
 	 * Add the two checksum bytes to an outgoing message
 	 */
-	void				addChecksumToMessage(uint8_t* message, const unsigned length);
+	void				add_checksum_to_message(uint8_t* message, const unsigned length);
 
 	/**
 	 * Helper to send a config packet
 	 */
-	void				sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length);
+	void				send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
 
+	int 				_fd;
+	struct vehicle_gps_position_s *_gps_position;
 	ubx_config_state_t	_config_state;
 	bool 				_waiting_for_ack;
+	uint8_t				_clsID_needed;
+	uint8_t				_msgID_needed;
 	ubx_decode_state_t	_decode_state;
 	uint8_t				_rx_buffer[RECV_BUFFER_SIZE];
 	unsigned			_rx_count;