Restructered the parsing/configuring, MTK working

5 files changed, 314 insertions, 303 deletions

diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
index c749e8b7f..76b5f4533 100644
--- a/apps/drivers/gps/gps.cpp
+++ b/apps/drivers/gps/gps.cpp
@@ -53,7 +53,6 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
-#include <termios.h>
 #include <fcntl.h>
 
 #include <nuttx/arch.h>
@@ -71,7 +70,7 @@
 
 #include <uORB/topics/vehicle_gps_position.h>
 
-#include "ubx.h"
+//#include "ubx.h"
 #include "mtk.h"
 
 #define SEND_BUFFER_LENGTH 100
@@ -113,10 +112,8 @@ private:
 	int				_serial_fd;					///< serial interface to GPS
 	unsigned			_baudrate;					///< current baudrate
 	char				_port[20];					///< device / serial port path
-	const unsigned			_baudrates_to_try[NUMBER_OF_TRIES];		///< try different baudrates that GPS could be set to
-	const gps_driver_mode_t	_modes_to_try[NUMBER_OF_TRIES];			///< try different modes
 	volatile int			_task;						//< worker task
-	bool				_config_needed;					///< flag to signal that configuration of GPS is needed
+	bool				_healthy;					///< flag to signal if the GPS is ok
 	bool 				_baudrate_changed;				///< flag to signal that the baudrate with the GPS has changed
 	bool				_mode_changed;					///< flag that the GPS mode has changed
 	gps_driver_mode_t		_mode;						///< current mode
@@ -171,11 +168,9 @@ GPS	*g_dev;
 GPS::GPS(const char* uart_path) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
-	_baudrates_to_try({9600, 38400, 57600, 115200, 38400}),
-	_modes_to_try({GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK}),
-	_config_needed(true),
-	_baudrate_changed(false),
+	_healthy(false),
 	_mode_changed(false),
+	_mode(GPS_DRIVER_MODE_MTK),
 	_Helper(nullptr),
 	_report_pub(-1),
 	_rate(0.0f)
@@ -251,20 +246,6 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
 }
 
 void
-GPS::config()
-{
-	_Helper->configure(_serial_fd, _baudrate_changed, _baudrate);
-
-	/* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed
-	 * from 9600 to 38400
-	 */
-	if (_baudrate_changed) {
-		set_baudrate(_baudrate);
-		_baudrate_changed = false;
-	}
-}
-
-void
 GPS::task_main_trampoline(void *arg)
 {
 	g_dev->task_main();
@@ -276,10 +257,7 @@ GPS::task_main()
 	log("starting");
 
 	/* open the serial port */
-	_serial_fd = ::open(_port, O_RDWR); //TODO make the device dynamic depending on startup parameters
-
-	/* buffer to read from the serial port */
-	uint8_t buf[32];
+	_serial_fd = ::open(_port, O_RDWR);
 
 	if (_serial_fd < 0) {
 		log("failed to open serial port: %s err: %d", _port, errno);
@@ -288,151 +266,65 @@ GPS::task_main()
 		_exit(1);
 	}
 
-	/* poll descriptor */
-	pollfd fds[1];
-	fds[0].fd = _serial_fd;
-	fds[0].events = POLLIN;
-
-	/* lock against the ioctl handler */
-	lock();
-
-	unsigned try_i = 0;
-	_baudrate = _baudrates_to_try[try_i];
-	_mode = _modes_to_try[try_i];
-	_mode_changed = true;
-	set_baudrate(_baudrate);
-
-	uint64_t time_before_configuration = hrt_absolute_time();
-
 	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) {
 
-		/* If a configuration does not finish in the config timeout, change the baudrate */
-		if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) {
-			/* loop through possible modes/baudrates */
-			try_i = (try_i + 1) % NUMBER_OF_TRIES;
-			_baudrate = _baudrates_to_try[try_i];
-			set_baudrate(_baudrate);
-			if (_mode != _modes_to_try[try_i]) {
-				_mode_changed = true;
-			}
-			_mode = _modes_to_try[try_i];
-
-			if (_Helper != nullptr) {
-				_Helper->reset();
-			}
-			time_before_configuration = hrt_absolute_time();
+		if (_Helper != nullptr) {
+			delete(_Helper);
+			/* set to zero to ensure parser is not used while not instantiated */
+			_Helper = nullptr;
 		}
 
-		if (_mode_changed) {
-			if (_Helper != nullptr) {
-				delete(_Helper);
-				/* set to zero to ensure parser is not used while not instantiated */
-				_Helper = nullptr;
-			}
-
-			switch (_mode) {
-			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX();
-				break;
+		switch (_mode) {
+//			case GPS_DRIVER_MODE_UBX:
+//				_Helper = new UBX();
+//				break;
 			case GPS_DRIVER_MODE_MTK:
+				printf("try new mtk\n");
 				_Helper = new MTK();
 				break;
 			case GPS_DRIVER_MODE_NMEA:
-				//_Helper = new NMEA();
+				//_Helper = new NMEA(); //TODO: add NMEA
 				break;
 			default:
 				break;
-			}
-			_mode_changed = false;
 		}
-
-
-
-		/* during configuration, the timeout should be small, so that we can send config messages in between parsing,
-		 * but during normal operation, it should never timeout because updates should arrive with 5Hz */
-		int poll_timeout;
-		if (_config_needed) {
-			poll_timeout = 50;
-		} else {
-			poll_timeout = 400;
-		}
-		/* sleep waiting for data, but no more than the poll timeout */
+		// XXX unlock/lock makes sense?
 		unlock();
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), poll_timeout);
-		lock();
+		if (_Helper->configure(_serial_fd, _baudrate) == 0) {
+
+			while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) {
+				/* 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++;
 
-		/* this would be bad... */
-		if (ret < 0) {
-			log("poll error %d", errno);
-		} else if (ret == 0) {
-			/* no response from the GPS */
-			if (_config_needed == false) {
-				_config_needed = true;
-				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) {
-				int count;
-
-				/*
-				 * 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(_serial_fd, buf, sizeof(buf));
-
-				/* pass received bytes to the packet decoder */
-				int j;
-				for (j = 0; j < count; j++) {
-					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 (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;
-					}
+				/* 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 (!_healthy) {
+					warnx("module found");
+					_healthy = true;
 				}
 			}
+			if (_healthy) {
+				warnx("module lost");
+				_healthy = false;
+				_rate = 0.0f;
+			}
 		}
+		lock();
 	}
 	debug("exiting");
 
@@ -443,59 +335,12 @@ GPS::task_main()
 	_exit(0);
 }
 
-int
-GPS::set_baudrate(unsigned baud)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
 
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n", baud);
-		return -EINVAL;
-	}
-	struct termios uart_config;
-	int termios_state;
-
-	/* fill the struct for the new configuration */
-	tcgetattr(_serial_fd, &uart_config);
-
-	/* clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-	/* no parity, one stop bit */
-	uart_config.c_cflag &= ~(CSTOPB | PARENB);
-
-	/* set baud rate */
-	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
-		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
-		return -1;
-	}
-	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
-		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
-		return -1;
-	}
-	if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate (tcsetattr)\n");
-		return -1;
-	}
-	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
-	return 0;
-}
 
 void
 GPS::cmd_reset()
 {
-	_config_needed = true;
+	//XXX add reset?
 }
 
 void
@@ -514,7 +359,7 @@ GPS::print_info()
 		default:
 			break;
 	}
-	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK");
+	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
 	if (_report.timestamp_position != 0) {
 		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
 			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp
new file mode 100644
index 000000000..2caa82af1
--- /dev/null
+++ b/apps/drivers/gps/gps_helper.cpp
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <termios.h>
+#include <errno.h>
+#include <systemlib/err.h>
+#include "gps_helper.h"
+
+/* @file gps_helper.cpp */
+
+int
+GPS_Helper::set_baudrate(const int &fd, unsigned baud)
+{
+	/* process baud rate */
+	int speed;
+
+	switch (baud) {
+	case 9600:   speed = B9600;   break;
+
+	case 19200:  speed = B19200;  break;
+
+	case 38400:  speed = B38400;  break;
+
+	case 57600:  speed = B57600;  break;
+
+	case 115200: speed = B115200; break;
+
+	default:
+		warnx("ERROR: Unsupported baudrate: %d\n", baud);
+		return -EINVAL;
+	}
+	struct termios uart_config;
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
+		return -1;
+	}
+	if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERROR setting baudrate (tcsetattr)\n");
+		return -1;
+	}
+	/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
+	return 0;
+}
diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h
index 576692c2a..537ca819c 100644
--- a/apps/drivers/gps/gps_helper.h
+++ b/apps/drivers/gps/gps_helper.h
@@ -33,17 +33,20 @@
  *
  ****************************************************************************/
 
-/* @file U-Blox protocol definitions */
+/* @file gps_helper.h */
 
 #ifndef GPS_HELPER_H
 #define GPS_HELPER_H
 
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
 class GPS_Helper
 {
 public:
-	virtual void			reset(void) = 0;
-	virtual void			configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) = 0;
-	virtual void 			parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated) = 0;
+	virtual int				configure(const int &fd, unsigned &baud) = 0;
+	virtual int 			receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0;
+	int 					set_baudrate(const int &fd, unsigned baud);
 };
 
 #endif /* GPS_HELPER_H */
diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp
index 026b0660b..4ccbbfbe4 100644
--- a/apps/drivers/gps/mtk.cpp
+++ b/apps/drivers/gps/mtk.cpp
@@ -37,85 +37,143 @@
 
 #include <unistd.h>
 #include <stdio.h>
+#include <poll.h>
 #include <math.h>
 #include <string.h>
 #include <assert.h>
 #include <systemlib/err.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_gps_position.h>
 #include <drivers/drv_hrt.h>
 
 #include "mtk.h"
 
 
 MTK::MTK() :
-_config_sent(false),
 _mtk_revision(0)
 {
-	decodeInit();
+	decode_init();
 }
 
 MTK::~MTK()
 {
 }
 
-void
-MTK::reset()
+int
+MTK::configure(const int &fd, unsigned &baudrate)
 {
+	/* set baudrate first */
+	if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0)
+		return -1;
 
-}
+	baudrate = MTK_BAUDRATE;
 
-void
-MTK::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
-{
-	if (_config_sent == false) {
+	/* Write config messages, don't wait for an answer */
+	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_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ)))
-			warnx("mtk: config write failed");
-		usleep(10000);
+	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(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY)))
-			warnx("mtk: config write failed");
-		usleep(10000);
+	if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
 
-		if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON)))
-			warnx("mtk: config write failed");
-		usleep(10000);
+	if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
 
-		if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON)))
-			warnx("mtk: config write failed");
-		usleep(10000);
+	if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
 
-		if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF)))
-			warnx("mtk: config write failed");
+	return 0;
+}
 
-		_config_sent = true;
-	}
+int
+MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	fds[0].fd = fd;
+	fds[0].events = POLLIN;
+
+	uint8_t buf[32];
+	gps_mtk_packet_t packet;
+
+	int j = 0;
+	ssize_t count = 0;
+
+	while (true) {
+
+		/* first read whatever is left */
+		if (j < count) {
+			/* pass received bytes to the packet decoder */
+			while (j < count) {
+				if (parse_char(buf[j], packet) > 0) {
+					handle_message(packet, gps_position);
+					return 1;
+				}
+				j++;
+			}
+			/* everything is read */
+			j = count = 0;
+		}
 
-	return;
+		/* then poll for new data */
+		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ);
+
+		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
-MTK::decodeInit(void)
+MTK::decode_init(void)
 {
 	_rx_ck_a = 0;
 	_rx_ck_b = 0;
 	_rx_count = 0;
 	_decode_state = MTK_DECODE_UNINIT;
 }
-
-void
-MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated)
+int
+MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
 {
+	int ret = 0;
+
 	if (_decode_state == MTK_DECODE_UNINIT) {
 
 		if (b == MTK_SYNC1_V16) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
-			config_needed = false;
 			_mtk_revision = 16;
 		} else if (b == MTK_SYNC1_V19) {
 			_decode_state = MTK_DECODE_GOT_CK_A;
-			config_needed = false;
 			_mtk_revision = 19;
 		}
 
@@ -125,78 +183,82 @@ MTK::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 
 		} else {
 			// Second start symbol was wrong, reset state machine
-			decodeInit();
+			decode_init();
 		}
 
 	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
 		// Add to checksum
 		if (_rx_count < 33)
-			addByteToChecksum(b);
+			add_byte_to_checksum(b);
 
 		// Fill packet buffer
-		_rx_buffer[_rx_count] = b;
+		((uint8_t*)(&packet))[_rx_count] = b;
 		_rx_count++;
 
-		/* Packet size minus checksum */
-		if (_rx_count >= 35) {
-			type_gps_mtk_packet *packet = (type_gps_mtk_packet *) _rx_buffer;;
-
-			/* Check if checksum is valid */
-			if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-				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
-				} else if (_mtk_revision == 19) {
-					gps_position->lat = packet->latitude; // both degrees*1e7
-					gps_position->lon = packet->longitude; // both degrees*1e7
-				} else {
-					warnx("mtk: unknown revision");
-				}
-				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
-				uint32_t timeinfo_conversion_temp;
-
-				timeinfo.tm_mday = packet->date * 1e-4;
-				timeinfo_conversion_temp = packet->date - timeinfo.tm_mday * 1e4;
-				timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
-				timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
-
-				timeinfo.tm_hour = packet->utc_time * 1e-7;
-				timeinfo_conversion_temp = packet->utc_time - timeinfo.tm_hour * 1e7;
-				timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
-				timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
-				timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
-				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();
-
-				pos_updated = true;
-
-
+		/* Packet size minus checksum, XXX ? */
+		if (_rx_count >= sizeof(packet)) {
+			/* Compare checksum */
+			if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
+				ret = 1;
 			} else {
-				warnx("mtk Checksum invalid, 0x%x, 0x%x instead of 0x%x, 0x%x", _rx_ck_a, packet->ck_a, _rx_ck_b, packet->ck_b);
+				warnx("MTK Checksum invalid");
+				ret = -1;
 			}
-
 			// Reset state machine to decode next packet
-			decodeInit();
+			decode_init();
 		}
 	}
+	return ret;
+}
+
+void
+MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position)
+{
+	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
+	} else if (_mtk_revision == 19) {
+		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.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
+	uint32_t timeinfo_conversion_temp;
+
+	timeinfo.tm_mday = packet.date * 1e-4;
+	timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 1e4;
+	timeinfo.tm_mon = timeinfo_conversion_temp * 1e-2 - 1;
+	timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 1e2) + 100;
+
+	timeinfo.tm_hour = packet.utc_time * 1e-7;
+	timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 1e7;
+	timeinfo.tm_min = timeinfo_conversion_temp * 1e-5;
+	timeinfo_conversion_temp -= timeinfo.tm_min * 1e5;
+	timeinfo.tm_sec = timeinfo_conversion_temp * 1e-3;
+	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();
+
 	return;
 }
 
 void
-MTK::addByteToChecksum(uint8_t b)
+MTK::add_byte_to_checksum(uint8_t b)
 {
 	_rx_ck_a = _rx_ck_a + b;
 	_rx_ck_b = _rx_ck_b + _rx_ck_a;
diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h
index e43637195..39063beab 100644
--- a/apps/drivers/gps/mtk.h
+++ b/apps/drivers/gps/mtk.h
@@ -50,6 +50,9 @@
 #define WAAS_ON				"$PMTK301,2*2E\r\n"
 #define MTK_NAVTHRES_OFF 	"$PMTK397,0*23\r\n"
 
+#define MTK_TIMEOUT_5HZ 400
+#define MTK_BAUDRATE 38400
+
 typedef enum {
 	MTK_DECODE_UNINIT = 0,
 	MTK_DECODE_GOT_CK_A = 1,
@@ -64,16 +67,16 @@ typedef struct {
 	int32_t latitude;  ///< Latitude in degrees * 10^7
 	int32_t longitude; ///< Longitude in degrees * 10^7
 	uint32_t msl_altitude;  ///< MSL altitude in meters * 10^2
-	uint32_t ground_speed; ///< FIXME SPEC UNCLEAR
-	int32_t heading;
-	uint8_t satellites;
-	uint8_t fix_type;
+	uint32_t ground_speed; ///< velocity in m/s
+	int32_t heading; ///< heading in degrees * 10^2
+	uint8_t satellites; ///< number of sattelites used
+	uint8_t fix_type;  ///< fix type: XXX correct for that
 	uint32_t date;
 	uint32_t utc_time;
-	uint16_t hdop;
+	uint16_t hdop; ///< horizontal dilution of position (without unit)
 	uint8_t ck_a;
 	uint8_t ck_b;
-} type_gps_mtk_packet;
+} gps_mtk_packet_t;
 
 #pragma pack(pop)
 
@@ -84,23 +87,31 @@ class MTK : public GPS_Helper
 public:
 	MTK();
 	~MTK();
-	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(const int &fd, struct vehicle_gps_position_s &gps_position);
+	int					configure(const int &fd, unsigned &baudrate);
 
 private:
 	/**
+	 * Parse the binary MTK packet
+	 */
+	int					parse_char(uint8_t b, gps_mtk_packet_t &packet);
+
+	/**
+	 * Handle the package once it has arrived
+	 */
+	void				handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position);
+
+	/**
 	 * 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);
 
 	mtk_decode_state_t	_decode_state;
-	bool				_config_sent;
 	uint8_t				_mtk_revision;
 	uint8_t				_rx_buffer[MTK_RECV_BUFFER_SIZE];
 	unsigned			_rx_count;