Merge branch 'master' into px4io_fix

40 files changed, 811 insertions, 180 deletions

diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp
index f0044d36f..1590cc182 100644
--- a/src/drivers/bma180/bma180.cpp
+++ b/src/drivers/bma180/bma180.cpp
@@ -690,6 +690,7 @@ BMA180::measure()
 	 * measurement flow without using the external interrupt.
 	 */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = 0;
 	/*
 	 * y of board is x of sensor and x of board is -y of sensor
 	 * perform only the axis assignment here.
diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 794de584b..eff5e7349 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -52,6 +52,7 @@
  */
 struct accel_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< acceleration in the NED X board axis in m/s^2 */
 	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
 	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index 176f798c0..aa251889f 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -55,6 +55,7 @@ struct baro_report {
 	float altitude;
 	float temperature;
 	uint64_t timestamp;
+	uint64_t error_count;
 };
 
 /*
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index 1d0fef6fc..fefcae50b 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -52,6 +52,7 @@
  */
 struct gyro_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;		/**< angular velocity in the NED X board axis in rad/s */
 	float y;		/**< angular velocity in the NED Y board axis in rad/s */
 	float z;		/**< angular velocity in the NED Z board axis in rad/s */
diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 3de5625fd..06107bd3d 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -54,6 +54,7 @@
  */
 struct mag_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float x;
 	float y;
 	float z;
diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h
index 03a82ec5d..cf91f7030 100644
--- a/src/drivers/drv_range_finder.h
+++ b/src/drivers/drv_range_finder.h
@@ -52,6 +52,7 @@
  */
 struct range_finder_report {
 	uint64_t timestamp;
+	uint64_t error_count;
 	float distance; 			/** in meters */
 	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
 };
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index dd8436b10..084671ae2 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -153,35 +153,36 @@ ETSAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 2);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
 		return ret;
 	}
 
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
         if (diff_pres_pa == 0) {
-            // a zero value means the pressure sensor cannot give us a
-            // value. We need to return, and not report a value or the
-            // caller could end up using this value as part of an
-            // average
-            log("zero value from sensor"); 
-            return -1;
+		// a zero value means the pressure sensor cannot give us a
+		// value. We need to return, and not report a value or the
+		// caller could end up using this value as part of an
+		// average
+		perf_count(_comms_errors);
+		log("zero value from sensor"); 
+		return -1;
         }
 
 	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
-
 	} else {
 		diff_pres_pa -= _diff_pres_offset;
 	}
 
 	// Track maximum differential pressure measured (so we can work out top speed).
 	if (diff_pres_pa > _max_differential_pressure_pa) {
-	    _max_differential_pressure_pa = diff_pres_pa;
+		_max_differential_pressure_pa = diff_pres_pa;
 	}
 
 	// XXX we may want to smooth out the readings to remove noise.
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 	report.differential_pressure_pa = diff_pres_pa;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
@@ -209,7 +210,7 @@ ETSAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			perf_count(_comms_errors);
 			/* restart the measurement state machine */
 			start();
 			return;
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index 5e891d7bb..5f0ce4ff8 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -791,6 +791,7 @@ HMC5883::collect()
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	new_report.timestamp = hrt_absolute_time();
+        new_report.error_count = perf_event_count(_comms_errors);
 
 	/*
 	 * @note  We could read the status register here, which could tell us that
diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 748809d3f..8f5674823 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -771,6 +771,7 @@ L3GD20::measure()
 	 *		  74 from all measurements centers them around zero.
 	 */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = 0; // not recorded
 	
 	switch (_orientation) {
 
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 2c56b6035..8bed8a8df 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1248,6 +1248,7 @@ LSM303D::measure()
 
 
 	accel_report.timestamp = hrt_absolute_time();
+        accel_report.error_count = 0; // not reported
 
 	accel_report.x_raw = raw_accel_report.x;
 	accel_report.y_raw = raw_accel_report.y;
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index ccc5bc15e..c910e2890 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -490,6 +490,7 @@ MB12XX::collect()
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
 	
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 03d7bbfb9..ee45d46ac 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -138,7 +138,6 @@ MEASAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
 		return ret;
 	}
 
@@ -161,7 +160,6 @@ MEASAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
                 perf_count(_comms_errors);
                 perf_end(_sample_perf);
 		return ret;
@@ -207,6 +205,7 @@ MEASAirspeed::collect()
 	}
 
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
 	report.voltage = 0;
@@ -235,7 +234,6 @@ MEASAirspeed::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 14a3571de..70359110e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1186,7 +1186,7 @@ MPU6000::measure()
 	 * Adjust and scale results to m/s^2.
 	 */
 	grb.timestamp = arb.timestamp = hrt_absolute_time();
-
+        grb.error_count = arb.error_count = 0; // not reported
 
 	/*
 	 * 1) Scale raw value to SI units using scaling from datasheet.
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 1c8a4d776..938786d3f 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -573,6 +573,7 @@ MS5611::collect()
 	struct baro_report report;
 	/* this should be fairly close to the end of the conversion, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
+        report.error_count = perf_event_count(_comms_errors);
 
 	/* read the most recent measurement - read offset/size are hardcoded in the interface */
 	ret = _interface->read(0, (void *)&raw, 0);
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index f3fa3ed29..9c5e4f9dd 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -80,6 +80,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/servorail_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <debug.h>
 
@@ -197,6 +198,11 @@ public:
 	int 			set_idle_values(const uint16_t *vals, unsigned len);
 
 	/**
+	 * Disable RC input handling
+	 */
+	int			disable_rc_handling();
+
+	/**
 	 * Print IO status.
 	 *
 	 * Print all relevant IO status information
@@ -204,9 +210,9 @@ public:
 	void			print_status();
 
 	/**
-	 * Disable RC input handling
+	 * Fetch and print debug console output.
 	 */
-	int			disable_rc_handling();
+	int			print_debug();
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	/**
@@ -268,6 +274,7 @@ private:
 	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
 	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
 	orb_advert_t		_to_battery;		///< battery status / voltage
+	orb_advert_t		_to_servorail;		///< servorail status
 	orb_advert_t		_to_safety;		///< status of safety
 
 	actuator_outputs_s	_outputs;		///<mixed outputs
@@ -426,8 +433,27 @@ private:
 	 */
 	void			dsm_bind_ioctl(int dsmMode);
 
-};
+	/**
+	 * Handle a battery update from IO.
+	 *
+	 * Publish IO battery information if necessary.
+	 *
+	 * @param vbatt		vbatt register
+	 * @param ibatt		ibatt register
+	 */
+	void			io_handle_battery(uint16_t vbatt, uint16_t ibatt);
+
+	/**
+	 * Handle a servorail update from IO.
+	 *
+	 * Publish servo rail information if necessary.
+	 *
+	 * @param vservo	vservo register
+	 * @param vrssi 	vrssi register
+	 */
+	void			io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
 
+};
 
 namespace
 {
@@ -463,6 +489,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_to_actuators_effective(0),
 	_to_outputs(0),
 	_to_battery(0),
+	_to_servorail(0),
 	_to_safety(0),
 	_primary_pwm_device(false),
 	_battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor
@@ -1232,10 +1259,70 @@ PX4IO::io_handle_alarms(uint16_t alarms)
 	return 0;
 }
 
+void
+PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
+{
+	/* only publish if battery has a valid minimum voltage */
+	if (vbatt <= 3300) {
+		return;
+	}
+
+	battery_status_s	battery_status;
+	battery_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	battery_status.voltage_v = vbatt / 1000.0f;
+
+	/*
+	  ibatt contains the raw ADC count, as 12 bit ADC
+	  value, with full range being 3.3v
+	*/
+	battery_status.current_a = ibatt * (3.3f / 4096.0f) * _battery_amp_per_volt;
+	battery_status.current_a += _battery_amp_bias;
+
+	/*
+	  integrate battery over time to get total mAh used
+	*/
+	if (_battery_last_timestamp != 0) {
+		_battery_mamphour_total += battery_status.current_a *
+					   (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
+	}
+
+	battery_status.discharged_mah = _battery_mamphour_total;
+	_battery_last_timestamp = battery_status.timestamp;
+
+	/* lazily publish the battery voltage */
+	if (_to_battery > 0) {
+		orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+
+	} else {
+		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+	}
+}
+
+void
+PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
+{
+	servorail_status_s servorail_status;
+	servorail_status.timestamp = hrt_absolute_time();
+
+	/* voltage is scaled to mV */
+	servorail_status.voltage_v = vservo * 0.001f;
+	servorail_status.rssi_v    = vrssi * 0.001f;
+
+	/* lazily publish the servorail voltages */
+	if (_to_servorail > 0) {
+		orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
+
+	} else {
+		_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
+	}
+}
+
 int
 PX4IO::io_get_status()
 {
-	uint16_t	regs[4];
+	uint16_t	regs[6];
 	int		ret;
 
 	/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
@@ -1247,41 +1334,13 @@ PX4IO::io_get_status()
 	io_handle_status(regs[0]);
 	io_handle_alarms(regs[1]);
 
-	/* only publish if battery has a valid minimum voltage */
-	if (regs[2] > 3300) {
-		battery_status_s	battery_status;
-
-		battery_status.timestamp = hrt_absolute_time();
-
-		/* voltage is scaled to mV */
-		battery_status.voltage_v = regs[2] / 1000.0f;
-
-		/*
-		  regs[3] contains the raw ADC count, as 12 bit ADC
-		  value, with full range being 3.3v
-		 */
-		battery_status.current_a = regs[3] * (3.3f / 4096.0f) * _battery_amp_per_volt;
-		battery_status.current_a += _battery_amp_bias;
-
-		/*
-		  integrate battery over time to get total mAh used
-		 */
-		if (_battery_last_timestamp != 0) {
-			_battery_mamphour_total += battery_status.current_a *
-						   (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
-		}
-
-		battery_status.discharged_mah = _battery_mamphour_total;
-		_battery_last_timestamp = battery_status.timestamp;
-
-		/* lazily publish the battery voltage */
-		if (_to_battery > 0) {
-			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+	io_handle_battery(regs[2], regs[3]);
+#endif
 
-		} else {
-			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
-		}
-	}
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	io_handle_vservo(regs[4], regs[5]);
+#endif
 
 	return ret;
 }
@@ -1528,8 +1587,54 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
 }
 
 int
+PX4IO::print_debug()
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+	int io_fd = -1;
+
+	if (io_fd < 0) {
+		io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
+	}
+
+	/* read IO's output */
+	if (io_fd > 0) {
+		pollfd fds[1];
+		fds[0].fd = io_fd;
+		fds[0].events = POLLIN;
+
+		usleep(500);
+		int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0);
+
+		if (pret > 0) {
+			int count;
+			char buf[65];
+
+			do {
+				count = ::read(io_fd, buf, sizeof(buf) - 1);
+
+				if (count > 0) {
+					/* enforce null termination */
+					buf[count] = '\0';
+					warnx("IO CONSOLE: %s", buf);
+				}
+
+			} while (count > 0);
+		}
+
+		::close(io_fd);
+		return 0;
+	}
+
+#endif
+	return 1;
+
+}
+
+int
 PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 {
+	/* get debug level */
+	int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG);
 
 	uint8_t	frame[_max_transfer];
 
@@ -1551,6 +1656,9 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 				memcpy(&msg->text[0], buf, count);
 				buf += count;
 				buflen -= count;
+
+			} else {
+				continue;
 			}
 
 			/*
@@ -1569,6 +1677,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 
 			int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
 
+			/* print mixer chunk */
+			if (debuglevel > 5 || ret) {
+
+				warnx("fmu sent: \"%s\"", msg->text);
+
+				/* read IO's output */
+				print_debug();
+			}
+
 			if (ret) {
 				log("mixer send error %d", ret);
 				return ret;
@@ -1578,6 +1695,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
 
 		} while (buflen > 0);
 
+		/* ensure a closing newline */
+		msg->text[0] = '\n';
+		msg->text[1] = '\0';
+
+		int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
+
 		retries--;
 
 		log("mixer sent");
@@ -2307,28 +2430,38 @@ test(void)
 void
 monitor(void)
 {
+	/* clear screen */
+	printf("\033[2J");
+
 	unsigned cancels = 3;
-	printf("Hit <enter> three times to exit monitor mode\n");
 
 	for (;;) {
 		pollfd fds[1];
 
 		fds[0].fd = 0;
 		fds[0].events = POLLIN;
-		poll(fds, 1, 500);
+		poll(fds, 1, 2000);
 
 		if (fds[0].revents == POLLIN) {
 			int c;
 			read(0, &c, 1);
 
-			if (cancels-- == 0)
+			if (cancels-- == 0) {
+				printf("\033[H"); /* move cursor home and clear screen */
 				exit(0);
+			}
 		}
 
-#warning implement this
+		if (g_dev != nullptr) {
+
+			printf("\033[H"); /* move cursor home and clear screen */
+			(void)g_dev->print_status();
+			(void)g_dev->print_debug();
+			printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
 
-//		if (g_dev != nullptr)
-//			g_dev->dump_one = true;
+		} else {
+			errx(1, "driver not loaded, exiting");
+		}
 	}
 }
 
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
new file mode 100644
index 000000000..06337be32
--- /dev/null
+++ b/src/examples/hwtest/hwtest.c
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author:    Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file hwtest.c
+ *
+ * Simple functional hardware test.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/actuator_controls.h>
+
+__EXPORT int ex_hwtest_main(int argc, char *argv[]);
+
+int ex_hwtest_main(int argc, char *argv[])
+{
+    struct actuator_controls_s actuators;
+    memset(&actuators, 0, sizeof(actuators));
+    orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
+
+    int i;
+    float rcvalue = -1.0f;
+    hrt_abstime stime;
+
+    while (true) {
+        stime = hrt_absolute_time();
+        while (hrt_absolute_time() - stime < 1000000) {
+            for (i=0; i<8; i++)
+                actuators.control[i] = rcvalue;
+            actuators.timestamp = hrt_absolute_time(); 
+            orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
+        }
+        warnx("servos set to %.1f", rcvalue);
+        rcvalue *= -1.0f;
+    }
+
+    return OK;
+}
diff --git a/src/examples/hwtest/module.mk b/src/examples/hwtest/module.mk
new file mode 100644
index 000000000..6e70863ed
--- /dev/null
+++ b/src/examples/hwtest/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Hardware test example application
+#
+
+MODULE_COMMAND	= ex_hwtest
+
+SRCS		= hwtest.c
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 87eb99f16..c5c0c7a3c 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
 float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 {
 	/* following [2], switching on L1 distance */
-	return math::min(wp_radius, _L1_distance);
+	return math::max(wp_radius, _L1_distance);
 }
 
 bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
 void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
 				       const math::Vector2f &ground_speed_vector)
 {
+
 	/* this follows the logic presented in [1] */
 
 	float eta;
@@ -128,7 +129,11 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float distance_A_to_airplane = vector_A_to_airplane.length();
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
-	/* extension from [2] */
+	/* estimate airplane position WRT to B */
+	math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
+
+	/* extension from [2], fly directly to A */
 	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
 
 		/* calculate eta to fly to waypoint A */
@@ -143,6 +148,21 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
 
+// XXX this can be useful as last-resort guard, but is currently not needed
+#if 0
+	} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
+		/* extension, fly back to waypoint */
+
+		/* calculate eta to fly to waypoint B */
+		
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = bearing_wp_b;
+#endif
 	} else {
 
 		/* calculate eta to fly along the line between A and B */
@@ -162,6 +182,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		eta = eta1 + eta2;
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+
 	}
 
 	/* limit angle to +-90 degrees */
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index a6a2c0156..5a17346cb 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -131,6 +131,14 @@ public:
 
 
 	/**
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circle_mode() {
+		return _circle_mode;
+	}
+
+
+	/**
 	 * Get the switch distance
 	 * 
 	 * This is the distance at which the system will
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 01b7b84d0..0c3546b95 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	case VEHICLE_CMD_DO_SET_MODE: {
 			uint8_t base_mode = (uint8_t) cmd->param1;
 			uint8_t custom_main_mode = (uint8_t) cmd->param2;
+			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 			/* set HIL state */
 			hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF;
-			hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+			int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd);
+
+			/* if HIL got enabled, reset battery status state */
+			if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
+				/* reset the arming mode to disarmed */
+				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+				if (arming_res != TRANSITION_DENIED) {
+					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+				} else {
+					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
+				}
+			}
 
 			// TODO remove debug code
 			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
-			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+			arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				if (safety->safety_switch_available && !safety->safety_off) {
+				if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) {
 					print_reject_arm("NOT ARMING: Press safety switch first.");
 					arming_res = TRANSITION_DENIED;
 
 				} else {
-					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
@@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			} else {
 				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
 					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
+					arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed);
 
 					if (arming_res == TRANSITION_CHANGED) {
 						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
@@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				arming_res = TRANSITION_DENIED;
 
 			} else {
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
 			}
 
 			if (arming_res == TRANSITION_CHANGED) {
@@ -532,6 +544,9 @@ static struct actuator_armed_s armed;
 
 static struct safety_s safety;
 
+/* flags for control apps */
+struct vehicle_control_mode_s control_mode;
+
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[])
 	/* Initialize armed with all false */
 	memset(&armed, 0, sizeof(armed));
 
-	/* flags for control apps */
-	struct vehicle_control_mode_s control_mode;
-
-
 	/* Initialize all flags to false */
 	memset(&control_mode, 0, sizeof(control_mode));
 
@@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[])
 
 			// warnx("bat v: %2.2f", battery.voltage_v);
 
-			/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
+			/* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) {
 				status.battery_voltage = battery.voltage_v;
 				status.condition_battery_voltage_valid = true;
 				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
@@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[])
 				battery_tune_played = false;
 
 				if (armed.armed) {
-					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
 				} else {
-					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
 
 				status_changed = true;
@@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[])
 			critical_voltage_counter++;
 
 		} else {
+
 			low_voltage_counter = 0;
 			critical_voltage_counter = 0;
 		}
@@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			// XXX check for sensors
-			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+			arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
 
 		} else {
 			// XXX: Add emergency stuff if sensors are lost
@@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[])
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+						res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed);
 						stick_off_counter = 0;
 
 					} else {
@@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[])
 							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 						} else {
-							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+							res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed);
 						}
 
 						stick_on_counter = 0;
@@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg)
 				/* try to go to INIT/PREFLIGHT arming state */
 
 				// XXX disable interrupts in arming_state_transition
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg)
 				else
 					tune_negative();
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+				arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
 
 				break;
 			}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 8ce31550f..efe12be57 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -67,7 +67,9 @@ static bool main_state_changed = true;
 static bool navigation_state_changed = true;
 
 transition_result_t
-arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
+arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety,
+	const struct vehicle_control_mode_s *control_mode,
+	arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
 	/*
 	 * Perform an atomic state update
@@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 
 	} else {
 
+		/* enforce lockdown in HIL */
+		if (control_mode->flag_system_hil_enabled) {
+			armed->lockdown = true;
+		} else {
+			armed->lockdown = false;
+		}
+
 		switch (new_arming_state) {
 		case ARMING_STATE_INIT:
 
@@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 
 			/* allow coming from INIT and disarming from ARMED */
 			if (status->arming_state == ARMING_STATE_INIT
-			    || status->arming_state == ARMING_STATE_ARMED) {
+			    || status->arming_state == ARMING_STATE_ARMED
+			    || control_mode->flag_system_hil_enabled) {
 
 				/* sensors need to be initialized for STANDBY state */
 				if (status->condition_system_sensors_initialized) {
@@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			/* allow arming from STANDBY and IN-AIR-RESTORE */
 			if ((status->arming_state == ARMING_STATE_STANDBY
 			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
-			    && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
+			    && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
 				armed->ready_to_arm = true;
@@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 		case HIL_STATE_OFF:
 
-			if (current_status->arming_state == ARMING_STATE_INIT
-			    || current_status->arming_state == ARMING_STATE_STANDBY) {
-
-				current_control_mode->flag_system_hil_enabled = false;
-				mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
-				valid_transition = true;
-			}
+			/* we're in HIL and unexpected things can happen if we disable HIL now */
+			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
+			valid_transition = false;
 
 			break;
 
 		case HIL_STATE_ON:
 
 			if (current_status->arming_state == ARMING_STATE_INIT
-			    || current_status->arming_state == ARMING_STATE_STANDBY) {
+			    || current_status->arming_state == ARMING_STATE_STANDBY
+			    || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) {
 
 				current_control_mode->flag_system_hil_enabled = true;
 				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 1641b6f60..0bfdf36a8 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -58,7 +58,7 @@ typedef enum {
 } transition_result_t;
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed);
+		const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
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 6b98003fd..ae9aaa2da 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -593,6 +593,10 @@ FixedwingAttitudeControl::task_main()
 					pitch_sp = _att_sp.pitch_body;
 					throttle_sp = _att_sp.thrust;
 
+					/* reset integrals where needed */
+					if (_att_sp.roll_reset_integral)
+						_roll_ctrl.reset_integrator();
+
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 3d5bce134..cd4a0d58e 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -74,6 +74,7 @@
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
@@ -129,9 +130,11 @@ private:
 	int		_accel_sub;			/**< body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
+	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct navigation_capabilities_s		_nav_capabilities;	/**< navigation capabilities */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct airspeed_s				_airspeed;		/**< airspeed */
 	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
@@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* publications */
 	_attitude_sp_pub(-1),
+	_nav_capabilities_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false)
 {
+	_nav_capabilities.turn_distance = 0.0f;
+
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
 	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
 	_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
@@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	// else integrators should be constantly reset.
 	if (_control_mode.flag_control_position_enabled) {
 
+		/* get circle mode */
+		bool was_circle_mode = _l1_control.circle_mode();
+
 		/* execute navigation once we have a setpoint */
 		if (_setpoint_valid) {
 
@@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else {
 				/*
-				 * No valid next waypoint, go for heading hold.
+				 * No valid previous waypoint, go for the current wp.
 				 * This is automatically handled by the L1 library.
 				 */
 				prev_wp.setX(global_triplet.current.lat / 1e7f);
@@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			}
 		}
 
+		if (was_circle_mode && !_l1_control.circle_mode()) {
+			/* just kicked out of loiter, reset roll integrals */
+			_att_sp.roll_reset_integral = true;
+		}
+
 	} else if (0/* seatbelt mode enabled */) {
 
 		/** SEATBELT FLIGHT **/
@@ -968,6 +982,21 @@ FixedwingPositionControl::task_main()
 					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 				}
 
+				float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy);
+
+				/* lazily publish navigation capabilities */
+				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+
+					/* set new turn distance */
+					_nav_capabilities.turn_distance = turn_distance;
+
+					if (_nav_capabilities_pub > 0) {
+						orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
+					} else {
+						_nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities);
+					}
+				}
+
 			}
 
 		}
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 7a5c2dab2..7c10e297b 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
 		/* send parameters at 20 Hz (if queued for sending) */
 		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
-		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap);
 
 		if (baudrate > 57600) {
 			mavlink_pm_queued_send();
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index 3175e64ce..e8d707948 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
 	sp->param2 = param2;
 	sp->param3 = param3;
 	sp->param4 = param4;
+
+
+	/* define the turn distance */
+	float orbit = 15.0f;
+
+	if (command == (int)MAV_CMD_NAV_WAYPOINT) {
+
+		orbit = param2;
+
+	} else if (command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+			command == (int)MAV_CMD_NAV_LOITER_TIME ||
+			command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+
+		orbit = param3;
+	} else {
+
+		// XXX set default orbit via param
+		// 15 initialized above
+	}
+
+	sp->turn_distance_xy = orbit;
+	sp->turn_distance_z = orbit;
 }
 
 /**
@@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		int last_setpoint_index = -1;
 		bool last_setpoint_valid = false;
 
-		/* at first waypoint, but cycled once through mission */
-		if (index == 0 && last_waypoint_index > 0) {
-			last_setpoint_index = last_waypoint_index;
-		} else {
+		if (index > 0) {
 			last_setpoint_index = index - 1;
 		}
 
@@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		int next_setpoint_index = -1;
 		bool next_setpoint_valid = false;
 
-		/* at last waypoint, try to re-loop through mission as default */
-		if (index == (wpm->size - 1) && wpm->size > 1) {
-			next_setpoint_index = 0;
-		} else if (wpm->size > 1) {
+		/* next waypoint */
+		if (wpm->size > 1) {
 			next_setpoint_index = index + 1;
 		}
 
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index cec2fdc43..0e0ce2723 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -67,6 +67,7 @@ extern bool gcs_link;
 
 struct vehicle_global_position_s global_pos;
 struct vehicle_local_position_s local_pos;
+struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
@@ -122,6 +123,7 @@ static void	l_optical_flow(const struct listener *l);
 static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
 static void	l_airspeed(const struct listener *l);
+static void	l_nav_cap(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -148,6 +150,7 @@ static const struct listener listeners[] = {
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
 	{l_home,			&mavlink_subs.home_sub,		0},
 	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
+	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -691,6 +694,19 @@ l_airspeed(const struct listener *l)
 	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
+void
+l_nav_cap(const struct listener *l)
+{
+
+	orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap);
+
+	mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+				   hrt_absolute_time() / 1000,
+				   "turn dist",
+				   nav_cap.turn_distance);
+
+}
+
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -837,6 +853,11 @@ uorb_receive_start(void)
 	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
 
+	/* --- NAVIGATION CAPABILITIES --- */
+	mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+	orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); 	/* 2Hz updates */
+	nav_cap.turn_distance = 0.0f;
+
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index c5cd0d03e..91773843b 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -67,6 +67,7 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
 #include <drivers/drv_rc_input.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 struct mavlink_subscriptions {
 	int sensor_sub;
@@ -92,6 +93,7 @@ struct mavlink_subscriptions {
 	int rates_setpoint_sub;
 	int home_sub;
 	int airspeed_sub;
+	int navigation_capabilities_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos;
 /** Local position */
 extern struct vehicle_local_position_s local_pos;
 
+/** navigation capabilities */
+extern struct navigation_capabilities_s nav_cap;
+
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 97472c233..ddad5f0df 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
  *
  * The distance calculation is based on the WGS84 geoid (GPS)
  */
-float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
+float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z)
 {
-	static uint16_t counter;
 
 	if (seq < wpm->size) {
 		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
@@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 		float dxy = radius_earth * c;
 		float dz = alt - wp->z;
 
+		*dist_xy = fabsf(dxy);
+		*dist_z = fabsf(dz);
+
 		return sqrtf(dxy * dxy + dz * dz);
 
 	} else {
 		return -1.0f;
 	}
 
-	counter++;
-
 }
 
 /*
  * Calculate distance in local frame (NED)
  */
-float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z)
+float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z)
 {
 	if (seq < wpm->size) {
 		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
@@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 		float dy = (cur->y - y);
 		float dz = (cur->z - z);
 
+		*dist_xy = sqrtf(dx * dx + dy * dy);
+		*dist_z = fabsf(dz);
+
 		return sqrtf(dx * dx + dy * dy + dz * dz);
 
 	} else {
@@ -303,11 +306,13 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float
 	}
 }
 
-void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos)
+void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance)
 {
 	static uint16_t counter;
 
-	if (!global_pos->valid && !local_pos->xy_valid) {
+	if ((!global_pos->valid && !local_pos->xy_valid) ||
+		/* no waypoint */
+		wpm->size == 0) {
 		/* nothing to check here, return */
 		return;
 	}
@@ -330,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			orbit = 15.0f;
 		}
 
+		/* keep vertical orbit */
+		float vertical_switch_distance = orbit;
+
+		/* Take the larger turn distance - orbit or turn_distance */
+		if (orbit < turn_distance)
+			orbit = turn_distance;
+
 		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
 		float dist = -1.0f;
 
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
 		if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
-			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
+			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_MISSION) {
 			/* Check if conditions of mission item are satisfied */
 			// XXX TODO
 		}
 
-		if (dist >= 0.f && dist <= orbit) {
+		if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
 			wpm->pos_reached = true;
 		}
+
 		// check if required yaw reached
 		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
 		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
@@ -463,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 }
 
 
-int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position)
+int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap)
 {
 	/* check for timed-out operations */
 	if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
@@ -486,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 		}
 	}
 
-	check_waypoints_reached(now, global_position, local_position);
+	check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance);
 
 	return OK;
 }
@@ -1009,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 		}
 	}
 
-	check_waypoints_reached(now, global_pos, local_pos);
+	// check_waypoints_reached(now, global_pos, local_pos);
 }
diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index 96a0ecd30..d7d6b31dc 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -55,6 +55,7 @@
 #include <stdbool.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/navigation_capabilities.h>
 
 // FIXME XXX - TO BE MOVED TO XML
 enum MAVLINK_WPM_STATES {
@@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage;
 
 void mavlink_wpm_init(mavlink_wpm_storage *state);
 int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position,
-			       struct vehicle_local_position_s *local_pos);
+			       struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap);
 void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos ,
 				 struct vehicle_local_position_s *local_pos);
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 8cb21e54f..9d9ef7c6d 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -674,27 +674,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 #ifdef ADC_VSERVO
 		/* PX4IO_P_STATUS_VSERVO */
 		{
-			/*
-			 * Coefficients here derived by measurement of the 5-16V
-			 * range on one unit:
-			 *
-			 * XXX pending measurements
-			 *
-			 * slope = xxx
-			 * intercept = xxx
-			 *
-			 * Intercept corrected for best results @ 5.0V.
-			 */
 			unsigned counts = adc_measure(ADC_VSERVO);
 			if (counts != 0xffff) {
-				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
-				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
-
-				r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+				// use 3:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 9900 / 4096;
+				r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
+			}
+		}
+#endif
+#ifdef ADC_RSSI
+		/* PX4IO_P_STATUS_VRSSI */
+		{
+			unsigned counts = adc_measure(ADC_RSSI);
+			if (counts != 0xffff) {
+				// use 1:1 scaling on 3.3V ADC input
+				unsigned mV = counts * 3300 / 4096;
+				r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
 			}
 		}
 #endif
-		/* XXX PX4IO_P_STATUS_VRSSI */
 		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 085da905c..9ef0d3c83 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced)
 void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
+	/* only read if publishing */
+	if (!_publishing)
+		return;
 
 	/* rate limit to 100 Hz */
 	if (hrt_absolute_time() - _last_adc >= 10000) {
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index 3c1e10287..bf84b7945 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle)
 	}
 }
 
+uint64_t
+perf_event_count(perf_counter_t handle)
+{
+	if (handle == NULL)
+		return 0;
+
+	switch (handle->type) {
+	case PC_COUNT:
+		return ((struct perf_ctr_count *)handle)->event_count;
+
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+		return pce->event_count;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		return pci->event_count;
+	}
+
+	default:
+		break;
+	}
+	return 0;
+}
+
 void
 perf_print_all(void)
 {
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 4cd8b67a1..e1e3cbe95 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -135,6 +135,14 @@ __EXPORT extern void		perf_print_all(void);
  */
 __EXPORT extern void		perf_reset_all(void);
 
+/**
+ * Return current event_count
+ *
+ * @param handle		The counter returned from perf_alloc.
+ * @return			event_count
+ */
+__EXPORT extern uint64_t	perf_event_count(perf_counter_t handle);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 1eb63a799..3514dca24 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s);
 #include "topics/battery_status.h"
 ORB_DEFINE(battery_status, struct battery_status_s);
 
+#include "topics/servorail_status.h"
+ORB_DEFINE(servorail_status, struct servorail_status_s);
+
 #include "topics/vehicle_global_position.h"
 ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 
diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 1ffeda764..e4d2c92ce 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -53,6 +53,7 @@
  */
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
+	uint64_t	error_count;
 	uint16_t	differential_pressure_pa;	/**< Differential pressure reading */
 	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
 	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h
new file mode 100644
index 000000000..55668790b
--- /dev/null
+++ b/src/modules/uORB/topics/servorail_status.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   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 servorail_status.h
+ *
+ * Definition of the servorail status uORB topic.
+ */
+
+#ifndef SERVORAIL_STATUS_H_
+#define SERVORAIL_STATUS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Servorail voltages and status
+ */
+struct servorail_status_s {
+	uint64_t	timestamp;		/**< microseconds since system boot */
+	float   	voltage_v;		/**< Servo rail voltage in volts */
+	float   	rssi_v;			/**< RSSI pin voltage in volts */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(servorail_status);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index 686fd765c..1a245132a 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s
 
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
+	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
+
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index 5c8ce1e4d..a56434d3b 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s
 	float param2;
 	float param3;
 	float param4;
+	float turn_distance_xy;		/**< The distance on the plane which will mark this as reached */
+	float turn_distance_z;		/**< The distance in Z direction which will mark this as reached */
 };
 
 /**
diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c
index f36c28061..7f3a654bf 100644
--- a/src/systemcmds/tests/tests_file.c
+++ b/src/systemcmds/tests/tests_file.c
@@ -54,7 +54,8 @@
 int
 test_file(int argc, char *argv[])
 {
-	const iterations = 200;
+	const unsigned iterations = 100;
+	const unsigned alignments = 65;
 
 	/* check if microSD card is mounted */
 	struct stat buffer;
@@ -63,68 +64,190 @@ test_file(int argc, char *argv[])
 		return 1;
 	}
 
-	uint8_t buf[512];
-	hrt_abstime start, end;
-	perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
+	/* perform tests for a range of chunk sizes */
+	unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500};
 
-	int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
-	memset(buf, 0, sizeof(buf));
+	for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
 
-	warnx("aligned write - please wait..");
+		printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
 
-	if ((0x3 & (uintptr_t)buf))
-		warnx("memory is unaligned!");
+		for (unsigned a = 0; a < alignments; a++) {
 
-	start = hrt_absolute_time();
-	for (unsigned i = 0; i < iterations; i++) {
-		perf_begin(wperf);
-		write(fd, buf, sizeof(buf));
-		fsync(fd);
-		perf_end(wperf);
-	}
-	end = hrt_absolute_time();
+			printf("\n");
+			warnx("----- alignment test: %u bytes -----", a);
 
-	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
-	perf_print_counter(wperf);
-	perf_free(wperf);
+			/* fill write buffer with known values */
+			for (int i = 0; i < sizeof(write_buf); i++) {
+				/* this will wrap, but we just need a known value with spacing */
+				write_buf[i] = i+11;
+			}
 
-	int ret = unlink("/fs/microsd/testfile");
+			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
+			hrt_abstime start, end;
+			//perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
 
-	if (ret)
-		err(1, "UNLINKING FILE FAILED");
+			int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
-	warnx("unaligned write - please wait..");
+			warnx("testing unaligned writes - please wait..");
 
-	struct {
-		uint8_t byte;
-		uint8_t unaligned[512];
-	} unaligned_buf;
+			start = hrt_absolute_time();
+			for (unsigned i = 0; i < iterations; i++) {
+				//perf_begin(wperf);
+				int wret = write(fd, write_buf + a, chunk_sizes[c]);
 
-	if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0)
-		warnx("creating unaligned memory failed.");
+				if (wret != chunk_sizes[c]) {
+					warn("WRITE ERROR!");
 
-	wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)");
+					if ((0x3 & (uintptr_t)(write_buf + a)))
+						errx(1, "memory is unaligned, align shift: %d", a);
 
-	start = hrt_absolute_time();
-	for (unsigned i = 0; i < iterations; i++) {
-		perf_begin(wperf);
-		write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned));
-		fsync(fd);
-		perf_end(wperf);
-	}
-	end = hrt_absolute_time();
+				}
 
-	close(fd);
+				fsync(fd);
+				//perf_end(wperf);
 
-	warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+			}
+			end = hrt_absolute_time();
 
-	perf_print_counter(wperf);
-	perf_free(wperf);
+			//warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
+
+			//perf_print_counter(wperf);
+			//perf_free(wperf);
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			/* read back data for validation */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf, chunk_sizes[c]);
+
+				if (rret != chunk_sizes[c]) {
+					errx(1, "READ ERROR!");
+				}
+				
+				/* compare value */
+				bool compare_ok = true;
+
+				for (int j = 0; j < chunk_sizes[c]; j++) {
+					if (read_buf[j] != write_buf[j + a]) {
+						warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
+						compare_ok = false;
+						break;
+					}
+				}
+
+				if (!compare_ok) {
+					errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
+				}
+
+			}
+
+			/*
+			 * ALIGNED WRITES AND UNALIGNED READS
+			 */
+
+			close(fd);
+			int ret = unlink("/fs/microsd/testfile");
+			fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
+
+			warnx("testing aligned writes - please wait..");
+
+			start = hrt_absolute_time();
+			for (unsigned i = 0; i < iterations; i++) {
+				int wret = write(fd, write_buf, chunk_sizes[c]);
+
+				if (wret != chunk_sizes[c]) {
+					err(1, "WRITE ERROR!");
+				}
+
+			}
+
+			fsync(fd);
+
+			warnx("reading data aligned..");
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			bool align_read_ok = true;
+
+			/* read back data unaligned */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf, chunk_sizes[c]);
+
+				if (rret != chunk_sizes[c]) {
+					err(1, "READ ERROR!");
+				}
+				
+				/* compare value */
+				bool compare_ok = true;
+
+				for (int j = 0; j < chunk_sizes[c]; j++) {
+					if (read_buf[j] != write_buf[j]) {
+						warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]);
+						align_read_ok = false;
+						break;
+					}
+				}
+
+				if (!align_read_ok) {
+					errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
+				}
+
+			}
+
+			warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR");
+
+			warnx("reading data unaligned..");
+
+			close(fd);
+			fd = open("/fs/microsd/testfile", O_RDONLY);
+
+			bool unalign_read_ok = true;
+			int unalign_read_err_count = 0;
+
+			memset(read_buf, 0, sizeof(read_buf));
+
+			/* read back data unaligned */
+			for (unsigned i = 0; i < iterations; i++) {
+				int rret = read(fd, read_buf + a, chunk_sizes[c]);
+
+				if (rret != chunk_sizes[c]) {
+					err(1, "READ ERROR!");
+				}
+
+				for (int j = 0; j < chunk_sizes[c]; j++) {
+
+					if ((read_buf + a)[j] != write_buf[j]) {
+						warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]);
+						unalign_read_ok = false;
+						unalign_read_err_count++;
+						
+						if (unalign_read_err_count > 10)
+							break;
+					}
+				}
+
+				if (!unalign_read_ok) {
+					errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR");
+				}
+
+			}
+
+			ret = unlink("/fs/microsd/testfile");
+			close(fd);
+
+			if (ret)
+				err(1, "UNLINKING FILE FAILED");
+
+		}
+	}
 
 	/* list directory */
-	DIR           *d;
-	struct dirent *dir;
+	DIR		*d;
+	struct dirent	*dir;
 	d = opendir("/fs/microsd");
 	if (d) {