Merged master

117 files changed, 1603 insertions, 206 deletions

diff --git a/.gitmodules b/.gitmodules
index 4b84afac2..0c8c91da4 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -5,5 +5,8 @@
 	path = NuttX
 	url = git://github.com/PX4/NuttX.git
 [submodule "uavcan"]
-	path = uavcan
+	path = src/lib/uavcan
 	url = git://github.com/pavel-kirienko/uavcan.git
+[submodule "unittests/gtest"]
+	path = unittests/gtest
+	url = https://github.com/sjwilks/gtest.git
diff --git a/.travis.yml b/.travis.yml
index d25080bed..e7dc6029c 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -85,3 +85,11 @@ addons:
     bucket: px4-travis
     region: us-east-1
     endpoint: s3-website-us-east-1.amazonaws.com
+
+notifications:
+  webhooks:
+    urls:
+      - https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba
+    on_success: always  # options: [always|never|change] default: always
+    on_failure: always  # options: [always|never|change] default: always
+    on_start: false     # default: false
diff --git a/Makefile b/Makefile
index 0f68e7b4b..905a60a3b 100644
--- a/Makefile
+++ b/Makefile
@@ -245,14 +245,16 @@ tests:
 #
 .PHONY:	clean
 clean:
-	$(Q) $(RMDIR) $(BUILD_DIR)*.build > /dev/null
-	$(Q) $(REMOVE) $(IMAGE_DIR)*.px4 > /dev/null
+	@echo > /dev/null
+	$(Q) $(RMDIR) $(BUILD_DIR)*.build
+	$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
 
 .PHONY:	distclean
 distclean: clean
-	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export > /dev/null
-	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean > /dev/null
-	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete) > /dev/null
+	@echo > /dev/null
+	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
+	$(Q) $(MAKE) -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	$(Q) (cd $(NUTTX_SRC)/configs && $(FIND) . -maxdepth 1 -type l -delete)
 
 #
 # Print some help text
diff --git a/NuttX b/NuttX
-Subproject b66de6506941dc7628efcf65e5543c0e3cb05a8
+Subproject 3c36467c0d5572431a09ae50013328a4693ee07
diff --git a/README.md b/README.md
index 302ea08c3..5e4bc478d 100644
--- a/README.md
+++ b/README.md
@@ -1,4 +1,4 @@
-## PX4 Aerial Middleware and Flight Control Stack ##
+## PX4 Flight Control Stack and Middleware ##
 
 [![Build Status](https://travis-ci.org/PX4/Firmware.svg?branch=master)](https://travis-ci.org/PX4/Firmware)
 
@@ -13,3 +13,21 @@
 *   Binaries (always up-to-date from master):
   * [Downloads](https://pixhawk.org/downloads)
 *   Mailing list: [Google Groups](http://groups.google.com/group/px4users)
+
+### Developers ###
+
+Contributing guide:
+http://px4.io/dev/contributing
+
+Developer guide:
+http://px4.io/dev/
+
+This repository contains code supporting these boards:
+  * PX4FMUv1.x
+  * PX4FMUv2.x
+  * AeroCore
+
+## NuttShell (NSH) ##
+
+NSH usage documentation:
+http://px4.io/users/serial_connection
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index fab2a7f18..e6de64054 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -10,7 +10,7 @@ then
 	param set NAV_LAND_ALT 90
 	param set NAV_RTL_ALT 100
 	param set NAV_RTL_LAND_T -1
-	param set NAV_ACCEPT_RAD 50
+	param set NAV_ACC_RAD 50
 	param set FW_T_HRATE_P 0.01
 	param set FW_T_RLL2THR 15
 	param set FW_T_SRATE_P 0.01
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
index 307a64c4d..24d9650a0 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -41,7 +41,9 @@ then
 	param set PE_POSNE_NOISE 0.5
 	param set PE_POSD_NOISE 1.0
 
-	param set NAV_ACCEPT_RAD 2.0
+	param set NAV_ACC_RAD 2.0
+	param set RTL_RETURN_ALT 30.0
+	param set RTL_DESCEND_ALT 10.0
 fi
 
 set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index e6fc535a6..973db9017 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -56,7 +56,6 @@ fi
 
 if meas_airspeed start
 then
-	echo "[i] Using MEAS airspeed sensor"
 else
 	if ets_airspeed start
 	then
@@ -67,6 +66,7 @@ else
 	fi
 fi
 
+# Check for flow sensor
 if px4flow start
 then
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index ee040a706..b3de8e590 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -25,6 +25,7 @@ mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
 usleep 100000
 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
 usleep 100000
+mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
 
 # Exit shell to make it available to MAVLink
 exit
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 14fc8d2b4..26b729aad 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -430,6 +430,16 @@ then
 	unset MAVLINK_F
 
 	#
+	# MAVLink onboard / TELEM2
+	#
+	if ver hwcmp PX4FMU_V2
+	then
+		if param compare SYS_COMPANION 921600
+		then
+			mavlink start -d /dev/ttyS2 -b 921600 -m onboard
+		fi
+	fi
+
 	# UAVCAN
 	#
 	sh /etc/init.d/rc.uavcan
diff --git a/Tools/px4params/srcparser.py b/Tools/px4params/srcparser.py
index 0a4d21d26..8e6092195 100644
--- a/Tools/px4params/srcparser.py
+++ b/Tools/px4params/srcparser.py
@@ -103,7 +103,7 @@ class SourceParser(object):
         Returns list of supported file extensions that can be parsed by this
         parser.
         """
-        return ["cpp", "c"]
+        return [".cpp", ".c"]
 
     def Parse(self, contents):
         """
diff --git a/Tools/px4params/srcscanner.py b/Tools/px4params/srcscanner.py
index d7eca72d7..1f0ea4e89 100644
--- a/Tools/px4params/srcscanner.py
+++ b/Tools/px4params/srcscanner.py
@@ -26,5 +26,10 @@ class SourceScanner(object):
         parser.Parse method.
         """
         with codecs.open(path, 'r', 'utf-8') as f:
-            contents = f.read()
+            try:
+                contents = f.read()
+            except:
+                contents = ''
+                print('Failed reading file: %s, skipping content.' % path)
+                pass
         parser.Parse(contents)
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index 335055cb1..fa5dff93c 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -32,6 +32,7 @@ MODULES		+= drivers/ll40ls
 # MODULES		+= drivers/trone
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
+MODULES		+= drivers/hott
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 # MODULES		+= drivers/blinkm
diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk
index 84e5cd08a..7119c723b 100644
--- a/makefiles/toolchain_gnu-arm-eabi.mk
+++ b/makefiles/toolchain_gnu-arm-eabi.mk
@@ -127,6 +127,7 @@ ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
 #
 ARCHWARNINGS		 = -Wall \
 			   -Wextra \
+			   -Werror \
 			   -Wdouble-promotion \
 			   -Wshadow \
 			   -Wfloat-equal \
diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs
index c7a1b71bb..c1f5a8ac4 100644
--- a/nuttx-configs/aerocore/nsh/Make.defs
+++ b/nuttx-configs/aerocore/nsh/Make.defs
@@ -119,6 +119,7 @@ endif
 ARCHCFLAGS		 = -std=gnu99
 ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
 ARCHWARNINGS		 = -Wall \
+			   -Wno-sign-compare \
 			   -Wextra \
 			   -Wdouble-promotion \
 			   -Wshadow \
diff --git a/nuttx-configs/px4fmu-v1/nsh/Make.defs b/nuttx-configs/px4fmu-v1/nsh/Make.defs
index f055cfddf..5e28f2473 100644
--- a/nuttx-configs/px4fmu-v1/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v1/nsh/Make.defs
@@ -119,6 +119,7 @@ endif
 ARCHCFLAGS		 = -std=gnu99
 ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
 ARCHWARNINGS		 = -Wall \
+			   -Wno-sign-compare \
 			   -Wextra \
 			   -Wdouble-promotion \
 			   -Wshadow \
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index edf49b26e..fade1f0c6 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -404,7 +404,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=38
+CONFIG_NFILE_DESCRIPTORS=42
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4fmu-v2/nsh/Make.defs b/nuttx-configs/px4fmu-v2/nsh/Make.defs
index f660deeca..99f3b3140 100644
--- a/nuttx-configs/px4fmu-v2/nsh/Make.defs
+++ b/nuttx-configs/px4fmu-v2/nsh/Make.defs
@@ -119,6 +119,7 @@ endif
 ARCHCFLAGS		 = -std=gnu99
 ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
 ARCHWARNINGS		 = -Wall \
+			   -Wno-sign-compare \
 			   -Wextra \
 			   -Wdouble-promotion \
 			   -Wshadow \
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index e31f40cd2..50d5b0296 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -323,7 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y
 #
 # CONFIG_STM32_I2C_DYNTIMEO is not set
 CONFIG_STM32_I2CTIMEOSEC=0
-CONFIG_STM32_I2CTIMEOMS=1
+CONFIG_STM32_I2CTIMEOMS=10
 # CONFIG_STM32_I2C_DUTY16_9 is not set
 
 #
@@ -438,7 +438,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=38
+CONFIG_NFILE_DESCRIPTORS=42
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
diff --git a/nuttx-configs/px4io-v1/nsh/Make.defs b/nuttx-configs/px4io-v1/nsh/Make.defs
index e96be6a1f..b4f5577ae 100644
--- a/nuttx-configs/px4io-v1/nsh/Make.defs
+++ b/nuttx-configs/px4io-v1/nsh/Make.defs
@@ -108,6 +108,7 @@ endif
 ARCHCFLAGS		 = -std=gnu99
 ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
 ARCHWARNINGS		 = -Wall \
+         -Wno-sign-compare \
 			   -Wextra \
 			   -Wdouble-promotion \
 			   -Wshadow \
diff --git a/nuttx-configs/px4io-v2/nsh/Make.defs b/nuttx-configs/px4io-v2/nsh/Make.defs
index 4a8df2738..51420eb23 100644
--- a/nuttx-configs/px4io-v2/nsh/Make.defs
+++ b/nuttx-configs/px4io-v2/nsh/Make.defs
@@ -108,6 +108,7 @@ endif
 ARCHCFLAGS		 = -std=gnu99
 ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x
 ARCHWARNINGS		 = -Wall \
+         -Wno-sign-compare \
 			   -Wextra \
 			   -Wdouble-promotion \
 			   -Wshadow \
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 9d2c1441d..7f1b21a95 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[])
 						    SCHED_PRIORITY_MAX - 15,
 						    1100,
 						    ardrone_interface_thread_main,
-						    (argv) ? (const char **)&argv[2] : (const char **)NULL);
+						    (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
new file mode 100644
index 000000000..dd83dacaf
--- /dev/null
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -0,0 +1,540 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Randy Mackay <rmackay9@yahoo.com>
+ *
+ * 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 batt_smbus.cpp
+ *
+ * Driver for a battery monitor connected via SMBus (I2C).
+ *
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <sched.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <ctype.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <board_config.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/battery_status.h>
+
+#include <float.h>
+
+#include <drivers/device/i2c.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_batt_smbus.h>
+#include <drivers/device/ringbuffer.h>
+
+#define BATT_SMBUS_ADDR_MIN				0x08	/* lowest possible address */
+#define BATT_SMBUS_ADDR_MAX				0x7F	/* highest possible address */
+
+#define BATT_SMBUS_I2C_BUS				PX4_I2C_BUS_EXPANSION
+#define BATT_SMBUS_ADDR					0x0B	/* I2C address */
+#define BATT_SMBUS_TEMP					0x08	/* temperature register */
+#define BATT_SMBUS_VOLTAGE				0x09	/* voltage register */
+#define BATT_SMBUS_DESIGN_CAPACITY		0x18	/* design capacity register */
+#define BATT_SMBUS_DESIGN_VOLTAGE		0x19	/* design voltage register */
+#define BATT_SMBUS_SERIALNUM			0x1c	/* serial number register */
+#define BATT_SMBUS_MANUFACTURE_NAME     0x20    /* manufacturer name */
+#define BATT_SMBUS_MANUFACTURE_INFO		0x25	/* cell voltage register */
+#define BATT_SMBUS_CURRENT				0x2a	/* current register */
+#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS	(1000000 / 10)	/* time in microseconds, measure at 10hz */
+
+#define BATT_SMBUS_PEC_POLYNOMIAL		0x07	/* Polynomial for calculating PEC */
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+class BATT_SMBUS : public device::I2C
+{
+public:
+	BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR);
+	virtual ~BATT_SMBUS();
+
+	virtual int		init();
+	virtual int		test();
+	int				search();		/* search all possible slave addresses for a smart battery */
+
+protected:
+	virtual int		probe();		// check if the device can be contacted
+
+private:
+
+	// start periodic reads from the battery
+	void			start();
+
+	// stop periodic reads from the battery
+	void			stop();
+
+	// static function that is called by worker queue
+	static void		cycle_trampoline(void *arg);
+
+	// perform a read from the battery
+	void			cycle();
+
+	// read_reg - read a word from specified register
+	int				read_reg(uint8_t reg, uint16_t &val);
+
+	// read_block - returns number of characters read if successful, zero if unsuccessful
+	uint8_t			read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero);
+
+	// get_PEC - calculate PEC for a read or write from the battery
+	//	buff is the data that was read or will be written
+	uint8_t			get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const;
+
+	// internal variables
+	work_s			_work;			// work queue for scheduling reads
+	RingBuffer		*_reports;		// buffer of recorded voltages, currents
+	struct battery_status_s _last_report;	// last published report, used for test()
+	orb_advert_t	_batt_topic;
+	orb_id_t		_batt_orb_id;
+};
+
+/* for now, we only support one BATT_SMBUS */
+namespace
+{
+BATT_SMBUS *g_batt_smbus;
+}
+
+void batt_smbus_usage();
+
+extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]);
+
+// constructor
+BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) :
+	I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000),
+	_work{},
+	_reports(nullptr),
+	_batt_topic(-1),
+	_batt_orb_id(nullptr)
+{
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+// destructor
+BATT_SMBUS::~BATT_SMBUS()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+}
+
+int
+BATT_SMBUS::init()
+{
+	int ret = ENOTTY;
+
+	// attempt to initialise I2C bus
+	ret = I2C::init();
+
+	if (ret != OK) {
+		errx(1,"failed to init I2C");
+		return ret;
+	} else {
+		/* allocate basic report buffers */
+		_reports = new RingBuffer(2, sizeof(struct battery_status_s));
+		if (_reports == nullptr) {
+			ret = ENOTTY;
+		} else {
+			// start work queue
+			start();
+		}
+	}
+
+	// init orb id
+	_batt_orb_id = ORB_ID(battery_status);
+
+	return ret;
+}
+
+int
+BATT_SMBUS::test()
+{
+	int sub = orb_subscribe(ORB_ID(battery_status));
+	bool updated = false;
+	struct battery_status_s status;
+	uint64_t start_time = hrt_absolute_time();
+
+	// loop for 5 seconds
+	while ((hrt_absolute_time() - start_time) < 5000000) {
+
+		// display new info that has arrived from the orb
+		orb_check(sub, &updated);
+		if (updated) {
+			if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) {
+				warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a);
+			}
+		}
+
+		// sleep for 0.05 seconds
+		usleep(50000);
+	}
+
+	return OK;
+}
+
+/* search all possible slave addresses for a smart battery */
+int
+BATT_SMBUS::search()
+{
+	bool found_slave = false;
+	uint16_t tmp;
+
+	// search through all valid SMBus addresses
+	for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) {
+		set_address(i);
+		if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
+			warnx("battery found at 0x%x",(int)i);
+			found_slave = true;
+		}
+		// short sleep
+		usleep(1);
+	}
+
+	// display completion message
+	if (found_slave) {
+		warnx("Done.");
+	} else {
+		warnx("No smart batteries found.");
+	}
+
+	return OK;
+}
+
+int
+BATT_SMBUS::probe()
+{
+	// always return OK to ensure device starts
+	return OK;
+}
+
+// start periodic reads from the battery
+void
+BATT_SMBUS::start()
+{
+	/* reset the report ring and state machine */
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1);
+}
+
+// stop periodic reads from the battery
+void
+BATT_SMBUS::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+// static function that is called by worker queue
+void
+BATT_SMBUS::cycle_trampoline(void *arg)
+{
+	BATT_SMBUS *dev = (BATT_SMBUS *)arg;
+
+	dev->cycle();
+}
+
+// perform a read from the battery
+void
+BATT_SMBUS::cycle()
+{
+	// read data from sensor
+	struct battery_status_s new_report;
+
+	// set time of reading
+	new_report.timestamp = hrt_absolute_time();
+
+	// read voltage
+	uint16_t tmp;
+	if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
+		// initialise new_report
+		memset(&new_report, 0, sizeof(new_report));
+
+		// convert millivolts to volts
+		new_report.voltage_v = ((float)tmp) / 1000.0f;
+
+		// read current
+		usleep(1);
+		uint8_t buff[4];
+		if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
+			new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f;
+		}
+
+		// publish to orb
+		if (_batt_topic != -1) {
+			orb_publish(_batt_orb_id, _batt_topic, &new_report);
+		} else {
+			_batt_topic = orb_advertise(_batt_orb_id, &new_report);
+			if (_batt_topic < 0) {
+				errx(1, "ADVERT FAIL");
+			}
+		}
+
+		// copy report for test()
+		_last_report = new_report;
+
+		/* post a report to the ring */
+		_reports->force(&new_report);
+
+		/* notify anyone waiting for data */
+		poll_notify(POLLIN);
+	}
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
+}
+
+int
+BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val)
+{
+	uint8_t buff[3];	// 2 bytes of data + PEC
+
+	// read from register
+	int ret = transfer(&reg, 1, buff, 3);
+	if (ret == OK) {
+		// check PEC
+		uint8_t pec = get_PEC(reg, true, buff, 2);
+		if (pec == buff[2]) {
+			val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0];
+		} else {
+			ret = ENOTTY;
+		}
+	}
+
+	// return success or failure
+	return ret;
+}
+
+// read_block - returns number of characters read if successful, zero if unsuccessful
+uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero)
+{
+	uint8_t buff[max_len+2];    // buffer to hold results
+
+	usleep(1);
+
+	// read bytes including PEC
+	int ret = transfer(&reg, 1, buff, max_len+2);
+
+	// return zero on failure
+	if (ret != OK) {
+		return 0;
+	}
+
+	// get length
+	uint8_t bufflen = buff[0];
+
+	// sanity check length returned by smbus
+	if (bufflen == 0 || bufflen > max_len) {
+		return 0;
+	}
+
+	// check PEC
+	uint8_t pec = get_PEC(reg, true, buff, bufflen+1);
+	if (pec != buff[bufflen+1]) {
+		// debug
+		warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec);
+		return 0;
+	} else {
+		warnx("CurPEC ok: %x",(int)pec);
+	}
+
+	// copy data
+	memcpy(data, &buff[1], bufflen);
+
+	// optionally add zero to end
+	if (append_zero) {
+		data[bufflen] = '\0';
+	}
+
+	// return success
+	return bufflen;
+}
+
+// get_PEC - calculate PEC for a read or write from the battery
+//	buff is the data that was read or will be written
+uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const
+{
+	// exit immediately if no data
+	if (len <= 0) {
+		return 0;
+	}
+
+	// prepare temp buffer for calcing crc
+	uint8_t tmp_buff[len+3];
+	tmp_buff[0] = (uint8_t)get_address() << 1;
+	tmp_buff[1] = cmd;
+	tmp_buff[2] = tmp_buff[0] | (uint8_t)reading;
+	memcpy(&tmp_buff[3],buff,len);
+
+	// initialise crc to zero
+	uint8_t crc = 0;
+	uint8_t shift_reg = 0;
+	bool do_invert;
+
+	// for each byte in the stream
+	for (uint8_t i=0; i<sizeof(tmp_buff); i++) {
+		// load next data byte into the shift register
+		shift_reg = tmp_buff[i];
+		// for each bit in the current byte
+		for (uint8_t j=0; j<8; j++) {
+			do_invert = (crc ^ shift_reg) & 0x80;
+			crc <<= 1;
+			shift_reg <<= 1;
+			if (do_invert) {
+				crc ^= BATT_SMBUS_PEC_POLYNOMIAL;
+			}
+		}
+	}
+
+	// return result
+	return crc;
+}
+
+///////////////////////// shell functions ///////////////////////
+
+void
+batt_smbus_usage()
+{
+	warnx("missing command: try 'start', 'test', 'stop', 'search'");
+	warnx("options:");
+		warnx("    -b i2cbus (%d)", BATT_SMBUS_I2C_BUS);
+		warnx("    -a addr (0x%x)", BATT_SMBUS_ADDR);
+}
+
+int
+batt_smbus_main(int argc, char *argv[])
+{
+	int i2cdevice = BATT_SMBUS_I2C_BUS;
+	int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */
+
+	int ch;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+		switch (ch) {
+		case 'a':
+			batt_smbusadr = strtol(optarg, NULL, 0);
+			break;
+
+		case 'b':
+			i2cdevice = strtol(optarg, NULL, 0);
+			break;
+
+		default:
+			batt_smbus_usage();
+			exit(0);
+		}
+	}
+
+	if (optind >= argc) {
+		batt_smbus_usage();
+		exit(1);
+	}
+
+	const char *verb = argv[optind];
+
+	if (!strcmp(verb, "start")) {
+		if (g_batt_smbus != nullptr) {
+			errx(1, "already started");
+		} else {
+			// create new global object
+			g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr);
+
+			if (g_batt_smbus == nullptr) {
+				errx(1, "new failed");
+			}
+
+			if (OK != g_batt_smbus->init()) {
+				delete g_batt_smbus;
+				g_batt_smbus = nullptr;
+				errx(1, "init failed");
+			}
+		}
+
+		exit(0);
+	}
+
+	/* need the driver past this point */
+	if (g_batt_smbus == nullptr) {
+		warnx("not started");
+		batt_smbus_usage();
+		exit(1);
+	}
+
+	if (!strcmp(verb, "test")) {
+		g_batt_smbus->test();
+		exit(0);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		delete g_batt_smbus;
+		g_batt_smbus = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(verb, "search")) {
+		g_batt_smbus->search();
+		exit(0);
+	}
+
+	batt_smbus_usage();
+	exit(0);
+}
diff --git a/src/drivers/batt_smbus/module.mk b/src/drivers/batt_smbus/module.mk
new file mode 100644
index 000000000..b32ea6e55
--- /dev/null
+++ b/src/drivers/batt_smbus/module.mk
@@ -0,0 +1,8 @@
+#
+# driver for SMBus smart batteries
+#
+
+MODULE_COMMAND	 = batt_smbus
+SRCS		 = batt_smbus.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c
index e40d1730c..94a716029 100644
--- a/src/drivers/boards/aerocore/aerocore_led.c
+++ b/src/drivers/boards/aerocore/aerocore_led.c
@@ -45,6 +45,7 @@
 #include "board_config.h"
 
 #include <arch/board/board.h>
+#include <systemlib/err.h>
 
 /*
  * Ideally we'd be able to get these from up_internal.h,
@@ -54,7 +55,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);
 extern void led_toggle(int led);
diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h
index ee365e47c..2fd8bc724 100644
--- a/src/drivers/boards/px4fmu-v2/board_config.h
+++ b/src/drivers/boards/px4fmu-v2/board_config.h
@@ -105,6 +105,7 @@ __BEGIN_DECLS
 #define GPIO_SPI_CS_ACCEL_MAG	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15)
 #define GPIO_SPI_CS_BARO	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7)
 #define GPIO_SPI_CS_FRAM	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_SPI_CS_HMC		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN1)
 #define GPIO_SPI_CS_MPU		(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2)
 #define GPIO_SPI_CS_EXT0	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4)
 #define GPIO_SPI_CS_EXT1	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14)
@@ -119,6 +120,7 @@ __BEGIN_DECLS
 #define PX4_SPIDEV_ACCEL_MAG	2
 #define PX4_SPIDEV_BARO		3
 #define PX4_SPIDEV_MPU		4
+#define PX4_SPIDEV_HMC		5
 
 /* External bus */
 #define PX4_SPIDEV_EXT0		1
diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
index 8c37d31a7..27f193513 100644
--- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c
@@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_configgpio(GPIO_SPI_CS_GYRO);
 	stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG);
 	stm32_configgpio(GPIO_SPI_CS_BARO);
+	stm32_configgpio(GPIO_SPI_CS_HMC);
 	stm32_configgpio(GPIO_SPI_CS_MPU);
 
 	/* De-activate all peripherals,
@@ -82,6 +83,7 @@ __EXPORT void weak_function stm32_spiinitialize(void)
 	stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+	stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
 	stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 
 	stm32_configgpio(GPIO_EXTI_GYRO_DRDY);
@@ -117,6 +119,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
@@ -125,6 +128,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
@@ -133,6 +137,16 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
+		break;
+
+	case PX4_SPIDEV_HMC:
+		/* Making sure the other peripherals are not selected */
+		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
 		break;
 
@@ -141,6 +155,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
 		stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_BARO, 1);
+		stm32_gpiowrite(GPIO_SPI_CS_HMC, 1);
 		stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
 		break;
 
diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h
index 705b398b0..206e71ddc 100644
--- a/src/drivers/device/i2c.h
+++ b/src/drivers/device/i2c.h
@@ -58,7 +58,7 @@ public:
 	/**
 	 * Get the address
 	 */
-	int16_t		get_address() { return _address; }
+	int16_t		get_address() const { return _address; }
 	
 protected:
 	/**
diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h
new file mode 100644
index 000000000..ca130c84e
--- /dev/null
+++ b/src/drivers/drv_batt_smbus.h
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_batt_smbus.h
+ *
+ * SMBus battery monitor device API
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+#include "drv_orb_dev.h"
+
+/* device path */
+#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus"
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index bccdf1190..5035600ef 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -221,7 +221,7 @@ int frsky_telemetry_main(int argc, char *argv[])
 					  SCHED_PRIORITY_DEFAULT,
 					  2000,
 					  frsky_telemetry_thread_main,
-					  (const char **)argv);
+					  (char * const *)argv);
 
 		while (!thread_running) {
 			usleep(200);
diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h
index f5608122f..0a586a8fd 100644
--- a/src/drivers/hott/comms.h
+++ b/src/drivers/hott/comms.h
@@ -41,6 +41,6 @@
 #ifndef COMMS_H_
 #define COMMS_H
 
-int open_uart(const char *device);
+__EXPORT int open_uart(const char *device);
 
 #endif /* COMMS_H_ */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index 8ab9d8d55..4b8e0c0b0 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.cpp
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -214,7 +214,7 @@ hott_sensors_main(int argc, char *argv[])
 					     SCHED_PRIORITY_DEFAULT,
 					     1024,
 					     hott_sensors_thread_main,
-					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
index 47aea6caf..25a6f0ac0 100644
--- a/src/drivers/hott/hott_sensors/module.mk
+++ b/src/drivers/hott/hott_sensors/module.mk
@@ -37,8 +37,6 @@
 
 MODULE_COMMAND	= hott_sensors
 
-SRCS		= hott_sensors.cpp \
-		  ../messages.cpp \
-		  ../comms.cpp
+SRCS		= hott_sensors.cpp
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index edbb14172..17a24d104 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -240,7 +240,7 @@ hott_telemetry_main(int argc, char *argv[])
 					 SCHED_PRIORITY_DEFAULT,
 					 2048,
 					 hott_telemetry_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
index cd7bdbc85..9de47b356 100644
--- a/src/drivers/hott/hott_telemetry/module.mk
+++ b/src/drivers/hott/hott_telemetry/module.mk
@@ -37,8 +37,6 @@
 
 MODULE_COMMAND	= hott_telemetry
 
-SRCS		= hott_telemetry.cpp \
-		  ../messages.cpp \
-		  ../comms.cpp
+SRCS		= hott_telemetry.cpp
 
 MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h
index 224f8fc56..a116a50dd 100644
--- a/src/drivers/hott/messages.h
+++ b/src/drivers/hott/messages.h
@@ -235,15 +235,15 @@ struct gps_module_msg {
 // The maximum size of a message.
 #define MAX_MESSAGE_BUFFER_SIZE 45
 
-void init_sub_messages(void);
-void init_pub_messages(void);
-void build_gam_request(uint8_t *buffer, size_t *size);
-void publish_gam_message(const uint8_t *buffer);
-void build_eam_response(uint8_t *buffer, size_t *size);
-void build_gam_response(uint8_t *buffer, size_t *size);
-void build_gps_response(uint8_t *buffer, size_t *size);
-float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
+__EXPORT void init_sub_messages(void);
+__EXPORT void init_pub_messages(void);
+__EXPORT void build_gam_request(uint8_t *buffer, size_t *size);
+__EXPORT void publish_gam_message(const uint8_t *buffer);
+__EXPORT void build_eam_response(uint8_t *buffer, size_t *size);
+__EXPORT void build_gam_response(uint8_t *buffer, size_t *size);
+__EXPORT void build_gps_response(uint8_t *buffer, size_t *size);
+__EXPORT float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
+__EXPORT void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
 
 
 #endif /* MESSAGES_H_ */
diff --git a/src/drivers/hott/module.mk b/src/drivers/hott/module.mk
new file mode 100644
index 000000000..31a66d491
--- /dev/null
+++ b/src/drivers/hott/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Graupner HoTT Sensors messages.
+#
+
+SRCS		= messages.cpp \
+			  comms.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk
index 460bec7b9..ecd3e804a 100644
--- a/src/drivers/px4flow/module.mk
+++ b/src/drivers/px4flow/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= px4flow
 SRCS		= px4flow.cpp
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wno-attributes
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 09ec4bf96..7e3461ba1 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -73,13 +73,14 @@
 #include <board_config.h>
 
 /* Configuration Constants */
-#define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
-//range 0x42 - 0x49
+#define I2C_FLOW_ADDRESS 		0x42	///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49
 
 /* PX4FLOW Registers addresses */
-#define PX4FLOW_REG		0x16	/* Measure Register 22*/
+#define PX4FLOW_REG			0x16	///< Measure Register 22
+
+#define PX4FLOW_CONVERSION_INTERVAL	20000	///< in microseconds! 20000 = 50 Hz 100000 = 10Hz
+#define PX4FLOW_I2C_MAX_BUS_SPEED	400000	///< 400 KHz maximum speed
 
-#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
 # undef ERROR
@@ -106,7 +107,7 @@ struct i2c_frame {
 };
 struct i2c_frame f;
 
-typedef struct i2c_integral_frame {
+struct i2c_integral_frame {
 	uint16_t frame_count_since_last_readout;
 	int16_t pixel_flow_x_integral;
 	int16_t pixel_flow_y_integral;
@@ -200,7 +201,7 @@ private:
 extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
 
 PX4FLOW::PX4FLOW(int bus, int address) :
-	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */
 	_reports(nullptr),
 	_sensor_ok(false),
 	_measure_ticks(0),
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
index 44ed03254..83086fd7c 100644
--- a/src/drivers/roboclaw/roboclaw_main.cpp
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -109,7 +109,7 @@ int roboclaw_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 2048,
 					 roboclaw_thread_main,
-					 (const char **)argv);
+					 (char * const *)argv);
 		exit(0);
 
 	} else if (!strcmp(argv[1], "test")) {
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 4301750f0..8e62e0d4b 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -90,7 +90,9 @@ static const int ERROR = -1;
 #define SF0X_TAKE_RANGE_REG		'd'
 #define SF02F_MIN_DISTANCE		0.0f
 #define SF02F_MAX_DISTANCE		40.0f
-#define SF0X_DEFAULT_PORT		"/dev/ttyS2"
+
+// designated SERIAL4/5 on Pixhawk
+#define SF0X_DEFAULT_PORT		"/dev/ttyS6"
 
 class SF0X : public device::CDev
 {
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index 281f918d7..603c2eb9d 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -253,9 +253,11 @@ static uint16_t			latency_baseline;
 static uint16_t			latency_actual;
 
 /* latency histogram */
-#define LATENCY_BUCKET_COUNT	8
-static const uint16_t		latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
-static uint32_t			latency_counters[LATENCY_BUCKET_COUNT + 1];
+#define LATENCY_BUCKET_COUNT 8
+__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+__EXPORT const uint16_t	latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t		latency_counters[LATENCY_BUCKET_COUNT + 1];
+
 
 /* timer-specific functions */
 static void		hrt_tim_init(void);
diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp
index 83b5c987e..cf3546669 100644
--- a/src/drivers/trone/trone.cpp
+++ b/src/drivers/trone/trone.cpp
@@ -206,7 +206,7 @@ static const uint8_t crc_table[] = {
     0xfa, 0xfd, 0xf4, 0xf3
 };
 
-uint8_t crc8(uint8_t *p, uint8_t len){
+static uint8_t crc8(uint8_t *p, uint8_t len) {
         uint16_t i;
         uint16_t crc = 0x0;
 
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 6a5cbcd30..fcbb54c8e 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -423,7 +423,7 @@ int ex_fixedwing_control_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 20,
 					 2048,
 					 fixedwing_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk
index a2a9eb113..f6c882ead 100644
--- a/src/examples/fixedwing_control/module.mk
+++ b/src/examples/fixedwing_control/module.mk
@@ -41,3 +41,5 @@ SRCS		= main.c \
 		  params.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wframe-larger-than=1200
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 0b8c01f79..a89ccf933 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -114,7 +114,7 @@ int flow_position_estimator_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 5,
 					 4000,
 					 flow_position_estimator_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk
index 88c9ceb93..5c6e29f8f 100644
--- a/src/examples/flow_position_estimator/module.mk
+++ b/src/examples/flow_position_estimator/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND		= flow_position_estimator
 
 SRCS		= flow_position_estimator_main.c \
 		  flow_position_estimator_params.c
+		  
+EXTRACFLAGS = -Wno-float-equal
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
index d3b10f46e..95ff346bb 100644
--- a/src/examples/hwtest/hwtest.c
+++ b/src/examples/hwtest/hwtest.c
@@ -39,13 +39,15 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <nuttx/config.h>
 #include <stdio.h>
-#include <systemlib/err.h>
+#include <string.h>
+
 #include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/actuator_controls.h>
+#include <nuttx/config.h>
+#include <systemlib/err.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/uORB.h>
 
 __EXPORT int ex_hwtest_main(int argc, char *argv[]);
 
@@ -53,7 +55,7 @@ int ex_hwtest_main(int argc, char *argv[])
 {
 	warnx("DO NOT FORGET TO STOP THE COMMANDER APP!");
 	warnx("(run <commander stop> to do so)");
-    warnx("usage: http://px4.io/dev/examples/write_output");
+	warnx("usage: http://px4.io/dev/examples/write_output");
 
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c
index c66bebeec..a95f45d1a 100644
--- a/src/examples/matlab_csv_serial/matlab_csv_serial.c
+++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c
@@ -107,7 +107,7 @@ int matlab_csv_serial_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 5,
 					 2000,
 					 matlab_csv_serial_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index 3eaf14148..45d541137 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -38,10 +38,13 @@
  * @author Example User <mail@example.com>
  */
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
 #include <nuttx/config.h>
 #include <nuttx/sched.h>
-#include <unistd.h>
-#include <stdio.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -100,7 +103,7 @@ int px4_daemon_app_main(int argc, char *argv[])
 					 SCHED_PRIORITY_DEFAULT,
 					 2000,
 					 px4_daemon_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index e17715733..1fe36d395 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -181,6 +181,11 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z)
         x = tmp;
         return;
     }
+    case ROTATION_ROLL_270_YAW_270: {
+        tmp = z; z = -y; y = tmp;
+        tmp = x; x = y; y = -tmp;
+        return;
+    }
     case ROTATION_PITCH_90: {
         tmp = z; z = -x; x = tmp;
         return;
diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c
index e8a791b8f..5c53a1602 100644
--- a/src/lib/rc/st24.c
+++ b/src/lib/rc/st24.c
@@ -72,7 +72,7 @@ const char *decode_states[] = {"UNSYNCED",
 #define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
 
 static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
-static unsigned _rxlen;
+static uint8_t _rxlen;
 
 static ReceiverFcPacket _rxpacket;
 
diff --git a/uavcan b/src/lib/uavcan
-Subproject 1efd24427539fa332a15151143466ec760fa5ff
+Subproject 1efd24427539fa332a15151143466ec760fa5ff
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 6068ab082..702894d60 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -63,6 +63,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vision_position_estimate.h>
 #include <drivers/drv_hrt.h>
 
 #include <lib/mathlib/mathlib.h>
@@ -134,7 +135,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 					      SCHED_PRIORITY_MAX - 5,
 					      7200,
 					      attitude_estimator_ekf_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
@@ -207,7 +208,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			     };		/**< init: identity matrix */
 
 	float debugOutput[4] = { 0.0f };
-
 	int overloadcounter = 19;
 
 	/* Initialize filter */
@@ -220,6 +220,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
+	gps.eph = 100000;
+	gps.epv = 100000;
 	struct vehicle_global_position_s global_pos;
 	memset(&global_pos, 0, sizeof(global_pos));
 	struct vehicle_attitude_s att;
@@ -258,9 +260,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to control mode*/
+	/* subscribe to control mode */
 	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
+	/* subscribe to vision estimate */
+	int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -268,14 +273,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 	thread_running = true;
 
-	/* advertise debug value */
-	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	// orb_advert_t pub_dbg = -1;
-
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_ekf_params ekf_params = { 0 };
+	struct attitude_estimator_ekf_params ekf_params;
+	memset(&ekf_params, 0, sizeof(ekf_params));
 
 	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
 
@@ -293,6 +295,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	math::Matrix<3, 3> R_decl;
 	R_decl.identity();
 
+	struct vision_position_estimate vision {};
+
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
 
@@ -313,8 +317,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
 			if (!control_mode.flag_system_hil_enabled) {
-				fprintf(stderr,
-					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
+				warnx("WARNING: Not getting sensors - sensor app running?");
 			}
 
 		} else {
@@ -449,9 +452,30 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
-					z_k[6] = raw.magnetometer_ga[0];
-					z_k[7] = raw.magnetometer_ga[1];
-					z_k[8] = raw.magnetometer_ga[2];
+					bool vision_updated = false;
+					orb_check(vision_sub, &vision_updated);
+
+					if (vision_updated) {
+						orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision);
+					}
+
+					if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) {
+
+						math::Quaternion q(vision.q);
+						math::Matrix<3, 3> Rvis = q.to_dcm();
+
+						math::Vector<3> v(1.0f, 0.0f, 0.4f);
+
+						math::Vector<3> vn = Rvis * v;
+
+						z_k[6] = vn(0);
+						z_k[7] = vn(1);
+						z_k[8] = vn(2);
+					} else {
+						z_k[6] = raw.magnetometer_ga[0];
+						z_k[7] = raw.magnetometer_ga[1];
+						z_k[8] = raw.magnetometer_ga[2];
+					}
 
 					uint64_t now = hrt_absolute_time();
 					unsigned int time_elapsed = now - last_run;
diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk
index 749b0a91c..1158536e1 100644
--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -42,3 +42,7 @@ SRCS		 = attitude_estimator_ekf_main.cpp \
 		   codegen/AttitudeEKF.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600
+
+EXTRACXXFLAGS = -Wframe-larger-than=2400
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
index e49027e47..9414225ca 100755
--- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
+++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp
@@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[])
 					      SCHED_PRIORITY_MAX - 5,
 					      14000,
 					      attitude_estimator_so3_thread_main,
-					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk
index f52715abb..7b2e206cc 100644
--- a/src/modules/attitude_estimator_so3/module.mk
+++ b/src/modules/attitude_estimator_so3/module.mk
@@ -8,3 +8,5 @@ SRCS		 = attitude_estimator_so3_main.cpp \
 		   attitude_estimator_so3_params.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS = -Wno-float-equal
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index e0bcbc6e9..4580b338d 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -523,6 +523,9 @@ BottleDrop::task_main()
 			}
 
 			switch (_drop_state) {
+				case DROP_STATE_INIT:
+					// do nothing
+					break;
 
 				case DROP_STATE_TARGET_VALID:
 				{
@@ -689,6 +692,10 @@ BottleDrop::task_main()
 						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
 					}
 					break;
+
+				case DROP_STATE_BAY_CLOSED:
+					// do nothing
+					break;
 			}
 
 			counter++;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index de3e69373..dc0594bf2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -81,6 +81,7 @@
 #include <uORB/topics/system_power.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
@@ -267,7 +268,7 @@ int commander_main(int argc, char *argv[])
 					     SCHED_PRIORITY_MAX - 40,
 					     3200,
 					     commander_thread_main,
-					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 
 		while (!thread_running) {
 			usleep(200);
@@ -926,6 +927,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct mission_result_s mission_result;
 	memset(&mission_result, 0, sizeof(mission_result));
 
+	/* Subscribe to geofence result topic */
+	int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
+	struct geofence_result_s geofence_result;
+	memset(&geofence_result, 0, sizeof(geofence_result));
+
 	/* Subscribe to manual control data */
 	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp_man;
@@ -1553,27 +1559,34 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
+		}
 
-			/* Check for geofence violation */
-			if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) {
-				//XXX: make this configurable to select different actions (e.g. navigation modes)
-				/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
-				armed.force_failsafe = true;
-				status_changed = true;
-				static bool flight_termination_printed = false;
-
-				if (!flight_termination_printed) {
-					warnx("Flight termination because of navigator request or geofence");
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-					flight_termination_printed = true;
-				}
+		/* start geofence result check */
+		orb_check(geofence_result_sub, &updated);
 
-				if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
-				}
-			} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+		if (updated) {
+			orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
 		}
 
+		/* Check for geofence violation */
+		if (armed.armed && (geofence_result.geofence_violated || mission_result.flight_termination)) {
+			//XXX: make this configurable to select different actions (e.g. navigation modes)
+			/* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */
+			armed.force_failsafe = true;
+			status_changed = true;
+			static bool flight_termination_printed = false;
+
+			if (!flight_termination_printed) {
+				warnx("Flight termination because of navigator request or geofence");
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				flight_termination_printed = true;
+			}
+
+			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+			}
+		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
+
 		/* RC input check */
 		if (!status.rc_input_blocked && sp_man.timestamp != 0 &&
 		    hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 27ca5c182..0e2a5356b 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -51,3 +51,6 @@ SRCS		 	= commander.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wframe-larger-than=2000
+
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index 36d854ddd..843dde978 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -42,4 +42,5 @@ SRCS		= ekf_att_pos_estimator_main.cpp \
 		  estimator_23states.cpp \
 		  estimator_utilities.cpp
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=6100
+
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index f4ea05088..71b387b1e 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
 					 control_demo_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 598408c9f..260b25a48 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -335,8 +335,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_actuators_0_pub(-1),
 	_actuators_2_pub(-1),
 
-	_rates_sp_id(ORB_ID(vehicle_rates_setpoint)),
-	_actuators_id(ORB_ID(actuator_controls_0)),
+	_rates_sp_id(0),
+	_actuators_id(0),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -588,12 +588,14 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (_vehicle_status.is_vtol) {
-			_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
-			_actuators_id = ORB_ID(actuator_controls_virtual_fw);
-		} else {
-			_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
-			_actuators_id = ORB_ID(actuator_controls_0);
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
 		}
 	}
 }
@@ -719,22 +721,24 @@ FixedwingAttitudeControl::task_main()
 				R.set(_att.R);
 				R_adapted.set(_att.R);
 
-				//move z to x
+				/* move z to x */
 				R_adapted(0, 0) = R(0, 2);
 				R_adapted(1, 0) = R(1, 2);
 				R_adapted(2, 0) = R(2, 2);
-				//move x to z
+
+				/* move x to z */
 				R_adapted(0, 2) = R(0, 0);
 				R_adapted(1, 2) = R(1, 0);
 				R_adapted(2, 2) = R(2, 0);
 
-				//change direction of pitch (convert to right handed system)
+				/* change direction of pitch (convert to right handed system) */
 				R_adapted(0, 0) = -R_adapted(0, 0);
 				R_adapted(1, 0) = -R_adapted(1, 0);
 				R_adapted(2, 0) = -R_adapted(2, 0);
 				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
 				euler_angles = R_adapted.to_euler();
-				//fill in new attitude data
+
+				/* fill in new attitude data */
 				_att.roll    = euler_angles(0);
 				_att.pitch   = euler_angles(1);
 				_att.yaw     = euler_angles(2);
@@ -748,7 +752,7 @@ FixedwingAttitudeControl::task_main()
 				_att.R[2][1] = R_adapted(2, 1);
 				_att.R[2][2] = R_adapted(2, 2);
 
-				// lastly, roll- and yawspeed have to be swaped
+				/* lastly, roll- and yawspeed have to be swaped */
 				float helper = _att.rollspeed;
 				_att.rollspeed = -_att.yawspeed;
 				_att.yawspeed = helper;
@@ -819,6 +823,7 @@ FixedwingAttitudeControl::task_main()
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
+				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
 				/* Read attitude setpoint from uorb if
@@ -879,6 +884,8 @@ FixedwingAttitudeControl::task_main()
 						+ _parameters.rollsp_offset_rad;
 					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
 						+ _parameters.pitchsp_offset_rad;
+					/* allow manual control of rudder deflection */
+					yaw_manual = _manual.r;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -978,6 +985,9 @@ FixedwingAttitudeControl::task_main()
 							_pitch_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+
+					/* add in manual rudder control */
+					_actuators.control[2] += yaw_manual;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
@@ -1017,7 +1027,7 @@ FixedwingAttitudeControl::task_main()
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude rates setpoint */
 					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
-				} else {
+				} else if (_rates_sp_id) {
 					/* advertise the attitude rates setpoint */
 					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
 				}
@@ -1042,7 +1052,7 @@ FixedwingAttitudeControl::task_main()
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
 				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-			} else {
+			} else if (_actuators_id) {
 				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
 			}
 
diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk
index 440bab2c5..98e5c0a1e 100644
--- a/src/modules/fw_pos_control_l1/module.mk
+++ b/src/modules/fw_pos_control_l1/module.mk
@@ -47,3 +47,5 @@ SRCS		= fw_pos_control_l1_main.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wno-float-equal
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index f17497aa8..4ba595a87 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req)
 
 /// @brief Copy file (with limited space)
 int
-MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
 {
 	char buff[512];
 	int src_fd = -1, dst_fd = -1;
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index bef6775a9..9693a92a9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -142,7 +142,7 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
-	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
+	int		_copy_file(const char *src_path, const char *dst_path, size_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
 	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 29b7ec7b7..f9a3681b3 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -948,7 +948,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
-				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
@@ -1412,9 +1411,13 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
 		break;
 
@@ -1638,7 +1641,7 @@ Mavlink::start(int argc, char *argv[])
 		       SCHED_PRIORITY_DEFAULT,
 		       2800,
 		       (main_t)&Mavlink::start_helper,
-		       (const char **)argv);
+		       (char * const *)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 378e3427d..0f25c969d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1486,6 +1486,7 @@ protected:
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
 			mavlink_position_target_global_int_t msg{};
 
+			msg.time_boot_ms = hrt_absolute_time()/1000;
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
 			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
 			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
@@ -1764,6 +1765,9 @@ protected:
 				case RC_INPUT_SOURCE_PX4IO_ST24:
 					msg.rssi |= (3 << 4);
 					break;
+				case RC_INPUT_SOURCE_UNKNOWN:
+					// do nothing
+					break;
 			}
 
 			if (rc.rc_lost) {
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index a3c127cdc..859d380fe 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -292,9 +292,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send(const hrt_abstime now)
 {
-	/* update interval for slow rate limiter */
-	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
-
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
 
@@ -312,6 +309,12 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 		send_mission_current(_current_seq);
 
+		if (mission_result.item_do_jump_changed) {
+			/* send a mission item again if the remaining DO_JUMPs has changed */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
+					  (uint16_t)mission_result.item_changed_index);
+		}
+
 	} else {
 		if (_slow_rate_limiter.check(now)) {
 			send_mission_current(_current_seq);
@@ -811,7 +814,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 	case NAV_CMD_DO_JUMP:
 		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
 		break;
 
 	default:
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 91fdd6154..f9d30dcbe 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -53,4 +53,6 @@ MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1024
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed
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 5918d6bc5..3b631e2ce 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* control position */
 			_pos_sp(0) = _pos_sp_triplet.current.x;
 			_pos_sp(1) = _pos_sp_triplet.current.y;
-			_pos_sp(2) = _pos_sp_triplet.current.z;
-
 		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* control velocity */
 			/* reset position setpoint to current position if needed */
@@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt)
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}
 
-		if (_control_mode.flag_control_altitude_enabled) {
+		if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) {
+			/* Control altitude */
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+		} else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* reset alt setpoint to current altitude if needed */
 			reset_alt_sp();
 
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index e789fd10d..87a6e023a 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -155,7 +155,7 @@ DataLinkLoss::set_dll_item()
 	case DLL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
@@ -188,7 +188,7 @@ DataLinkLoss::advance_dll()
 					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
@@ -209,7 +209,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
@@ -217,7 +217,7 @@ DataLinkLoss::advance_dll()
 		warnx("time is up, state should have been changed manually by now");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 0f431ded2..4482fb36b 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -279,8 +279,14 @@ Geofence::loadFromFile(const char *filename)
 		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
 
 		/* if the line starts with #, skip */
-		if (line[textStart] == commentChar)
+		if (line[textStart] == commentChar) {
 			continue;
+		}
+
+		/* if there is only a linefeed, skip it */
+		if (line[0] == '\n') {
+			continue;
+		}
 
 		if (gotVertical) {
 			/* Parse the line as a geofence point */
@@ -291,8 +297,10 @@ Geofence::loadFromFile(const char *filename)
 				/* Handle degree minute second format */
 				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
 
-				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6)
+				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
+					warnx("Scanf to parse DMS geofence vertex failed.");
 					return ERROR;
+				}
 
 //				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
 
@@ -301,9 +309,10 @@ Geofence::loadFromFile(const char *filename)
 
 			} else {
 				/* Handle decimal degree format */
-
-				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
+					warnx("Scanf to parse geofence vertex failed.");
 					return ERROR;
+				}
 			}
 
 			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp
index cd55f60b0..e370796c0 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item()
 	case GPSF_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("gps fail: request flight termination");
 	}
 	default:
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 0765e9b7c..9b0a092da 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,6 +38,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #include <sys/types.h>
@@ -149,18 +150,12 @@ Mission::on_active()
 
 	/* lets check if we reached the current mission item */
 	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		set_mission_item_reached();
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
 			advance_mission();
 			set_mission_items();
 
-		} else {
-			/* else just report that item reached */
-			if (_mission_type == MISSION_TYPE_OFFBOARD) {
-				if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) {
-					set_mission_item_reached();
-				}
-			}
 		}
 
 	} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
@@ -395,7 +390,6 @@ Mission::set_mission_items()
 		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
 
-		reset_mission_item_reached();
 		set_mission_finished();
 
 		_navigator->set_position_setpoint_triplet_updated();
@@ -636,6 +630,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
+					report_do_jump_mission_changed(*mission_index_ptr,
+								       mission_item_tmp.do_jump_repeat_count);
 				}
 				/* set new mission item index and repeat
 				* we don't have to validate here, if it's invalid, we should realize this later .*/
@@ -707,22 +703,31 @@ Mission::save_offboard_mission_state()
 }
 
 void
+Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
+{
+	/* inform about the change */
+	_navigator->get_mission_result()->item_do_jump_changed = true;
+	_navigator->get_mission_result()->item_changed_index = index;
+	_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+	_navigator->set_mission_result_updated();
+}
+
+void
 Mission::set_mission_item_reached()
 {
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 	reset_mission_item_reached();
 }
 
 void
 Mission::set_current_offboard_mission_item()
 {
-	warnx("current offboard mission index: %d", _current_offboard_mission_index);
 	_navigator->get_mission_result()->reached = false;
 	_navigator->get_mission_result()->finished = false;
 	_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 
 	save_offboard_mission_state();
 }
@@ -731,5 +736,5 @@ void
 Mission::set_mission_finished()
 {
 	_navigator->get_mission_result()->finished = true;
-	_navigator->publish_mission_result();
+	_navigator->set_mission_result_updated();
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index ea7cc0927..a8a644b0f 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -38,6 +38,7 @@
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -131,6 +132,11 @@ private:
 	void save_offboard_mission_state();
 
 	/**
+	 * Inform about a changed mission item after a DO_JUMP
+	 */
+	void report_do_jump_mission_changed(int index, int do_jumps_remaining);
+
+	/**
 	 * Set a mission item as reached
 	 */
 	void set_mission_item_reached();
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index c44d4c35e..0d7d6b9ef 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -62,3 +62,5 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION = -Os
+
+EXTRACXXFLAGS	= -Wno-sign-compare
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 9cd609955..d9d911d9c 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -54,6 +54,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/geofence_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 
 #include "navigator_mode.h"
@@ -107,9 +108,9 @@ public:
 	void		load_fence_from_file(const char *filename);
 
 	/**
-	 * Publish the mission result so commander and mavlink know what is going on
+	 * Publish the geofence result
 	 */
-	void publish_mission_result();
+	void publish_geofence_result();
 
 	/**
 	 * Publish the attitude sp, only to be used in very special modes when position control is deactivated
@@ -122,6 +123,7 @@ public:
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
+	void		set_mission_result_updated() { _mission_result_updated = true; }
 
 	/**
 	 * Getters
@@ -134,6 +136,7 @@ public:
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
+	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
 	struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; }
 
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
@@ -164,6 +167,7 @@ private:
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;
+	orb_advert_t	_geofence_result_pub;
 	orb_advert_t	_att_sp_pub;			/**< publish att sp
 							  used only in very special failsafe modes
 							  when pos control is deactivated */
@@ -179,7 +183,8 @@ private:
 	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
 
 	mission_result_s				_mission_result;
-	vehicle_attitude_setpoint_s					_att_sp;
+	geofence_result_s				_geofence_result;
+	vehicle_attitude_setpoint_s			_att_sp;
 
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
@@ -206,6 +211,7 @@ private:
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
 	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
+	bool		_mission_result_updated;		/**< flags if mission result has seen an update */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
@@ -271,6 +277,12 @@ private:
 	 */
 	void		publish_position_setpoint_triplet();
 
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void		publish_mission_result();
+
 	/* this class has ptr data members, so it should not be copied,
 	 * consequently the copy constructors are private.
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index df620e5e7..3f7670ec4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -110,6 +110,7 @@ Navigator::Navigator() :
 	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
+	_geofence_result_pub(-1),
 	_att_sp_pub(-1),
 	_vstatus{},
 	_control_mode{},
@@ -138,6 +139,7 @@ Navigator::Navigator() :
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
 	_pos_sp_triplet_published_invalid_once(false),
+	_mission_result_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -398,8 +400,8 @@ Navigator::task_main()
 			have_geofence_position_data = false;
 			if (!inside) {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = true;
-				publish_mission_result();
+				_geofence_result.geofence_violated = true;
+				publish_geofence_result();
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
@@ -408,8 +410,8 @@ Navigator::task_main()
 				}
 			} else {
 				/* inform other apps via the mission result */
-				_mission_result.geofence_violated = false;
-				publish_mission_result();
+				_geofence_result.geofence_violated = false;
+				publish_geofence_result();
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
@@ -490,6 +492,11 @@ Navigator::task_main()
 			_pos_sp_triplet_updated = false;
 		}
 
+		if (_mission_result_updated) {
+			publish_mission_result();
+			_mission_result_updated = false;
+		}
+
 		perf_end(_loop_perf);
 	}
 	warnx("exiting.");
@@ -639,6 +646,28 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
+
+	/* reset some of the flags */
+	_mission_result.seq_reached = false;
+	_mission_result.seq_current = 0;
+	_mission_result.item_do_jump_changed = false;
+	_mission_result.item_changed_index = 0;
+	_mission_result.item_do_jump_remaining = 0;
+}
+
+void
+Navigator::publish_geofence_result()
+{
+
+	/* lazily publish the geofence result only once available */
+	if (_geofence_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(geofence_result), _geofence_result_pub, &_geofence_result);
+
+	} else {
+		/* advertise and publish */
+		_geofence_result_pub = orb_advertise(ORB_ID(geofence_result), &_geofence_result);
+	}
 }
 
 void
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 3807c5ea8..2f322031c 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -65,7 +65,7 @@ NavigatorMode::run(bool active) {
 			_first_run = false;
 			/* Reset stay in failsafe flag */
 			_navigator->get_mission_result()->stay_in_failsafe = false;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			on_activation();
 
 		} else {
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 1f40e634e..5f8f8d02f 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -50,7 +50,8 @@
  * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
  *
  * @unit meters
- * @min 0.0
+ * @min 20
+ * @max 200
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
@@ -61,10 +62,11 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
  * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
- * @min 1.0
+ * @min 0.05
+ * @max 200
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f);
 
 /**
  * Set OBC mode for data link loss
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 42392e739..a7cde6325 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -128,7 +128,7 @@ RCLoss::set_rcl_item()
 	case RCL_STATE_TERMINATE: {
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		warnx("rc not recovered: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -162,7 +162,7 @@ RCLoss::advance_rcl()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
-			_navigator->publish_mission_result();
+			_navigator->set_mission_result_updated();
 			reset_mission_item_reached();
 		}
 		break;
@@ -171,7 +171,7 @@ RCLoss::advance_rcl()
 		warnx("time is up, no RC regain, terminating");
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
-		_navigator->publish_mission_result();
+		_navigator->set_mission_result_updated();
 		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index bfe6ce7e1..1568233b0 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -53,7 +53,8 @@
  * Default value of loiter radius after RTL (fixedwing only).
  *
  * @unit meters
- * @min 0.0
+ * @min 20
+ * @max 200
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
@@ -65,10 +66,10 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
  *
  * @unit meters
  * @min 0
- * @max 1
+ * @max 150
  * @group RTL
  */
-PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
 
 
 /**
@@ -78,7 +79,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
  * Land (i.e. slowly descend) from this altitude if autolanding allowed.
  *
  * @unit meters
- * @min 0
+ * @min 2
  * @max 100
  * @group RTL
  */
@@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  *
  * @unit seconds
  * @min -1
- * @max
+ * @max 300
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index 0658d3f09..45c876299 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -41,3 +41,6 @@ SRCS		 	= position_estimator_inav_main.c \
 			inertial_filter.c
 
 MODULE_STACKSIZE = 1200
+
+EXTRACFLAGS = -Wframe-larger-than=3500
+
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 0af01cba1..1962151fa 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
 					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
-					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
+					       (argv) ? (char * const *) &argv[2] : (char * const *) NULL);
 		exit(0);
 	}
 
@@ -233,8 +233,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	float eph_flow = 1.0f;
 
-	float eph_vision = 0.5f;
-	float epv_vision = 0.5f;
+	float eph_vision = 0.2f;
+	float epv_vision = 0.2f;
 
 	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
@@ -640,6 +640,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				if (updated) {
 					orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision);
 
+					static float last_vision_x = 0.0f;
+					static float last_vision_y = 0.0f;
+					static float last_vision_z = 0.0f;
+
 					/* reset position estimate on first vision update */
 					if (!vision_valid) {
 						x_est[0] = vision.x;
@@ -654,6 +658,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						}
 						
 						vision_valid = true;
+
+						last_vision_x = vision.x;
+						last_vision_y = vision.y;
+						last_vision_z = vision.z;
+
 						warnx("VISION estimate valid");
 						mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid");
 					}
@@ -663,10 +672,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					corr_vision[1][0] = vision.y - y_est[0];
 					corr_vision[2][0] = vision.z - z_est[0];
 
-					/* calculate correction for velocity */
-					corr_vision[0][1] = vision.vx - x_est[1];
-					corr_vision[1][1] = vision.vy - y_est[1];
-					corr_vision[2][1] = vision.vz - z_est[1];
+					static hrt_abstime last_vision_time = 0;
+
+					float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f;
+					last_vision_time = vision.timestamp_boot;
+
+					if (vision_dt > 0.000001f && vision_dt < 0.2f) {
+						vision.vx = (vision.x - last_vision_x) / vision_dt;
+						vision.vy = (vision.y - last_vision_y) / vision_dt;
+						vision.vz = (vision.z - last_vision_z) / vision_dt;
+
+						last_vision_x = vision.x;
+						last_vision_y = vision.y;
+						last_vision_z = vision.z;
+
+						/* calculate correction for velocity */
+						corr_vision[0][1] = vision.vx - x_est[1];
+						corr_vision[1][1] = vision.vy - y_est[1];
+						corr_vision[2][1] = vision.vz - z_est[1];
+					} else {
+						/* assume zero motion */
+						corr_vision[0][1] = 0.0f - x_est[1];
+						corr_vision[1][1] = 0.0f - y_est[1];
+						corr_vision[2][1] = 0.0f - z_est[1];
+					}
 
 				}
 			}
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 cc0fb4155..b7f6509d7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -116,7 +116,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
  * @max 10.0
  * @group Position Estimator INAV
  */
-PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f);
+PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f);
 
 /**
  * XY axis weight for vision velocity
diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index d4a00af39..f1118000e 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -45,3 +45,6 @@ SRCS = sdlog2.c \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS = -Wframe-larger-than=1200
+
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 6df677338..0a8564da6 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -304,7 +304,7 @@ int sdlog2_main(int argc, char *argv[])
 						 SCHED_PRIORITY_DEFAULT - 30,
 						 3000,
 						 sdlog2_thread_main,
-						 (const char **)argv);
+						 (char * const *)argv);
 		exit(0);
 	}
 
@@ -1089,6 +1089,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	if (_extended_logging) {
 		subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info));
+	} else {
+		subs.sat_info_sub = 0;
 	}
 
 	/* close non-needed fd's */
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index b78b430aa..5b047f538 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -468,7 +468,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
-	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
+	LOG_FORMAT(FLOW, "QBffffffLLHhB",	"IntT,ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"),
 	LOG_FORMAT(GPOS, "LLfffffff",		"Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV,TALT"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHffiffH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp
index 061fbf9b9..ee492f85a 100644
--- a/src/modules/segway/segway_main.cpp
+++ b/src/modules/segway/segway_main.cpp
@@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[])
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
 					 segway_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+					 (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index dfbba92d9..f37bc9327 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -44,3 +44,5 @@ SRCS		= sensors.cpp \
 MODULE_STACKSIZE = 1200
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS	= -Wno-type-limits
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index fe8b7e75a..f4dff2838 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -57,3 +57,5 @@ SRCS		 = err.c \
 		   mcu_version.c
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS	= -Wno-sign-compare
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index d6d8284d2..f9e90652d 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -41,7 +41,7 @@
 #include <stdio.h>
 #include <sys/queue.h>
 #include <drivers/drv_hrt.h>
-
+#include <math.h>
 #include "perf_counter.h"
 
 /**
@@ -84,7 +84,8 @@ struct perf_ctr_interval {
 	uint64_t		time_last;
 	uint64_t		time_least;
 	uint64_t		time_most;
-
+	float			mean;
+	float			M2;
 };
 
 /**
@@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name)
 
 	case PC_INTERVAL:
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+
 		break;
 
 	default:
@@ -156,15 +158,23 @@ perf_count(perf_counter_t handle)
 		case 1:
 			pci->time_least = now - pci->time_last;
 			pci->time_most = now - pci->time_last;
+			pci->mean = pci->time_least / 1e6f;
+			pci->M2 = 0;
 			break;
 		default: {
-			hrt_abstime interval = now - pci->time_last;
-			if (interval < pci->time_least)
-				pci->time_least = interval;
-			if (interval > pci->time_most)
-				pci->time_most = interval;
-			break;
-		}
+				hrt_abstime interval = now - pci->time_last;
+				if (interval < pci->time_least)
+					pci->time_least = interval;
+				if (interval > pci->time_most)
+					pci->time_most = interval;
+				// maintain mean and variance of interval in seconds
+				// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+				float dt = interval / 1e6f;
+				float delta_intvl = dt - pci->mean;
+				pci->mean += delta_intvl / pci->event_count;
+				pci->M2 += delta_intvl * (dt - pci->mean);
+				break;
+			}
 		}
 		pci->time_last = now;
 		pci->event_count++;
@@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle)
 
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		float rms = sqrtf(pci->M2 / (pci->event_count-1));
 
-		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
 		       pci->time_least,
-		       pci->time_most);
+		       pci->time_most,
+			   (double)(1000 * pci->mean),
+			   (double)(1000 * rms));
 		break;
 	}
 
@@ -365,6 +378,21 @@ perf_print_all(int fd)
 	}
 }
 
+extern const uint16_t latency_bucket_count;
+extern uint32_t latency_counters[];
+extern const uint16_t latency_buckets[];
+
+void
+perf_print_latency(int fd)
+{
+	dprintf(fd, "bucket : events\n");
+	for (int i = 0; i < latency_bucket_count; i++) {
+		printf("  %4i : %i\n", latency_buckets[i], latency_counters[i]);
+	}
+	// print the overflow bucket value
+	dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]);
+}
+
 void
 perf_reset_all(void)
 {
@@ -374,4 +402,7 @@ perf_reset_all(void)
 		perf_reset(handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
+	for (int i = 0; i <= latency_bucket_count; i++) {
+		latency_counters[i] = 0;
+	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 668d9dfdf..d06606a5d 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -94,7 +94,7 @@ __EXPORT extern void		perf_begin(perf_counter_t handle);
  * End a performance event.
  *
  * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
- * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * If a call is made without a corresponding perf_begin call, or if perf_cancel
  * has been called subsequently, no change is made to the counter.
  *
  * @param handle		The handle returned from perf_alloc.
@@ -143,6 +143,13 @@ __EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
 __EXPORT extern void		perf_print_all(int fd);
 
 /**
+ * Print hrt latency counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ */
+__EXPORT extern void		perf_print_latency(int fd);
+
+/**
  * Reset all of the performance counters.
  */
 __EXPORT extern void		perf_reset_all(void);
diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c
index 702e435ac..a0988035c 100644
--- a/src/modules/systemlib/system_params.c
+++ b/src/modules/systemlib/system_params.c
@@ -82,3 +82,17 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 * @group System
 */
 PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
+
+/**
+* Companion computer interface
+*
+* Configures the baud rate of the companion computer interface.
+* Set to zero to disable, set to 921600 to enable.
+* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
+* other baud rates.
+*
+* @min 0
+* @max 921600
+* @group System
+*/
+PARAM_DEFINE_INT32(SYS_COMPANION, 0);
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 90d8dd77c..82183b0d7 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[])
 {
 	int pid;
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 6e22a557e..2f24215a9 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -64,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name,
 			int scheduler,
 			int stack_size,
 			main_t entry,
-			const char *argv[]);
+			char * const argv[]);
 
 enum MULT_PORTS {
 	MULT_0_US2_RXTX = 0,
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 1141431cc..3d5755a46 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -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
@@ -148,6 +148,9 @@ ORB_DEFINE(onboard_mission, struct mission_s);
 #include "topics/mission_result.h"
 ORB_DEFINE(mission_result, struct mission_result_s);
 
+#include "topics/geofence_result.h"
+ORB_DEFINE(geofence_result, struct geofence_result_s);
+
 #include "topics/fence.h"
 ORB_DEFINE(fence, unsigned);
 
diff --git a/src/modules/uORB/topics/geofence_result.h b/src/modules/uORB/topics/geofence_result.h
new file mode 100644
index 000000000..b07e04499
--- /dev/null
+++ b/src/modules/uORB/topics/geofence_result.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   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 geofence_result.h
+ * Status of the plance concerning the geofence
+ *
+ * @author Ban Siesta <bansiesta@gmail.com>
+ */
+
+#ifndef TOPIC_GEOFENCE_RESULT_H
+#define TOPIC_GEOFENCE_RESULT_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct geofence_result_s
+{
+	bool geofence_violated;		/**< true if the geofence is violated */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(geofence_result);
+
+#endif
diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h
index c7d25d1f0..2ddc529a3 100644
--- a/src/modules/uORB/topics/mission_result.h
+++ b/src/modules/uORB/topics/mission_result.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 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>
+ *   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
@@ -37,6 +34,11 @@
 /**
  * @file mission_result.h
  * Mission results that navigator needs to pass on to commander and mavlink.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Ban Siesta <bansiesta@gmail.com>
  */
 
 #ifndef TOPIC_MISSION_RESULT_H
@@ -58,8 +60,10 @@ struct mission_result_s
 	bool reached;			/**< true if mission has been reached					 */
 	bool finished;			/**< true if mission has been completed					 */
 	bool stay_in_failsafe;		/**< true if the commander should not switch out of the failsafe mode*/
-	bool geofence_violated;		/**< true if the geofence is violated */
 	bool flight_termination;	/**< true if the navigator demands a flight termination from the commander app */
+	bool item_do_jump_changed;	/**< true if the number of do jumps remaining has changed */
+	unsigned item_changed_index;	/**< indicate which item has changed */
+	unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
 };
 
 /**
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index 82ff46ad2..beb23f61d 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -68,6 +68,33 @@ typedef const struct orb_metadata *orb_id_t;
 #define ORB_ID(_name)		&__orb_##_name
 
 /**
+ * Generates a pointer to the uORB metadata structure for
+ * a given topic.
+ *
+ * The topic must have been declared previously in scope
+ * with ORB_DECLARE().
+ *
+ * @param _name		The name of the topic.
+ * @param _count	The class ID of the topic
+ */
+#define ORB_ID_DOUBLE(_name, _count)	((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1)
+
+/**
+ * Generates a pointer to the uORB metadata structure for
+ * a given topic.
+ *
+ * The topic must have been declared previously in scope
+ * with ORB_DECLARE().
+ *
+ * @param _name		The name of the topic.
+ * @param _count	The class ID of the topic
+ */
+#define ORB_ID_TRIPLE(_name, _count)					\
+ 	((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \
+			((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \
+				(((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0))))
+
+/**
  * Declare (prototype) the uORB metadata for a topic.
  *
  * Note that optional topics are declared weak; this allows a potential
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
index f92bc754f..e5d30f6c4 100644
--- a/src/modules/uavcan/module.mk
+++ b/src/modules/uavcan/module.mk
@@ -57,7 +57,7 @@ SRCS += sensors/sensor_bridge.cpp   \
 #
 # libuavcan
 #
-include $(UAVCAN_DIR)/libuavcan/include.mk
+include $(PX4_LIB_DIR)/uavcan/libuavcan/include.mk
 SRCS += $(LIBUAVCAN_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
@@ -67,7 +67,7 @@ override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAV
 #
 # libuavcan drivers for STM32
 #
-include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+include $(PX4_LIB_DIR)/uavcan/libuavcan_drivers/stm32/driver/include.mk
 SRCS += $(LIBUAVCAN_STM32_SRC)
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk
index 0d5155e90..0cf3072c8 100644
--- a/src/modules/vtol_att_control/module.mk
+++ b/src/modules/vtol_att_control/module.mk
@@ -39,3 +39,6 @@ MODULE_COMMAND	= vtol_att_control
 
 SRCS		= vtol_att_control_main.cpp \
 			  vtol_att_control_params.c
+
+EXTRACXXFLAGS = -Wno-write-strings
+
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 958907d1b..9a80562f3 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -74,11 +74,8 @@
 #include <lib/geo/geo.h>
 
 #include "drivers/drv_pwm_output.h"
-#include <nuttx/fs/nxffs.h>
 #include <nuttx/fs/ioctl.h>
 
-#include <nuttx/mtd.h>
-
 #include <fcntl.h>
 
 
diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c
index 0569d89f5..ec9269d39 100644
--- a/src/systemcmds/bl_update/bl_update.c
+++ b/src/systemcmds/bl_update/bl_update.c
@@ -52,6 +52,8 @@
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
 
+#define BL_FILE_SIZE_LIMIT	16384
+
 __EXPORT int bl_update_main(int argc, char *argv[]);
 
 static void setopt(void);
@@ -72,12 +74,12 @@ bl_update_main(int argc, char *argv[])
 
 	struct stat s;
 
-	if (stat(argv[1], &s) < 0)
+	if (!stat(argv[1], &s))
 		err(1, "stat %s", argv[1]);
 
 	/* sanity-check file size */
-	if (s.st_size > 16384)
-		errx(1, "%s: file too large", argv[1]);
+	if (s.st_size > BL_FILE_SIZE_LIMIT)
+		errx(1, "%s: file too large (limit: %u, actual: %d)", argv[1], BL_FILE_SIZE_LIMIT, s.st_size);
 
 	uint8_t *buf = malloc(s.st_size);
 
diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk
index cdbff75f0..0fb899c67 100644
--- a/src/systemcmds/mixer/module.mk
+++ b/src/systemcmds/mixer/module.mk
@@ -41,3 +41,6 @@ SRCS		 = mixer.cpp
 MODULE_STACKSIZE = 4096
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACXXFLAGS = -Wframe-larger-than=2048
+
diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk
index 1bc4f414e..bca1cdcc1 100644
--- a/src/systemcmds/mtd/module.mk
+++ b/src/systemcmds/mtd/module.mk
@@ -6,3 +6,6 @@ MODULE_COMMAND	 = mtd
 SRCS		 = mtd.c 24xxxx_mtd.c
 
 MAXOPTIMIZATION	 = -Os
+
+EXTRACFLAGS	= -Wno-error
+
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index e110335e7..80ee204e8 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -212,7 +212,7 @@ static void
 do_show(const char* search_string)
 {
 	printf(" + = saved, * = unsaved\n");
-	param_foreach(do_show_print, search_string, false);
+	param_foreach(do_show_print, (char *)search_string, false);
 
 	exit(0);
 }
diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c
index b08a2e3b7..a788dfc11 100644
--- a/src/systemcmds/perf/perf.c
+++ b/src/systemcmds/perf/perf.c
@@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[])
 		if (strcmp(argv[1], "reset") == 0) {
 			perf_reset_all();
 			return 0;
+		} else if (strcmp(argv[1], "latency") == 0) {
+			perf_print_latency(0 /* stdout */);
+			fflush(stdout);
+			return 0;
 		}
-		printf("Usage: perf <reset>\n");
+		printf("Usage: perf [reset | latency]\n");
 		return -1;
 	}
 
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 622a0faf3..6eed3922c 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -34,3 +34,6 @@ SRCS			 = test_adc.c \
 			   test_conv.cpp \
 			   test_mount.c \
 			   test_mtd.c
+
+EXTRACXXFLAGS = -Wframe-larger-than=2500
+
diff --git a/unittests/.gitignore b/unittests/.gitignore
index 37e923b5e..aa316941f 100644
--- a/unittests/.gitignore
+++ b/unittests/.gitignore
@@ -1,6 +1,8 @@
 ./obj/*
+gtest_main.a
 mixer_test
 sf0x_test
 sbus2_test
 autodeclination_test
 st24_test
+sample_unittest
diff --git a/unittests/Makefile b/unittests/Makefile
index dfd5ccb3f..e55411a75 100644
--- a/unittests/Makefile
+++ b/unittests/Makefile
@@ -3,7 +3,44 @@ CC=g++
 CFLAGS=-I. -I../src/modules -I ../src/include -I../src/drivers \
 	-I../src -I../src/lib -D__EXPORT="" -Dnullptr="0" -lm
 
-all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test
+# Points to the root of Google Test, relative to where this file is.
+# Remember to tweak this if you move this file.
+GTEST_DIR = gtest
+
+# Flags passed to the preprocessor.
+# Set Google Test's header directory as a system directory, such that
+# the compiler doesn't generate warnings in Google Test headers.
+CFLAGS += -isystem $(GTEST_DIR)/include
+
+# All Google Test headers.  Usually you shouldn't change this
+# definition.
+GTEST_HEADERS = $(GTEST_DIR)/include/gtest/*.h \
+                $(GTEST_DIR)/include/gtest/internal/*.h
+
+# Usually you shouldn't tweak such internal variables, indicated by a
+# trailing _.
+GTEST_SRCS_ = $(GTEST_DIR)/src/*.cc $(GTEST_DIR)/src/*.h $(GTEST_HEADERS)
+
+# For simplicity and to avoid depending on Google Test's
+# implementation details, the dependencies specified below are
+# conservative and not optimized.  This is fine as Google Test
+# compiles fast and for ordinary users its source rarely changes.
+gtest-all.o: $(GTEST_SRCS_)
+	$(CC) $(CFLAGS) -I$(GTEST_DIR) -c \
+            $(GTEST_DIR)/src/gtest-all.cc
+
+gtest_main.o: $(GTEST_SRCS_)
+	$(CC) $(CFLAGS) -I$(GTEST_DIR) -c \
+            $(GTEST_DIR)/src/gtest_main.cc
+
+gtest.a: gtest-all.o
+	$(AR) $(ARFLAGS) $@ $^
+
+gtest_main.a: gtest-all.o gtest_main.o
+	$(AR) $(ARFLAGS) $@ $^
+
+
+all: sample_unittest mixer_test sbus2_test autodeclination_test st24_test sf0x_test
 
 MIXER_FILES=../src/systemcmds/tests/test_mixer.cpp \
 		../src/systemcmds/tests/test_conv.cpp \
@@ -33,6 +70,20 @@ AUTODECLINATION_FILES= ../src/lib/geo_lookup/geo_mag_declination.c \
 		hrt.cpp \
 		autodeclination_test.cpp
 
+# Builds a sample test. A test should link with either gtest.a or
+# gtest_main.a, depending on whether it defines its own main()
+# function.
+sample.o: sample.cc sample.h $(GTEST_HEADERS)
+	$(CC) $(CFLAGS) -c sample.cc
+
+sample_unittest.o: sample_unittest.cc \
+                     sample.h $(GTEST_HEADERS)
+	$(CC) $(CFLAGS) -c sample_unittest.cc
+
+sample_unittest: sample.o sample_unittest.o gtest_main.a
+	$(CC) $(CFLAGS) $^ -o $@
+
+
 mixer_test: $(MIXER_FILES)
 	$(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS)
 
@@ -57,4 +108,4 @@ unittests: clean mixer_test sbus2_test sf0x_test autodeclination_test st24_test
 
 .PHONY: clean
 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test
+	rm -f gtest.a gtest_main.a *.o $(ODIR)/*.o *~ core $(INCDIR)/*~ sample_unittest mixer_test sbus2_test autodeclination_test st24_test sf0x_test
diff --git a/unittests/debug.h b/unittests/debug.h
index 9824d13fc..ac321312f 100644
--- a/unittests/debug.h
+++ b/unittests/debug.h
@@ -2,4 +2,5 @@
 #pragma once
 
 #include <systemlib/err.h>
-#define lowsyslog warnx
\ No newline at end of file
+#define lowsyslog warnx
+#define dbg warnx
diff --git a/unittests/gtest b/unittests/gtest
new file mode 160000
+Subproject cdef6e4ce097f953445446e8da4cb8bb68478bc
diff --git a/unittests/sample.cc b/unittests/sample.cc
new file mode 100644
index 000000000..3f0c838f2
--- /dev/null
+++ b/unittests/sample.cc
@@ -0,0 +1,68 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of Google Inc. 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.
+
+// A sample program demonstrating using Google C++ testing framework.
+//
+// Author: wan@google.com (Zhanyong Wan)
+
+#include "sample.h"
+
+// Returns n! (the factorial of n).  For negative n, n! is defined to be 1.
+int Factorial(int n) {
+  int result = 1;
+  for (int i = 1; i <= n; i++) {
+    result *= i;
+  }
+
+  return result;
+}
+
+// Returns true iff n is a prime number.
+bool IsPrime(int n) {
+  // Trivial case 1: small numbers
+  if (n <= 1) return false;
+
+  // Trivial case 2: even numbers
+  if (n % 2 == 0) return n == 2;
+
+  // Now, we have that n is odd and n >= 3.
+
+  // Try to divide n by every odd number i, starting from 3
+  for (int i = 3; ; i += 2) {
+    // We only have to try i up to the squre root of n
+    if (i > n/i) break;
+
+    // Now, we have i <= n/i < n.
+    // If n is divisible by i, n is not prime.
+    if (n % i == 0) return false;
+  }
+
+  // n has no integer factor in the range (1, n), and thus is prime.
+  return true;
+}
diff --git a/unittests/sample.h b/unittests/sample.h
new file mode 100644
index 000000000..3dfeb98c4
--- /dev/null
+++ b/unittests/sample.h
@@ -0,0 +1,43 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of Google Inc. 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.
+
+// A sample program demonstrating using Google C++ testing framework.
+//
+// Author: wan@google.com (Zhanyong Wan)
+
+#ifndef GTEST_SAMPLES_SAMPLE1_H_
+#define GTEST_SAMPLES_SAMPLE1_H_
+
+// Returns n! (the factorial of n).  For negative n, n! is defined to be 1.
+int Factorial(int n);
+
+// Returns true iff n is a prime number.
+bool IsPrime(int n);
+
+#endif  // GTEST_SAMPLES_SAMPLE1_H_
diff --git a/unittests/sample_unittest.cc b/unittests/sample_unittest.cc
new file mode 100644
index 000000000..8dc3f5c38
--- /dev/null
+++ b/unittests/sample_unittest.cc
@@ -0,0 +1,153 @@
+// Copyright 2005, Google Inc.
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+//     * Redistributions of source code must retain the above copyright
+// notice, this list of conditions and the following disclaimer.
+//     * 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.
+//     * Neither the name of Google Inc. 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.
+
+// A sample program demonstrating using Google C++ testing framework.
+//
+// Author: wan@google.com (Zhanyong Wan)
+
+
+// This sample shows how to write a simple unit test for a function,
+// using Google C++ testing framework.
+//
+// Writing a unit test using Google C++ testing framework is easy as 1-2-3:
+
+
+// Step 1. Include necessary header files such that the stuff your
+// test logic needs is declared.
+//
+// Don't forget gtest.h, which declares the testing framework.
+
+#include <limits.h>
+#include "sample.h"
+#include "gtest/gtest.h"
+
+
+// Step 2. Use the TEST macro to define your tests.
+//
+// TEST has two parameters: the test case name and the test name.
+// After using the macro, you should define your test logic between a
+// pair of braces.  You can use a bunch of macros to indicate the
+// success or failure of a test.  EXPECT_TRUE and EXPECT_EQ are
+// examples of such macros.  For a complete list, see gtest.h.
+//
+// <TechnicalDetails>
+//
+// In Google Test, tests are grouped into test cases.  This is how we
+// keep test code organized.  You should put logically related tests
+// into the same test case.
+//
+// The test case name and the test name should both be valid C++
+// identifiers.  And you should not use underscore (_) in the names.
+//
+// Google Test guarantees that each test you define is run exactly
+// once, but it makes no guarantee on the order the tests are
+// executed.  Therefore, you should write your tests in such a way
+// that their results don't depend on their order.
+//
+// </TechnicalDetails>
+
+
+// Tests Factorial().
+
+// Tests factorial of negative numbers.
+TEST(FactorialTest, Negative) {
+  // This test is named "Negative", and belongs to the "FactorialTest"
+  // test case.
+  EXPECT_EQ(1, Factorial(-5));
+  EXPECT_EQ(1, Factorial(-1));
+  EXPECT_GT(Factorial(-10), 0);
+
+  // <TechnicalDetails>
+  //
+  // EXPECT_EQ(expected, actual) is the same as
+  //
+  //   EXPECT_TRUE((expected) == (actual))
+  //
+  // except that it will print both the expected value and the actual
+  // value when the assertion fails.  This is very helpful for
+  // debugging.  Therefore in this case EXPECT_EQ is preferred.
+  //
+  // On the other hand, EXPECT_TRUE accepts any Boolean expression,
+  // and is thus more general.
+  //
+  // </TechnicalDetails>
+}
+
+// Tests factorial of 0.
+TEST(FactorialTest, Zero) {
+  EXPECT_EQ(1, Factorial(0));
+}
+
+// Tests factorial of positive numbers.
+TEST(FactorialTest, Positive) {
+  EXPECT_EQ(1, Factorial(1));
+  EXPECT_EQ(2, Factorial(2));
+  EXPECT_EQ(6, Factorial(3));
+  EXPECT_EQ(40320, Factorial(8));
+}
+
+
+// Tests IsPrime()
+
+// Tests negative input.
+TEST(IsPrimeTest, Negative) {
+  // This test belongs to the IsPrimeTest test case.
+
+  EXPECT_FALSE(IsPrime(-1));
+  EXPECT_FALSE(IsPrime(-2));
+  EXPECT_FALSE(IsPrime(INT_MIN));
+}
+
+// Tests some trivial cases.
+TEST(IsPrimeTest, Trivial) {
+  EXPECT_FALSE(IsPrime(0));
+  EXPECT_FALSE(IsPrime(1));
+  EXPECT_TRUE(IsPrime(2));
+  EXPECT_TRUE(IsPrime(3));
+}
+
+// Tests positive input.
+TEST(IsPrimeTest, Positive) {
+  EXPECT_FALSE(IsPrime(4));
+  EXPECT_TRUE(IsPrime(5));
+  EXPECT_FALSE(IsPrime(6));
+  EXPECT_TRUE(IsPrime(23));
+}
+
+// Step 3. Call RUN_ALL_TESTS() in main().
+//
+// We do this by linking in src/gtest_main.cc file, which consists of
+// a main() function which calls RUN_ALL_TESTS() for us.
+//
+// This runs all the tests you've defined, prints the result, and
+// returns 0 if successful, or 1 otherwise.
+//
+// Did you notice that we didn't register the tests?  The
+// RUN_ALL_TESTS() macro magically knows about all the tests we
+// defined.  Isn't this convenient?