Merge branch 'master' into navigator_rewrite_drton

54 files changed, 1896 insertions, 978 deletions

diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index 98c491ce6..92e089217 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -559,13 +559,7 @@ BlinkM::led()
 		}
 
 		/* get number of used satellites in navigation */
-		num_of_used_sats = 0;
-
-		for(unsigned satloop=0; satloop<sizeof(vehicle_gps_position_raw.satellite_used); satloop++) {
-			if(vehicle_gps_position_raw.satellite_used[satloop] == 1) {
-				num_of_used_sats++;
-			}
-		}
+		num_of_used_sats = vehicle_gps_position_raw.satellites_used;
 
 		if (new_data_vehicle_status || no_data_vehicle_status < 3) {
 			if (num_of_cells == 0) {
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_init.c b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
index 4b12b75f9..293021f8b 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_init.c
@@ -99,7 +99,7 @@
  * CONFIG_ARCH_LEDS configuration switch.
  */
 __BEGIN_DECLS
-extern void led_init();
+extern void led_init(void);
 extern void led_on(int led);
 extern void led_off(int led);
 __END_DECLS
diff --git a/src/drivers/boards/px4fmu-v1/px4fmu_led.c b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
index ea91f34ad..ee53fc43d 100644
--- a/src/drivers/boards/px4fmu-v1/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu-v1/px4fmu_led.c
@@ -54,13 +54,13 @@
  * CONFIG_ARCH_LEDS configuration switch.
  */
 __BEGIN_DECLS
-extern void led_init();
+extern void led_init(void);
 extern void led_on(int led);
 extern void led_off(int led);
 extern void led_toggle(int led);
 __END_DECLS
 
-__EXPORT void led_init()
+__EXPORT void led_init(void)
 {
 	/* Configure LED1-2 GPIOs for output */
 
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index dd505abdb..401b65dd4 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -63,6 +63,7 @@
 #include <drivers/drv_gps.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 
 #include <board_config.h>
 
@@ -79,10 +80,18 @@
 #endif
 static const int ERROR = -1;
 
+/* class for dynamic allocation of satellite info data */
+class GPS_Sat_Info
+{
+public:
+	struct satellite_info_s 	_data;
+};
+
+
 class GPS : public device::CDev
 {
 public:
-	GPS(const char *uart_path, bool fake_gps);
+	GPS(const char *uart_path, bool fake_gps, bool enable_sat_info);
 	virtual ~GPS();
 
 	virtual int			init();
@@ -100,14 +109,17 @@ private:
 	int				_serial_fd;					///< serial interface to GPS
 	unsigned			_baudrate;					///< current baudrate
 	char				_port[20];					///< device / serial port path
-	volatile int			_task;						//< worker task
+	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				_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
+	GPS_Sat_Info			*_Sat_Info;					///< instance of GPS sat info data object
+	struct vehicle_gps_position_s	_report_gps_pos;				///< uORB topic for gps position
+	orb_advert_t			_report_gps_pos_pub;				///< uORB pub for gps position
+	struct satellite_info_s		*_p_report_sat_info;				///< pointer to uORB topic for satellite info
+	orb_advert_t			_report_sat_info_pub;				///< uORB pub for satellite info
 	float				_rate;						///< position update rate
 	bool				_fake_gps;					///< fake gps output
 
@@ -154,14 +166,17 @@ GPS	*g_dev;
 }
 
 
-GPS::GPS(const char *uart_path, bool fake_gps) :
+GPS::GPS(const char *uart_path, bool fake_gps, bool enable_sat_info) :
 	CDev("gps", GPS_DEVICE_PATH),
 	_task_should_exit(false),
 	_healthy(false),
 	_mode_changed(false),
 	_mode(GPS_DRIVER_MODE_UBX),
 	_Helper(nullptr),
-	_report_pub(-1),
+	_Sat_Info(nullptr),
+	_report_gps_pos_pub(-1),
+	_p_report_sat_info(nullptr),
+	_report_sat_info_pub(-1),
 	_rate(0.0f),
 	_fake_gps(fake_gps)
 {
@@ -172,7 +187,14 @@ GPS::GPS(const char *uart_path, bool fake_gps) :
 
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
-	memset(&_report, 0, sizeof(_report));
+	memset(&_report_gps_pos, 0, sizeof(_report_gps_pos));
+
+	/* create satellite info data object if requested */
+	if (enable_sat_info) {
+		_Sat_Info = new(GPS_Sat_Info);
+		_p_report_sat_info = &_Sat_Info->_data;
+		memset(_p_report_sat_info, 0, sizeof(*_p_report_sat_info));
+	}
 
 	_debug_enabled = true;
 }
@@ -207,7 +229,7 @@ GPS::init()
 
 	/* start the GPS driver worker task */
 	_task = task_spawn_cmd("gps", SCHED_DEFAULT,
-				SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
+				SCHED_PRIORITY_SLOW_DRIVER, 1500, (main_t)&GPS::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
@@ -271,33 +293,32 @@ GPS::task_main()
 
 		if (_fake_gps) {
 
-			_report.timestamp_position = hrt_absolute_time();
-			_report.lat = (int32_t)47.378301e7f;
-			_report.lon = (int32_t)8.538777e7f;
-			_report.alt = (int32_t)1200e3f;
-			_report.timestamp_variance = hrt_absolute_time();
-			_report.s_variance_m_s = 10.0f;
-			_report.p_variance_m = 10.0f;
-			_report.c_variance_rad = 0.1f;
-			_report.fix_type = 3;
-			_report.eph = 0.9f;
-			_report.epv = 1.8f;
-			_report.timestamp_velocity = hrt_absolute_time();
-			_report.vel_n_m_s = 0.0f;
-			_report.vel_e_m_s = 0.0f;
-			_report.vel_d_m_s = 0.0f;
-			_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
-			_report.cog_rad = 0.0f;
-			_report.vel_ned_valid = true;
+			_report_gps_pos.timestamp_position = hrt_absolute_time();
+			_report_gps_pos.lat = (int32_t)47.378301e7f;
+			_report_gps_pos.lon = (int32_t)8.538777e7f;
+			_report_gps_pos.alt = (int32_t)1200e3f;
+			_report_gps_pos.timestamp_variance = hrt_absolute_time();
+			_report_gps_pos.s_variance_m_s = 10.0f;
+			_report_gps_pos.c_variance_rad = 0.1f;
+			_report_gps_pos.fix_type = 3;
+			_report_gps_pos.eph = 0.9f;
+			_report_gps_pos.epv = 1.8f;
+			_report_gps_pos.timestamp_velocity = hrt_absolute_time();
+			_report_gps_pos.vel_n_m_s = 0.0f;
+			_report_gps_pos.vel_e_m_s = 0.0f;
+			_report_gps_pos.vel_d_m_s = 0.0f;
+			_report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s);
+			_report_gps_pos.cog_rad = 0.0f;
+			_report_gps_pos.vel_ned_valid = true;
 
 			//no time and satellite information simulated
 
 			if (!(_pub_blocked)) {
-				if (_report_pub > 0) {
-					orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+				if (_report_gps_pos_pub > 0) {
+					orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
 				} else {
-					_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+					_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
 				}
 			}
 
@@ -313,11 +334,11 @@ GPS::task_main()
 
 			switch (_mode) {
 			case GPS_DRIVER_MODE_UBX:
-				_Helper = new UBX(_serial_fd, &_report);
+				_Helper = new UBX(_serial_fd, &_report_gps_pos, _p_report_sat_info);
 				break;
 
 			case GPS_DRIVER_MODE_MTK:
-				_Helper = new MTK(_serial_fd, &_report);
+				_Helper = new MTK(_serial_fd, &_report_gps_pos);
 				break;
 
 			default:
@@ -332,20 +353,33 @@ GPS::task_main()
 				// GPS is obviously detected successfully, reset statistics
 				_Helper->reset_update_rates();
 
-				while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
+				int helper_ret;
+				while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
 	//				lock();
 					/* opportunistic publishing - else invalid data would end up on the bus */
 
 					if (!(_pub_blocked)) {
-						if (_report_pub > 0) {
-							orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+						if (helper_ret & 1) {
+							if (_report_gps_pos_pub > 0) {
+								orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
 
-						} else {
-							_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+							} else {
+								_report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos);
+							}
+						}
+						if (_p_report_sat_info && (helper_ret & 2)) {
+							if (_report_sat_info_pub > 0) {
+								orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info);
+
+							} else {
+								_report_sat_info_pub = orb_advertise(ORB_ID(satellite_info), _p_report_sat_info);
+							}
 						}
 					}
 
-					last_rate_count++;
+					if (helper_ret & 1) {	// consider only pos info updates for rate calculation */
+						last_rate_count++;
+					}
 
 					/* measure update rate every 5 seconds */
 					if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
@@ -446,12 +480,13 @@ GPS::print_info()
 	}
 
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
+	warnx("sat info: %s", (_p_report_sat_info != nullptr) ? "enabled" : "disabled");
 
-	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
-				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d);
-		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv);
+	if (_report_gps_pos.timestamp_position != 0) {
+		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type,
+				_report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0);
+		warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv);
 		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);
@@ -469,7 +504,7 @@ namespace gps
 
 GPS	*g_dev;
 
-void	start(const char *uart_path, bool fake_gps);
+void	start(const char *uart_path, bool fake_gps, bool enable_sat_info);
 void	stop();
 void	test();
 void	reset();
@@ -479,7 +514,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *uart_path, bool fake_gps)
+start(const char *uart_path, bool fake_gps, bool enable_sat_info)
 {
 	int fd;
 
@@ -487,7 +522,7 @@ start(const char *uart_path, bool fake_gps)
 		errx(1, "already started");
 
 	/* create the driver */
-	g_dev = new GPS(uart_path, fake_gps);
+	g_dev = new GPS(uart_path, fake_gps, enable_sat_info);
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -580,6 +615,7 @@ gps_main(int argc, char *argv[])
 	/* set to default */
 	const char *device_name = GPS_DEFAULT_UART_PORT;
 	bool fake_gps = false;
+	bool enable_sat_info = false;
 
 	/*
 	 * Start/load the driver.
@@ -601,7 +637,13 @@ gps_main(int argc, char *argv[])
 				fake_gps = true;
 		}
 
-		gps::start(device_name, fake_gps);
+		/* Detect sat info option */
+		for (int i = 2; i < argc; i++) {
+			if (!strcmp(argv[i], "-s"))
+				enable_sat_info = true;
+		}
+
+		gps::start(device_name, fake_gps, enable_sat_info);
 	}
 
 	if (!strcmp(argv[1], "stop"))
@@ -626,5 +668,5 @@ gps_main(int argc, char *argv[])
 		gps::info();
 
 out:
-	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]");
+	errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f][-s]");
 }
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index d14a95afe..3623397b2 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -62,8 +62,8 @@ protected:
 	uint8_t _rate_count_lat_lon;
 	uint8_t _rate_count_vel;
 
-	float _rate_lat_lon;
-	float _rate_vel;
+	float _rate_lat_lon = 0.0f;
+	float _rate_vel = 0.0f;
 
 	uint64_t _interval_rate_start;
 };
diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 41716cd97..c4f4f7bec 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -263,7 +263,7 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 	_gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
-	_gps_position->satellites_visible = packet.satellites;
+	_gps_position->satellites_used = packet.satellites;
 
 	/* convert time and date information to unix timestamp */
 	struct tm timeinfo; //TODO: test this conversion
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 404607571..d0854f5e9 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013, 2014 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
@@ -34,14 +34,18 @@
 /**
  * @file ubx.cpp
  *
- * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * U-Blox protocol implementation. Following u-blox 6/7/8 Receiver Description
  * including Prototol Specification.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  *
+ * @author Hannes Delago
+ *   (rework, add ubx7+ compatibility)
+ *
  * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_%28UBX-13003221%29.pdf
  */
 
 #include <assert.h>
@@ -55,24 +59,44 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 #include <drivers/drv_hrt.h>
 
 #include "ubx.h"
 
-#define UBX_CONFIG_TIMEOUT		200		// ms, timeout for waiting ACK
-#define UBX_PACKET_TIMEOUT		2		// ms, if now data during this delay assume that full update received
+#define UBX_CONFIG_TIMEOUT	200		// ms, timeout for waiting ACK
+#define UBX_PACKET_TIMEOUT	2		// ms, if now data during this delay assume that full update received
 #define UBX_WAIT_BEFORE_READ	20		// ms, wait before reading to save read() calls
-#define DISABLE_MSG_INTERVAL	1000000	// us, try to disable message with this interval
+#define DISABLE_MSG_INTERVAL	1000000		// us, try to disable message with this interval
+
+#define MIN(X,Y)	((X) < (Y) ? (X) : (Y))
+#define SWAP16(X)	((((X) >>  8) & 0x00ff) | (((X) << 8) & 0xff00))
+
+#define FNV1_32_INIT	((uint32_t)0x811c9dc5)	// init value for FNV1 hash algorithm
+#define FNV1_32_PRIME	((uint32_t)0x01000193)	// magic prime for FNV1 hash algorithm
+
 
-UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
+/**** Trace macros, disable for production builds */
+#define UBX_TRACE_PARSER(s, ...)	{/*printf(s, ## __VA_ARGS__);*/}	/* decoding progress in parse_char() */
+#define UBX_TRACE_RXMSG(s, ...)		{/*printf(s, ## __VA_ARGS__);*/}	/* Rx msgs in payload_rx_done() */
+#define UBX_TRACE_SVINFO(s, ...)	{/*printf(s, ## __VA_ARGS__);*/}	/* NAV-SVINFO processing (debug use only, will cause rx buffer overflows) */
+
+/**** Warning macros, disable to save memory */
+#define UBX_WARN(s, ...)		{warnx(s, ## __VA_ARGS__);}
+
+
+UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info) :
 	_fd(fd),
 	_gps_position(gps_position),
+	_satellite_info(satellite_info),
 	_configured(false),
-	_waiting_for_ack(false),
+	_ack_state(UBX_ACK_IDLE),
 	_got_posllh(false),
 	_got_velned(false),
-	_got_timeutc(false),
-	_disable_cmd_last(0)
+	_disable_cmd_last(0),
+	_ack_waiting_msg(0),
+	_ubx_version(0),
+	_use_nav_pvt(false)
 {
 	decode_init();
 }
@@ -86,182 +110,167 @@ UBX::configure(unsigned &baudrate)
 {
 	_configured = false;
 	/* try different baudrates */
-	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
+	const unsigned baudrates[] = {9600, 38400, 19200, 57600, 115200};
 
-	int baud_i;
+	unsigned baud_i;
 
-	for (baud_i = 0; baud_i < 5; baud_i++) {
-		baudrate = baudrates_to_try[baud_i];
+	for (baud_i = 0; baud_i < sizeof(baudrates) / sizeof(baudrates[0]); baud_i++) {
+		baudrate = baudrates[baud_i];
 		set_baudrate(_fd, baudrate);
 
+		/* flush input and wait for at least 20 ms silence */
+		decode_init();
+		receive(20);
+		decode_init();
+
 		/* 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));
-
-		_message_class_needed = UBX_CLASS_CFG;
-		_message_id_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 (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		 * and leave the baudrate as it is, we just want an ACK-ACK for this */
+		memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
+		_buf.payload_tx_cfg_prt.portID		= UBX_TX_CFG_PRT_PORTID;
+		_buf.payload_tx_cfg_prt.mode		= UBX_TX_CFG_PRT_MODE;
+		_buf.payload_tx_cfg_prt.baudRate	= baudrate;
+		_buf.payload_tx_cfg_prt.inProtoMask	= UBX_TX_CFG_PRT_INPROTOMASK;
+		_buf.payload_tx_cfg_prt.outProtoMask	= UBX_TX_CFG_PRT_OUTPROTOMASK;
+
+		send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
+
+		if (wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false) < 0) {
 			/* try next baudrate */
 			continue;
 		}
 
 		/* Send a CFG-PRT message again, this time change the baudrate */
+		memset(&_buf.payload_tx_cfg_prt, 0, sizeof(_buf.payload_tx_cfg_prt));
+		_buf.payload_tx_cfg_prt.portID		= UBX_TX_CFG_PRT_PORTID;
+		_buf.payload_tx_cfg_prt.mode		= UBX_TX_CFG_PRT_MODE;
+		_buf.payload_tx_cfg_prt.baudRate	= UBX_TX_CFG_PRT_BAUDRATE;
+		_buf.payload_tx_cfg_prt.inProtoMask	= UBX_TX_CFG_PRT_INPROTOMASK;
+		_buf.payload_tx_cfg_prt.outProtoMask	= UBX_TX_CFG_PRT_OUTPROTOMASK;
 
-		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));
+		send_message(UBX_MSG_CFG_PRT, _buf.raw, sizeof(_buf.payload_tx_cfg_prt));
 
 		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-		wait_for_ack(UBX_CONFIG_TIMEOUT);
+		wait_for_ack(UBX_MSG_CFG_PRT, UBX_CONFIG_TIMEOUT, false);
 
-		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
-			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
-			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
+		if (UBX_TX_CFG_PRT_BAUDRATE != baudrate) {
+			set_baudrate(_fd, UBX_TX_CFG_PRT_BAUDRATE);
+			baudrate = UBX_TX_CFG_PRT_BAUDRATE;
 		}
 
 		/* at this point we have correct baudrate on both ends */
 		break;
 	}
 
-	if (baud_i >= 5) {
-		return 1;
+	if (baud_i >= sizeof(baudrates) / sizeof(baudrates[0])) {
+		return 1;	// connection and/or baudrate detection failed
 	}
 
-	/* send a CFG-RATE message to define update rate */
-	type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
-	memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+	/* Send a CFG-RATE message to define update rate */
+	memset(&_buf.payload_tx_cfg_rate, 0, sizeof(_buf.payload_tx_cfg_rate));
+	_buf.payload_tx_cfg_rate.measRate	= UBX_TX_CFG_RATE_MEASINTERVAL;
+	_buf.payload_tx_cfg_rate.navRate	= UBX_TX_CFG_RATE_NAVRATE;
+	_buf.payload_tx_cfg_rate.timeRef	= UBX_TX_CFG_RATE_TIMEREF;
 
-	_message_class_needed = UBX_CLASS_CFG;
-	_message_id_needed = UBX_MESSAGE_CFG_RATE;
+	send_message(UBX_MSG_CFG_RATE, _buf.raw, sizeof(_buf.payload_tx_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) {
-		warnx("CFG FAIL: RATE");
+	if (wait_for_ack(UBX_MSG_CFG_RATE, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
 	/* 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));
-
-	_message_class_needed = UBX_CLASS_CFG;
-	_message_id_needed = UBX_MESSAGE_CFG_NAV5;
+	memset(&_buf.payload_tx_cfg_nav5, 0, sizeof(_buf.payload_tx_cfg_nav5));
+	_buf.payload_tx_cfg_nav5.mask		= UBX_TX_CFG_NAV5_MASK;
+	_buf.payload_tx_cfg_nav5.dynModel	= UBX_TX_CFG_NAV5_DYNMODEL;
+	_buf.payload_tx_cfg_nav5.fixMode	= UBX_TX_CFG_NAV5_FIXMODE;
 
-	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_message(UBX_MSG_CFG_NAV5, _buf.raw, sizeof(_buf.payload_tx_cfg_nav5));
 
-	send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("CFG FAIL: NAV5");
+	if (wait_for_ack(UBX_MSG_CFG_NAV5, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
 
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: NAV POSLLH");
-		return 1;
+	/* try to set rate for NAV-PVT */
+	/* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */
+	configure_message_rate(UBX_MSG_NAV_PVT, 1);
+	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+		_use_nav_pvt = false;
+	} else {
+		_use_nav_pvt = true;
 	}
+	UBX_WARN("%susing NAV-PVT", _use_nav_pvt ? "" : "not ");
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
+	if (!_use_nav_pvt) {
+		configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: NAV TIMEUTC");
-		return 1;
-	}
+		configure_message_rate(UBX_MSG_NAV_POSLLH, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
+		configure_message_rate(UBX_MSG_NAV_SOL, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: NAV SOL");
-		return 1;
+		configure_message_rate(UBX_MSG_NAV_VELNED, 1);
+		if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
+			return 1;
+		}
 	}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: NAV VELNED");
+	configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0);
+	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
-	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: NAV SVINFO");
+	configure_message_rate(UBX_MSG_MON_HW, 1);
+	if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) {
 		return 1;
 	}
 
-	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
-
-	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("MSG CFG FAIL: MON HW");
-		return 1;
-	}
+	/* request module version information by sending an empty MON-VER message */
+	send_message(UBX_MSG_MON_VER, nullptr, 0);
 
 	_configured = true;
 	return 0;
 }
 
-int
-UBX::wait_for_ack(unsigned timeout)
+int	// -1 = NAK, error or timeout, 0 = ACK
+UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report)
 {
-	_waiting_for_ack = true;
-	uint64_t time_started = hrt_absolute_time();
+	int ret = -1;
 
-	while (hrt_absolute_time() < time_started + timeout * 1000) {
-		if (receive(timeout) > 0) {
-			if (!_waiting_for_ack) {
-				return 1;
-			}
+	_ack_state = UBX_ACK_WAITING;
+	_ack_waiting_msg = msg;	// memorize sent msg class&ID for ACK check
+
+	hrt_abstime time_started = hrt_absolute_time();
 
+	while ((_ack_state == UBX_ACK_WAITING) && (hrt_absolute_time() < time_started + timeout * 1000)) {
+		receive(timeout);
+	}
+
+	if (_ack_state == UBX_ACK_GOT_ACK) {
+		ret = 0;	// ACK received ok
+	} else if (report) {
+		if (_ack_state == UBX_ACK_GOT_NAK) {
+			UBX_WARN("ubx msg 0x%04x NAK", SWAP16((unsigned)msg));
 		} else {
-			return -1;	// timeout or error receiving, or NAK
+			UBX_WARN("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg));
 		}
 	}
 
-	return -1;	// timeout
+	_ack_state = UBX_ACK_IDLE;
+	return ret;
 }
 
-int
-UBX::receive(unsigned timeout)
+int	// -1 = error, 0 = no message handled, 1 = message handled, 2 = sat info message handled
+UBX::receive(const unsigned timeout)
 {
 	/* poll descriptor */
 	pollfd fds[1];
@@ -275,17 +284,17 @@ UBX::receive(unsigned timeout)
 
 	ssize_t count = 0;
 
-	bool handled = false;
+	int handled = 0;
 
 	while (true) {
-		bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled;
+		bool ready_to_return = _configured ? (_got_posllh && _got_velned) : handled;
 
 		/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
 		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
-			warnx("poll error");
+			UBX_WARN("ubx poll() err");
 			return -1;
 
 		} else if (ret == 0) {
@@ -293,8 +302,7 @@ UBX::receive(unsigned timeout)
 			if (ready_to_return) {
 				_got_posllh = false;
 				_got_velned = false;
-				_got_timeutc = false;
-				return 1;
+				return handled;
 
 			} else {
 				return -1;
@@ -314,440 +322,675 @@ UBX::receive(unsigned timeout)
 
 				/* pass received bytes to the packet decoder */
 				for (int i = 0; i < count; i++) {
-					if (parse_char(buf[i]) > 0) {
-						if (handle_message() > 0)
-							handled = true;
-					}
+					handled |= parse_char(buf[i]);
 				}
 			}
 		}
 
 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
-			warnx("timeout - no useful messages");
 			return -1;
 		}
 	}
 }
 
-int
-UBX::parse_char(uint8_t b)
+int	// 0 = decoding, 1 = message handled, 2 = sat info message handled
+UBX::parse_char(const uint8_t b)
 {
+	int ret = 0;
+
 	switch (_decode_state) {
-		/* First, look for sync1 */
-	case UBX_DECODE_UNINIT:
-		if (b == UBX_SYNC1) {
-			_decode_state = UBX_DECODE_GOT_SYNC1;
-		}
 
+	/* Expecting Sync1 */
+	case UBX_DECODE_SYNC1:
+		if (b == UBX_SYNC1) {	// Sync1 found --> expecting Sync2
+			UBX_TRACE_PARSER("\nA");
+			_decode_state = UBX_DECODE_SYNC2;
+		}
 		break;
 
-		/* Second, look for sync2 */
-	case UBX_DECODE_GOT_SYNC1:
-		if (b == UBX_SYNC2) {
-			_decode_state = UBX_DECODE_GOT_SYNC2;
+	/* Expecting Sync2 */
+	case UBX_DECODE_SYNC2:
+		if (b == UBX_SYNC2) {	// Sync2 found --> expecting Class
+			UBX_TRACE_PARSER("B");
+			_decode_state = UBX_DECODE_CLASS;
 
-		} else {
-			/* Second start symbol was wrong, reset state machine */
+		} else {		// Sync1 not followed by Sync2: reset parser
 			decode_init();
-			/* don't return error, it can be just false sync1 */
 		}
+		break;
 
+	/* Expecting Class */
+	case UBX_DECODE_CLASS:
+		UBX_TRACE_PARSER("C");
+		add_byte_to_checksum(b);  // checksum is calculated for everything except Sync and Checksum bytes
+		_rx_msg = b;
+		_decode_state = UBX_DECODE_ID;
 		break;
 
-		/* Now look for class */
-	case UBX_DECODE_GOT_SYNC2:
-		/* everything except sync1 and sync2 needs to be added to the checksum */
+	/* Expecting ID */
+	case UBX_DECODE_ID:
+		UBX_TRACE_PARSER("D");
 		add_byte_to_checksum(b);
-		_message_class = b;
-		_decode_state = UBX_DECODE_GOT_CLASS;
+		_rx_msg |= b << 8;
+		_decode_state = UBX_DECODE_LENGTH1;
 		break;
 
-	case UBX_DECODE_GOT_CLASS:
+	/* Expecting first length byte */
+	case UBX_DECODE_LENGTH1:
+		UBX_TRACE_PARSER("E");
 		add_byte_to_checksum(b);
-		_message_id = b;
-		_decode_state = UBX_DECODE_GOT_MESSAGEID;
+		_rx_payload_length = b;
+		_decode_state = UBX_DECODE_LENGTH2;
 		break;
 
-	case UBX_DECODE_GOT_MESSAGEID:
+	/* Expecting second length byte */
+	case UBX_DECODE_LENGTH2:
+		UBX_TRACE_PARSER("F");
 		add_byte_to_checksum(b);
-		_payload_size = b; //this is the first length byte
-		_decode_state = UBX_DECODE_GOT_LENGTH1;
+		_rx_payload_length |= b << 8;	// calculate payload size
+		if (payload_rx_init() != 0) {	// start payload reception
+			// payload will not be handled, discard message
+			decode_init();
+		} else {
+			_decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1;
+		}
 		break;
 
-	case UBX_DECODE_GOT_LENGTH1:
+	/* Expecting payload */
+	case UBX_DECODE_PAYLOAD:
+		UBX_TRACE_PARSER(".");
 		add_byte_to_checksum(b);
-		_payload_size += b << 8; // here comes the second byte of length
-		_decode_state = UBX_DECODE_GOT_LENGTH2;
+		switch (_rx_msg) {
+		case UBX_MSG_NAV_SVINFO:
+			ret = payload_rx_add_nav_svinfo(b);	// add a NAV-SVINFO payload byte
+			break;
+		case UBX_MSG_MON_VER:
+			ret = payload_rx_add_mon_ver(b);	// add a MON-VER payload byte
+			break;
+		default:
+			ret = payload_rx_add(b);		// add a payload byte
+			break;
+		}
+		if (ret < 0) {
+			// payload not handled, discard message
+			decode_init();
+		} else if (ret > 0) {
+			// payload complete, expecting checksum
+			_decode_state = UBX_DECODE_CHKSUM1;
+		} else {
+			// expecting more payload, stay in state UBX_DECODE_PAYLOAD
+		}
+		ret = 0;
 		break;
 
-	case UBX_DECODE_GOT_LENGTH2:
+	/* Expecting first checksum byte */
+	case UBX_DECODE_CHKSUM1:
+		if (_rx_ck_a != b) {
+			UBX_WARN("ubx checksum err");
+			decode_init();
+		} else {
+			_decode_state = UBX_DECODE_CHKSUM2;
+		}
+		break;
 
-		/* Add to checksum if not yet at checksum byte */
-		if (_rx_count < _payload_size)
-			add_byte_to_checksum(b);
+	/* Expecting second checksum byte */
+	case UBX_DECODE_CHKSUM2:
+		if (_rx_ck_b != b) {
+			UBX_WARN("ubx checksum err");
+		} else {
+			ret = payload_rx_done();	// finish payload processing
+		}
+		decode_init();
+		break;
 
-		_rx_buffer[_rx_count] = b;
+	default:
+		break;
+	}
 
-		/* 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]) {
-				decode_init();
-				return 1;	// message received successfully
+	return ret;
+}
 
-			} else {
-				warnx("checksum wrong");
-				decode_init();
-				return -1;
-			}
+/**
+ * Start payload rx
+ */
+int	// -1 = abort, 0 = continue
+UBX::payload_rx_init()
+{
+	int ret = 0;
 
-		} else if (_rx_count < RECV_BUFFER_SIZE) {
-			_rx_count++;
+	_rx_state = UBX_RXMSG_HANDLE;	// handle by default
+
+	switch (_rx_msg) {
+	case UBX_MSG_NAV_PVT:
+		if (   (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7)		/* u-blox 7 msg format */
+		    && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8))	/* u-blox 8+ msg format */
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (!_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if not using NAV-PVT
+		break;
 
-		} else {
-			warnx("buffer full");
-			decode_init();
-			return -1;
-		}
+	case UBX_MSG_NAV_POSLLH:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
 
+	case UBX_MSG_NAV_SOL:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_NAV_TIMEUTC:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_NAV_SVINFO:
+		if (_satellite_info == nullptr)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if sat info not requested
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else
+			memset(_satellite_info, 0, sizeof(*_satellite_info));	// initialize sat info
+		break;
+
+	case UBX_MSG_NAV_VELNED:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		else if (_use_nav_pvt)
+			_rx_state = UBX_RXMSG_DISABLE;	// disable if using NAV-PVT instead
+		break;
+
+	case UBX_MSG_MON_VER:
+		break;		// unconditionally handle this message
+
+	case UBX_MSG_MON_HW:
+		if (   (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t))	/* u-blox 6 msg format */
+		    && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t)))	/* u-blox 7+ msg format */
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (!_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if not _configured
+		break;
+
+	case UBX_MSG_ACK_ACK:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if _configured
+		break;
+
+	case UBX_MSG_ACK_NAK:
+		if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t))
+			_rx_state = UBX_RXMSG_ERROR_LENGTH;
+		else if (_configured)
+			_rx_state = UBX_RXMSG_IGNORE;	// ignore if _configured
 		break;
 
 	default:
+		_rx_state = UBX_RXMSG_DISABLE;	// disable all other messages
 		break;
 	}
 
-	return 0;	// message decoding in progress
+	switch (_rx_state) {
+	case UBX_RXMSG_HANDLE:	// handle message
+	case UBX_RXMSG_IGNORE:	// ignore message but don't report error
+		ret = 0;
+		break;
+
+	case UBX_RXMSG_DISABLE:	// disable unexpected messages
+		UBX_WARN("ubx msg 0x%04x len %u unexpected", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+
+		{
+			hrt_abstime t = hrt_absolute_time();
+
+			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
+				/* don't attempt for every message to disable, some might not be disabled */
+				_disable_cmd_last = t;
+				UBX_WARN("ubx disabling msg 0x%04x", SWAP16((unsigned)_rx_msg));
+				configure_message_rate(_rx_msg, 0);
+			}
+		}
+
+		ret = -1;	// return error, abort handling this message
+		break;
+
+	case UBX_RXMSG_ERROR_LENGTH:	// error: invalid length
+		UBX_WARN("ubx msg 0x%04x invalid len %u", SWAP16((unsigned)_rx_msg), (unsigned)_rx_payload_length);
+		ret = -1;	// return error, abort handling this message
+		break;
+
+	default:	// invalid message state
+		UBX_WARN("ubx internal err1");
+		ret = -1;	// return error, abort handling this message
+		break;
+	}
+
+	return ret;
 }
 
+/**
+ * Add payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add(const uint8_t b)
+{
+	int ret = 0;
 
+	_buf.raw[_rx_payload_index] = b;
 
-int
-UBX::handle_message()
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
+
+	return ret;
+}
+
+/**
+ * Add NAV-SVINFO payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_nav_svinfo(const uint8_t b)
 {
 	int ret = 0;
 
-	if (_configured) {
-		/* handle only info messages when configured */
-		switch (_message_class) {
-		case UBX_CLASS_NAV:
-			switch (_message_id) {
-			case UBX_MESSAGE_NAV_POSLLH: {
-					// printf("GOT NAV_POSLLH\n");
-					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 = (float)packet->hAcc * 1e-3f; // from mm to m
-					_gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m
-					_gps_position->timestamp_position = hrt_absolute_time();
-
-					_rate_count_lat_lon++;
-
-					_got_posllh = true;
-					ret = 1;
-					break;
-				}
+	if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+		// Fill Part 1 buffer
+		_buf.raw[_rx_payload_index] = b;
+	} else {
+		if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) {
+			// Part 1 complete: decode Part 1 buffer
+			_satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, SAT_INFO_MAX_SATELLITES);
+			UBX_TRACE_SVINFO("SVINFO len %u  numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh);
+		}
+		if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) {
+			// Still room in _satellite_info: fill Part 2 buffer
+			unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+			_buf.raw[buf_index] = b;
+			if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) {
+				// Part 2 complete: decode Part 2 buffer
+				unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t);
+				_satellite_info->used[sat_index]	= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01);
+				_satellite_info->snr[sat_index]		= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno);
+				_satellite_info->elevation[sat_index]	= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev);
+				_satellite_info->azimuth[sat_index]	= (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f);
+				_satellite_info->svid[sat_index]	= (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid);
+				UBX_TRACE_SVINFO("SVINFO #%02u  used %u  snr %3u  elevation %3u  azimuth %3u  svid %3u\n",
+						(unsigned)sat_index + 1,
+						(unsigned)_satellite_info->used[sat_index],
+						(unsigned)_satellite_info->snr[sat_index],
+						(unsigned)_satellite_info->elevation[sat_index],
+						(unsigned)_satellite_info->azimuth[sat_index],
+						(unsigned)_satellite_info->svid[sat_index]
+				);
+			}
+		}
+	}
+
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
 
-			case UBX_MESSAGE_NAV_SOL: {
-					// printf("GOT NAV_SOL\n");
-					gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+	return ret;
+}
 
-					_gps_position->fix_type = packet->gpsFix;
-					_gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s
-					_gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m
-					_gps_position->timestamp_variance = hrt_absolute_time();
+/**
+ * Add MON-VER payload rx byte
+ */
+int	// -1 = error, 0 = ok, 1 = payload completed
+UBX::payload_rx_add_mon_ver(const uint8_t b)
+{
+	int ret = 0;
 
-					ret = 1;
-					break;
-				}
+	if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+		// Fill Part 1 buffer
+		_buf.raw[_rx_payload_index] = b;
+	} else {
+		if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) {
+			// Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings
+			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.swVersion, FNV1_32_INIT);
+			_ubx_version = fnv1_32_str(_buf.payload_rx_mon_ver_part1.hwVersion, _ubx_version);
+			UBX_WARN("VER hash 0x%08x", _ubx_version);
+			UBX_WARN("VER hw  \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion);
+			UBX_WARN("VER sw  \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion);
+		}
+		// fill Part 2 buffer
+		unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t);
+		_buf.raw[buf_index] = b;
+		if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) {
+			// Part 2 complete: decode Part 2 buffer
+			UBX_WARN("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension);
+		}
+	}
 
-			case UBX_MESSAGE_NAV_TIMEUTC: {
-					// printf("GOT NAV_TIMEUTC\n");
-					gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+	if (++_rx_payload_index >= _rx_payload_length) {
+		ret = 1;	// payload received completely
+	}
 
-					/* 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);
+	return ret;
+}
+
+/**
+ * Finish payload rx
+ */
+int	// 0 = no message handled, 1 = message handled, 2 = sat info message handled
+UBX::payload_rx_done(void)
+{
+	int ret = 0;
+
+	// return if no message handled
+	if (_rx_state != UBX_RXMSG_HANDLE) {
+		return ret;
+	}
+
+	// handle message
+	switch (_rx_msg) {
+
+	case UBX_MSG_NAV_PVT:
+		UBX_TRACE_RXMSG("Rx NAV-PVT\n");
+
+		_gps_position->fix_type		= _buf.payload_rx_nav_pvt.fixType;
+		_gps_position->satellites_used	= _buf.payload_rx_nav_pvt.numSV;
+
+		_gps_position->lat		= _buf.payload_rx_nav_pvt.lat;
+		_gps_position->lon		= _buf.payload_rx_nav_pvt.lon;
+		_gps_position->alt		= _buf.payload_rx_nav_pvt.hMSL;
+
+		_gps_position->eph		= (float)_buf.payload_rx_nav_pvt.hAcc * 1e-3f;
+		_gps_position->epv		= (float)_buf.payload_rx_nav_pvt.vAcc * 1e-3f;
+		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_pvt.sAcc * 1e-3f;
+
+		_gps_position->vel_m_s		= (float)_buf.payload_rx_nav_pvt.gSpeed * 1e-3f;
+
+		_gps_position->vel_n_m_s	= (float)_buf.payload_rx_nav_pvt.velN * 1e-3f;
+		_gps_position->vel_e_m_s	= (float)_buf.payload_rx_nav_pvt.velE * 1e-3f;
+		_gps_position->vel_d_m_s	= (float)_buf.payload_rx_nav_pvt.velD * 1e-3f;
+		_gps_position->vel_ned_valid	= true;
+
+		_gps_position->cog_rad		= (float)_buf.payload_rx_nav_pvt.headMot * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->c_variance_rad	= (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f;
+
+		{
+			/* convert to unix timestamp */
+			struct tm timeinfo;
+			timeinfo.tm_year	= _buf.payload_rx_nav_pvt.year - 1900;
+			timeinfo.tm_mon		= _buf.payload_rx_nav_pvt.month - 1;
+			timeinfo.tm_mday	= _buf.payload_rx_nav_pvt.day;
+			timeinfo.tm_hour	= _buf.payload_rx_nav_pvt.hour;
+			timeinfo.tm_min		= _buf.payload_rx_nav_pvt.min;
+			timeinfo.tm_sec		= _buf.payload_rx_nav_pvt.sec;
+			time_t epoch = mktime(&timeinfo);
 
 #ifndef CONFIG_RTC
-					//Since we lack a hardware RTC, set the system time clock based on GPS UTC
-					//TODO generalize this by moving into gps.cpp?
-					timespec ts;
-					ts.tv_sec = epoch;
-					ts.tv_nsec = packet->time_nanoseconds;
-					clock_settime(CLOCK_REALTIME, &ts);
+			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+			//TODO generalize this by moving into gps.cpp?
+			timespec ts;
+			ts.tv_sec = epoch;
+			ts.tv_nsec = _buf.payload_rx_nav_pvt.nano;
+			clock_settime(CLOCK_REALTIME, &ts);
 #endif
 
-					_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();
+			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f);
+		}
 
-					_got_timeutc = true;
-					ret = 1;
-					break;
-				}
+		_gps_position->timestamp_time		= hrt_absolute_time();
+		_gps_position->timestamp_velocity 	= hrt_absolute_time();
+		_gps_position->timestamp_variance 	= hrt_absolute_time();
+		_gps_position->timestamp_position	= hrt_absolute_time();
 
-			case UBX_MESSAGE_NAV_SVINFO: {
-					//printf("GOT NAV_SVINFO\n");
-					const int length_part1 = 8;
-					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
-					const int length_part2 = 12;
-					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-
-					uint8_t satellites_used = 0;
-					int i;
-
-					//printf("Number of Channels: %d\n", packet_part1->numCh);
-					for (i = 0; i < packet_part1->numCh; i++) {
-						/* set pointer to sattelite_i information */
-						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
-
-						/* write satellite information to global storage */
-						uint8_t sv_used = packet_part2->flags & 0x01;
-
-						if (sv_used) {
-							/* count SVs used for NAV */
-							satellites_used++;
-						}
-
-						/* record info for all channels, whether or not the SV is used for NAV */
-						_gps_position->satellite_used[i] = sv_used;
-						_gps_position->satellite_snr[i] = packet_part2->cno;
-						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-						_gps_position->satellite_prn[i] = packet_part2->svid;
-						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
-					}
-
-					for (i = packet_part1->numCh; i < 20; i++) {
-						/* unused channels have to be set to zero for e.g. MAVLink */
-						_gps_position->satellite_prn[i] = 0;
-						_gps_position->satellite_used[i] = 0;
-						_gps_position->satellite_snr[i] = 0;
-						_gps_position->satellite_elevation[i] = 0;
-						_gps_position->satellite_azimuth[i] = 0;
-					}
-
-					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
-
-					if (packet_part1->numCh > 0) {
-						_gps_position->satellite_info_available = true;
-
-					} else {
-						_gps_position->satellite_info_available = false;
-					}
-
-					_gps_position->timestamp_satellites = hrt_absolute_time();
-
-					ret = 1;
-					break;
-				}
+		_rate_count_vel++;
+		_rate_count_lat_lon++;
 
-			case UBX_MESSAGE_NAV_VELNED: {
-					// printf("GOT NAV_VELNED\n");
-					gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+		_got_posllh = true;
+		_got_velned = true;
 
-					_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();
+		ret = 1;
+		break;
 
-					_rate_count_vel++;
+	case UBX_MSG_NAV_POSLLH:
+		UBX_TRACE_RXMSG("Rx NAV-POSLLH\n");
 
-					_got_velned = true;
-					ret = 1;
-					break;
-				}
+		_gps_position->lat	= _buf.payload_rx_nav_posllh.lat;
+		_gps_position->lon	= _buf.payload_rx_nav_posllh.lon;
+		_gps_position->alt	= _buf.payload_rx_nav_posllh.hMSL;
+		_gps_position->eph	= (float)_buf.payload_rx_nav_posllh.hAcc * 1e-3f; // from mm to m
+		_gps_position->epv	= (float)_buf.payload_rx_nav_posllh.vAcc * 1e-3f; // from mm to m
 
-			default:
-				break;
-			}
+		_gps_position->timestamp_position = hrt_absolute_time();
 
-			break;
+		_rate_count_lat_lon++;
+		_got_posllh = true;
 
-		case UBX_CLASS_ACK: {
-				/* ignore ACK when already configured */
-				ret = 1;
-				break;
-			}
+		ret = 1;
+		break;
 
-		case UBX_CLASS_MON: {
-			switch (_message_id) {
-			case UBX_MESSAGE_MON_HW: {
+	case UBX_MSG_NAV_SOL:
+		UBX_TRACE_RXMSG("Rx NAV-SOL\n");
 
-					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+		_gps_position->fix_type		= _buf.payload_rx_nav_sol.gpsFix;
+		_gps_position->s_variance_m_s	= (float)_buf.payload_rx_nav_sol.sAcc * 1e-2f;	// from cm to m
+		_gps_position->satellites_used	= _buf.payload_rx_nav_sol.numSV;
 
-					_gps_position->noise_per_ms = p->noisePerMS;
-					_gps_position->jamming_indicator = p->jamInd;
+		_gps_position->timestamp_variance = hrt_absolute_time();
 
-					ret = 1;
-					break;
-				}
+		ret = 1;
+		break;
 
-			default:
-				break;
-			}
+	case UBX_MSG_NAV_TIMEUTC:
+		UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n");
+
+		{
+			/* convert to unix timestamp */
+			struct tm timeinfo;
+			timeinfo.tm_year	= _buf.payload_rx_nav_timeutc.year - 1900;
+			timeinfo.tm_mon		= _buf.payload_rx_nav_timeutc.month - 1;
+			timeinfo.tm_mday	= _buf.payload_rx_nav_timeutc.day;
+			timeinfo.tm_hour	= _buf.payload_rx_nav_timeutc.hour;
+			timeinfo.tm_min		= _buf.payload_rx_nav_timeutc.min;
+			timeinfo.tm_sec		= _buf.payload_rx_nav_timeutc.sec;
+			time_t epoch = mktime(&timeinfo);
+
+#ifndef CONFIG_RTC
+			//Since we lack a hardware RTC, set the system time clock based on GPS UTC
+			//TODO generalize this by moving into gps.cpp?
+			timespec ts;
+			ts.tv_sec = epoch;
+			ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano;
+			clock_settime(CLOCK_REALTIME, &ts);
+#endif
+
+			_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+			_gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f);
 		}
 
-		default:
+		_gps_position->timestamp_time = hrt_absolute_time();
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_NAV_SVINFO:
+		UBX_TRACE_RXMSG("Rx NAV-SVINFO\n");
+
+		// _satellite_info already populated by payload_rx_add_svinfo(), just add a timestamp
+		_satellite_info->timestamp = hrt_absolute_time();
+
+		ret = 2;
+		break;
+
+	case UBX_MSG_NAV_VELNED:
+		UBX_TRACE_RXMSG("Rx NAV-VELNED\n");
+
+		_gps_position->vel_m_s		= (float)_buf.payload_rx_nav_velned.speed * 1e-2f;
+		_gps_position->vel_n_m_s	= (float)_buf.payload_rx_nav_velned.velN * 1e-2f; /* NED NORTH velocity */
+		_gps_position->vel_e_m_s	= (float)_buf.payload_rx_nav_velned.velE * 1e-2f; /* NED EAST velocity */
+		_gps_position->vel_d_m_s	= (float)_buf.payload_rx_nav_velned.velD * 1e-2f; /* NED DOWN velocity */
+		_gps_position->cog_rad		= (float)_buf.payload_rx_nav_velned.heading * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->c_variance_rad	= (float)_buf.payload_rx_nav_velned.cAcc * M_DEG_TO_RAD_F * 1e-5f;
+		_gps_position->vel_ned_valid	= true;
+
+		_gps_position->timestamp_velocity = hrt_absolute_time();
+
+		_rate_count_vel++;
+		_got_velned = true;
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_MON_VER:
+		UBX_TRACE_RXMSG("Rx MON-VER\n");
+
+		ret = 1;
+		break;
+
+	case UBX_MSG_MON_HW:
+		UBX_TRACE_RXMSG("Rx MON-HW\n");
+
+		switch (_rx_payload_length) {
+
+		case sizeof(ubx_payload_rx_mon_hw_ubx6_t):	/* u-blox 6 msg format */
+			_gps_position->noise_per_ms		= _buf.payload_rx_mon_hw_ubx6.noisePerMS;
+			_gps_position->jamming_indicator	= _buf.payload_rx_mon_hw_ubx6.jamInd;
+
+			ret = 1;
 			break;
-		}
 
-		if (ret == 0) {
-			/* message not handled */
-			warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+		case sizeof(ubx_payload_rx_mon_hw_ubx7_t):	/* u-blox 7+ msg format */
+			_gps_position->noise_per_ms		= _buf.payload_rx_mon_hw_ubx7.noisePerMS;
+			_gps_position->jamming_indicator	= _buf.payload_rx_mon_hw_ubx7.jamInd;
 
-			hrt_abstime t = hrt_absolute_time();
+			ret = 1;
+			break;
 
-			if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
-				/* don't attempt for every message to disable, some might not be disabled */
-				_disable_cmd_last = t;
-				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
-				configure_message_rate(_message_class, _message_id, 0);
-			}
+		default:		// unexpected payload size:
+			ret = 0;	// don't handle message
+			break;
 		}
+		break;
 
-	} else {
-		/* handle only ACK while configuring */
-		if (_message_class == UBX_CLASS_ACK) {
-			switch (_message_id) {
-			case UBX_MESSAGE_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 == _message_class_needed && packet->msgID == _message_id_needed) {
-							_waiting_for_ack = false;
-							ret = 1;
-						}
-					}
-
-					break;
-				}
+	case UBX_MSG_ACK_ACK:
+		UBX_TRACE_RXMSG("Rx ACK-ACK\n");
 
-			case UBX_MESSAGE_ACK_NAK: {
-					// printf("GOT ACK_NAK\n");
-					warnx("ubx: not acknowledged");
-					/* configuration obviously not successful */
-					_waiting_for_ack = false;
-					ret = -1;
-					break;
-				}
+		if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+			_ack_state = UBX_ACK_GOT_ACK;
+		}
 
-			default:
-				break;
-			}
+		ret = 1;
+		break;
+
+	case UBX_MSG_ACK_NAK:
+		UBX_TRACE_RXMSG("Rx ACK-NAK\n");
+
+		if ((_ack_state == UBX_ACK_WAITING) && (_buf.payload_rx_ack_ack.msg == _ack_waiting_msg)) {
+			_ack_state = UBX_ACK_GOT_NAK;
 		}
+
+		ret = 1;
+		break;
+
+	default:
+		break;
 	}
 
-	decode_init();
 	return ret;
 }
 
 void
 UBX::decode_init(void)
 {
+	_decode_state = UBX_DECODE_SYNC1;
 	_rx_ck_a = 0;
 	_rx_ck_b = 0;
-	_rx_count = 0;
-	_decode_state = UBX_DECODE_UNINIT;
-	_payload_size = 0;
-	/* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */
+	_rx_payload_length = 0;
+	_rx_payload_index = 0;
 }
 
 void
-UBX::add_byte_to_checksum(uint8_t b)
+UBX::add_byte_to_checksum(const 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)
+UBX::calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum)
 {
-	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;
+	for (uint16_t i = 0; i < length; i++) {
+		checksum->ck_a = checksum->ck_a + buffer[i];
+		checksum->ck_b = checksum->ck_b + checksum->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)
+UBX::configure_message_rate(const uint16_t msg, const uint8_t rate)
 {
-	for (unsigned i = 0; i < length; i++) {
-		ck_a = ck_a + message[i];
-		ck_b = ck_b + ck_a;
-	}
+	ubx_payload_tx_cfg_msg_t cfg_msg;	// don't use _buf (allow interleaved operation)
+
+	cfg_msg.msg	= msg;
+	cfg_msg.rate	= rate;
+
+	send_message(UBX_MSG_CFG_MSG, (uint8_t *)&cfg_msg, sizeof(cfg_msg));
 }
 
 void
-UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
+UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length)
 {
-	struct ubx_cfg_msg_rate msg;
-	msg.msg_class	= msg_class;
-	msg.msg_id		= msg_id;
-	msg.rate		= rate;
-	send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+	ubx_header_t   header = {UBX_SYNC1, UBX_SYNC2};
+	ubx_checksum_t checksum = {0, 0};
+
+	// Populate header
+	header.msg	= msg;
+	header.length	= length;
+
+	// Calculate checksum
+	calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum);  // skip 2 sync bytes
+	if (payload != nullptr)
+		calc_checksum(payload, length, &checksum);
+
+	// Send message
+	write(_fd, (const void *)&header, sizeof(header));
+	if (payload != nullptr)
+		write(_fd, (const void *)payload, length);
+	write(_fd, (const void *)&checksum, sizeof(checksum));
 }
 
-void
-UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
+uint32_t
+UBX::fnv1_32_str(uint8_t *str, uint32_t hval)
 {
-	ssize_t ret = 0;
-
-	/* calculate the checksum now */
-	add_checksum_to_message(packet, length);
-
-	const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
+    uint8_t *s = str;
+
+    /*
+     * FNV-1 hash each octet in the buffer
+     */
+    while (*s) {
+
+	/* multiply by the 32 bit FNV magic prime mod 2^32 */
+#if defined(NO_FNV_GCC_OPTIMIZATION)
+	hval *= FNV1_32_PRIME;
+#else
+	hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24);
+#endif
 
-	/* start with the two sync bytes */
-	ret += write(fd, sync_bytes, sizeof(sync_bytes));
-	ret += write(fd, packet, length);
+	/* xor the bottom with the current octet */
+	hval ^= (uint32_t)*s++;
+    }
 
-	if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning?
-		warnx("ubx: configuration write fail");
+    /* return our new hash value */
+    return hval;
 }
 
-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 ACK check */
-	_message_class_needed = msg_class;
-	_message_id_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
index 43d688893..219a5762a 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013, 2014 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
@@ -34,13 +34,16 @@
 /**
  * @file ubx.h
  *
- * U-Blox protocol definition. Following u-blox 6/7 Receiver Description
+ * U-Blox protocol definition. Following u-blox 6/7/8 Receiver Description
  * including Prototol Specification.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  *
+ * @author Hannes Delago
+ *   (rework, add ubx7+ compatibility)
+ *
  */
 
 #ifndef UBX_H_
@@ -51,319 +54,433 @@
 #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
-#define UBX_CLASS_MON 0x0A
-
-/* MessageIDs (the ones that are used) */
-#define UBX_MESSAGE_NAV_POSLLH 0x02
-//#define UBX_MESSAGE_NAV_DOP 0x04
-#define UBX_MESSAGE_NAV_SOL 0x06
-#define UBX_MESSAGE_NAV_VELNED 0x12
-//#define UBX_MESSAGE_RXM_SVSI 0x20
-#define UBX_MESSAGE_NAV_TIMEUTC 0x21
-#define UBX_MESSAGE_NAV_SVINFO 0x30
-#define UBX_MESSAGE_ACK_NAK 0x00
-#define UBX_MESSAGE_ACK_ACK 0x01
-#define UBX_MESSAGE_CFG_PRT 0x00
-#define UBX_MESSAGE_CFG_MSG 0x01
-#define UBX_MESSAGE_CFG_RATE 0x08
-#define UBX_MESSAGE_CFG_NAV5 0x24
-
-#define UBX_MESSAGE_MON_HW	0x09
-
-#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		/**< 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 */
+/* Message Classes */
+#define UBX_CLASS_NAV		0x01
+#define UBX_CLASS_ACK		0x05
+#define UBX_CLASS_CFG		0x06
+#define UBX_CLASS_MON		0x0A
+
+/* Message IDs */
+#define UBX_ID_NAV_POSLLH	0x02
+#define UBX_ID_NAV_SOL		0x06
+#define UBX_ID_NAV_PVT		0x07
+#define UBX_ID_NAV_VELNED	0x12
+#define UBX_ID_NAV_TIMEUTC	0x21
+#define UBX_ID_NAV_SVINFO	0x30
+#define UBX_ID_ACK_NAK		0x00
+#define UBX_ID_ACK_ACK		0x01
+#define UBX_ID_CFG_PRT		0x00
+#define UBX_ID_CFG_MSG		0x01
+#define UBX_ID_CFG_RATE		0x08
+#define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_MON_VER		0x04
+#define UBX_ID_MON_HW		0x09
+
+/* Message Classes & IDs */
+#define UBX_MSG_NAV_POSLLH	((UBX_CLASS_NAV) | UBX_ID_NAV_POSLLH << 8)
+#define UBX_MSG_NAV_SOL		((UBX_CLASS_NAV) | UBX_ID_NAV_SOL << 8)
+#define UBX_MSG_NAV_PVT		((UBX_CLASS_NAV) | UBX_ID_NAV_PVT << 8)
+#define UBX_MSG_NAV_VELNED	((UBX_CLASS_NAV) | UBX_ID_NAV_VELNED << 8)
+#define UBX_MSG_NAV_TIMEUTC	((UBX_CLASS_NAV) | UBX_ID_NAV_TIMEUTC << 8)
+#define UBX_MSG_NAV_SVINFO	((UBX_CLASS_NAV) | UBX_ID_NAV_SVINFO << 8)
+#define UBX_MSG_ACK_NAK		((UBX_CLASS_ACK) | UBX_ID_ACK_NAK << 8)
+#define UBX_MSG_ACK_ACK		((UBX_CLASS_ACK) | UBX_ID_ACK_ACK << 8)
+#define UBX_MSG_CFG_PRT		((UBX_CLASS_CFG) | UBX_ID_CFG_PRT << 8)
+#define UBX_MSG_CFG_MSG		((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
+#define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
+#define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
+#define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
+
+/* RX NAV-PVT message content details */
+/*   Bitfield "valid" masks */
+#define UBX_RX_NAV_PVT_VALID_VALIDDATE		0x01	/**< validDate (Valid UTC Date) */
+#define UBX_RX_NAV_PVT_VALID_VALIDTIME		0x02	/**< validTime (Valid UTC Time) */
+#define UBX_RX_NAV_PVT_VALID_FULLYRESOLVED	0x04	/**< fullyResolved (1 = UTC Time of Day has been fully resolved (no seconds uncertainty)) */
+
+/*   Bitfield "flags" masks */
+#define UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK		0x01	/**< gnssFixOK (A valid fix (i.e within DOP & accuracy masks)) */
+#define UBX_RX_NAV_PVT_FLAGS_DIFFSOLN		0x02	/**< diffSoln (1 if differential corrections were applied) */
+#define UBX_RX_NAV_PVT_FLAGS_PSMSTATE		0x1C	/**< psmState (Power Save Mode state (see Power Management)) */
+#define UBX_RX_NAV_PVT_FLAGS_HEADVEHVALID	0x20	/**< headVehValid (Heading of vehicle is valid) */
+
+/* RX NAV-TIMEUTC message content details */
+/*   Bitfield "valid" masks */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDTOW	0x01	/**< validTOW (1 = Valid Time of Week) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDKWN	0x02	/**< validWKN (1 = Valid Week Number) */
+#define UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC	0x04	/**< validUTC (1 = Valid UTC Time) */
+#define UBX_RX_NAV_TIMEUTC_VALID_UTCSTANDARD	0xF0	/**< utcStandard (0..15 = UTC standard identifier) */
+
+/* TX CFG-PRT message contents */
+#define UBX_TX_CFG_PRT_PORTID		0x01		/**< UART1 */
+#define UBX_TX_CFG_PRT_MODE		0x000008D0	/**< 0b0000100011010000: 8N1 */
+#define UBX_TX_CFG_PRT_BAUDRATE		38400		/**< choose 38400 as GPS baudrate */
+#define UBX_TX_CFG_PRT_INPROTOMASK	0x01		/**< UBX in */
+#define UBX_TX_CFG_PRT_OUTPROTOMASK	0x01		/**< UBX out */
+
+/* TX CFG-RATE message contents */
+#define UBX_TX_CFG_RATE_MEASINTERVAL	200		/**< 200ms for 5Hz */
+#define UBX_TX_CFG_RATE_NAVRATE		1		/**< cannot be changed */
+#define UBX_TX_CFG_RATE_TIMEREF		0		/**< 0: UTC, 1: GPS time */
+
+/* TX CFG-NAV5 message contents */
+#define UBX_TX_CFG_NAV5_MASK		0x0005		/**< Only update dynamic model and fix mode */
+#define UBX_TX_CFG_NAV5_DYNMODEL	7		/**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+#define UBX_TX_CFG_NAV5_FIXMODE		2		/**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
+
+/* TX CFG-MSG message contents */
+#define UBX_TX_CFG_MSG_RATE1_5HZ	0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_TX_CFG_MSG_RATE1_1HZ	0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
+#define UBX_TX_CFG_MSG_RATE1_05HZ	10
+
+
+/*** u-blox protocol binary message and payload definitions ***/
 #pragma pack(push, 1)
 
-struct ubx_header {
-	uint8_t sync1;
-	uint8_t sync2;
-	uint8_t msg_class;
-	uint8_t msg_id;
-	uint16_t length;
-};
-
+/* General: Header */
 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;
+	uint8_t		sync1;
+	uint8_t		sync2;
+	uint16_t	msg;
+	uint16_t	length;
+} ubx_header_t;
 
+/* General: Checksum */
 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;
+	uint8_t		ck_a;
+	uint8_t		ck_b;
+} ubx_checksum_t ;
 
+/* Rx NAV-POSLLH */
 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;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	int32_t		lon;		/**< Longitude [1e-7 deg] */
+	int32_t		lat;		/**< Latitude [1e-7 deg] */
+	int32_t		height;		/**< Height above ellipsoid [mm] */
+	int32_t		hMSL;		/**< Height above mean sea level [mm] */
+	uint32_t	hAcc;  		/**< Horizontal accuracy estimate [mm] */
+	uint32_t	vAcc;  		/**< Vertical accuracy estimate [mm] */
+} ubx_payload_rx_nav_posllh_t;
+
+/* Rx NAV-SOL */
 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;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	int32_t		fTOW;		/**< Fractional part of iTOW (range: +/-500000) [ns] */
+	int16_t		week;		/**< GPS week */
+	uint8_t		gpsFix;		/**< GPSfix type: 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;		/**< Number of SVs used in Nav Solution */
+	uint32_t	reserved2;
+} ubx_payload_rx_nav_sol_t;
+
+/* Rx NAV-PVT (ubx8) */
 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;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint16_t	year; 		/**< Year (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..60 (UTC) */
+	uint8_t		valid; 		/**< Validity flags (see UBX_RX_NAV_PVT_VALID_...) */
+	uint32_t	tAcc; 		/**< Time accuracy estimate (UTC) [ns] */
+	int32_t		nano;		/**< Fraction of second (UTC) [-1e9...1e9 ns] */
+	uint8_t		fixType;	/**< GNSSfix type: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GNSS + dead reckoning, 5 = time only fix */
+	uint8_t		flags;		/**< Fix Status Flags (see UBX_RX_NAV_PVT_FLAGS_...) */
+	uint8_t		reserved1;
+	uint8_t		numSV;		/**< Number of SVs used in Nav Solution */
+	int32_t		lon;		/**< Longitude [1e-7 deg] */
+	int32_t		lat;		/**< Latitude [1e-7 deg] */
+	int32_t		height;		/**< Height above ellipsoid [mm] */
+	int32_t		hMSL;		/**< Height above mean sea level [mm] */
+	uint32_t	hAcc;  		/**< Horizontal accuracy estimate [mm] */
+	uint32_t	vAcc;  		/**< Vertical accuracy estimate [mm] */
+	int32_t		velN;		/**< NED north velocity [mm/s]*/
+	int32_t		velE;		/**< NED east velocity [mm/s]*/
+	int32_t		velD;		/**< NED down velocity [mm/s]*/
+	int32_t		gSpeed;		/**< Ground Speed (2-D) [mm/s] */
+	int32_t		headMot;	/**< Heading of motion (2-D) [1e-5 deg] */
+	uint32_t	sAcc;		/**< Speed accuracy estimate [mm/s] */
+	uint32_t	headAcc;	/**< Heading accuracy estimate (motion and vehicle) [1e-5 deg] */
+	uint16_t	pDOP;		/**< Position DOP [0.01] */
+	uint16_t	reserved2;
+	uint32_t	reserved3;
+	int32_t		headVeh;	/**< (ubx8+ only) Heading of vehicle (2-D) [1e-5 deg] */
+	uint32_t	reserved4;	/**< (ubx8+ only) */
+} ubx_payload_rx_nav_pvt_t;
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7	(sizeof(ubx_payload_rx_nav_pvt_t) - 8)
+#define UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8	(sizeof(ubx_payload_rx_nav_pvt_t))
+
+/* Rx NAV-TIMEUTC */
 typedef struct {
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_svinfo_part3_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint32_t	tAcc; 		/**< Time accuracy estimate (UTC) [ns] */
+	int32_t		nano;		/**< Fraction of second, range -1e9 .. 1e9 (UTC) [ns] */
+	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..60 (UTC) */
+	uint8_t		valid; 		/**< Validity Flags (see UBX_RX_NAV_TIMEUTC_VALID_...) */
+} ubx_payload_rx_nav_timeutc_t;
+
+/* Rx NAV-SVINFO Part 1 */
 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;
-
-struct gps_bin_mon_hw_packet {
-	uint32_t pinSel;
-	uint32_t pinBank;
-	uint32_t pinDir;
-	uint32_t pinVal;
-	uint16_t noisePerMS;
-	uint16_t agcCnt;
-	uint8_t aStatus;
-	uint8_t aPower;
-	uint8_t flags;
-	uint8_t __reserved1;
-	uint32_t usedMask;
-	uint8_t VP[25];
-	uint8_t jamInd;
-	uint16_t __reserved3;
-	uint32_t pinIrq;
-	uint32_t pulLH;
-	uint32_t pullL;
-};
-
-
-//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;
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	uint8_t		numCh; 		/**< Number of channels */
+	uint8_t		globalFlags;
+	uint16_t	reserved2;
+} ubx_payload_rx_nav_svinfo_part1_t;
 
+/* Rx NAV-SVINFO Part 2 (repeated) */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_ack_ack_packet_t;
-
+	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 [deg] */
+	int16_t		azim; 		/**< Azimuth [deg] */
+	int32_t		prRes; 		/**< Pseudo range residual [cm] */
+} ubx_payload_rx_nav_svinfo_part2_t;
+
+/* Rx NAV-VELNED */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_ack_nak_packet_t;
-
+	uint32_t	iTOW;		/**< GPS Time of Week [ms] */
+	int32_t		velN;		/**< North velocity component [cm/s]*/
+	int32_t		velE;		/**< East velocity component [cm/s]*/
+	int32_t		velD;		/**< Down velocity component [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 [1e-5 deg] */
+	uint32_t	sAcc;		/**< Speed accuracy estimate [cm/s] */
+	uint32_t	cAcc;		/**< Course / Heading accuracy estimate [1e-5 deg] */
+} ubx_payload_rx_nav_velned_t;
+
+/* Rx MON-HW (ubx6) */
 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;
-
+	uint32_t	pinSel;
+	uint32_t	pinBank;
+	uint32_t	pinDir;
+	uint32_t	pinVal;
+	uint16_t	noisePerMS;
+	uint16_t	agcCnt;
+	uint8_t		aStatus;
+	uint8_t		aPower;
+	uint8_t		flags;
+	uint8_t		reserved1;
+	uint32_t	usedMask;
+	uint8_t		VP[25];
+	uint8_t		jamInd;
+	uint16_t	reserved3;
+	uint32_t	pinIrq;
+	uint32_t	pullH;
+	uint32_t	pullL;
+} ubx_payload_rx_mon_hw_ubx6_t;
+
+/* Rx MON-HW (ubx7+) */
+typedef struct {
+	uint32_t	pinSel;
+	uint32_t	pinBank;
+	uint32_t	pinDir;
+	uint32_t	pinVal;
+	uint16_t	noisePerMS;
+	uint16_t	agcCnt;
+	uint8_t		aStatus;
+	uint8_t		aPower;
+	uint8_t		flags;
+	uint8_t		reserved1;
+	uint32_t	usedMask;
+	uint8_t		VP[17];
+	uint8_t		jamInd;
+	uint16_t	reserved3;
+	uint32_t	pinIrq;
+	uint32_t	pullH;
+	uint32_t	pullL;
+} ubx_payload_rx_mon_hw_ubx7_t;
+
+/* Rx MON-VER Part 1 */
 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;
+	uint8_t		swVersion[30];
+	uint8_t		hwVersion[10];
+} ubx_payload_rx_mon_ver_part1_t;
 
+/* Rx MON-VER Part 2 (repeated) */
 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;
+	uint8_t		extension[30];
+} ubx_payload_rx_mon_ver_part2_t;
+
+/* Rx ACK-ACK */
+typedef	union {
+	uint16_t	msg;
+	struct {
+		uint8_t	clsID;
+		uint8_t	msgID;
+	};
+} ubx_payload_rx_ack_ack_t;
+
+/* Rx ACK-NAK */
+typedef	union {
+	uint16_t	msg;
+	struct {
+		uint8_t	clsID;
+		uint8_t	msgID;
+	};
+} ubx_payload_rx_ack_nak_t;
+
+/* Tx CFG-PRT */
+typedef struct {
+	uint8_t		portID;
+	uint8_t		reserved0;
+	uint16_t	txReady;
+	uint32_t	mode;
+	uint32_t	baudRate;
+	uint16_t	inProtoMask;
+	uint16_t	outProtoMask;
+	uint16_t	flags;
+	uint16_t	reserved5;
+} ubx_payload_tx_cfg_prt_t;
+
+/* Tx CFG-RATE */
+typedef struct {
+	uint16_t	measRate;	/**< Measurement Rate, GPS measurements are taken every measRate milliseconds */
+	uint16_t	navRate;	/**< Navigation Rate, in number of measurement cycles. This parameter cannot be changed, and must be set to 1 */
+	uint16_t	timeRef;	/**< Alignment to reference time: 0 = UTC time, 1 = GPS time */
+} ubx_payload_tx_cfg_rate_t;
 
+/* Tx CFG-NAV5 */
+typedef struct {
+	uint16_t	mask;
+	uint8_t		dynModel;	/**< Dynamic Platform model: 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
+	uint8_t		fixMode;	/**< Position Fixing Mode: 1 2D only, 2 3D only, 3 Auto 2D/3D */
+	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;
+	uint8_t		cnoThreshNumSVs;	/**< (ubx7+ only, else 0) */
+	uint8_t		cnoThresh;		/**< (ubx7+ only, else 0) */
+	uint16_t	reserved;
+	uint16_t	staticHoldMaxDist;	/**< (ubx8+ only, else 0) */
+	uint8_t		utcStandard;		/**< (ubx8+ only, else 0) */
+	uint8_t		reserved3;
+	uint32_t	reserved4;
+} ubx_payload_tx_cfg_nav5_t;
+
+/* Tx CFG-MSG */
 typedef struct {
-	uint8_t clsID;
-	uint8_t msgID;
-	uint16_t length;
-	uint8_t msgClass_payload;
-	uint8_t msgID_payload;
+	union {
+		uint16_t	msg;
+		struct {
+			uint8_t	msgClass;
+			uint8_t	msgID;
+		};
+	};
 	uint8_t rate;
-	uint8_t ck_a;
-	uint8_t ck_b;
-} type_gps_bin_cfg_msg_packet_t;
+} ubx_payload_tx_cfg_msg_t;
+
+/* General message and payload buffer union */
+typedef union {
+	ubx_payload_rx_nav_pvt_t		payload_rx_nav_pvt;
+	ubx_payload_rx_nav_posllh_t		payload_rx_nav_posllh;
+	ubx_payload_rx_nav_sol_t		payload_rx_nav_sol;
+	ubx_payload_rx_nav_timeutc_t		payload_rx_nav_timeutc;
+	ubx_payload_rx_nav_svinfo_part1_t	payload_rx_nav_svinfo_part1;
+	ubx_payload_rx_nav_svinfo_part2_t	payload_rx_nav_svinfo_part2;
+	ubx_payload_rx_nav_velned_t		payload_rx_nav_velned;
+	ubx_payload_rx_mon_hw_ubx6_t		payload_rx_mon_hw_ubx6;
+	ubx_payload_rx_mon_hw_ubx7_t		payload_rx_mon_hw_ubx7;
+	ubx_payload_rx_mon_ver_part1_t		payload_rx_mon_ver_part1;
+	ubx_payload_rx_mon_ver_part2_t		payload_rx_mon_ver_part2;
+	ubx_payload_rx_ack_ack_t		payload_rx_ack_ack;
+	ubx_payload_rx_ack_nak_t		payload_rx_ack_nak;
+	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
+	ubx_payload_tx_cfg_rate_t		payload_tx_cfg_rate;
+	ubx_payload_tx_cfg_nav5_t		payload_tx_cfg_nav5;
+	ubx_payload_tx_cfg_msg_t		payload_tx_cfg_msg;
+	uint8_t					raw[];
+} ubx_buf_t;
 
-struct ubx_cfg_msg_rate {
-	uint8_t msg_class;
-	uint8_t msg_id;
-	uint8_t rate;
-};
-
-
-// END the structures of the binary packets
-// ************
+#pragma pack(pop)
+/*** END OF u-blox protocol binary message and payload definitions ***/
 
+/* Decoder state */
 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_SYNC1 = 0,
+	UBX_DECODE_SYNC2,
+	UBX_DECODE_CLASS,
+	UBX_DECODE_ID,
+	UBX_DECODE_LENGTH1,
+	UBX_DECODE_LENGTH2,
+	UBX_DECODE_PAYLOAD,
+	UBX_DECODE_CHKSUM1,
+	UBX_DECODE_CHKSUM2
 } ubx_decode_state_t;
 
-//typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
-#pragma pack(pop)
+/* Rx message state */
+typedef enum {
+	UBX_RXMSG_IGNORE = 0,
+	UBX_RXMSG_HANDLE,
+	UBX_RXMSG_DISABLE,
+	UBX_RXMSG_ERROR_LENGTH
+} ubx_rxmsg_state_t;
+
+/* ACK state */
+typedef enum {
+	UBX_ACK_IDLE = 0,
+	UBX_ACK_WAITING,
+	UBX_ACK_GOT_ACK,
+	UBX_ACK_GOT_NAK
+} ubx_ack_state_t;
 
-#define RECV_BUFFER_SIZE 300 //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(const int &fd, struct vehicle_gps_position_s *gps_position, struct satellite_info_s *satellite_info);
 	~UBX();
-	int			receive(unsigned timeout);
+	int			receive(const unsigned timeout);
 	int			configure(unsigned &baudrate);
 
 private:
 
 	/**
-	 * Parse the binary MTK packet
+	 * Parse the binary UBX packet
+	 */
+	int			parse_char(const uint8_t b);
+
+	/**
+	 * Start payload rx
 	 */
-	int			parse_char(uint8_t b);
+	int			payload_rx_init(void);
 
 	/**
-	 * Handle the package once it has arrived
+	 * Add payload rx byte
 	 */
-	int			handle_message(void);
+	int			payload_rx_add(const uint8_t b);
+	int			payload_rx_add_nav_svinfo(const uint8_t b);
+	int			payload_rx_add_mon_ver(const uint8_t b);
+
+	/**
+	 * Finish payload rx
+	 */
+	int			payload_rx_done(void);
 
 	/**
 	 * Reset the parse state machine for a fresh start
@@ -373,44 +490,53 @@ private:
 	/**
 	 * While parsing add every byte (except the sync bytes) to the checksum
 	 */
-	void			add_byte_to_checksum(uint8_t);
+	void			add_byte_to_checksum(const uint8_t);
 
 	/**
-	 * Add the two checksum bytes to an outgoing message
+	 * Send a message
 	 */
-	void			add_checksum_to_message(uint8_t *message, const unsigned length);
+	void			send_message(const uint16_t msg, const uint8_t *payload, const uint16_t length);
 
 	/**
-	 * Helper to send a config packet
+	 * Configure message rate
 	 */
-	void			send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
+	void			configure_message_rate(const uint16_t msg, const uint8_t rate);
 
-	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);
+	/**
+	 * Calculate & add checksum for given buffer
+	 */
+	void			calc_checksum(const uint8_t *buffer, const uint16_t length, ubx_checksum_t *checksum);
 
-	void			add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b);
+	/**
+	 * Wait for message acknowledge
+	 */
+	int			wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report);
 
-	int			wait_for_ack(unsigned timeout);
+	/**
+	 * Calculate FNV1 hash
+	 */
+	uint32_t		fnv1_32_str(uint8_t *str, uint32_t hval);
 
 	int			_fd;
 	struct vehicle_gps_position_s *_gps_position;
+	struct satellite_info_s *_satellite_info;
+	bool			_enable_sat_info;
 	bool			_configured;
-	bool			_waiting_for_ack;
+	ubx_ack_state_t		_ack_state;
 	bool			_got_posllh;
 	bool			_got_velned;
-	bool			_got_timeutc;
-	uint8_t			_message_class_needed;
-	uint8_t			_message_id_needed;
 	ubx_decode_state_t	_decode_state;
-	uint8_t			_rx_buffer[RECV_BUFFER_SIZE];
-	unsigned		_rx_count;
+	uint16_t		_rx_msg;
+	ubx_rxmsg_state_t	_rx_state;
+	uint16_t		_rx_payload_length;
+	uint16_t		_rx_payload_index;
 	uint8_t			_rx_ck_a;
 	uint8_t			_rx_ck_b;
-	uint8_t			_message_class;
-	uint8_t			_message_id;
-	unsigned		_payload_size;
 	hrt_abstime		_disable_cmd_last;
+	uint16_t		_ack_waiting_msg;
+	ubx_buf_t		_buf;
+	uint32_t		_ubx_version;
+	bool			_use_nav_pvt;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 1e779e8dc..086132573 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -226,7 +226,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	msg.sensor_id = GPS_SENSOR_ID;
 	msg.sensor_text_id = GPS_SENSOR_TEXT_ID;
 
-	msg.gps_num_sat = gps.satellites_visible;
+	msg.gps_num_sat = gps.satellites_used;
 
 	/* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */
 	msg.gps_fix_char = (uint8_t)(gps.fix_type + 48);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 992ab9623..24da4c68b 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -72,6 +72,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/param/param.h>
+#include <systemlib/circuit_breaker.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -1012,6 +1013,19 @@ PX4IO::task_main()
 					}
 				}
 
+				int32_t safety_param_val;
+				param_t safety_param = param_find("RC_FAILS_THR");
+
+				if (safety_param != PARAM_INVALID) {
+
+					param_get(safety_param, &safety_param_val);
+
+					if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) {
+						/* disable IO safety if circuit breaker asked for it */
+						(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val);
+					}
+				}
+
 			}
 
 		}
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 3730b1920..6386e37a0 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -310,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
-			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (_THRmaxf - nomThr);
 
 		} else {
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 24da452b1..be465ab76 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -131,6 +131,7 @@
 #include <fcntl.h>
 #include <sys/prctl.h>
 #include <math.h>
+#include <float.h>
 #include <mathlib/mathlib.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
@@ -526,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
 		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
 		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
 
-	if (det == 0.0f) {
+	if (fabsf(det) < FLT_EPSILON) {
 		return ERROR;        // Singular matrix
 	}
 
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 9d79124e7..43f341ae7 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -40,6 +40,7 @@
  */
 
 #include <math.h>
+#include <float.h>
 
 #include "calibration_routines.h"
 
@@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
 		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
 		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
-		aA = (aA == 0.0f) ? 1.0f : aA;
-		aB = (aB == 0.0f) ? 1.0f : aB;
-		aC = (aC == 0.0f) ? 1.0f : aC;
+		aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA;
+		aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB;
+		aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC;
 
 		//Compute next iteration
 		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bb42889ea..efa26eb97 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -52,6 +52,7 @@
 #include <fcntl.h>
 #include <errno.h>
 #include <systemlib/err.h>
+#include <systemlib/circuit_breaker.h>
 #include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
@@ -76,6 +77,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/telemetry_status.h>
 
@@ -371,16 +373,16 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
+transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy)
 {
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
-	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
-		mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
 
 	} else if (arming_res == TRANSITION_DENIED) {
 		tune_negative(true);
@@ -507,7 +509,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
-				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+				mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f",
+					(double)cmd->param1,
+					(double)cmd->param2,
+					(double)cmd->param3,
+					(double)cmd->param4,
+					(double)cmd->param5,
+					(double)cmd->param6,
+					(double)cmd->param7);
 			}
 		}
 		break;
@@ -700,6 +709,12 @@ int commander_thread_main(int argc, char *argv[])
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
 
+	status.condition_power_input_valid = true;
+	status.avionics_power_rail_voltage = -1.0f;
+
+	// CIRCUIT BREAKERS
+	status.circuit_breaker_engaged_power_check = false;
+
 	/* publish initial state */
 	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
 
@@ -752,7 +767,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	hrt_abstime last_idle_time = 0;
 	hrt_abstime start_time = 0;
-	hrt_abstime last_auto_state_valid = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
@@ -846,6 +860,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct position_setpoint_triplet_s pos_sp_triplet;
 	memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
 
+	/* Subscribe to system power */
+	int system_power_sub = orb_subscribe(ORB_ID(system_power));
+	struct system_power_s system_power;
+	memset(&system_power, 0, sizeof(system_power));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -862,6 +881,7 @@ int commander_thread_main(int argc, char *argv[])
 	bool arming_state_changed = false;
 	bool main_state_changed = false;
 	bool failsafe_old = false;
+	bool system_checked = false;
 
 	while (!thread_should_exit) {
 
@@ -903,6 +923,9 @@ int commander_thread_main(int argc, char *argv[])
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
 				param_get(_param_component_id, &(status.component_id));
+
+				status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY);
+
 				status_changed = true;
 
 				/* re-check RC calibration */
@@ -915,6 +938,15 @@ int commander_thread_main(int argc, char *argv[])
 			param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
 		}
 
+		/* Perform system checks (again) once params are loaded and MAVLink is up. */
+		if (!system_checked && mavlink_fd && 
+			(telemetry.heartbeat_time > 0) &&
+			(hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) {
+
+			(void)rc_calibration_check(mavlink_fd);
+			system_checked = true;
+		}
+
 		orb_check(sp_man_sub, &updated);
 
 		if (updated) {
@@ -945,6 +977,26 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
 		}
 
+		orb_check(system_power_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
+
+			if (hrt_elapsed_time(&system_power.timestamp) < 200000) {
+				if (system_power.servo_valid &&
+					!system_power.brick_valid &&
+					!system_power.usb_connected) {
+					/* flying only on servo rail, this is unsafe */
+					status.condition_power_input_valid = false;
+				} else {
+					status.condition_power_input_valid = true;
+				}
+
+				/* copy avionics voltage */
+				status.avionics_power_rail_voltage = system_power.voltage5V_v;
+			}
+		}
+
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
 		/* update safety topic */
@@ -1254,12 +1306,13 @@ int commander_thread_main(int argc, char *argv[])
 			if (status.arming_state == ARMING_STATE_STANDBY &&
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
-						print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
 
-					} else if (status.main_state != MAIN_STATE_MANUAL) {
+					/* we check outside of the transition function here because the requirement
+					 * for being in manual mode only applies to manual arming actions.
+					 * the system can be armed in auto if armed via the GCS.
+					 */
+					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
-
 					} else {
 						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
 						if (arming_ret == TRANSITION_CHANGED) {
@@ -1289,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[])
 			} else if (arming_ret == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
+				tune_negative(true);
 			}
 
 			/* evaluate the main state machine according to mode switches */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 940a04aa1..80e6861f6 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -209,12 +209,18 @@ int led_init()
 	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
 	(void)ioctl(leds, LED_ON, LED_BLUE);
 
+	/* switch blue off */
+	led_off(LED_BLUE);
+
 	/* we consider the amber led mandatory */
 	if (ioctl(leds, LED_ON, LED_AMBER)) {
 		warnx("Amber LED: ioctl fail\n");
 		return ERROR;
 	}
 
+	/* switch amber off */
+	led_off(LED_AMBER);
+
 	/* then try RGB LEDs, this can fail on FMUv1*/
 	rgbleds = open(RGBLED_DEVICE_PATH, 0);
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 74885abf6..423ce2f23 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 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
@@ -46,20 +46,32 @@
 #include <dirent.h>
 #include <fcntl.h>
 #include <string.h>
+#include <math.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/differential_pressure.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_airspeed.h>
 #include <drivers/drv_device.h>
 #include <mavlink/mavlink_log.h>
 
 #include "state_machine_helper.h"
 #include "commander_helper.h"
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
+
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
@@ -98,18 +110,31 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_INIT == 0);
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
-	/*
-	 * Perform an atomic state update
-	 */
-	irqstate_t flags = irqsave();
-
 	transition_result_t ret = TRANSITION_DENIED;
 
+	arming_state_t current_arming_state = status->arming_state;
+
 	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == status->arming_state) {
+	if (new_arming_state == current_arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
 
 	} else {
+
+		/*
+		 * Get sensing state if necessary
+		 */
+		int prearm_ret = OK;
+
+		/* only perform the check if we have to */
+		if (new_arming_state == ARMING_STATE_ARMED) {
+			prearm_ret = prearm_check(status, mavlink_fd);
+		}
+
+		/*
+		 * Perform an atomic state update
+		 */
+		irqstate_t flags = irqsave();
+
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
@@ -124,15 +149,44 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		if (valid_transition) {
 			// We have a good transition. Now perform any secondary validation.
 			if (new_arming_state == ARMING_STATE_ARMED) {
-				// Fail transition if we need safety switch press
-				//      Allow if coming from in air restore
+
+				//      Do not perform pre-arm checks if coming from in air restore
 				//      Allow if HIL_STATE_ON
-				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
-					if (mavlink_fd) {
-						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
+				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE &&
+					status->hil_state == HIL_STATE_OFF) {
+
+					// Fail transition if pre-arm check fails
+					if (prearm_ret) {
+						valid_transition = false;
+
+					// Fail transition if we need safety switch press
+					} else if (safety->safety_switch_available && !safety->safety_off) {
+
+						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!");
+
+						valid_transition = false;
+					}
+
+					// Perform power checks only if circuit breaker is not
+					// engaged for these checks
+					if (!status->circuit_breaker_engaged_power_check) {
+						// Fail transition if power is not good
+						if (!status->condition_power_input_valid) {
+
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module.");
+							valid_transition = false;
+						}
+
+						// Fail transition if power levels on the avionics rail
+						// are measured but are insufficient
+						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) &&
+							(status->avionics_power_rail_voltage < 4.9f)) {
+
+							mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							valid_transition = false;
+						}
 					}
 
-					valid_transition = false;
 				}
 
 			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
@@ -157,17 +211,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 			ret = TRANSITION_CHANGED;
 			status->arming_state = new_arming_state;
 		}
-	}
 
-	/* end of atomic state update */
-	irqrestore(flags);
+		/* end of atomic state update */
+		irqrestore(flags);
+	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "Invalid arming transition from %s to %s";
+		static const char *errMsg = "INVAL: %s - %s";
 
-		if (mavlink_fd) {
-			mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-		}
+		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
 
 		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
 	}
@@ -539,3 +591,80 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	return status->nav_state != nav_state_old;
 }
 
+int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
+{
+	int ret;
+
+	int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
+		ret = fd;
+		goto system_eval;
+	}
+
+	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
+
+	if (ret != OK) {
+		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
+		goto system_eval;
+	}
+
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
+
+		if (accel_scale < 9.78f || accel_scale > 9.83f) {
+			mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
+		}
+
+		if (accel_scale > 30.0f /* m/s^2 */) {
+			mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
+			/* this is frickin' fatal */
+			ret = ERROR;
+			goto system_eval;
+		} else {
+			ret = OK;
+		}
+	} else {
+		mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		ret = ERROR;
+		goto system_eval;
+	}
+
+	if (!status->is_rotary_wing) {
+		fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
+
+		if (fd < 0) {
+			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
+			ret = fd;
+			goto system_eval;
+		}
+
+		struct differential_pressure_s diff_pres;
+
+		ret = read(fd, &diff_pres, sizeof(diff_pres));
+
+		if (ret == sizeof(diff_pres)) {
+			if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) {
+				mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
+				// XXX do not make this fatal yet
+				ret = OK;
+			}
+		} else {
+			mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ");
+			/* this is frickin' fatal */
+			ret = ERROR;
+			goto system_eval;
+		}
+	}
+
+system_eval:
+	close(fd);
+	return ret;
+}
diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index 9622f7e40..deaaf55fe 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -2503,12 +2503,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 
     // Calculate initial Tbn matrix and rotate Mag measurements into NED
     // to set initial NED magnetic field states
-    Mat3f DCM;
-    quat2Tbn(DCM, initQuat);
+    quat2Tbn(Tbn, initQuat);
+    Tnb = Tbn.transpose();
     Vector3f initMagNED;
-    initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z;
-    initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z;
-    initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z;
+    initMagNED.x = Tbn.x.x*initMagXYZ.x + Tbn.x.y*initMagXYZ.y + Tbn.x.z*initMagXYZ.z;
+    initMagNED.y = Tbn.y.x*initMagXYZ.x + Tbn.y.y*initMagXYZ.y + Tbn.y.z*initMagXYZ.z;
+    initMagNED.z = Tbn.z.x*initMagXYZ.x + Tbn.z.y*initMagXYZ.y + Tbn.z.z*initMagXYZ.z;
 
     magstate.q0 = initQuat[0];
     magstate.q1 = initQuat[1];
@@ -2521,7 +2521,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
     magstate.magYbias = magBias.y;
     magstate.magZbias = magBias.z;
     magstate.R_MAG = sq(magMeasurementSigma);
-    magstate.DCM = DCM;
+    magstate.DCM = Tbn;
 
     // write to state vector
     for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 43acee96f..026a4d6c9 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -474,7 +474,7 @@ Mavlink::get_instance_id()
 	return _instance_id;
 }
 
-const mavlink_channel_t
+mavlink_channel_t
 Mavlink::get_channel()
 {
 	return _channel;
@@ -2109,7 +2109,7 @@ Mavlink::task_main(int argc, char *argv[])
                 write_ptr = (uint8_t*)&msg;
 
                 // Pull a single message from the buffer
-                int read_count = available;
+                size_t read_count = available;
                 if (read_count > sizeof(mavlink_message_t)) {
                     read_count = sizeof(mavlink_message_t);
                 }
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 5f4117ae5..f94036a17 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -213,15 +213,15 @@ public:
 	 */
 	int			enable_flow_control(bool enabled);
 
-	const mavlink_channel_t	get_channel();
+	mavlink_channel_t	get_channel();
 
-	void configure_stream_threadsafe(const char *stream_name, const float rate);
+	void configure_stream_threadsafe(const char *stream_name, float rate);
 
 	bool			_task_should_exit;	/**< if true, mavlink task should exit */
 
 	int			get_mavlink_fd() { return _mavlink_fd; }
 
-	MavlinkStream * get_streams() { return _streams; } const
+	MavlinkStream * get_streams() const { return _streams; }
 
 
 	/* Functions for waiting to start transmission until message received. */
@@ -311,15 +311,15 @@ private:
 
 	pthread_mutex_t		_message_buffer_mutex;
 
-	perf_counter_t		_loop_perf;			/**< loop performance counter */
-	perf_counter_t		_txerr_perf;			/**< TX error counter */
-
 	bool			_param_initialized;
 	param_t			_param_system_id;
 	param_t			_param_component_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
 
+	perf_counter_t		_loop_perf;			/**< loop performance counter */
+	perf_counter_t		_txerr_perf;			/**< TX error counter */
+
 	/**
 	 * Send one parameter.
 	 *
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index e1ebc16cc..667be87b7 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -45,7 +45,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -678,7 +677,7 @@ protected:
 						     cm_uint16_from_m_float(gps.epv),
 						     gps.vel_m_s * 100.0f,
 						     _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
-						     gps.satellites_visible);
+						     gps.satellites_used);
 		}
 	}
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bb977d277..6d361052c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -741,7 +741,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 
 	hil_gps.timestamp_variance = timestamp;
 	hil_gps.s_variance_m_s = 5.0f;
-	hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph;
 
 	hil_gps.timestamp_velocity = timestamp;
 	hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
@@ -751,9 +750,8 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	hil_gps.vel_ned_valid = true;
 	hil_gps.cog_rad = _wrap_pi(gps.cog * M_DEG_TO_RAD_F * 1e-2f);
 
-	hil_gps.timestamp_satellites = timestamp;
 	hil_gps.fix_type = gps.fix_type;
-	hil_gps.satellites_visible = gps.satellites_visible;
+	hil_gps.satellites_used = gps.satellites_visible;  //TODO: rename mavlink_hil_gps_t sats visible to used?
 
 	if (_gps_pub < 0) {
 		_gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index ed6776e5c..7279206db 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -44,10 +44,10 @@
 #include "mavlink_main.h"
 
 MavlinkStream::MavlinkStream() :
-	_last_sent(0),
+	next(nullptr),
 	_channel(MAVLINK_COMM_0),
 	_interval(1000000),
-	next(nullptr)
+	_last_sent(0)
 {
 }
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 20e016da3..19c10198c 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -72,6 +72,7 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 
@@ -123,6 +124,8 @@ private:
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
+
 	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
 	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
@@ -267,6 +270,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
 
+	_actuators_0_circuit_breaker_enabled(false),
+
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
@@ -402,6 +407,8 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_params_handles.acro_yaw_max, &v);
 	_params.acro_rate_max(2) = math::radians(v);
 
+	_actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
+
 	return OK;
 }
 
@@ -840,11 +847,13 @@ MulticopterAttitudeControl::task_main()
 				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 				_actuators.timestamp = hrt_absolute_time();
 
-				if (_actuators_0_pub > 0) {
-					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
+				if (!_actuators_0_circuit_breaker_enabled) {
+					if (_actuators_0_pub > 0) {
+						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
 
-				} else {
-					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+					} else {
+						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+					}
 				}
 			}
 		}
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index c24c172af..bcc3e2ae7 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -466,7 +466,7 @@ MulticopterPositionControl::update_ref()
 {
 	if (_local_pos.ref_timestamp != _ref_timestamp) {
 		double lat_sp, lon_sp;
-		float alt_sp;
+		float alt_sp = 0.0f;
 
 		if (_ref_timestamp != 0) {
 			/* calculate current position setpoint in global frame */
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index cbf7ac987..a1c6f09d0 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -42,6 +42,8 @@
 #include <string.h>
 #include <stdlib.h>
 #include <unistd.h>
+#include <math.h>
+#include <float.h>
 
 #include <systemlib/err.h>
 #include <geo/geo.h>
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 6d7afcf4b..142a73409 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -222,7 +222,7 @@ RTL::set_rtl_item()
 		_navigator->set_can_loiter_at_sp(true);
 
 		if (autoland) {
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside);
 
 		} else {
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 9870ebd05..05eae047c 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -49,6 +49,7 @@
 #include <sys/prctl.h>
 #include <termios.h>
 #include <math.h>
+#include <float.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_controls.h>
@@ -477,7 +478,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f;
 				flow_prev = flow.flow_timestamp;
 
-				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) {
+				if ((flow.ground_distance_m > 0.31f) &&
+					(flow.ground_distance_m < 4.0f) &&
+					(att.R[2][2] > 0.7f) &&
+					(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
+
 					sonar_time = t;
 					sonar_prev = flow.ground_distance_m;
 					corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
@@ -646,25 +651,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 						} else if (t > ref_init_start + ref_init_delay) {
 							ref_inited = true;
-							/* update baro offset */
-							baro_offset -= z_est[0];
 
 							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
 							x_est[0] = 0.0f;
 							x_est[1] = gps.vel_n_m_s;
 							y_est[0] = 0.0f;
 							y_est[1] = gps.vel_e_m_s;
-							z_est[0] = 0.0f;
 
 							local_pos.ref_lat = lat;
 							local_pos.ref_lon = lon;
-							local_pos.ref_alt = alt;
+							local_pos.ref_alt = alt + z_est[0];
 							local_pos.ref_timestamp = t;
 
 							/* initialize projection */
 							map_projection_init(&ref, lat, lon);
 							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
-							mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
 						}
 					}
 
@@ -913,6 +915,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				memcpy(x_est_prev, x_est, sizeof(x_est));
 				memcpy(y_est_prev, y_est, sizeof(y_est));
 			}
+		} else {
+			/* gradually reset xy velocity estimates */
+			inertial_filter_correct(-x_est[1], dt, x_est, 1, params.w_xy_res_v);
+			inertial_filter_correct(-y_est[1], dt, y_est, 1, params.w_xy_res_v);
 		}
 
 		/* detect land */
@@ -928,6 +934,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				landed = false;
 				landed_time = 0;
 			}
+			/* reset xy velocity estimates when landed */
+			x_est[1] = 0.0f;
+			y_est[1] = 0.0f;
 
 		} else {
 			if (alt_disp2 < land_disp2 && thrust < params.land_thr) {
@@ -952,11 +961,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				float updates_dt = (t - updates_counter_start) * 0.000001f;
 				warnx(
 					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
-					accel_updates / updates_dt,
-					baro_updates / updates_dt,
-					gps_updates / updates_dt,
-					attitude_updates / updates_dt,
-					flow_updates / updates_dt);
+					(double)(accel_updates / updates_dt),
+					(double)(baro_updates / updates_dt),
+					(double)(gps_updates / updates_dt),
+					(double)(attitude_updates / updates_dt),
+					(double)(flow_updates / updates_dt));
 				updates_counter_start = t;
 				accel_updates = 0;
 				baro_updates = 0;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index d88419c25..0581f8236 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -46,6 +46,7 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
@@ -65,6 +66,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
 	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
+	h->w_xy_res_v = param_find("INAV_W_XY_RES_V");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
 	h->flow_k = param_find("INAV_FLOW_K");
@@ -87,6 +89,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
 	param_get(h->w_xy_flow, &(p->w_xy_flow));
+	param_get(h->w_xy_res_v, &(p->w_xy_res_v));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
 	param_get(h->flow_k, &(p->flow_k));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index df1cfaa71..423f0d879 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -47,6 +47,7 @@ struct position_estimator_inav_params {
 	float w_xy_gps_p;
 	float w_xy_gps_v;
 	float w_xy_flow;
+	float w_xy_res_v;
 	float w_gps_flow;
 	float w_acc_bias;
 	float flow_k;
@@ -66,6 +67,7 @@ struct position_estimator_inav_param_handles {
 	param_t w_xy_gps_p;
 	param_t w_xy_gps_v;
 	param_t w_xy_flow;
+	param_t w_xy_res_v;
 	param_t w_gps_flow;
 	param_t w_acc_bias;
 	param_t flow_k;
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 62e6b12cb..185cb20dd 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -47,8 +47,8 @@
 #include "px4io.h"
 
 #define RC_FAILSAFE_TIMEOUT		2000000		/**< two seconds failsafe timeout */
-#define RC_CHANNEL_HIGH_THRESH		5000
-#define RC_CHANNEL_LOW_THRESH		-5000
+#define RC_CHANNEL_HIGH_THRESH		5000	/* 75% threshold */
+#define RC_CHANNEL_LOW_THRESH		-8000	/* 10% threshold */
 
 static bool	ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len);
 
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 79b6546b3..76762c0fc 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -331,6 +331,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
+#ifdef DEBUG
 static void
 i2c_dump(void)
 {
@@ -339,3 +340,4 @@ i2c_dump(void)
 	debug("CCR   0x%08x  TRISE 0x%08x", rCCR,  rTRISE);
 	debug("SR1   0x%08x  SR2   0x%08x", rSR1,  rSR2);
 }
+#endif
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 0e7dc621c..70ccab180 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -119,13 +119,15 @@ sbus_init(const char *device)
 bool
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	write(sbus_fd, 'A', 1);
+	char a = 'A';
+	write(sbus_fd, &a, 1);
 }
 
 bool
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
-	write(sbus_fd, 'B', 1);
+	char b = 'B';
+	write(sbus_fd, &b, 1);
 }
 
 bool
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index e483b35e8..39e5b6c41 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -74,6 +74,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/satellite_info.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -141,6 +142,8 @@ PARAM_DEFINE_INT32(SDLOG_EXT, -1);
 	fds[fdsc_count].events = POLLIN; \
 	fdsc_count++;
 
+#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
@@ -944,6 +947,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct tecs_status_s tecs_status;
 		struct system_power_s system_power;
 		struct servorail_status_s servorail_status;
+		struct satellite_info_s sat_info;
 		struct wind_estimate_s wind_estimate;
 	} buf;
 
@@ -1007,6 +1011,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int global_pos_sub;
 		int triplet_sub;
 		int gps_pos_sub;
+		int sat_info_sub;
 		int vicon_pos_sub;
 		int flow_sub;
 		int rc_sub;
@@ -1026,6 +1031,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	subs.status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
@@ -1066,11 +1072,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
 
+	/* initialize calculated mean SNR */
+	float snr_mean = 0.0f;
+
 	/* enable logging on start if needed */
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
-			if (copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
+			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
 				gps_time = buf_gps_pos.time_gps_usec;
 			}
 		}
@@ -1128,14 +1137,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- GPS POSITION - UNIT #1 --- */
 		if (gps_pos_updated) {
 
-			float snr_mean = 0.0f;
-
-			for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
-				snr_mean += buf_gps_pos.satellite_snr[i];
-			}
-
-			snr_mean /= buf_gps_pos.satellites_visible;
-
 			log_msg.msg_type = LOG_GPS_MSG;
 			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@@ -1148,44 +1149,55 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
 			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
 			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
-			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
+			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_used;
 			log_msg.body.log_GPS.snr_mean = snr_mean;
 			log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
 			log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
 			LOGBUFFER_WRITE_AND_COUNT(GPS);
+		}
+
+		/* --- SATELLITE INFO - UNIT #1 --- */
+		if (_extended_logging) {
+
+			if (copy_if_updated(ORB_ID(satellite_info), subs.sat_info_sub, &buf.sat_info)) {
 
-			if (_extended_logging) {
 				/* log the SNR of each satellite for a detailed view of signal quality */
-				unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
+				unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
 				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
 
 				log_msg.msg_type = LOG_GS0A_MSG;
 				memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
-				/* fill set A */
-				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+				snr_mean = 0.0f;
 
-					int satindex = buf_gps_pos.satellite_prn[i] - 1;
+				/* fill set A and calculate mean SNR */
+				for (unsigned i = 0; i < sat_info_count; i++) {
+
+					snr_mean += buf.sat_info.snr[i];
+
+					int satindex = buf.sat_info.svid[i] - 1;
 
 					/* handles index exceeding and wraps to to arithmetic errors */
 					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
 						/* map satellites by their ID so that logs from two receivers can be compared */
-						log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+						log_msg.body.log_GS0A.satellite_snr[satindex] = buf.sat_info.snr[i];
 					}
 				}
 				LOGBUFFER_WRITE_AND_COUNT(GS0A);
+				snr_mean /= sat_info_count;
 
 				log_msg.msg_type = LOG_GS0B_MSG;
 				memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+
 				/* fill set B */
-				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+				for (unsigned i = 0; i < sat_info_count; i++) {
 
 					/* get second bank of satellites, thus deduct bank size from index */
-					int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
+					int satindex = buf.sat_info.svid[i] - 1 - log_max_snr;
 
 					/* handles index exceeding and wraps to to arithmetic errors */
 					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
 						/* map satellites by their ID so that logs from two receivers can be compared */
-						log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+						log_msg.body.log_GS0B.satellite_snr[satindex] = buf.sat_info.snr[i];
 					}
 				}
 				LOGBUFFER_WRITE_AND_COUNT(GS0B);
@@ -1394,8 +1406,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		if (copy_if_updated(ORB_ID(rc_channels), subs.rc_sub, &buf.rc)) {
 			log_msg.msg_type = LOG_RC_MSG;
 			/* Copy only the first 8 channels of 14 */
-			memcpy(log_msg.body.log_RC.channel, buf.rc.chan, sizeof(log_msg.body.log_RC.channel));
-			log_msg.body.log_RC.channel_count = buf.rc.chan_count;
+			memcpy(log_msg.body.log_RC.channel, buf.rc.channels, sizeof(log_msg.body.log_RC.channel));
+			log_msg.body.log_RC.channel_count = buf.rc.channel_count;
 			log_msg.body.log_RC.signal_lost = buf.rc.signal_lost;
 			LOGBUFFER_WRITE_AND_COUNT(RC);
 		}
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 8908adf4c..3307354a0 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1223,9 +1223,9 @@ Sensors::parameter_update_poll(bool forced)
 		}
 
 #if 0
-		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor * 10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]);
-		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor * 10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]);
-		printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled * 100), (int)(_rc.chan[1].scaled * 100));
+		printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.channels[0].scaling_factor * 10000), (int)(_rc.channels[0].mid), (int)_rc.function[0]);
+		printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.channels[1].scaling_factor * 10000), (int)(_rc.channels[1].mid), (int)_rc.function[1]);
+		printf("MAN: %d %d\n", (int)(_rc.channels[0] * 100), (int)(_rc.channels[1] * 100));
 		fflush(stdout);
 		usleep(5000);
 #endif
@@ -1315,7 +1315,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					 * a valid voltage from a connected sensor. Also assume a non-
 					 * zero offset from the sensor if its connected.
 					 */
-					if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
+					if (voltage > 0.4f && (_parameters.diff_pres_analog_enabled > 0)) {
 
 						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
 
@@ -1355,7 +1355,7 @@ float
 Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
 {
 	if (_rc.function[func] >= 0) {
-		float value = _rc.chan[_rc.function[func]].scaled;
+		float value = _rc.channels[_rc.function[func]];
 
 		if (value < min_value) {
 			return min_value;
@@ -1376,7 +1376,7 @@ switch_pos_t
 Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
-		float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
+		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
 
 		if (on_inv ? value < on_th : value > on_th) {
 			return SWITCH_POS_ON;
@@ -1397,7 +1397,7 @@ switch_pos_t
 Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
 {
 	if (_rc.function[func] >= 0) {
-		float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
+		float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
 
 		if (on_inv ? value < on_th : value > on_th) {
 			return SWITCH_POS_ON;
@@ -1489,25 +1489,25 @@ Sensors::rc_poll()
 			 * DO NOT REMOVE OR ALTER STEP 1!
 			 */
 			if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
-				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
+				_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
 
 			} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
-				_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
+				_rc.channels[i] = (rc_input.values[i] - _parameters.trim[i] + _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
 
 			} else {
 				/* in the configured dead zone, output zero */
-				_rc.chan[i].scaled = 0.0f;
+				_rc.channels[i] = 0.0f;
 			}
 
-			_rc.chan[i].scaled *= _parameters.rev[i];
+			_rc.channels[i] *= _parameters.rev[i];
 
 			/* handle any parameter-induced blowups */
-			if (!isfinite(_rc.chan[i].scaled)) {
-				_rc.chan[i].scaled = 0.0f;
+			if (!isfinite(_rc.channels[i])) {
+				_rc.channels[i] = 0.0f;
 			}
 		}
 
-		_rc.chan_count = rc_input.channel_count;
+		_rc.channel_count = rc_input.channel_count;
 		_rc.rssi = rc_input.rssi;
 		_rc.signal_lost = signal_lost;
 		_rc.timestamp = rc_input.timestamp_last_signal;
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
new file mode 100644
index 000000000..8f697749e
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker.c
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 circuit_breaker.c
+ *
+ * Circuit breaker parameters.
+ * Analog to real aviation circuit breakers these parameters
+ * allow to disable subsystems. They are not supported as standard
+ * operation procedure and are only provided for development purposes.
+ * To ensure they are not activated accidentally, the associated
+ * parameter needs to set to the key (magic).
+ */
+
+#include <systemlib/param/param.h>
+#include <systemlib/circuit_breaker.h>
+
+/**
+ * Circuit breaker for power supply check
+ *
+ * Setting this parameter to 894281 will disable the power valid
+ * checks in the commander.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 894281
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
+
+/**
+ * Circuit breaker for rate controller output
+ *
+ * Setting this parameter to 140253 will disable the rate
+ * controller uORB publication.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 140253
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
+
+/**
+ * Circuit breaker for IO safety
+ *
+ * Setting this parameter to 894281 will disable IO safety.
+ * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
+ *
+ * @min 0
+ * @max 22027
+ * @group Circuit Breaker
+ */
+PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
+
+bool circuit_breaker_enabled(const char* breaker, int32_t magic)
+{
+	int32_t val;
+	(void)param_get(param_find(breaker), &val);
+
+	return (val == magic);
+}
+
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
new file mode 100644
index 000000000..1175dbce8
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 circuit_breaker.h
+ *
+ * Circuit breaker functionality.
+ */
+
+#ifndef CIRCUIT_BREAKER_H_
+#define CIRCUIT_BREAKER_H_
+
+/* SAFETY WARNING  --  SAFETY WARNING  --  SAFETY WARNING
+ *
+ * OBEY THE DOCUMENTATION FOR ALL CIRCUIT BREAKERS HERE,
+ * ENSURE TO READ CAREFULLY ALL SAFETY WARNINGS.
+ * http://pixhawk.org/dev/circuit_breakers
+ *
+ * CIRCUIT BREAKERS ARE NOT PART OF THE STANDARD OPERATION PROCEDURE
+ * AND MAY DISABLE CHECKS THAT ARE VITAL FOR SAFE FLIGHT.
+ */
+#define CBRK_SUPPLY_CHK_KEY	894281
+#define CBRK_RATE_CTRL_KEY	140253
+#define CBRK_IO_SAFETY_KEY	22027
+
+#include <stdbool.h>
+
+__BEGIN_DECLS
+
+__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
+
+__END_DECLS
+
+#endif /* CIRCUIT_BREAKER_H_ */
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index 6c0e876d1..998b5ac7d 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -86,7 +86,7 @@ warnerr_core(int errcode, const char *fmt, va_list args)
 	fprintf(stderr, "\n");
 #elif CONFIG_ARCH_LOWPUTC
 	lowsyslog("%s: ", getprogname());
-	lowvyslog(fmt, args);
+	lowvsyslog(fmt, args);
 
 	/* convenience as many parts of NuttX use negative errno */
 	if (errcode < 0)
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 3953b757d..147903aa0 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2012-2014 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
@@ -52,5 +52,6 @@ SRCS		 = err.c \
 		   rc_check.c \
 		   otp.c \
 		   board_serial.c \
-		   pwm_limit/pwm_limit.c
+		   pwm_limit/pwm_limit.c \
+		   circuit_breaker.c
 
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index c0c1a5cb4..b35b333af 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -42,6 +42,7 @@
 #include <stdio.h>
 #include <fcntl.h>
 
+#include <systemlib/err.h>
 #include <systemlib/rc_check.h>
 #include <systemlib/param/param.h>
 #include <mavlink/mavlink_log.h>
@@ -98,32 +99,32 @@ int rc_calibration_check(int mavlink_fd) {
 		/* assert min..center..max ordering */
 		if (param_min < 500) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MIN < 500", i+1);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_max > 2500) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_MAX > 2500", i+1);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_trim < param_min) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_trim > param_max) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 
 		/* assert deadzone is sane */
 		if (param_dz > 500) {
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
+			mavlink_log_critical(mavlink_fd, "#audio ERR: RC_%d_DZ > 500", i+1);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 			count++;
@@ -139,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) {
 
 		/* sanity checks pass, enable channel */
 		if (count) {
-			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			mavlink_log_critical(mavlink_fd, "#audio ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 		}
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 57a751e1c..9fff3eb88 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -64,6 +64,9 @@ systemreset(bool to_bootloader)
 		*(uint32_t *)0x40002850 = 0xb007b007;
 	}
 	up_systemreset();
+
+	/* lock up here */
+	while(true);
 }
 
 static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index c1d1a938f..44b6debc7 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -40,6 +40,7 @@
 #include "topics/parameter_update.h"
 #include "topics/actuator_controls.h"
 #include "topics/vehicle_gps_position.h"
+#include "topics/satellite_info.h"
 #include "topics/sensor_combined.h"
 #include "topics/vehicle_attitude.h"
 #include "topics/vehicle_global_position.h"
@@ -88,6 +89,7 @@ T Subscription<T>::getData() {
 template class __EXPORT Subscription<parameter_update_s>;
 template class __EXPORT Subscription<actuator_controls_s>;
 template class __EXPORT Subscription<vehicle_gps_position_s>;
+template class __EXPORT Subscription<satellite_info_s>;
 template class __EXPORT Subscription<sensor_combined_s>;
 template class __EXPORT Subscription<vehicle_attitude_s>;
 template class __EXPORT Subscription<vehicle_global_position_s>;
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 7c3bb0009..687fc1d4a 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -75,6 +75,9 @@ ORB_DEFINE(sensor_combined, struct sensor_combined_s);
 #include "topics/vehicle_gps_position.h"
 ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
 
+#include "topics/satellite_info.h"
+ORB_DEFINE(satellite_info, struct satellite_info_s);
+
 #include "topics/home_position.h"
 ORB_DEFINE(home_position, struct home_position_s);
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 370c54442..829d8e57d 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 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
@@ -43,59 +43,43 @@
 #include "../uORB.h"
 
 /**
- * The number of RC channel inputs supported.
- * Current (Q4/2013) radios support up to 18 channels,
- * leaving at a sane value of 16.
- * This number can be greater then number of RC channels,
- * because single RC channel can be mapped to multiple
- * functions, e.g. for various mode switches.
- */
-#define RC_CHANNELS_MAPPED_MAX   16
-
-/**
  * This defines the mapping of the RC functions.
  * The value assigned to the specific function corresponds to the entry of
- * the channel array chan[].
+ * the channel array channels[].
  */
 enum RC_CHANNELS_FUNCTION {
 	THROTTLE = 0,
-	ROLL     = 1,
-	PITCH    = 2,
-	YAW      = 3,
-	MODE = 4,
-	RETURN = 5,
-	POSCTL = 6,
-	LOITER = 7,
-	OFFBOARD_MODE = 8,
-	ACRO    = 9,
-	FLAPS   = 10,
-	AUX_1   = 11,
-	AUX_2   = 12,
-	AUX_3   = 13,
-	AUX_4   = 14,
-	AUX_5   = 15,
-	RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
+	ROLL,
+	PITCH,
+	YAW,
+	MODE,
+	RETURN,
+	POSCTL,
+	LOITER,
+	OFFBOARD_MODE,
+	ACRO,
+	FLAPS,
+	AUX_1,
+	AUX_2,
+	AUX_3,
+	AUX_4,
+	AUX_5,
+	RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
 };
 
 /**
  * @addtogroup topics
  * @{
  */
-
 struct rc_channels_s {
-
-	uint64_t timestamp;                 /**< In microseconds since boot time. */
-	uint64_t timestamp_last_valid;      /**< timestamp of last valid RC signal. */
-	struct {
-		float scaled;                     /**< Scaled to -1..1 (throttle: 0..1) */
-	} chan[RC_CHANNELS_MAPPED_MAX];
-	uint8_t chan_count;                 /**< number of valid channels */
-
-	/*String array to store the names of the functions*/
-	char function_name[RC_CHANNELS_FUNCTION_MAX][20];
-	int8_t function[RC_CHANNELS_FUNCTION_MAX];
-	uint8_t rssi;                       /**< Overall receive signal strength */
-	bool signal_lost;		/**< control signal lost, should be checked together with topic timeout */
+	uint64_t timestamp;									/**< Timestamp in microseconds since boot time */
+	uint64_t timestamp_last_valid;						/**< Timestamp of last valid RC signal */
+	float channels[RC_CHANNELS_FUNCTION_MAX];			/**< Scaled to -1..1 (throttle: 0..1) */
+	uint8_t channel_count;								/**< Number of valid channels */
+	char function_name[RC_CHANNELS_FUNCTION_MAX][20];	/**< String array to store the names of the functions */
+	int8_t function[RC_CHANNELS_FUNCTION_MAX];			/**< Functions mapping */
+	uint8_t rssi;										/**< Receive signal strength index */
+	bool signal_lost;									/**< Control signal lost, should be checked together with topic timeout */
 }; /**< radio control channels. */
 
 /**
diff --git a/src/modules/uORB/topics/satellite_info.h b/src/modules/uORB/topics/satellite_info.h
new file mode 100644
index 000000000..37c2faa96
--- /dev/null
+++ b/src/modules/uORB/topics/satellite_info.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.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 satellite_info.h
+ * Definition of the GNSS satellite info uORB topic.
+ */
+
+#ifndef TOPIC_SAT_INFO_H_
+#define TOPIC_SAT_INFO_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * GNSS Satellite Info.
+ */
+
+#define SAT_INFO_MAX_SATELLITES  20
+
+struct satellite_info_s {
+	uint64_t timestamp;				/**< Timestamp of satellite info */
+	uint8_t count;					/**< Number of satellites in satellite info */
+	uint8_t svid[SAT_INFO_MAX_SATELLITES]; 		/**< Space vehicle ID [1..255], see scheme below  */
+	uint8_t used[SAT_INFO_MAX_SATELLITES];		/**< 0: Satellite not used, 1: used for navigation */
+	uint8_t elevation[SAT_INFO_MAX_SATELLITES];	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t azimuth[SAT_INFO_MAX_SATELLITES];	/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t snr[SAT_INFO_MAX_SATELLITES];		/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
+};
+
+/**
+ * NAV_SVINFO space vehicle ID (svid) scheme according to u-blox protocol specs
+ * u-bloxM8-V15_ReceiverDescriptionProtocolSpec_Public_(UBX-13003221).pdf
+ *
+ * GPS		1-32
+ * SBAS		120-158
+ * Galileo	211-246
+ * BeiDou	159-163, 33-64
+ * QZSS		193-197
+ * GLONASS	65-96, 255
+ *
+ */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(satellite_info);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index bbacb733a..80d65cd69 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -61,7 +61,6 @@ struct vehicle_gps_position_s {
 
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
-	float p_variance_m;				/**< position accuracy estimate m */
 	float c_variance_rad;				/**< course accuracy estimate rad */
 	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
@@ -82,14 +81,7 @@ struct vehicle_gps_position_s {
 	uint64_t timestamp_time;			/**< Timestamp for time information */
 	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
 
-	uint64_t timestamp_satellites;			/**< Timestamp for sattelite information */
-	uint8_t satellites_visible;			/**< Number of satellites visible. If unknown, set to 255 */
-	uint8_t satellite_prn[20]; 			/**< Global satellite ID */
-	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
-	uint8_t satellite_elevation[20]; 		/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
-	uint8_t satellite_azimuth[20];			/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
-	uint8_t satellite_snr[20];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
-	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
+	uint8_t satellites_used;			/**< Number of satellites used */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 5dc46dacb..56590047f 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -191,6 +191,8 @@ struct vehicle_status_s {
 	bool condition_local_altitude_valid;
 	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
+	bool condition_power_input_valid;		/**< set if input power is valid */
+	float avionics_power_rail_voltage;		/**< voltage of the avionics power rail */
 
 	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception lost */
@@ -220,6 +222,8 @@ struct vehicle_status_s {
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
+
+	bool circuit_breaker_engaged_power_check;
 };
 
 /**
diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/src/systemcmds/mtd/24xxxx_mtd.c
index a3f9dffff..72200f418 100644
--- a/src/systemcmds/mtd/24xxxx_mtd.c
+++ b/src/systemcmds/mtd/24xxxx_mtd.c
@@ -237,8 +237,10 @@ void at24c_test(void)
 		} else if (result != 1) {
 			vdbg("unexpected %u\n", result);
 		}
-		if ((count % 100) == 0)
+
+		if ((count % 100) == 0) {
 			vdbg("test %u errors %u\n", count, errors);
+		}
 	}
 }
 
diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index 7d9484d3e..fca1798e6 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -60,7 +60,7 @@ nshterm_main(int argc, char *argv[])
         printf("Usage: nshterm <device>\n");
         exit(1);
     }
-    uint8_t retries = 0;
+    unsigned retries = 0;
     int fd = -1;
 
     /* try the first 30 seconds */
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 328205e29..f8bff2f6f 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -64,6 +64,7 @@ static void	do_show_print(void *arg, param_t param);
 static void	do_set(const char* name, const char* val, bool fail_on_not_found);
 static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
 static void	do_reset(void);
+static void	do_reset_nostart(void);
 
 int
 param_main(int argc, char *argv[])
@@ -142,6 +143,10 @@ param_main(int argc, char *argv[])
 		if (!strcmp(argv[1], "reset")) {
 			do_reset();
 		}
+
+		if (!strcmp(argv[1], "reset_nostart")) {
+			do_reset_nostart();
+		}
 	}
 	
 	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
@@ -427,3 +432,26 @@ do_reset(void)
 		exit(0);
 	}
 }
+
+static void
+do_reset_nostart(void)
+{
+
+	int32_t autostart;
+	int32_t autoconfig;
+
+	(void)param_get(param_find("SYS_AUTOSTART"), &autostart);
+	(void)param_get(param_find("SYS_AUTOCONFIG"), &autoconfig);
+
+	param_reset_all();
+
+	(void)param_set(param_find("SYS_AUTOSTART"), &autostart);
+	(void)param_set(param_find("SYS_AUTOCONFIG"), &autoconfig);
+
+	if (param_save_default()) {
+		warnx("Param export failed.");
+		exit(1);
+	} else {
+		exit(0);
+	}
+}
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 1828c660f..e0e6ca537 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -515,7 +515,8 @@ pwm_main(int argc, char *argv[])
 				ret = poll(&fds, 1, 0);
 				if (ret > 0) {
 
-				int ret = read(0, &c, 1);
+					ret = read(0, &c, 1);
+
 					if (ret > 0) {
 						/* reset output to the last value */
 						for (unsigned i = 0; i < servo_count; i++) {
diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp
index 0ecdc8eb9..fda949c61 100644
--- a/src/systemcmds/tests/test_conv.cpp
+++ b/src/systemcmds/tests/test_conv.cpp
@@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[])
 		float f = i/10000.0f;
 		float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
 		if (fabsf(f - fres) > 0.0001f) {
-			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
+			warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", (double)f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres);
 			return 1;
 		}
 	}
diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c
index 86f23b485..570583dee 100644
--- a/src/systemcmds/tests/test_file.c
+++ b/src/systemcmds/tests/test_file.c
@@ -149,6 +149,8 @@ test_file(int argc, char *argv[])
 			}
 			end = hrt_absolute_time();
 
+			warnx("write took %llu us", (end - start));
+
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -192,7 +194,6 @@ test_file(int argc, char *argv[])
 
 			warnx("testing aligned writes - please wait.. (CTRL^C to abort)");
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 				int wret = write(fd, write_buf, chunk_sizes[c]);
 
diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c
index e33cc6ef7..bb8b6c7f5 100644
--- a/src/systemcmds/tests/test_float.c
+++ b/src/systemcmds/tests/test_float.c
@@ -68,7 +68,7 @@ int test_float(int argc, char *argv[])
 	float sinf_one = sinf(1.0f);
 	float sqrt_two = sqrt(2.0f);
 
-	if (sinf_zero == 0.0f) {
+	if (fabsf(sinf_zero) < FLT_EPSILON) {
 		printf("\t success: sinf(0.0f) == 0.0f\n");
 
 	} else {
@@ -136,7 +136,7 @@ int test_float(int argc, char *argv[])
 		ret = -2;
 	}
 
-	if (sqrt_two == 1.41421356f) {
+	if (fabsf(sqrt_two - 1.41421356f) < FLT_EPSILON) {
 		printf("\t success: sqrt(2.0f) == 1.41421f\n");
 
 	} else {
@@ -188,7 +188,7 @@ int test_float(int argc, char *argv[])
 
 	double d1d2 = d1 * d2;
 
-	if (d1d2 == 2.022200000000000219557705349871) {
+	if (fabs(d1d2 - 2.022200000000000219557705349871) < DBL_EPSILON) {
 		printf("\t success: 1.0111 * 2.0 == 2.0222\n");
 
 	} else {
@@ -201,7 +201,7 @@ int test_float(int argc, char *argv[])
 	// Assign value of f1 to d1
 	d1 = f1;
 
-	if (f1 == (float)d1) {
+	if (fabsf(f1 - (float)d1) < FLT_EPSILON) {
 		printf("\t success: (float) 1.55f == 1.55 (double)\n");
 
 	} else {
@@ -216,7 +216,7 @@ int test_float(int argc, char *argv[])
 	double sin_one = sin(1.0);
 	double atan2_ones = atan2(1.0, 1.0);
 
-	if (sin_zero == 0.0) {
+	if (fabs(sin_zero - 0.0) < DBL_EPSILON) {
 		printf("\t success: sin(0.0) == 0.0\n");
 
 	} else {
@@ -224,7 +224,7 @@ int test_float(int argc, char *argv[])
 		ret = -9;
 	}
 
-	if (sin_one == 0.841470984807896504875657228695) {
+	if (fabs(sin_one - 0.841470984807896504875657228695) < DBL_EPSILON) {
 		printf("\t success: sin(1.0) == 0.84147098480\n");
 
 	} else {
@@ -232,7 +232,7 @@ int test_float(int argc, char *argv[])
 		ret = -10;
 	}
 
-	if (atan2_ones != 0.785398) {
+	if (fabs(atan2_ones - 0.785398) < DBL_EPSILON) {
 		printf("\t success: atan2(1.0, 1.0) == 0.785398\n");
 
 	} else {
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
index cdb4362ba..43231ccad 100644
--- a/src/systemcmds/tests/test_mtd.c
+++ b/src/systemcmds/tests/test_mtd.c
@@ -171,6 +171,8 @@ test_mtd(int argc, char *argv[])
 		}
 		end = hrt_absolute_time();
 
+		warnx("write took %llu us", (end - start));
+
 		close(fd);
 		fd = open(PARAM_FILE_NAME, O_RDONLY);