Merge remote-tracking branch 'upstream/master' into new_state_machine

Conflicts:
	src/drivers/px4io/px4io.cpp
	src/modules/commander/commander.c
	src/modules/commander/state_machine_helper.c

8 files changed, 2383 insertions, 0 deletions

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
new file mode 100644
index 000000000..38835418b
--- /dev/null
+++ b/src/drivers/gps/gps.cpp
@@ -0,0 +1,545 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gps.cpp
+ * Driver for the GPS on a serial port
+ */
+
+#include <nuttx/clock.h>
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <nuttx/arch.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 "mtk.h"
+
+
+#define TIMEOUT_5HZ 500
+#define RATE_MEASUREMENT_PERIOD 5000000
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+
+
+class GPS : public device::CDev
+{
+public:
+	GPS(const char* uart_path);
+	virtual ~GPS();
+
+	virtual int			init();
+
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void				print_info();
+
+private:
+
+	bool				_task_should_exit;				///< flag to make the main worker task exit
+	int				_serial_fd;					///< serial interface to GPS
+	unsigned			_baudrate;					///< current baudrate
+	char				_port[20];					///< device / serial port path
+	volatile int			_task;						//< worker task
+	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
+	GPS_Helper			*_Helper;					///< instance of GPS parser
+	struct vehicle_gps_position_s 	_report;					///< uORB topic for gps position
+	orb_advert_t			_report_pub;					///< uORB pub for gps position
+	float				_rate;						///< position update rate
+
+
+	/**
+	 * Try to configure the GPS, handle outgoing communication to the GPS
+	 */
+	void				config();
+
+	/**
+	 * Trampoline to the worker task
+	 */
+	static void			task_main_trampoline(void *arg);
+
+
+	/**
+	 * Worker task: main GPS thread that configures the GPS and parses incoming data, always running
+	 */
+	void				task_main(void);
+
+	/**
+	 * Set the baudrate of the UART to the GPS
+	 */
+	int				set_baudrate(unsigned baud);
+
+	/**
+	 * Send a reset command to the GPS
+	 */
+	void				cmd_reset();
+
+};
+
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int gps_main(int argc, char *argv[]);
+
+namespace
+{
+
+GPS	*g_dev;
+
+}
+
+
+GPS::GPS(const char* uart_path) :
+	CDev("gps", GPS_DEVICE_PATH),
+	_task_should_exit(false),
+	_healthy(false),
+	_mode_changed(false),
+	_mode(GPS_DRIVER_MODE_UBX),
+	_Helper(nullptr),
+	_report_pub(-1),
+	_rate(0.0f)
+{
+	/* store port name */
+	strncpy(_port, uart_path, sizeof(_port));
+	/* enforce null termination */
+	_port[sizeof(_port) - 1] = '\0';
+
+	/* we need this potentially before it could be set in task_main */
+	g_dev = this;
+	memset(&_report, 0, sizeof(_report));
+
+	_debug_enabled = true;
+}
+
+GPS::~GPS()
+{
+	/* tell the task we want it to go away */
+	_task_should_exit = true;
+
+	/* spin waiting for the task to stop */
+	for (unsigned i = 0; (i < 10) && (_task != -1); i++) {
+		/* give it another 100ms */
+		usleep(100000);
+	}
+
+	/* well, kill it anyway, though this will probably crash */
+	if (_task != -1)
+		task_delete(_task);
+	g_dev = nullptr;
+
+}
+
+int
+GPS::init()
+{
+	int ret = ERROR;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK)
+		goto out;
+
+	/* start the GPS driver worker task */
+	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+
+	if (_task < 0) {
+		warnx("task start failed: %d", errno);
+		return -errno;
+	}
+
+	ret = OK;
+out:
+	return ret;
+}
+
+int
+GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	lock();
+
+	int ret = OK;
+
+	switch (cmd) {
+	case SENSORIOCRESET:
+		cmd_reset();
+		break;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+GPS::task_main_trampoline(void *arg)
+{
+	g_dev->task_main();
+}
+
+void
+GPS::task_main()
+{
+	log("starting");
+
+	/* open the serial port */
+	_serial_fd = ::open(_port, O_RDWR);
+
+	if (_serial_fd < 0) {
+		log("failed to open serial port: %s err: %d", _port, errno);
+		/* tell the dtor that we are exiting, set error code */
+		_task = -1;
+		_exit(1);
+	}
+
+	uint64_t last_rate_measurement = hrt_absolute_time();
+	unsigned last_rate_count = 0;
+
+	/* loop handling received serial bytes and also configuring in between */
+	while (!_task_should_exit) {
+
+		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(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_MTK:
+				_Helper = new MTK(_serial_fd, &_report);
+				break;
+			case GPS_DRIVER_MODE_NMEA:
+				//_Helper = new NMEA(); //TODO: add NMEA
+				break;
+			default:
+				break;
+		}
+		unlock();
+		if (_Helper->configure(_baudrate) == 0) {
+			unlock();
+
+			// GPS is obviously detected successfully, reset statistics
+			_Helper->reset_update_rates();
+
+			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);
+				} 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 > 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;
+					_Helper->store_update_rates();
+					_Helper->reset_update_rates();
+				}
+
+				if (!_healthy) {
+					warnx("module found");
+					_healthy = true;
+				}
+			}
+			if (_healthy) {
+				warnx("module lost");
+				_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");
+
+	::close(_serial_fd);
+
+	/* tell the dtor that we are exiting */
+	_task = -1;
+	_exit(0);
+}
+
+
+
+void
+GPS::cmd_reset()
+{
+	//XXX add reset?
+}
+
+void
+GPS::print_info()
+{
+	switch (_mode) {
+		case GPS_DRIVER_MODE_UBX:
+			warnx("protocol: UBX");
+			break;
+		case GPS_DRIVER_MODE_MTK:
+			warnx("protocol: MTK");
+			break;
+		case GPS_DRIVER_MODE_NMEA:
+			warnx("protocol: NMEA");
+			break;
+		default:
+			break;
+	}
+	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));
+		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
+		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
+		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
+		warnx("rate publication:\t%6.2f Hz", (double)_rate);
+
+	}
+
+	usleep(100000);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace gps
+{
+
+GPS	*g_dev;
+
+void	start(const char *uart_path);
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *uart_path)
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new GPS(uart_path);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
+		goto fail;
+	}
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver.
+ */
+void
+stop()
+{
+	delete g_dev;
+	g_dev = nullptr;
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(GPS_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	exit(0);
+}
+
+/**
+ * Print the status of the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+
+int
+gps_main(int argc, char *argv[])
+{
+
+	/* set to default */
+	char* device_name = GPS_DEFAULT_UART_PORT;
+
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		/* work around getopt unreliability */
+		if (argc > 3) {
+			if (!strcmp(argv[2], "-d")) {
+				device_name = argv[3];
+			} else {
+				goto out;
+			}
+		}
+		gps::start(device_name);
+	}
+
+	if (!strcmp(argv[1], "stop"))
+		gps::stop();
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		gps::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		gps::reset();
+
+	/*
+	 * Print driver status.
+	 */
+	if (!strcmp(argv[1], "status"))
+		gps::info();
+
+out:
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]");
+}
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
new file mode 100644
index 000000000..ba86d370a
--- /dev/null
+++ b/src/drivers/gps/gps_helper.cpp
@@ -0,0 +1,122 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012,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 <drivers/drv_hrt.h>
+#include "gps_helper.h"
+
+/**
+ * @file gps_helper.cpp
+ */
+
+float
+GPS_Helper::get_position_update_rate()
+{
+	return _rate_lat_lon;
+}
+
+float
+GPS_Helper::get_velocity_update_rate()
+{
+	return _rate_vel;
+}
+
+float
+GPS_Helper::reset_update_rates()
+{
+	_rate_count_vel = 0;
+	_rate_count_lat_lon = 0;
+	_interval_rate_start = hrt_absolute_time();
+}
+
+float
+GPS_Helper::store_update_rates()
+{
+	_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+	_rate_lat_lon = _rate_count_lat_lon / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
+}
+
+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;
+
+	warnx("try baudrate: %d\n", speed);
+
+	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/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
new file mode 100644
index 000000000..defc1a074
--- /dev/null
+++ b/src/drivers/gps/gps_helper.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 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.
+ *
+ ****************************************************************************/
+
+/** 
+ * @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 int			configure(unsigned &baud) = 0;
+	virtual int 			receive(unsigned timeout) = 0;
+	int 				set_baudrate(const int &fd, unsigned baud);
+	float				get_position_update_rate();
+	float				get_velocity_update_rate();
+	float				reset_update_rates();
+	float				store_update_rates();
+
+protected:
+	uint8_t _rate_count_lat_lon;
+	uint8_t _rate_count_vel;
+
+	float _rate_lat_lon;
+	float _rate_vel;
+
+	uint64_t _interval_rate_start;
+};
+
+#endif /* GPS_HELPER_H */
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
new file mode 100644
index 000000000..097db2abf
--- /dev/null
+++ b/src/drivers/gps/module.mk
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# 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.
+#
+############################################################################
+
+#
+# GPS driver
+#
+
+MODULE_COMMAND	= gps
+
+SRCS		= gps.cpp \
+		  gps_helper.cpp \
+		  mtk.cpp \
+		  ubx.cpp
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
new file mode 100644
index 000000000..62941d74b
--- /dev/null
+++ b/src/drivers/gps/mtk.cpp
@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/* @file mkt.cpp */
+
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "mtk.h"
+
+
+MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_mtk_revision(0)
+{
+	decode_init();
+}
+
+MTK::~MTK()
+{
+}
+
+int
+MTK::configure(unsigned &baudrate)
+{
+	/* set baudrate first */
+	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))) {
+		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");
+		return -1;
+	}
+	usleep(10000);
+
+	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))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+	usleep(10000);
+
+	if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
+		warnx("mtk: config write failed");
+		return -1;
+	}
+
+	return 0;
+}
+
+int
+MTK::receive(unsigned timeout)
+{
+	/* poll descriptor */
+	pollfd fds[1];
+	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;
+
+	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);
+					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 */
+			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
+MTK::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = MTK_DECODE_UNINIT;
+}
+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;
+			_mtk_revision = 16;
+		} else if (b == MTK_SYNC1_V19) {
+			_decode_state = MTK_DECODE_GOT_CK_A;
+			_mtk_revision = 19;
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_A) {
+		if (b == MTK_SYNC2) {
+			_decode_state = MTK_DECODE_GOT_CK_B;
+
+		} else {
+			// Second start symbol was wrong, reset state machine
+			decode_init();
+		}
+
+	} else if (_decode_state == MTK_DECODE_GOT_CK_B) {
+		// Add to checksum
+		if (_rx_count < 33)
+			add_byte_to_checksum(b);
+
+		// Fill packet buffer
+		((uint8_t*)(&packet))[_rx_count] = b;
+		_rx_count++;
+
+		/* 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");
+				ret = -1;
+			}
+			// Reset state machine to decode next packet
+			decode_init();
+		}
+	}
+	return ret;
+}
+
+void
+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
+	} 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();
+
+	// Position and velocity update always at the same time
+	_rate_count_vel++;
+	_rate_count_lat_lon++;
+
+	return;
+}
+
+void
+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/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h
new file mode 100644
index 000000000..b5cfbf0a6
--- /dev/null
+++ b/src/drivers/gps/mtk.h
@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/* @file mtk.h */
+
+#ifndef MTK_H_
+#define MTK_H_
+
+#include "gps_helper.h"
+
+#define MTK_SYNC1_V16 0xd0
+#define MTK_SYNC1_V19 0xd1
+#define MTK_SYNC2 0xdd
+
+#define MTK_OUTPUT_5HZ		"$PMTK220,200*2C\r\n"
+#define MTK_SET_BINARY		"$PGCMD,16,0,0,0,0,0*6A\r\n"
+#define SBAS_ON	        	"$PMTK313,1*2E\r\n"
+#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,
+	MTK_DECODE_GOT_CK_B = 2
+} mtk_decode_state_t;
+
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+typedef struct {
+	uint8_t payload; ///< Number of payload bytes
+	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; ///< 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; ///< horizontal dilution of position (without unit)
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_mtk_packet_t;
+
+#pragma pack(pop)
+
+#define MTK_RECV_BUFFER_SIZE 40
+
+class MTK : public GPS_Helper
+{
+public:
+	MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~MTK();
+	int				receive(unsigned timeout);
+	int				configure(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);
+
+	/**
+	 * Reset the parse state machine for a fresh start
+	 */
+	void				decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	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];
+	unsigned			_rx_count;
+	uint8_t 			_rx_ck_a;
+	uint8_t				_rx_ck_b;
+};
+
+#endif /* MTK_H_ */
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
new file mode 100644
index 000000000..b3093b0f6
--- /dev/null
+++ b/src/drivers/gps/ubx.cpp
@@ -0,0 +1,785 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ubx.cpp
+ *
+ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ */
+
+#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 "ubx.h"
+
+#define UBX_CONFIG_TIMEOUT 100
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+_fd(fd),
+_gps_position(gps_position),
+_waiting_for_ack(false),
+_disable_cmd_counter(0)
+{
+	decode_init();
+}
+
+UBX::~UBX()
+{
+}
+
+int
+UBX::configure(unsigned &baudrate)
+{
+	_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;
+		}
+
+		/* 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));
+		
+		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
+		receive(UBX_CONFIG_TIMEOUT);
+		
+		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
+			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		}
+
+		/* 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));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_RATE;
+
+		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_MEASINTERVAL;
+		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
+		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
+
+		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		/* 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));
+
+		_clsID_needed = UBX_CLASS_CFG;
+		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
+
+		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_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+			/* try next baudrate */
+			continue;
+		}
+
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
+			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
+		/* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
+			1);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
+		// 	0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
+			0);
+		// /* insist of receiving the ACK for this packet */
+		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
+		// 	continue;
+
+		_waiting_for_ack = false;
+		return 0;
+	}
+	return -1;
+}
+
+int
+UBX::wait_for_ack(unsigned timeout)
+{
+	_waiting_for_ack = true;
+	int ret = receive(timeout);
+	_waiting_for_ack = false;
+	return ret;
+}
+
+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++;
+		}
+
+		/* 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));
+			}
+		}
+	}
+}
+
+int
+UBX::parse_char(uint8_t b)
+{
+	switch (_decode_state) {
+		/* First, look for sync1 */
+		case UBX_DECODE_UNINIT:
+			if (b == UBX_SYNC1) {
+				_decode_state = UBX_DECODE_GOT_SYNC1;
+			}
+			break;
+		/* Second, look for sync2 */
+		case UBX_DECODE_GOT_SYNC1:
+			if (b == UBX_SYNC2) {
+				_decode_state = UBX_DECODE_GOT_SYNC2;
+			} else {
+				/* Second start symbol was wrong, reset state machine */
+				decode_init();
+			}
+			break;
+		/* Now look for class */
+		case UBX_DECODE_GOT_SYNC2:
+			/* everything except sync1 and sync2 needs to be added to the checksum */
+			add_byte_to_checksum(b);
+			/* check for known class */
+			switch (b) {
+				case UBX_CLASS_ACK:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = ACK;
+					break;
+
+				case UBX_CLASS_NAV:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = NAV;
+					break;
+
+//				case UBX_CLASS_RXM:
+//					_decode_state = UBX_DECODE_GOT_CLASS;
+//					_message_class = RXM;
+//					break;
+
+				case UBX_CLASS_CFG:
+					_decode_state = UBX_DECODE_GOT_CLASS;
+					_message_class = CFG;
+					break;
+				default: //unknown class: reset state machine
+					decode_init();
+					break;
+			}
+			break;
+		case UBX_DECODE_GOT_CLASS:
+			add_byte_to_checksum(b);
+			switch (_message_class) {
+				case NAV:
+					switch (b) {
+					case UBX_MESSAGE_NAV_POSLLH:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_POSLLH;
+						break;
+
+					case UBX_MESSAGE_NAV_SOL:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SOL;
+						break;
+
+					case UBX_MESSAGE_NAV_TIMEUTC:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_TIMEUTC;
+						break;
+
+//					case UBX_MESSAGE_NAV_DOP:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = NAV_DOP;
+//						break;
+
+					case UBX_MESSAGE_NAV_SVINFO:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_SVINFO;
+						break;
+
+					case UBX_MESSAGE_NAV_VELNED:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = NAV_VELNED;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+//				case RXM:
+//					switch (b) {
+//					case UBX_MESSAGE_RXM_SVSI:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = RXM_SVSI;
+//						break;
+//
+//					default: //unknown class: reset state machine, should not happen
+//						decode_init();
+//						break;
+//					}
+//					break;
+
+				case CFG:
+					switch (b) {
+					case UBX_MESSAGE_CFG_NAV5:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = CFG_NAV5;
+						break;
+
+					default: //unknown class: reset state machine, should not happen
+						decode_init();
+						break;
+					}
+					break;
+
+				case ACK:
+					switch (b) {
+					case UBX_MESSAGE_ACK_ACK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_ACK;
+						break;
+					case UBX_MESSAGE_ACK_NAK:
+						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+						_message_id = ACK_NAK;
+						break;
+					default: //unknown class: reset state machine, should not happen
+						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");
+					decode_init();
+//					config_needed = true;
+					break;
+				}
+				break;
+			case UBX_DECODE_GOT_MESSAGEID:
+				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:
+				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)
+					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
+
+						/* 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");
+						}
+
+						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 ?
+}
+
+
+int
+UBX::handle_message()
+{
+	int ret = 0;
+
+	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;
+
+				_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
+
+				_rate_count_lat_lon++;
+
+				/* Add timestamp to finish the report */
+				_gps_position->timestamp_position = hrt_absolute_time();
+				/* only return 1 when new position is available */
+				ret = 1;
+			}
+			break;
+		}
+
+		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;
+
+				_gps_position->fix_type = packet->gpsFix;
+				_gps_position->s_variance_m_s = packet->sAcc;
+				_gps_position->p_variance_m = packet->pAcc;
+
+				_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;
+//
+//			_gps_position->eph_m =  packet->hDOP;
+//			_gps_position->epv =  packet->vDOP;
+//
+//			_gps_position->timestamp_posdilution = hrt_absolute_time();
+//
+//			_new_nav_dop = true;
+//
+//			break;
+//		}
+
+		case NAV_TIMEUTC: {
+//			printf("GOT NAV_TIMEUTC MESSAGE\n");
+
+			if (!_waiting_for_ack) {
+				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+
+				//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();
+			}
+			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;
+						}
+
+						_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();
+			}
+
+			break;
+		}
+
+		case NAV_VELNED: {
+
+			if (!_waiting_for_ack) {
+				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
+				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; /* NED NORTH velocity */
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
+				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->vel_ned_valid = true;
+				_gps_position->timestamp_velocity = hrt_absolute_time();
+
+				_rate_count_vel++;
+			}
+
+			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);
+			if (_disable_cmd_counter++ == 0) {
+				// Don't attempt for every message to disable, some might not be disabled */
+				warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+				configure_message_rate(_message_class, _message_id, 0);
+			}
+		return ret;
+			ret = -1;
+			break;
+		}
+	// end if _rx_count high enough
+	decode_init();
+	return ret; //XXX?
+}
+
+void
+UBX::decode_init(void)
+{
+	_rx_ck_a = 0;
+	_rx_ck_b = 0;
+	_rx_count = 0;
+	_decode_state = UBX_DECODE_UNINIT;
+	_message_class = CLASS_UNKNOWN;
+	_message_id = ID_UNKNOWN;
+	_payload_size = 0;
+}
+
+void
+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::add_checksum_to_message(uint8_t* message, const unsigned length)
+{
+	uint8_t ck_a = 0;
+	uint8_t ck_b = 0;
+	unsigned i;
+
+	for (i = 0; i < length-2; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+	/* The checksum is written to the last to bytes of a message */
+	message[length-2] = ck_a;
+	message[length-1] = ck_b;
+}
+
+void
+UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+{
+	for (unsigned i = 0; i < length; i++) {
+		ck_a = ck_a + message[i];
+		ck_b = ck_b + ck_a;
+	}
+}
+
+void
+UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+{
+    struct ubx_cfg_msg_rate msg;
+    msg.msg_class	= msg_class;
+    msg.msg_id		= msg_id;
+    msg.rate		= rate;
+    send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+}
+
+void
+UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+{
+	ssize_t ret = 0;
+
+	/* Calculate the checksum now */
+	add_checksum_to_message(packet, length);
+
+	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+
+	/* Start with the two sync bytes */
+	ret += write(fd, sync_bytes, sizeof(sync_bytes));
+	ret += write(fd, packet, length);
+
+	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
+		warnx("ubx: config write fail");
+}
+
+void
+UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
+{
+	struct ubx_header header;
+	uint8_t ck_a=0, ck_b=0;
+	header.sync1 = UBX_SYNC1;
+	header.sync2 = UBX_SYNC2;
+	header.msg_class = msg_class;
+	header.msg_id    = msg_id;
+	header.length    = size;
+
+	add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
+	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
+
+	// Configure receive check
+	_clsID_needed = msg_class;
+	_msgID_needed = msg_id;
+
+	write(_fd, (const char *)&header, sizeof(header));
+	write(_fd, (const char *)msg, size);
+	write(_fd, (const char *)&ck_a, 1);
+	write(_fd, (const char *)&ck_b, 1);
+}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
new file mode 100644
index 000000000..5a433642c
--- /dev/null
+++ b/src/drivers/gps/ubx.h
@@ -0,0 +1,419 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/* @file U-Blox protocol definitions */
+
+#ifndef UBX_H_
+#define UBX_H_
+
+#include "gps_helper.h"
+
+#define UBX_SYNC1 0xB5
+#define UBX_SYNC2 0x62
+
+/* ClassIDs (the ones that are used) */
+#define UBX_CLASS_NAV 0x01
+//#define UBX_CLASS_RXM 0x02
+#define UBX_CLASS_ACK 0x05
+#define UBX_CLASS_CFG 0x06
+
+/* MessageIDs (the ones that are used) */
+#define UBX_MESSAGE_NAV_POSLLH 0x02
+#define UBX_MESSAGE_NAV_SOL 0x06
+#define UBX_MESSAGE_NAV_TIMEUTC 0x21
+//#define UBX_MESSAGE_NAV_DOP 0x04
+#define UBX_MESSAGE_NAV_SVINFO 0x30
+#define UBX_MESSAGE_NAV_VELNED 0x12
+//#define UBX_MESSAGE_RXM_SVSI 0x20
+#define UBX_MESSAGE_ACK_ACK 0x01
+#define UBX_MESSAGE_ACK_NAK 0x00
+#define UBX_MESSAGE_CFG_PRT 0x00
+#define UBX_MESSAGE_CFG_NAV5 0x24
+#define UBX_MESSAGE_CFG_MSG 0x01
+#define UBX_MESSAGE_CFG_RATE 0x08
+
+#define UBX_CFG_PRT_LENGTH 20
+#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
+#define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400		/**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01		/**< UBX in */
+#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01		/**< UBX out */
+
+#define UBX_CFG_RATE_LENGTH 6
+#define UBX_CFG_RATE_PAYLOAD_MEASINTERVAL 200		/**< 200ms for 5Hz */
+#define UBX_CFG_RATE_PAYLOAD_NAVRATE 1			/**< cannot be changed */
+#define UBX_CFG_RATE_PAYLOAD_TIMEREF 0			/**< 0: UTC, 1: GPS time */
+
+
+#define UBX_CFG_NAV5_LENGTH 36
+#define UBX_CFG_NAV5_PAYLOAD_MASK 0x0005		/**< XXX only update dynamic model and fix mode */
+#define UBX_CFG_NAV5_PAYLOAD_DYNMODEL 7			/**< 0: portable, 2: stationary, 3: pedestrian, 4: automotive, 5: sea, 6: airborne <1g, 7: airborne <2g, 8: airborne <4g */
+#define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2			/**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */
+
+#define UBX_CFG_MSG_LENGTH 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_CFG_MSG_PAYLOAD_RATE1_05HZ 10
+
+#define UBX_MAX_PAYLOAD_LENGTH 500
+
+// ************
+/** the structures of the binary packets */
+#pragma pack(push, 1)
+
+struct ubx_header {
+	uint8_t sync1;
+	uint8_t sync2;
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint16_t length;
+};
+
+typedef struct {
+	uint32_t time_milliseconds;		/**<  GPS Millisecond Time of Week */
+	int32_t lon;					/**<  Longitude * 1e-7, deg */
+	int32_t lat;					/**<  Latitude * 1e-7, deg */
+	int32_t height;					/**<  Height above Ellipsoid, mm */
+	int32_t height_msl;				/**<  Height above mean sea level, mm */
+	uint32_t hAcc;  				/**< Horizontal Accuracy Estimate, mm */
+	uint32_t vAcc;  				/**< Vertical Accuracy Estimate, mm */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_posllh_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**< GPS Millisecond Time of Week */
+	int32_t time_nanoseconds;		/**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */
+	int16_t week;					/**< GPS week (GPS time) */
+	uint8_t gpsFix;					/**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */
+	uint8_t flags;
+	int32_t ecefX;
+	int32_t ecefY;
+	int32_t ecefZ;
+	uint32_t pAcc;
+	int32_t ecefVX;
+	int32_t ecefVY;
+	int32_t ecefVZ;
+	uint32_t sAcc;
+	uint16_t pDOP;
+	uint8_t reserved1;
+	uint8_t numSV;
+	uint32_t reserved2;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_sol_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds;		/**< GPS Millisecond Time of Week */
+	uint32_t time_accuracy; 		/**< Time Accuracy Estimate, ns */
+	int32_t time_nanoseconds; 		/**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */
+	uint16_t year; 					/**< Year, range 1999..2099 (UTC) */
+	uint8_t month; 					/**< Month, range 1..12 (UTC) */
+	uint8_t day; 					/**< Day of Month, range 1..31 (UTC) */
+	uint8_t hour; 					/**< Hour of Day, range 0..23 (UTC) */
+	uint8_t min; 					/**< Minute of Hour, range 0..59 (UTC) */
+	uint8_t sec; 					/**< Seconds of Minute, range 0..59 (UTC) */
+	uint8_t valid_flag; 			/**< Validity Flags (see ubx documentation) */
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_timeutc_packet_t;
+
+//typedef struct {
+//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
+//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
+//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
+//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
+//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
+//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
+//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
+//	uint8_t ck_a;
+//	uint8_t ck_b;
+//} gps_bin_nav_dop_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+	uint8_t numCh; 					/**< Number of channels */
+	uint8_t globalFlags;
+	uint16_t reserved2;
+
+} gps_bin_nav_svinfo_part1_packet_t;
+
+typedef struct {
+	uint8_t chn; 					/**< Channel number, 255 for SVs not assigned to a channel */
+	uint8_t svid; 					/**< Satellite ID */
+	uint8_t flags;
+	uint8_t quality;
+	uint8_t cno; 					/**< Carrier to Noise Ratio (Signal Strength), dbHz */
+	int8_t elev; 					/**< Elevation in integer degrees */
+	int16_t azim; 					/**< Azimuth in integer degrees */
+	int32_t prRes; 					/**< Pseudo range residual in centimetres */
+
+} gps_bin_nav_svinfo_part2_packet_t;
+
+typedef struct {
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_svinfo_part3_packet_t;
+
+typedef struct {
+	uint32_t time_milliseconds; // GPS Millisecond Time of Week
+	int32_t velN; //NED north velocity, cm/s
+	int32_t velE; //NED east velocity, cm/s
+	int32_t velD; //NED down velocity, cm/s
+	uint32_t speed; //Speed (3-D), cm/s
+	uint32_t gSpeed; //Ground Speed (2-D), cm/s
+	int32_t heading; //Heading of motion 2-D, deg, scaling: 1e-5
+	uint32_t sAcc; //Speed Accuracy Estimate, cm/s
+	uint32_t cAcc; //Course / Heading Accuracy Estimate, scaling: 1e-5
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_nav_velned_packet_t;
+
+//typedef struct {
+//	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
+//	int16_t week; 					/**< Measurement GPS week number */
+//	uint8_t numVis;					/**< Number of visible satellites */
+//
+//	//... rest of package is not used in this implementation
+//
+//} gps_bin_rxm_svsi_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_ack_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} gps_bin_ack_nak_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t portID;
+	uint8_t res0;
+	uint16_t res1;
+	uint32_t mode;
+	uint32_t baudRate;
+	uint16_t inProtoMask;
+	uint16_t outProtoMask;
+	uint16_t flags;
+	uint16_t pad;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_prt_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t measRate;
+	uint16_t navRate;
+	uint16_t timeRef;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_rate_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint16_t mask;
+	uint8_t dynModel;
+	uint8_t fixMode;
+	int32_t fixedAlt;
+	uint32_t fixedAltVar;
+	int8_t minElev;
+	uint8_t drLimit;
+	uint16_t pDop;
+	uint16_t tDop;
+	uint16_t pAcc;
+	uint16_t tAcc;
+	uint8_t staticHoldThresh;
+	uint8_t dgpsTimeOut;
+	uint32_t reserved2;
+	uint32_t reserved3;
+	uint32_t reserved4;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_nav5_packet_t;
+
+typedef struct {
+	uint8_t clsID;
+	uint8_t msgID;
+	uint16_t length;
+	uint8_t msgClass_payload;
+	uint8_t msgID_payload;
+	uint8_t rate;
+	uint8_t ck_a;
+	uint8_t ck_b;
+} type_gps_bin_cfg_msg_packet_t;
+
+struct ubx_cfg_msg_rate {
+	uint8_t msg_class;
+	uint8_t msg_id;
+	uint8_t rate;
+};
+
+
+// END the structures of the binary packets
+// ************
+
+typedef enum {
+	UBX_CONFIG_STATE_PRT = 0,
+	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
+	UBX_CONFIG_STATE_RATE,
+	UBX_CONFIG_STATE_NAV5,
+	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
+	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
+	UBX_CONFIG_STATE_MSG_NAV_DOP,
+	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
+	UBX_CONFIG_STATE_MSG_NAV_SOL,
+	UBX_CONFIG_STATE_MSG_NAV_VELNED,
+//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
+	UBX_CONFIG_STATE_CONFIGURED
+} ubx_config_state_t;
+
+typedef enum {
+	CLASS_UNKNOWN = 0,
+	NAV = 1,
+	RXM = 2,
+	ACK = 3,
+	CFG = 4
+} ubx_message_class_t;
+
+typedef enum {
+	//these numbers do NOT correspond to the message id numbers of the ubx protocol
+	ID_UNKNOWN = 0,
+	NAV_POSLLH,
+	NAV_SOL,
+	NAV_TIMEUTC,
+//	NAV_DOP,
+	NAV_SVINFO,
+	NAV_VELNED,
+//	RXM_SVSI,
+	CFG_NAV5,
+	ACK_ACK,
+	ACK_NAK,
+} ubx_message_id_t;
+
+typedef enum {
+	UBX_DECODE_UNINIT = 0,
+	UBX_DECODE_GOT_SYNC1,
+	UBX_DECODE_GOT_SYNC2,
+	UBX_DECODE_GOT_CLASS,
+	UBX_DECODE_GOT_MESSAGEID,
+	UBX_DECODE_GOT_LENGTH1,
+	UBX_DECODE_GOT_LENGTH2
+} ubx_decode_state_t;
+
+//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
+#pragma pack(pop)
+
+#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+
+class UBX : public GPS_Helper
+{
+public:
+	UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
+	~UBX();
+	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			decode_init(void);
+
+	/**
+	 * While parsing add every byte (except the sync bytes) to the checksum
+	 */
+	void			add_byte_to_checksum(uint8_t);
+
+	/**
+	 * Add the two checksum bytes to an outgoing message
+	 */
+	void			add_checksum_to_message(uint8_t* message, const unsigned length);
+
+	/**
+	 * Helper to send a config packet
+	 */
+	void			send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+
+	void			configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate);
+
+	void			send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size);
+
+	void			add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+
+	int			wait_for_ack(unsigned timeout);
+
+	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;
+	uint8_t			_rx_ck_a;
+	uint8_t			_rx_ck_b;
+	ubx_message_class_t	_message_class;
+	ubx_message_id_t	_message_id;
+	unsigned		_payload_size;
+	uint8_t			_disable_cmd_counter;
+};
+
+#endif /* UBX_H_ */