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

26 files changed, 414 insertions, 979 deletions

diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 146a06e7c..2de7063ea 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -154,8 +154,9 @@ ETSAirspeed::collect()
 		return ret;
 	}
 
-	uint16_t diff_pres_pa = val[1] << 8 | val[0];
-        if (diff_pres_pa == 0) {
+	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
+	uint16_t diff_pres_pa;
+        if (diff_pres_pa_raw == 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
@@ -165,10 +166,10 @@ ETSAirspeed::collect()
 		return -1;
         }
 
-	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
+	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 	} else {
-		diff_pres_pa -= _diff_pres_offset;
+		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
 	}
 
 	// Track maximum differential pressure measured (so we can work out top speed).
@@ -183,6 +184,7 @@ ETSAirspeed::collect()
 
 	// XXX we may want to smooth out the readings to remove noise.
 	report.differential_pressure_filtered_pa = (float)diff_pres_pa;
+	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
 	report.temperature = -1000.0f;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 6195cd6ea..5342ccf78 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -448,10 +448,10 @@ GPS::print_info()
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
 
 	if (_report.timestamp_position != 0) {
-		warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type,
-				_report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
+		warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type,
+				_report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f);
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
-		warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m);
+		warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m);
 		warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate());
 		warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate());
 		warnx("rate publication:\t%6.2f Hz", (double)_rate);
diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp
index 2360ff39b..3b92f1bf4 100644
--- a/src/drivers/gps/gps_helper.cpp
+++ b/src/drivers/gps/gps_helper.cpp
@@ -56,7 +56,7 @@ GPS_Helper::get_velocity_update_rate()
 	return _rate_vel;
 }
 
-float
+void
 GPS_Helper::reset_update_rates()
 {
 	_rate_count_vel = 0;
@@ -64,7 +64,7 @@ GPS_Helper::reset_update_rates()
 	_interval_rate_start = hrt_absolute_time();
 }
 
-float
+void
 GPS_Helper::store_update_rates()
 {
 	_rate_vel = _rate_count_vel / (((float)(hrt_absolute_time() - _interval_rate_start)) / 1000000.0f);
diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h
index cfb9e0d43..d14a95afe 100644
--- a/src/drivers/gps/gps_helper.h
+++ b/src/drivers/gps/gps_helper.h
@@ -46,13 +46,17 @@
 class GPS_Helper
 {
 public:
+
+	GPS_Helper() {};
+	virtual ~GPS_Helper() {};
+
 	virtual int			configure(unsigned &baud) = 0;
 	virtual int 			receive(unsigned timeout) = 0;
 	int 				set_baudrate(const int &fd, unsigned baud);
 	float				get_position_update_rate();
 	float				get_velocity_update_rate();
-	float				reset_update_rates();
-	float				store_update_rates();
+	void				reset_update_rates();
+	void				store_update_rates();
 
 protected:
 	uint8_t _rate_count_lat_lon;
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 8a2afecb7..19cf5beec 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -164,7 +164,7 @@ UBX::configure(unsigned &baudrate)
 	send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: configuration failed: RATE");
+		warnx("CFG FAIL: RATE");
 		return 1;
 	}
 
@@ -185,7 +185,7 @@ UBX::configure(unsigned &baudrate)
 	send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: configuration failed: NAV5");
+		warnx("CFG FAIL: NAV5");
 		return 1;
 	}
 
@@ -194,35 +194,42 @@ UBX::configure(unsigned &baudrate)
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV POSLLH");
+		warnx("MSG CFG FAIL: NAV POSLLH");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV TIMEUTC");
+		warnx("MSG CFG FAIL: NAV TIMEUTC");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SOL");
+		warnx("MSG CFG FAIL: NAV SOL");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV VELNED");
+		warnx("MSG CFG FAIL: NAV VELNED");
 		return 1;
 	}
 
 	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
 
 	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-		warnx("ubx: msg rate configuration failed: NAV SVINFO");
+		warnx("MSG CFG FAIL: NAV SVINFO");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("MSG CFG FAIL: MON HW");
 		return 1;
 	}
 
@@ -274,7 +281,7 @@ UBX::receive(unsigned timeout)
 
 		if (ret < 0) {
 			/* something went wrong when polling */
-			warnx("ubx: poll error");
+			warnx("poll error");
 			return -1;
 
 		} else if (ret == 0) {
@@ -310,7 +317,7 @@ UBX::receive(unsigned timeout)
 
 		/* abort after timeout if no useful packets received */
 		if (time_started + timeout * 1000 < hrt_absolute_time()) {
-			warnx("ubx: timeout - no useful messages");
+			warnx("timeout - no useful messages");
 			return -1;
 		}
 	}
@@ -383,7 +390,7 @@ UBX::parse_char(uint8_t b)
 				return 1;	// message received successfully
 
 			} else {
-				warnx("ubx: checksum wrong");
+				warnx("checksum wrong");
 				decode_init();
 				return -1;
 			}
@@ -392,7 +399,7 @@ UBX::parse_char(uint8_t b)
 			_rx_count++;
 
 		} else {
-			warnx("ubx: buffer full");
+			warnx("buffer full");
 			decode_init();
 			return -1;
 		}
@@ -566,6 +573,24 @@ UBX::handle_message()
 				break;
 			}
 
+		case UBX_CLASS_MON: {
+			switch (_message_id) {
+			case UBX_MESSAGE_MON_HW: {
+
+					struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer;
+
+					_gps_position->noise_per_ms = p->noisePerMS;
+					_gps_position->jamming_indicator = p->jamInd;
+
+					ret = 1;
+					break;
+				}
+
+			default:
+				break;
+			}
+		}
+
 		default:
 			break;
 		}
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 79a904f4a..5cf47b60b 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -56,6 +56,7 @@
 //#define UBX_CLASS_RXM 0x02
 #define UBX_CLASS_ACK 0x05
 #define UBX_CLASS_CFG 0x06
+#define UBX_CLASS_MON 0x0A
 
 /* MessageIDs (the ones that are used) */
 #define UBX_MESSAGE_NAV_POSLLH 0x02
@@ -72,6 +73,8 @@
 #define UBX_MESSAGE_CFG_RATE 0x08
 #define UBX_MESSAGE_CFG_NAV5 0x24
 
+#define UBX_MESSAGE_MON_HW	0x09
+
 #define UBX_CFG_PRT_LENGTH 20
 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
 #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
@@ -210,6 +213,27 @@ typedef struct {
 	uint8_t ck_b;
 } gps_bin_nav_velned_packet_t;
 
+struct gps_bin_mon_hw_packet {
+	uint32_t pinSel;
+	uint32_t pinBank;
+	uint32_t pinDir;
+	uint32_t pinVal;
+	uint16_t noisePerMS;
+	uint16_t agcCnt;
+	uint8_t aStatus;
+	uint8_t aPower;
+	uint8_t flags;
+	uint8_t __reserved1;
+	uint32_t usedMask;
+	uint8_t VP[25];
+	uint8_t jamInd;
+	uint16_t __reserved3;
+	uint32_t pinIrq;
+	uint32_t pulLH;
+	uint32_t pullL;
+};
+
+
 //typedef struct {
 //	int32_t time_milliseconds; 		/**< Measurement integer millisecond GPS time of week */
 //	int16_t week; 					/**< Measurement GPS week number */
@@ -319,7 +343,7 @@ typedef enum {
 //typedef type_gps_bin_ubx_state gps_bin_ubx_state_t;
 #pragma pack(pop)
 
-#define RECV_BUFFER_SIZE 500 //The NAV-SOL messages really need such a big buffer
+#define RECV_BUFFER_SIZE 300 //The NAV-SOL messages really need such a big buffer
 
 class UBX : public GPS_Helper
 {
@@ -383,7 +407,7 @@ private:
 	uint8_t			_message_class;
 	uint8_t			_message_id;
 	unsigned		_payload_size;
-	uint8_t			_disable_cmd_last;
+	hrt_abstime		_disable_cmd_last;
 };
 
 #endif /* UBX_H_ */
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index fd69cf795..8a4bfa18c 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -648,11 +648,9 @@ PX4FMU::task_main()
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
-					/* last resort: catch NaN, INF and out-of-band errors */
-					if (i >= outputs.noutputs ||
-						!isfinite(outputs.output[i]) ||
-						outputs.output[i] < -1.0f ||
-						outputs.output[i] > 1.0f) {
+					/* last resort: catch NaN and INF */
+					if ((i >= outputs.noutputs) ||
+						!isfinite(outputs.output[i])) {
 						/*
 						 * Value is NaN, INF or out of band - set to the minimum value.
 						 * This will be clearly visible on the servo status and will limit the risk of accidentally
@@ -664,6 +662,7 @@ PX4FMU::task_main()
 
 				uint16_t pwm_limited[num_outputs];
 
+				/* the PWM limit call takes care of out of band errors and constrains */
 				pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
 				/* output to the servos */
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 4099e5522..972f45148 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -581,8 +581,10 @@ PX4IO::init()
 	ASSERT(_task == -1);
 
 	sys_restart_param = param_find("SYS_RESTART_TYPE");
-	/* Indicate restart type is unknown */
-	param_set(sys_restart_param, &sys_restart_val);
+	if (sys_restart_param != PARAM_INVALID) {
+		/* Indicate restart type is unknown */
+		param_set(sys_restart_param, &sys_restart_val);
+	}
 
 	/* do regular cdev init */
 	ret = CDev::init();
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
deleted file mode 100644
index 391e40ac1..000000000
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ /dev/null
@@ -1,613 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 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 flow_position_control.c
- *
- * Optical flow position controller
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <math.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
-#include <uORB/topics/filtered_bottom_flow.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-#include <poll.h>
-#include <mavlink/mavlink_log.h>
-
-#include "flow_position_control_params.h"
-
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-__EXPORT int flow_position_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of position controller.
- */
-static int flow_position_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- * 
- * The actual stack size should be set in the call
- * to task_spawn_cmd().
- */
-int flow_position_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start"))
-	{
-		if (thread_running)
-		{
-			printf("flow position control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("flow_position_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 6,
-					 4096,
-					 flow_position_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop"))
-	{
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status"))
-	{
-		if (thread_running)
-			printf("\tflow position control app is running\n");
-		else
-			printf("\tflow position control app not started\n");
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static int
-flow_position_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	thread_running = true;
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[fpc] started");
-
-	uint32_t counter = 0;
-	const float time_scale = powf(10.0f,-6.0f);
-
-	/* structures */
-	struct actuator_armed_s armed;
-	memset(&armed, 0, sizeof(armed));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct manual_control_setpoint_s manual;
-	memset(&manual, 0, sizeof(manual));
-	struct filtered_bottom_flow_s filtered_flow;
-	memset(&filtered_flow, 0, sizeof(filtered_flow));
-	struct vehicle_local_position_s local_pos;
-	memset(&local_pos, 0, sizeof(local_pos));
-	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
-	memset(&speed_sp, 0, sizeof(speed_sp));
-
-	/* subscribe to attitude, motor setpoints and system state */
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
-	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-
-	orb_advert_t speed_sp_pub;
-	bool speed_setpoint_adverted = false;
-
-	/* parameters init*/
-	struct flow_position_control_params params;
-	struct flow_position_control_param_handles param_handles;
-	parameters_init(&param_handles);
-	parameters_update(&param_handles, &params);
-
-	/* init flow sum setpoint */
-	float flow_sp_sumx = 0.0f;
-	float flow_sp_sumy = 0.0f;
-
-	/* init yaw setpoint */
-	float yaw_sp = 0.0f;
-
-	/* init height setpoint */
-	float height_sp = params.height_min;
-
-	/* height controller states */
-	bool start_phase = true;
-	bool landing_initialized = false;
-	float landing_thrust_start = 0.0f;
-
-	/* states */
-	float integrated_h_error = 0.0f;
-	float last_local_pos_z = 0.0f;
-	bool update_flow_sp_sumx = false;
-	bool update_flow_sp_sumy = false;
-	uint64_t last_time = 0.0f;
-	float dt = 0.0f; // s
-
-
-	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
-	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
-	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
-
-	static bool sensors_ready = false;
-	static bool status_changed = false;
-
-	while (!thread_should_exit)
-	{
-		/* wait for first attitude msg to be sure all data are available */
-		if (sensors_ready)
-		{
-			/* polling */
-			struct pollfd fds[2] = {
-				{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
-				{ .fd = parameter_update_sub,   .events = POLLIN }
-
-			};
-
-			/* wait for a position update, check for exit condition every 500 ms */
-			int ret = poll(fds, 2, 500);
-
-			if (ret < 0)
-			{
-				/* poll error, count it in perf */
-				perf_count(mc_err_perf);
-			}
-			else if (ret == 0)
-			{
-				/* no return value, ignore */
-//				printf("[flow position control] no filtered flow updates\n");
-			}
-			else
-			{
-				/* parameter update available? */
-				if (fds[1].revents & POLLIN)
-				{
-					/* read from param to clear updated flag */
-					struct parameter_update_s update;
-					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-
-					parameters_update(&param_handles, &params);
-					mavlink_log_info(mavlink_fd,"[fpc] parameters updated.");
-				}
-
-				/* only run controller if position/speed changed */
-				if (fds[0].revents & POLLIN)
-				{
-					perf_begin(mc_loop_perf);
-
-					/* get a local copy of the vehicle state */
-					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-					/* get a local copy of manual setpoint */
-					orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
-					/* get a local copy of attitude */
-					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-					/* get a local copy of filtered bottom flow */
-					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
-					/* get a local copy of local position */
-					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
-					/* get a local copy of control mode */
-					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-
-					if (control_mode.flag_control_velocity_enabled)
-					{
-						float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
-						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
-						float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
-
-						if(status_changed == false)
-							mavlink_log_info(mavlink_fd,"[fpc] flow POSITION control engaged");
-
-						status_changed = true;
-
-						/* calc dt */
-						if(last_time == 0)
-						{
-							last_time = hrt_absolute_time();
-							continue;
-						}
-						dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
-						last_time = hrt_absolute_time();
-
-						/* update flow sum setpoint */
-						if (update_flow_sp_sumx)
-						{
-							flow_sp_sumx = filtered_flow.sumx;
-							update_flow_sp_sumx = false;
-						}
-						if (update_flow_sp_sumy)
-						{
-							flow_sp_sumy = filtered_flow.sumy;
-							update_flow_sp_sumy = false;
-						} 
-
-						/* calc new bodyframe speed setpoints */
-						float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
-						float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d;
-						float speed_limit_height_factor = height_sp; // the settings are for 1 meter
-
-						/* overwrite with rc input if there is any */
-						if(isfinite(manual_pitch) && isfinite(manual_roll))
-						{
-							if(fabsf(manual_pitch) > params.manual_threshold)
-							{
-								speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
-								update_flow_sp_sumx = true;
-							}
-
-							if(fabsf(manual_roll) > params.manual_threshold)
-							{
-								speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
-								update_flow_sp_sumy = true;
-							}
-						}
-
-						/* limit speed setpoints */
-						if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
-								(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
-						{
-							speed_sp.vx = speed_body_x;
-						}
-						else
-						{
-							if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
-								speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
-							if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
-								speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
-						}
-
-						if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
-								(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
-						{
-							speed_sp.vy = speed_body_y;
-						}
-						else
-						{
-							if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
-								speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
-							if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
-								speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
-						}
-
-						/* manual yaw change */
-						if(isfinite(manual_yaw) && isfinite(manual.throttle))
-						{
-							if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
-							{
-								yaw_sp += manual_yaw * params.limit_yaw_step;
-
-								/* modulo for rotation -pi +pi */
-								if(yaw_sp < -M_PI_F)
-									yaw_sp = yaw_sp + M_TWOPI_F;
-								else if(yaw_sp > M_PI_F)
-									yaw_sp = yaw_sp - M_TWOPI_F;
-							}
-						}
-
-						/* forward yaw setpoint */
-						speed_sp.yaw_sp = yaw_sp;
-
-
-						/* manual height control
-						 * 0-20%: thrust linear down
-						 * 20%-40%: down
-						 * 40%-60%: stabilize altitude
-						 * 60-100%: up
-						 */
-						float thrust_control = 0.0f;
-
-						if (isfinite(manual.throttle))
-						{
-							if (start_phase)
-							{
-								/* control start thrust with stick input */
-								if (manual.throttle < 0.4f)
-								{
-									/* first 40% for up to feedforward */
-									thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
-								}
-								else
-								{
-									/* second 60% for up to feedforward + 10% */
-									thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
-								}
-
-								/* exit start phase if setpoint is reached */
-								if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
-								{
-									start_phase = false;
-									/* switch to stabilize */
-									thrust_control = params.thrust_feedforward;
-								}
-							}
-							else
-							{
-								if (manual.throttle < 0.2f)
-								{
-									/* landing initialization */
-									if (!landing_initialized)
-									{
-										/* consider last thrust control to avoid steps */
-										landing_thrust_start = speed_sp.thrust_sp;
-										landing_initialized = true;
-									}
-
-									/* set current height as setpoint to avoid steps */
-									if (-local_pos.z > params.height_min)
-										height_sp = -local_pos.z;
-									else
-										height_sp = params.height_min;
-
-									/* lower 20% stick range controls thrust down */
-									thrust_control = manual.throttle / 0.2f * landing_thrust_start;
-
-									/* assume ground position here */
-									if (thrust_control < 0.1f)
-									{
-										/* reset integral if on ground */
-										integrated_h_error = 0.0f;
-										/* switch to start phase */
-										start_phase = true;
-										/* reset height setpoint */
-										height_sp = params.height_min;
-									}
-								}
-								else
-								{
-									/* stabilized mode */
-									landing_initialized = false;
-
-									/* calc new thrust with PID */
-									float height_error = (local_pos.z - (-height_sp));
-
-									/* update height setpoint if needed*/
-									if (manual.throttle < 0.4f)
-									{
-										/* down */
-										if (height_sp > params.height_min + params.height_rate &&
-												fabsf(height_error) < params.limit_height_error)
-											height_sp -= params.height_rate * dt;
-									}
-
-									if (manual.throttle > 0.6f)
-									{
-										/* up */
-										if (height_sp < params.height_max &&
-												fabsf(height_error) < params.limit_height_error)
-											height_sp += params.height_rate * dt;
-									}
-
-									/* instead of speed limitation, limit height error (downwards) */
-									if(height_error > params.limit_height_error)
-										height_error = params.limit_height_error;
-									else if(height_error < -params.limit_height_error)
-										height_error = -params.limit_height_error;
-
-									integrated_h_error = integrated_h_error + height_error;
-									float integrated_thrust_addition = integrated_h_error * params.height_i;
-
-									if(integrated_thrust_addition > params.limit_thrust_int)
-										integrated_thrust_addition = params.limit_thrust_int;
-									if(integrated_thrust_addition < -params.limit_thrust_int)
-										integrated_thrust_addition = -params.limit_thrust_int;
-
-									float height_speed = last_local_pos_z - local_pos.z;
-									float thrust_diff = height_error * params.height_p - height_speed * params.height_d;
-
-									thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;
-
-									/* add attitude component
-									 * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
-									 */
-//									// TODO problem with attitude
-//									if (att.R_valid && att.R[2][2] > 0)
-//										thrust_control = thrust_control / att.R[2][2];
-
-									/* set thrust lower limit */
-									if(thrust_control < params.limit_thrust_lower)
-										thrust_control = params.limit_thrust_lower;
-								}
-							}
-
-							/* set thrust upper limit */
-							if(thrust_control > params.limit_thrust_upper)
-								thrust_control = params.limit_thrust_upper;
-						}
-						/* store actual height for speed estimation */
-						last_local_pos_z = local_pos.z;
-
-						speed_sp.thrust_sp =  thrust_control; //manual.throttle;
-						speed_sp.timestamp = hrt_absolute_time();
-
-						/* publish new speed setpoint */
-						if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
-						{
-
-							if(speed_setpoint_adverted)
-							{
-								orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
-							}
-							else
-							{
-								speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
-								speed_setpoint_adverted = true;
-							}
-						}
-						else
-						{
-							warnx("NaN in flow position controller!");
-						}
-					}
-					else
-					{
-						/* in manual or stabilized state just reset speed and flow sum setpoint */
-						//mavlink_log_info(mavlink_fd,"[fpc] reset speed sp, flow_sp_sumx,y (%f,%f)",filtered_flow.sumx, filtered_flow.sumy);
-						if(status_changed == true)
-							mavlink_log_info(mavlink_fd,"[fpc] flow POSITION controller disengaged.");
-
-						status_changed = false;
-						speed_sp.vx = 0.0f;
-						speed_sp.vy = 0.0f;
-						flow_sp_sumx = filtered_flow.sumx;
-						flow_sp_sumy = filtered_flow.sumy;
-						if(isfinite(att.yaw))
-						{
-							yaw_sp = att.yaw;
-							speed_sp.yaw_sp = att.yaw;
-						}
-						if(isfinite(manual.throttle))
-							speed_sp.thrust_sp = manual.throttle;
-					}
-					/* measure in what intervals the controller runs */
-					perf_count(mc_interval_perf);
-					perf_end(mc_loop_perf);
-				}
-			}
-
-			counter++;
-		}
-		else
-		{
-			/* sensors not ready waiting for first attitude msg */
-
-			/* polling */
-			struct pollfd fds[1] = {
-				{ .fd = vehicle_attitude_sub, .events = POLLIN },
-			};
-
-			/* wait for a flow msg, check for exit condition every 5 s */
-			int ret = poll(fds, 1, 5000);
-
-			if (ret < 0)
-			{
-				/* poll error, count it in perf */
-				perf_count(mc_err_perf);
-			}
-			else if (ret == 0)
-			{
-				/* no return value, ignore */
-				mavlink_log_info(mavlink_fd,"[fpc] no attitude received.\n");
-			}
-			else
-			{
-				if (fds[0].revents & POLLIN)
-				{
-					sensors_ready = true;
-					mavlink_log_info(mavlink_fd,"[fpc] initialized.\n");
-				}
-			}
-		}
-	}
-
-	mavlink_log_info(mavlink_fd,"[fpc] ending now...\n");
-
-	thread_running = false;
-
-	close(parameter_update_sub);
-	close(vehicle_attitude_sub);
-	close(vehicle_local_position_sub);
-	close(armed_sub);
-	close(control_mode_sub);
-	close(manual_control_setpoint_sub);
-	close(speed_sp_pub);
-
-	perf_print_counter(mc_loop_perf);
-	perf_free(mc_loop_perf);
-
-	fflush(stdout);
-	return 0;
-}
-
diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c
deleted file mode 100644
index eb1473647..000000000
--- a/src/examples/flow_position_control/flow_position_control_params.c
+++ /dev/null
@@ -1,124 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 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 flow_position_control_params.c
- */
-
-#include "flow_position_control_params.h"
-
-/* controller parameters */
-
-// Position control P gain
-PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
-// Position control D / damping gain
-PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
-// Altitude control P gain
-PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
-// Altitude control I (integrator) gain
-PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
-// Altitude control D gain
-PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
-// Altitude control rate limiter
-PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
-// Altitude control minimum altitude
-PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
-// Altitude control maximum altitude (higher than 1.5m is untested)
-PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
-// Altitude control feed forward throttle - adjust to the
-// throttle position (0..1) where the copter hovers in manual flight
-PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight
-PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f);
-PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f);
-PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f);
-PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f);
-PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f);
-PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f);
-PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f);
-PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f);
-
-
-int parameters_init(struct flow_position_control_param_handles *h)
-{
-	/* PID parameters */
-	h->pos_p	 			=	param_find("FPC_POS_P");
-	h->pos_d 				=	param_find("FPC_POS_D");
-	h->height_p 			=	param_find("FPC_H_P");
-	h->height_i 			=	param_find("FPC_H_I");
-	h->height_d 			=	param_find("FPC_H_D");
-	h->height_rate 			=	param_find("FPC_H_RATE");
-	h->height_min			=	param_find("FPC_H_MIN");
-	h->height_max			=	param_find("FPC_H_MAX");
-	h->thrust_feedforward 	=	param_find("FPC_T_FFWD");
-	h->limit_speed_x 		=	param_find("FPC_L_S_X");
-	h->limit_speed_y 		=	param_find("FPC_L_S_Y");
-	h->limit_height_error	=	param_find("FPC_L_H_ERR");
-	h->limit_thrust_int 	=	param_find("FPC_L_TH_I");
-	h->limit_thrust_upper 	=	param_find("FPC_L_TH_U");
-	h->limit_thrust_lower 	=	param_find("FPC_L_TH_L");
-	h->limit_yaw_step		=	param_find("FPC_L_YAW_STEP");
-	h->manual_threshold 	=	param_find("FPC_MAN_THR");
-	h->rc_scale_pitch		=   param_find("RC_SCALE_PITCH");
-	h->rc_scale_roll		=   param_find("RC_SCALE_ROLL");
-	h->rc_scale_yaw			=   param_find("RC_SCALE_YAW");
-
-	return OK;
-}
-
-int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p)
-{
-	param_get(h->pos_p, &(p->pos_p));
-	param_get(h->pos_d, &(p->pos_d));
-	param_get(h->height_p, &(p->height_p));
-	param_get(h->height_i, &(p->height_i));
-	param_get(h->height_d, &(p->height_d));
-	param_get(h->height_rate, &(p->height_rate));
-	param_get(h->height_min, &(p->height_min));
-	param_get(h->height_max, &(p->height_max));
-	param_get(h->thrust_feedforward, &(p->thrust_feedforward));
-	param_get(h->limit_speed_x, &(p->limit_speed_x));
-	param_get(h->limit_speed_y, &(p->limit_speed_y));
-	param_get(h->limit_height_error, &(p->limit_height_error));
-	param_get(h->limit_thrust_int, &(p->limit_thrust_int));
-	param_get(h->limit_thrust_upper, &(p->limit_thrust_upper));
-	param_get(h->limit_thrust_lower, &(p->limit_thrust_lower));
-	param_get(h->limit_yaw_step, &(p->limit_yaw_step));
-	param_get(h->manual_threshold, &(p->manual_threshold));
-	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
-	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
-	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
-
-	return OK;
-}
diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h
deleted file mode 100644
index d0c8fc722..000000000
--- a/src/examples/flow_position_control/flow_position_control_params.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 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 flow_position_control_params.h
- * 
- * Parameters for position controller
- */
-
-#include <systemlib/param/param.h>
-
-struct flow_position_control_params {
-	float pos_p;
-	float pos_d;
-	float height_p;
-	float height_i;
-	float height_d;
-	float height_rate;
-	float height_min;
-	float height_max;
-	float thrust_feedforward;
-	float limit_speed_x;
-	float limit_speed_y;
-	float limit_height_error;
-	float limit_thrust_int;
-	float limit_thrust_upper;
-	float limit_thrust_lower;
-	float limit_yaw_step;
-	float manual_threshold;
-	float rc_scale_pitch;
-	float rc_scale_roll;
-	float rc_scale_yaw;
-};
-
-struct flow_position_control_param_handles {
-	param_t pos_p;
-	param_t pos_d;
-	param_t height_p;
-	param_t height_i;
-	param_t height_d;
-	param_t height_rate;
-	param_t height_min;
-	param_t height_max;
-	param_t thrust_feedforward;
-	param_t limit_speed_x;
-	param_t limit_speed_y;
-	param_t limit_height_error;
-	param_t limit_thrust_int;
-	param_t limit_thrust_upper;
-	param_t limit_thrust_lower;
-	param_t limit_yaw_step;
-	param_t manual_threshold;
-	param_t rc_scale_pitch;
-	param_t rc_scale_roll;
-	param_t rc_scale_yaw;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct flow_position_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p);
diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk
deleted file mode 100644
index b10dc490a..000000000
--- a/src/examples/flow_position_control/module.mk
+++ /dev/null
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Build multirotor position control
-#
-
-MODULE_COMMAND	= flow_position_control
-
-SRCS		= flow_position_control_main.c \
-		  flow_position_control_params.c
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c26200048..5c0628a16 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1233,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[])
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
-						print_reject_arm("NOT ARMING: Press safety switch first.");
+						print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
 
 					} else if (status.main_state != MAIN_STATE_MANUAL) {
-						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
 						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
@@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
 				home.alt = global_position.alt;
 
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+				mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1848,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
+		/* this needs additional hints to the user - so let other messages pass and be spoken */
+		mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
 		tune_negative(true);
 		break;
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f1a353d5b..87309b238 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 				//      Allow if HIL_STATE_ON
 				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
 					if (mavlink_fd) {
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
+						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
 					}
 
 					valid_transition = false;
@@ -312,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		case HIL_STATE_OFF:
 
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
+			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
 			valid_transition = false;
 
 			break;
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index ebf4f3e8e..2f721bf1e 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -213,6 +213,7 @@ mixer_tick(void)
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 		in_mixer = false;
 
+		/* the pwm limit call takes care of out of band errors */
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 70ce76806..577cadfbb 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -89,6 +89,7 @@
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
 #include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
@@ -97,6 +98,36 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
+/**
+ * Logging rate.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 sets the minimum rate,
+ * any other value is interpreted as rate in Hertz. This
+ * parameter is only read out before logging starts (which
+ * commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_RATE, -1);
+
+/**
+ * Enable extended logging mode.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 disables extended
+ * logging mode, a value of 1 enables it. This
+ * parameter is only read out before logging starts
+ * (which commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_EXT, -1);
+
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
 	} else { \
@@ -112,12 +143,14 @@ static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
 static bool logwriter_should_exit = false;	/**< Logwriter thread exit flag */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
-static const int MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
+static const unsigned MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
+static const unsigned MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
 static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
 static const int MAX_WRITE_CHUNK = 512;
 static const int MIN_BYTES_TO_WRITE = 512;
 
+static bool _extended_logging = false;
+
 static const char *log_root = "/fs/microsd/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
@@ -218,6 +251,8 @@ static int create_log_dir(void);
  */
 static int open_log_file(void);
 
+static int open_perf_file(const char* str);
+
 static void
 sdlog2_usage(const char *reason)
 {
@@ -225,12 +260,13 @@ sdlog2_usage(const char *reason)
 		fprintf(stderr, "%s\n", reason);
 	}
 
-	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
+	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 8\n"
 		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
 		 "\t-a\tLog only when armed (can be still overriden by command)\n"
-		 "\t-t\tUse date/time for naming log directories and files\n");
+		 "\t-t\tUse date/time for naming log directories and files\n"
+		 "\t-x\tExtended logging");
 }
 
 /**
@@ -349,8 +385,8 @@ int create_log_dir()
 int open_log_file()
 {
 	/* string to hold the path to the log */
-	char log_file_name[16] = "";
-	char log_file_path[48] = "";
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
 
 	if (log_name_timestamp && gps_time != 0) {
 		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
@@ -378,7 +414,7 @@ int open_log_file()
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			warnx("all %d possible files exist already", MAX_NO_LOGFILE);
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -387,7 +423,58 @@ int open_log_file()
 
 	if (fd < 0) {
 		warn("failed opening log: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+
+	} else {
+		warnx("log file: %s", log_file_name);
+		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+	}
+
+	return fd;
+}
+
+int open_perf_file(const char* str)
+{
+	/* string to hold the path to the log */
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
+
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
+		snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+	} else {
+		unsigned file_number = 1; // start with file log001
+
+		/* look for the next file that does not exist */
+		while (file_number <= MAX_NO_LOGFILE) {
+			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
+			snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+			if (!file_exist(log_file_path)) {
+				break;
+			}
+
+			file_number++;
+		}
+
+		if (file_number > MAX_NO_LOGFILE) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			return -1;
+		}
+	}
+
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
+
+	if (fd < 0) {
+		warn("failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
 
 	} else {
 		warnx("log file: %s", log_file_name);
@@ -529,6 +616,12 @@ void sdlog2_start_log()
 		errx(1, "error creating logwriter thread");
 	}
 
+	/* write all performance counters */
+	int perf_fd = open_perf_file("preflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	logging_enabled = true;
 }
 
@@ -556,6 +649,12 @@ void sdlog2_stop_log()
 	logwriter_pthread = 0;
 	pthread_attr_destroy(&logwriter_attr);
 
+	/* write all performance counters */
+	int perf_fd = open_perf_file("postflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	sdlog2_status();
 }
 
@@ -572,7 +671,7 @@ int write_formats(int fd)
 	int written = 0;
 
 	/* fill message format packet for each format and write it */
-	for (int i = 0; i < log_formats_num; i++) {
+	for (unsigned i = 0; i < log_formats_num; i++) {
 		log_msg_format.body = log_formats[i];
 		written += write(fd, &log_msg_format, sizeof(log_msg_format));
 	}
@@ -679,7 +778,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
+	while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(optarg, NULL, 10);
@@ -715,6 +814,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_name_timestamp = true;
 			break;
 
+		case 'x':
+			_extended_logging = true;
+			break;
+
 		case '?':
 			if (optopt == 'c') {
 				warnx("option -%c requires an argument", optopt);
@@ -741,6 +844,44 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	gps_time = 0;
 
+	/* interpret logging params */
+
+	param_t log_rate_ph = param_find("SDLOG_RATE");
+
+	if (log_rate_ph != PARAM_INVALID) {
+		int32_t param_log_rate;
+		param_get(log_rate_ph, &param_log_rate);
+
+		if (param_log_rate > 0) {
+
+			/* we can't do more than ~ 500 Hz, even with a massive buffer */
+			if (param_log_rate > 500) {
+				param_log_rate = 500;
+			}
+
+			sleep_delay = 1000000 / param_log_rate;
+		} else if (param_log_rate == 0) {
+			/* we need at minimum 10 Hz to be able to see anything */
+			sleep_delay = 1000000 / 10;
+		}
+	}
+
+	param_t log_ext_ph = param_find("SDLOG_EXT");
+
+	if (log_ext_ph != PARAM_INVALID) {
+
+		int32_t param_log_extended;
+		param_get(log_ext_ph, &param_log_extended);
+
+		if (param_log_extended > 0) {
+			_extended_logging = true;
+		} else if (param_log_extended == 0) {
+			_extended_logging = false;
+		}
+		/* any other value means to ignore the parameter, so no else case */
+
+	}
+
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
@@ -834,8 +975,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
-			struct log_GSN0_s log_GSN0;
-			struct log_GSN1_s log_GSN1;
+			struct log_GS0A_s log_GS0A;
+			struct log_GS0B_s log_GS0B;
+			struct log_GS1A_s log_GS1A;
+			struct log_GS1B_s log_GS1B;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -969,8 +1112,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
-		/* --- GPS POSITION --- */
+		/* --- GPS POSITION - UNIT #1 --- */
 		if (gps_pos_updated) {
+
+			float snr_mean = 0.0f;
+
+			for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
+				snr_mean += buf_gps_pos.satellite_snr[i];
+			}
+
+			snr_mean /= buf_gps_pos.satellites_visible;
+
 			log_msg.msg_type = LOG_GPS_MSG;
 			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@@ -983,19 +1135,48 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
 			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
 			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
+			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
+			log_msg.body.log_GPS.snr_mean = snr_mean;
+			log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
+			log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
 			LOGBUFFER_WRITE_AND_COUNT(GPS);
 
-			/* log the SNR of each satellite for a detailed view of signal quality */
-			log_msg.msg_type = LOG_GSN0_MSG;
-			/* pick the smaller number so we do not overflow any of the arrays */
-			unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
-			unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]);
-			unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr;
+			if (_extended_logging) {
+				/* log the SNR of each satellite for a detailed view of signal quality */
+				unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
+				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
+
+				log_msg.msg_type = LOG_GS0A_MSG;
+				memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
+				/* fill set A */
+				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+
+					int satindex = buf_gps_pos.satellite_prn[i] - 1;
 
-			for (unsigned i = 0; i < sat_max_snr; i++) {
-				log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+					}
+				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0A);
+
+				log_msg.msg_type = LOG_GS0B_MSG;
+				memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+				/* fill set B */
+				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+
+					/* get second bank of satellites, thus deduct bank size from index */
+					int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
+
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+					}
+				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0B);
 			}
-			LOGBUFFER_WRITE_AND_COUNT(GSN0);
 		}
 
 		/* --- SENSOR COMBINED --- */
@@ -1340,6 +1521,7 @@ void sdlog2_status()
 	float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
 
 	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+	warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
 	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
 }
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 90025b9ff..f4d88f079 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -139,6 +139,10 @@ struct log_GPS_s {
 	float vel_e;
 	float vel_d;
 	float cog;
+	uint8_t sats;
+	uint16_t snr_mean;
+	uint16_t noise_per_ms;
+	uint16_t jamming_indicator;
 };
 
 /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
@@ -318,16 +322,28 @@ struct log_VICN_s {
 	float yaw;
 };
 
-/* --- GSN0 - GPS SNR #0 --- */
-#define LOG_GSN0_MSG 26
-struct log_GSN0_s {
-	uint8_t satellite_snr[16];			/**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
+/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
+#define LOG_GS0A_MSG 26
+struct log_GS0A_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
 };
 
-/* --- GSN1 - GPS SNR #1 --- */
-#define LOG_GSN1_MSG 27
-struct log_GSN1_s {
-	uint8_t satellite_snr[16];			/**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
+/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
+#define LOG_GS0B_MSG 27
+struct log_GS0B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
+#define LOG_GS1A_MSG 28
+struct log_GS1A_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
+#define LOG_GS1B_MSG 29
+struct log_GS1B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
 };
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -363,7 +379,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
 	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
-	LOG_FORMAT(GPS, "QBffLLfffff",		"GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
+	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
@@ -381,8 +397,10 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
-	LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 4ad21d818..092c0e2b0 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -351,9 +351,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		scale_out = 1.0f;
 	}
 
-	/* scale outputs to range _idle_speed..1 */
+	/* scale outputs to range _idle_speed..1, and do final limiting */
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out);
+		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
 	}
 
 	return _rotor_count;
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index b4ca0ed3e..22182e39e 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -282,12 +282,18 @@ perf_reset(perf_counter_t handle)
 void
 perf_print_counter(perf_counter_t handle)
 {
+	perf_print_counter_fd(0, handle);
+}
+
+void
+perf_print_counter_fd(int fd, perf_counter_t handle)
+{
 	if (handle == NULL)
 		return;
 
 	switch (handle->type) {
 	case PC_COUNT:
-		printf("%s: %llu events\n",
+		dprintf(fd, "%s: %llu events\n",
 		       handle->name,
 		       ((struct perf_ctr_count *)handle)->event_count);
 		break;
@@ -295,7 +301,7 @@ perf_print_counter(perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -308,7 +314,7 @@ perf_print_counter(perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
@@ -349,12 +355,12 @@ perf_event_count(perf_counter_t handle)
 }
 
 void
-perf_print_all(void)
+perf_print_all(int fd)
 {
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != NULL) {
-		perf_print_counter(handle);
+		perf_print_counter_fd(fd, handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index d8f69fdbf..6835ee4a2 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -121,16 +121,26 @@ __EXPORT extern void		perf_cancel(perf_counter_t handle);
 __EXPORT extern void		perf_reset(perf_counter_t handle);
 
 /**
- * Print one performance counter.
+ * Print one performance counter to stdout
  *
  * @param handle		The counter to print.
  */
 __EXPORT extern void		perf_print_counter(perf_counter_t handle);
 
 /**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
+
+/**
  * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_all(void);
+__EXPORT extern void		perf_print_all(int fd);
 
 /**
  * Reset all of the performance counters.
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index 190b315f1..ea5ba9e52 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -136,12 +136,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 					}
 
 					effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+
+					/* last line of defense against invalid inputs */
+					if (effective_pwm[i] < ramp_min_pwm) {
+						effective_pwm[i] = ramp_min_pwm;
+					} else if (effective_pwm[i] > max_pwm[i]) {
+						effective_pwm[i] = max_pwm[i];
+					}
 				}
 			}
 			break;
 		case PWM_LIMIT_STATE_ON:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+
+				/* last line of defense against invalid inputs */
+				if (effective_pwm[i] < min_pwm[i]) {
+					effective_pwm[i] = min_pwm[i];
+				} else if (effective_pwm[i] > max_pwm[i]) {
+					effective_pwm[i] = max_pwm[i];
+				}
 			}
 			break;
 		default:
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 794c3f8bc..5924a324d 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -68,6 +68,9 @@ struct vehicle_gps_position_s {
 	float eph_m;					/**< GPS HDOP horizontal dilution of position in m */
 	float epv_m;					/**< GPS VDOP horizontal dilution of position in m */
 
+	unsigned noise_per_ms;				/**< */
+	unsigned jamming_indicator;			/**< */
+
 	uint64_t timestamp_velocity;			/**< Timestamp for velocity informations */
 	float vel_m_s;					/**< GPS ground speed (m/s) */
 	float vel_n_m_s;				/**< GPS ground speed in m/s */
@@ -85,7 +88,7 @@ struct vehicle_gps_position_s {
 	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
 	uint8_t satellite_elevation[20]; 		/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
 	uint8_t satellite_azimuth[20];			/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
-	uint8_t satellite_snr[20];			/**< Signal to noise ratio of satellite   */
+	uint8_t satellite_snr[20];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
 	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
 };
 
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index e6d4b763b..4a97d328c 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -1,8 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *   Author: Julian Oes <joes@student.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
@@ -94,7 +92,6 @@ do_device(int argc, char *argv[])
 	}
 
 	int	fd;
-	int	ret;
 
 	fd = open(argv[0], 0);
 
@@ -104,6 +101,8 @@ do_device(int argc, char *argv[])
 
 	} else {
 
+		int	ret;
+
 		if (argc == 2 && !strcmp(argv[1], "block")) {
 
 			/* disable the device publications */
@@ -132,7 +131,6 @@ static void
 do_gyro(int argc, char *argv[])
 {
 	int	fd;
-	int	ret;
 
 	fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -142,6 +140,8 @@ do_gyro(int argc, char *argv[])
 
 	} else {
 
+		int ret;
+
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the gyro internal sampling rate up to at least i Hz */
@@ -173,8 +173,13 @@ do_gyro(int argc, char *argv[])
 				warnx("gyro self test FAILED! Check calibration:");
 				struct gyro_scale scale;
 				ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
-				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+
+				if (ret) {
+					err(1, "failed getting gyro scale");
+				}
+
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
 			} else {
 				warnx("gyro calibration and self test OK");
 			}
@@ -199,7 +204,6 @@ static void
 do_mag(int argc, char *argv[])
 {
 	int fd;
-	int ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -209,6 +213,8 @@ do_mag(int argc, char *argv[])
 
 	} else {
 
+		int ret;
+
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the mag internal sampling rate up to at least i Hz */
@@ -240,8 +246,13 @@ do_mag(int argc, char *argv[])
 				warnx("mag self test FAILED! Check calibration:");
 				struct mag_scale scale;
 				ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
-				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+
+				if (ret) {
+					err(ret, "failed getting mag scale");
+				}
+
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
 			} else {
 				warnx("mag calibration and self test OK");
 			}
@@ -266,7 +277,6 @@ static void
 do_accel(int argc, char *argv[])
 {
 	int	fd;
-	int	ret;
 
 	fd = open(ACCEL_DEVICE_PATH, 0);
 
@@ -276,6 +286,8 @@ do_accel(int argc, char *argv[])
 
 	} else {
 
+		int ret;
+
 		if (argc == 2 && !strcmp(argv[0], "sampling")) {
 
 			/* set the accel internal sampling rate up to at least i Hz */
@@ -307,8 +319,13 @@ do_accel(int argc, char *argv[])
 				warnx("accel self test FAILED! Check calibration:");
 				struct accel_scale scale;
 				ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
-				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_offset, scale.y_offset, scale.z_offset);
-				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", scale.x_scale, scale.y_scale, scale.z_scale);
+
+				if (ret) {
+					err(ret, "failed getting accel scale");
+				}
+
+				warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset);
+				warnx("scale:   X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
 			} else {
 				warnx("accel calibration and self test OK");
 			}
diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c
index b69ea597b..b08a2e3b7 100644
--- a/src/systemcmds/perf/perf.c
+++ b/src/systemcmds/perf/perf.c
@@ -73,7 +73,7 @@ int perf_main(int argc, char *argv[])
 		return -1;
 	}
 
-	perf_print_all();
+	perf_print_all(0 /* stdout */);
 	fflush(stdout);
 	return 0;
 }
diff --git a/src/systemcmds/tests/test_mtd.c b/src/systemcmds/tests/test_mtd.c
index bac9efbdb..cdb4362ba 100644
--- a/src/systemcmds/tests/test_mtd.c
+++ b/src/systemcmds/tests/test_mtd.c
@@ -57,6 +57,8 @@
 #define PARAM_FILE_NAME "/fs/mtd_params"
 
 static int check_user_abort(int fd);
+static void print_fail(void);
+static void print_success(void);
 
 int check_user_abort(int fd) {
 	/* check if user wants to abort */
@@ -126,7 +128,7 @@ test_mtd(int argc, char *argv[])
 		uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
 
 		/* fill write buffer with known values */
-		for (int i = 0; i < sizeof(write_buf); i++) {
+		for (unsigned i = 0; i < sizeof(write_buf); i++) {
 			/* this will wrap, but we just need a known value with spacing */
 			write_buf[i] = i+11;
 		}
@@ -137,11 +139,14 @@ test_mtd(int argc, char *argv[])
 		int fd = open(PARAM_FILE_NAME, O_RDONLY);
 		int rret = read(fd, read_buf, chunk_sizes[c]);
 		close(fd);
+		if (rret <= 0) {
+			err(1, "read error");
+		}
 
 		fd = open(PARAM_FILE_NAME, O_WRONLY);
 
 		printf("printing 2 percent of the first chunk:\n");
-		for (int i = 0; i < sizeof(read_buf) / 50; i++) {
+		for (unsigned i = 0; i < sizeof(read_buf) / 50; i++) {
 			printf("%02X", read_buf[i]);
 		}
 		printf("\n");
@@ -171,9 +176,9 @@ test_mtd(int argc, char *argv[])
 
 		/* read back data for validation */
 		for (unsigned i = 0; i < iterations; i++) {
-			int rret = read(fd, read_buf, chunk_sizes[c]);
+			int rret2 = read(fd, read_buf, chunk_sizes[c]);
 
-			if (rret != chunk_sizes[c]) {
+			if (rret2 != (int)chunk_sizes[c]) {
 				warnx("READ ERROR!");
 				print_fail();
 				return 1;
@@ -182,7 +187,7 @@ test_mtd(int argc, char *argv[])
 			/* compare value */
 			bool compare_ok = true;
 
-			for (int j = 0; j < chunk_sizes[c]; j++) {
+			for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 				if (read_buf[j] != write_buf[j]) {
 					warnx("COMPARISON ERROR: byte %d", j);
 					print_fail();
@@ -211,7 +216,7 @@ test_mtd(int argc, char *argv[])
 	char ffbuf[64];
 	memset(ffbuf, 0xFF, sizeof(ffbuf));
 	int fd = open(PARAM_FILE_NAME, O_WRONLY);
-	for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
+	for (unsigned i = 0; i < file_size / sizeof(ffbuf); i++) {
 		int ret = write(fd, ffbuf, sizeof(ffbuf));
 
 		if (ret != sizeof(ffbuf)) {
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index fe22f6177..e3f26924f 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -235,7 +235,7 @@ test_perf(int argc, char *argv[])
 	printf("perf: expect count of 1\n");
 	perf_print_counter(ec);
 	printf("perf: expect at least two counters\n");
-	perf_print_all();
+	perf_print_all(0);
 
 	perf_free(cc);
 	perf_free(ec);