Merge branch '6point_accel_calibration' into seatbelt_multirotor

36 files changed, 575 insertions, 176 deletions

diff --git a/ROMFS/Makefile b/ROMFS/Makefile
index ed39ab825..11a4650fa 100644
--- a/ROMFS/Makefile
+++ b/ROMFS/Makefile
@@ -32,6 +32,7 @@ ROMFS_FSSPEC	:= $(SRCROOT)/scripts/rcS~init.d/rcS \
 		   $(SRCROOT)/mixers/FMU_RET.mix~mixers/FMU_ERT.mix \
 		   $(SRCROOT)/mixers/FMU_quad_x.mix~mixers/FMU_quad_x.mix \
 		   $(SRCROOT)/mixers/FMU_quad_+.mix~mixers/FMU_quad_+.mix \
+                   $(SRCROOT)/mixers/FMU_quad_v.mix~mixers/FMU_quad_v.mix \
 		   $(SRCROOT)/mixers/FMU_hex_x.mix~mixers/FMU_hex_x.mix \
 		   $(SRCROOT)/mixers/FMU_hex_+.mix~mixers/FMU_hex_+.mix \
 		   $(SRCROOT)/mixers/FMU_octo_x.mix~mixers/FMU_octo_x.mix \
diff --git a/ROMFS/mixers/FMU_quad_v.mix b/ROMFS/mixers/FMU_quad_v.mix
new file mode 100644
index 000000000..2a4a0f341
--- /dev/null
+++ b/ROMFS/mixers/FMU_quad_v.mix
@@ -0,0 +1,7 @@
+Multirotor mixer for PX4FMU
+===========================
+
+This file defines a single mixer for a quadrotor in the V configuration.  All controls
+are mixed 100%.
+
+R: 4v 10000 10000 10000 0
diff --git a/apps/attitude_estimator_ekf/Makefile b/apps/attitude_estimator_ekf/Makefile
index 734af7cc1..46a54c660 100755
--- a/apps/attitude_estimator_ekf/Makefile
+++ b/apps/attitude_estimator_ekf/Makefile
@@ -35,8 +35,9 @@ APPNAME		 = attitude_estimator_ekf
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
-CSRCS		 = attitude_estimator_ekf_main.c \
-		   attitude_estimator_ekf_params.c \
+CXXSRCS		 = attitude_estimator_ekf_main.cpp
+
+CSRCS		 = attitude_estimator_ekf_params.c \
 		   codegen/eye.c \
 		   codegen/attitudeKalmanfilter.c \
 		   codegen/mrdivide.c \
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index bd972f03f..1a50dde0f 100755
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -66,11 +66,17 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 #include "codegen/attitudeKalmanfilter_initialize.h"
 #include "codegen/attitudeKalmanfilter.h"
 #include "attitude_estimator_ekf_params.h"
+#ifdef __cplusplus
+}
+#endif
 
-__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
+extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;		/**< Deamon status flag */
@@ -265,10 +271,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* Main loop*/
 	while (!thread_should_exit) {
 
-		struct pollfd fds[2] = {
-			{ .fd = sub_raw,   .events = POLLIN },
-			{ .fd = sub_params, .events = POLLIN }
-		};
+		struct pollfd fds[2];
+		fds[0].fd = sub_raw;
+		fds[0].events = POLLIN;
+		fds[1].fd = sub_params;
+		fds[1].events = POLLIN;
 		int ret = poll(fds, 2, 1000);
 
 		if (ret < 0) {
diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c
new file mode 100644
index 000000000..180736908
--- /dev/null
+++ b/apps/commander/accelerometer_calibration.c
@@ -0,0 +1,418 @@
+/*
+ * accelerometer_calibration.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Transform acceleration vector to true orientation and scale
+ *
+ * * * * Model * * *
+ * accel_corr = accel_T * (accel_raw - accel_offs)
+ *
+ * accel_corr[3] - fully corrected acceleration vector in body frame
+ * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform
+ * accel_raw[3]  - raw acceleration vector
+ * accel_offs[3] - acceleration offset vector
+ *
+ * * * * Calibration * * *
+ *
+ * Reference vectors
+ * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
+ *                        | -g  0  0 |     // nose down
+ *                        |  0  g  0 |     // left side down
+ *                        |  0 -g  0 |     // right side down
+ *                        |  0  0  g |     // on back
+ *                        [  0  0 -g ]     // level
+ * accel_raw_ref[6][3]
+ *
+ * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5
+ *
+ * 6 reference vectors * 3 axes = 18 equations
+ * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants
+ *
+ * Find accel_offs
+ *
+ * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
+ *
+ *
+ * Find accel_T
+ *
+ * 9 unknown constants
+ * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations
+ *
+ * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2
+ *
+ * Solve separate system for each row of accel_T:
+ *
+ * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2
+ *
+ * A * x = b
+ *
+ * x = [ accel_T[0][i] ]
+ *     | accel_T[1][i] |
+ *     [ accel_T[2][i] ]
+ *
+ * b = [ accel_corr_ref[0][i] ]	// One measurement per axis is enough
+ *     | accel_corr_ref[2][i] |
+ *     [ accel_corr_ref[4][i] ]
+ *
+ * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2
+ *
+ * Matrix A is common for all three systems:
+ * A = [ a[0][0]  a[0][1]  a[0][2] ]
+ *     | a[2][0]  a[2][1]  a[2][2] |
+ *     [ a[4][0]  a[4][1]  a[4][2] ]
+ *
+ * x = A^-1 * b
+ *
+ * accel_T = A^-1 * g
+ * g = 9.80665
+ */
+
+#include "accelerometer_calibration.h"
+
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_accel.h>
+#include <systemlib/conversions.h>
+#include <mavlink/mavlink_log.h>
+
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+int detect_orientation(int mavlink_fd, int sub_sensor_combined);
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
+int mat_invert3(float src[3][3], float dst[3][3]);
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
+
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) {
+	/* announce change */
+	mavlink_log_info(mavlink_fd, "accel calibration started");
+	/* set to accel calibration mode */
+	status->flag_preflight_accel_calibration = true;
+	state_machine_publish(status_pub, status, mavlink_fd);
+
+	/* measure and calculate offsets & scales */
+	float accel_offs[3];
+	float accel_scale[3];
+	int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale);
+
+	if (res == OK) {
+		/* measurements complete successfully, set parameters */
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
+			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
+			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
+			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
+			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
+			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
+		}
+
+		int fd = open(ACCEL_DEVICE_PATH, 0);
+		struct accel_scale ascale = {
+			accel_offs[0],
+			accel_scale[0],
+			accel_offs[1],
+			accel_scale[1],
+			accel_offs[2],
+			accel_scale[2],
+		};
+
+		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+			warn("WARNING: failed to set scale / offsets for accel");
+
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warn("WARNING: auto-save of params to storage failed");
+		}
+
+		mavlink_log_info(mavlink_fd, "accel calibration done");
+		tune_confirm();
+		sleep(2);
+		tune_confirm();
+		sleep(2);
+		/* third beep by cal end routine */
+	} else {
+		/* measurements error */
+		mavlink_log_info(mavlink_fd, "accel calibration aborted");
+		tune_error();
+		sleep(2);
+	}
+
+	/* exit accel calibration mode */
+	status->flag_preflight_accel_calibration = false;
+	state_machine_publish(status_pub, status, mavlink_fd);
+}
+
+int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+	const int samples_num = 2500;
+	float accel_ref[6][3];
+	bool data_collected[6] = { false, false, false, false, false, false };
+	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
+
+	/* reset existing calibration */
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	struct accel_scale ascale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+	int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
+	close(fd);
+
+	if (OK != ioctl_res) {
+		warn("ERROR: failed to set scale / offsets for accel");
+		return ERROR;
+	}
+
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	while (true) {
+		bool done = true;
+		char str[80];
+		int str_ptr;
+		str_ptr = sprintf(str, "keep vehicle still:");
+		for (int i = 0; i < 6; i++) {
+			if (!data_collected[i]) {
+				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
+				done = false;
+			}
+		}
+		if (done)
+			break;
+		mavlink_log_info(mavlink_fd, str);
+
+		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+		if (orient < 0)
+			return ERROR;
+
+		sprintf(str, "meas started: %s", orientation_strs[orient]);
+		mavlink_log_info(mavlink_fd, str);
+		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
+		str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
+		mavlink_log_info(mavlink_fd, str);
+		data_collected[orient] = true;
+		tune_confirm();
+	}
+	close(sensor_combined_sub);
+
+	/* calculate offsets and rotation+scale matrix */
+	float accel_T[3][3];
+	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+	if (res != 0) {
+		mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error");
+		return ERROR;
+	}
+
+	/* convert accel transform matrix to scales,
+	 * rotation part of transform matrix is not used by now
+	 */
+	for (int i = 0; i < 3; i++) {
+		accel_scale[i] = accel_T[i][i];
+	}
+
+	return OK;
+}
+
+/*
+ * Wait for vehicle become still and detect it's orientation.
+ *
+ * @return 0..5 according to orientation when vehicle is still and ready for measurements,
+ * ERROR if vehicle is not still after 10s or orientation error is more than 20%
+ */
+int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
+	struct sensor_combined_s sensor;
+	/* exponential moving average of accel */
+	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
+	/* max-hold dispersion of accel */
+	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
+	float accel_len2 = 0.0f;
+	/* EMA time constant in seconds*/
+	float ema_len = 0.2f;
+	/* set "still" threshold to 0.005 m/s^2 */
+	float still_thr2 = pow(0.05f / CONSTANTS_ONE_G, 2);
+	/* set accel error threshold to 30% of accel vector length */
+	float accel_err_thr = 0.3f;
+	/* still time required in us */
+	int64_t still_time = 2000000;
+	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+
+	hrt_abstime t_start = hrt_absolute_time();
+	/* set deadline to 20s */
+	hrt_abstime timeout = 20000000;
+	hrt_abstime t_timeout = t_start + timeout;
+	hrt_abstime t = t_start;
+	hrt_abstime t_prev = t_start;
+	hrt_abstime t_still = 0;
+	while (true) {
+		/* wait blocking for new data */
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
+			t = hrt_absolute_time();
+			float dt = (t - t_prev) / 1000000.0f;
+			t_prev = t;
+			float w = dt / ema_len;
+			for (int i = 0; i < 3; i++) {
+				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
+				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
+				d = d * d;
+				accel_disp[i] = accel_disp[i] * (1.0f - w);
+				if (d > accel_disp[i])
+					accel_disp[i] = d;
+			}
+			accel_len2 = accel_ema[0] * accel_ema[0] + accel_ema[1] * accel_ema[1] + accel_ema[2] * accel_ema[2];
+			float still_thr_raw2 = still_thr2 * accel_len2;
+			if (  accel_disp[0] < still_thr_raw2 &&
+				  accel_disp[1] < still_thr_raw2 &&
+				  accel_disp[2] < still_thr_raw2 ) {
+				/* is still now */
+				if (t_still == 0) {
+					/* first time */
+					mavlink_log_info(mavlink_fd, "still...");
+					t_still = t;
+					t_timeout = t + timeout;
+				} else {
+					/* still since t_still */
+					if ((int64_t) t - (int64_t) t_still > still_time) {
+						/* vehicle is still, exit from the loop to detection of its orientation */
+						break;
+					}
+				}
+			} else if ( accel_disp[0] > still_thr_raw2 * 2.0f ||
+					    accel_disp[1] > still_thr_raw2 * 2.0f ||
+					    accel_disp[2] > still_thr_raw2 * 2.0f) {
+				/* not still, reset still start time */
+				if (t_still != 0) {
+					mavlink_log_info(mavlink_fd, "moving...");
+					t_still = 0;
+				}
+			}
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_info(mavlink_fd, "ERROR: poll failure");
+			return -3;
+		}
+		if (t > t_timeout) {
+			mavlink_log_info(mavlink_fd, "ERROR: timeout");
+			return -1;
+		}
+	}
+
+	float accel_len = sqrt(accel_len2);
+	float accel_err_thr_raw = accel_len * accel_err_thr;
+	if (  fabs(accel_ema[0] - accel_len) < accel_err_thr_raw &&
+		  fabs(accel_ema[1]) < accel_err_thr_raw &&
+		  fabs(accel_ema[2]) < accel_err_thr_raw  )
+		return 0;	// [ g, 0, 0 ]
+	if (  fabs(accel_ema[0] + accel_len) < accel_err_thr_raw &&
+		  fabs(accel_ema[1]) < accel_err_thr_raw &&
+		  fabs(accel_ema[2]) < accel_err_thr_raw  )
+		return 1;	// [ -g, 0, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr_raw &&
+		  fabs(accel_ema[1] - accel_len) < accel_err_thr_raw &&
+		  fabs(accel_ema[2]) < accel_err_thr_raw  )
+		return 2;	// [ 0, g, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr_raw &&
+		  fabs(accel_ema[1] + accel_len) < accel_err_thr_raw &&
+		  fabs(accel_ema[2]) < accel_err_thr_raw  )
+		return 3;	// [ 0, -g, 0 ]
+	if (  fabs(accel_ema[0]) < accel_err_thr_raw &&
+		  fabs(accel_ema[1]) < accel_err_thr_raw &&
+		  fabs(accel_ema[2] - accel_len) < accel_err_thr_raw  )
+		return 4;	// [ 0, 0, g ]
+	if (  fabs(accel_ema[0]) < accel_err_thr_raw &&
+		  fabs(accel_ema[1]) < accel_err_thr_raw &&
+		  fabs(accel_ema[2] + accel_len) < accel_err_thr_raw  )
+		return 5;	// [ 0, 0, -g ]
+
+	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+
+	return -2;	// Can't detect orientation
+}
+
+/*
+ * Read specified number of accelerometer samples, calculate average and dispersion.
+ */
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
+	struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
+	int count = 0;
+	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
+
+	while (count < samples_num) {
+		int poll_ret = poll(fds, 1, 1000);
+		if (poll_ret == 1) {
+			struct sensor_combined_s sensor;
+			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+			for (int i = 0; i < 3; i++)
+				accel_sum[i] += sensor.accelerometer_m_s2[i];
+			count++;
+		} else {
+			return ERROR;
+		}
+	}
+
+	for (int i = 0; i < 3; i++) {
+		accel_avg[i] = accel_sum[i] / count;
+	}
+
+	return OK;
+}
+
+int mat_invert3(float src[3][3], float dst[3][3]) {
+	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
+			    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+			    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+	if (det == 0.0)
+		return ERROR;	// Singular matrix
+
+	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
+	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
+	dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det;
+	dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det;
+	dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det;
+	dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det;
+	dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det;
+	dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det;
+	dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det;
+
+	return OK;
+}
+
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
+	/* calculate offsets */
+	for (int i = 0; i < 3; i++) {
+		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
+	}
+
+	/* fill matrix A for linear equations system*/
+	float mat_A[3][3];
+	memset(mat_A, 0, sizeof(mat_A));
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			float a = accel_ref[i * 2][j] - accel_offs[j];
+			mat_A[i][j] = a;
+		}
+	}
+
+	/* calculate inverse matrix for A */
+	float mat_A_inv[3][3];
+	if (mat_invert3(mat_A, mat_A_inv) != OK)
+		return ERROR;
+
+	/* copy results to accel_T */
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			/* simplify matrices mult because b has only one non-zero element == g at index i */
+			accel_T[j][i] = mat_A_inv[j][i] * g;
+		}
+	}
+
+	return OK;
+}
diff --git a/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h
new file mode 100644
index 000000000..c0169c2a1
--- /dev/null
+++ b/apps/commander/accelerometer_calibration.h
@@ -0,0 +1,16 @@
+/*
+ * accelerometer_calibration.h
+ *
+ *  Created on: 25.04.2013
+ *      Author: ton
+ */
+
+#ifndef ACCELEROMETER_CALIBRATION_H_
+#define ACCELEROMETER_CALIBRATION_H_
+
+#include <stdint.h>
+#include <uORB/topics/vehicle_status.h>
+
+void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd);
+
+#endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 3c74c15ef..c66301d48 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -94,7 +94,7 @@
 #include <drivers/drv_baro.h>
 
 #include "calibration_routines.h"
-
+#include "accelerometer_calibration.h"
 
 PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
 //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0);	/**< Go into high-level failsafe after 0 ms */
@@ -158,7 +158,6 @@ static int led_off(int led);
 static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_rc_calibration(int status_pub, struct vehicle_status_s *status);
-static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
 static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
 
 int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
@@ -666,126 +665,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
 	close(sub_sensor_combined);
 }
 
-void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it level and still");
-	/* set to accel calibration mode */
-	status->flag_preflight_accel_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float accel_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	int fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null))
-		warn("WARNING: failed to set scale / offsets for accel");
-
-	close(fd);
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			accel_offset[0] += raw.accelerometer_m_s2[0];
-			accel_offset[1] += raw.accelerometer_m_s2[1];
-			accel_offset[2] += raw.accelerometer_m_s2[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
-			return;
-		}
-	}
-
-	accel_offset[0] = accel_offset[0] / calibration_count;
-	accel_offset[1] = accel_offset[1] / calibration_count;
-	accel_offset[2] = accel_offset[2] / calibration_count;
-
-	if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) {
-
-		/* add the removed length from x / y to z, since we induce a scaling issue else */
-		float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]);
-
-		/* if length is correct, zero results here */
-		accel_offset[2] = accel_offset[2] + total_len;
-
-		float scale = 9.80665f / total_len;
-
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2]))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(scale))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(scale))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!");
-		}
-
-		fd = open(ACCEL_DEVICE_PATH, 0);
-		struct accel_scale ascale = {
-			accel_offset[0],
-			scale,
-			accel_offset[1],
-			scale,
-			accel_offset[2],
-			scale,
-		};
-
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-			warn("WARNING: failed to set scale / offsets for accel");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)");
-	}
-
-	/* exit accel calibration mode */
-	status->flag_preflight_accel_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_sensor_combined);
-}
-
 void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
 {
 	/* announce change */
@@ -1040,7 +919,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
 					mavlink_log_info(mavlink_fd, "CMD starting accel cal");
 					tune_confirm();
-					do_accel_calibration(status_pub, &current_status);
+					do_accel_calibration(status_pub, &current_status, mavlink_fd);
 					tune_confirm();
 					mavlink_log_info(mavlink_fd, "CMD finished accel cal");
 					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp
index 56265995f..3fabfd9a5 100644
--- a/apps/drivers/blinkm/blinkm.cpp
+++ b/apps/drivers/blinkm/blinkm.cpp
@@ -92,6 +92,7 @@
 
 #include <nuttx/config.h>
 
+#include <arch/board/board.h>
 #include <drivers/device/i2c.h>
 
 #include <sys/types.h>
@@ -841,7 +842,7 @@ int
 blinkm_main(int argc, char *argv[])
 {
 
-	int i2cdevice = 3;
+	int i2cdevice = PX4_I2C_BUS_EXPANSION;
 	int blinkmadr = 9;
 
 	int x;
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index 0d4894b8d..c150f5271 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -33,7 +33,14 @@
  *
  ****************************************************************************/
 
-/* @file U-Blox protocol implementation */
+/**
+ * @file ubx.cpp
+ *
+ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ */
 
 #include <unistd.h>
 #include <stdio.h>
@@ -633,16 +640,17 @@ UBX::handle_message()
 		}
 
 		case NAV_VELNED: {
-//			printf("GOT NAV_VELNED MESSAGE\n");
 
 			if (!_waiting_for_ack) {
+				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
 				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
 
 				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
-				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
-				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
-				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
+				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
 				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
 				_gps_position->vel_ned_valid = true;
 				_gps_position->timestamp_velocity = hrt_absolute_time();
 			}
diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c
index bb67d5e6d..cec0c49ff 100644
--- a/apps/drivers/stm32/drv_hrt.c
+++ b/apps/drivers/stm32/drv_hrt.c
@@ -125,7 +125,7 @@
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM8CC
 # define HRT_TIMER_CLOCK	STM32_APB2_TIM8_CLKIN
 # if CONFIG_STM32_TIM8
-#  error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6
+#  error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=8
 # endif
 #elif HRT_TIMER == 9
 # define HRT_TIMER_BASE		STM32_TIM9_BASE
diff --git a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
index baa652d8a..ac5511e60 100644
--- a/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/apps/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -494,7 +494,7 @@ ToneAlarm::init()
 		return ret;
 
 	/* configure the GPIO to the idle state */
-	stm32_configgpio(GPIO_TONE_ALARM);
+	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
 
 	/* clock/power on our timer */
 	modifyreg32(STM32_RCC_APB1ENR, 0, TONE_ALARM_CLOCK_ENABLE);
@@ -606,6 +606,8 @@ ToneAlarm::start_note(unsigned note)
 	rEGR = GTIM_EGR_UG;	// force a reload of the period
 	rCCER |= TONE_CCER;	// enable the output
 
+	// configure the GPIO to enable timer output
+	stm32_configgpio(GPIO_TONE_ALARM);
 }
 
 void
@@ -616,10 +618,8 @@ ToneAlarm::stop_note()
 
 	/*
 	 * Make sure the GPIO is not driving the speaker.
-	 *
-	 * XXX this presumes PX4FMU and the onboard speaker driver FET.
 	 */
-	stm32_gpiowrite(GPIO_TONE_ALARM, 0);
+	stm32_configgpio(GPIO_TONE_ALARM_IDLE);
 }
 
 void
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 6df454a55..4ef150da1 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -355,6 +355,8 @@ int KalmanNav::predictState(float dt)
 		float LDot = vN / R;
 		float lDot = vE / (cosLSing * R);
 		float rotRate = 2 * omega + lDot;
+
+		// XXX position prediction using speed
 		float vNDot = fN - vE * rotRate * sinL +
 			      vD * LDot;
 		float vDDot = fD - vE * rotRate * cosL -
diff --git a/apps/examples/kalman_demo/Makefile b/apps/examples/kalman_demo/Makefile
index 99c34d934..6c592d645 100644
--- a/apps/examples/kalman_demo/Makefile
+++ b/apps/examples/kalman_demo/Makefile
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#   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
@@ -32,7 +32,7 @@
 ############################################################################
 
 #
-# Basic example application
+# Full attitude / position Extended Kalman Filter
 #
 
 APPNAME		 = kalman_demo
diff --git a/apps/examples/nsh/Makefile b/apps/examples/nsh/Makefile
index c7d212fc2..ad687958f 100644
--- a/apps/examples/nsh/Makefile
+++ b/apps/examples/nsh/Makefile
@@ -64,6 +64,8 @@ ROOTDEPPATH	= --dep-path .
 
 VPATH		= 
 
+MAXOPTIMIZATION = -Os
+
 all: .built
 .PHONY: clean depend distclean
 
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index a30f0bf3c..22c2fcdad 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -477,25 +477,27 @@ handle_message(mavlink_message_t *msg)
 
 			/* gps */
 			hil_gps.timestamp_position = gps.time_usec;
-//			hil_gps.counter = hil_counter++;
 			hil_gps.time_gps_usec = gps.time_usec;
 			hil_gps.lat = gps.lat;
 			hil_gps.lon = gps.lon;
 			hil_gps.alt = gps.alt;
-//			hil_gps.counter_pos_valid = hil_counter++;
 			hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
 			hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
 			hil_gps.s_variance_m_s = 5.0f;
 			hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
 			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
 
-			/* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */
-			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F;
+			/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
+			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
+			/* go back to -PI..PI */
+			if (heading_rad > M_PI_F)
+				heading_rad -= 2.0f * M_PI_F;
 			hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
 			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
 			hil_gps.vel_d_m_s = 0.0f;
-			/* COG (course over ground) is speced as 0..360 degrees (compass) */
-			hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad
+			hil_gps.vel_ned_valid = true;
+			/* COG (course over ground) is speced as -PI..+PI */
+			hil_gps.cog_rad = heading_rad;
 			hil_gps.fix_type = gps.fix_type;
 			hil_gps.satellites_visible = gps.satellites_visible;
 
diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c
index 1dd3fc2d8..5f15facf8 100644
--- a/apps/mavlink/orb_listener.c
+++ b/apps/mavlink/orb_listener.c
@@ -229,6 +229,13 @@ l_vehicle_gps_position(const struct listener *l)
 	/* copy gps data into local buffer */
 	orb_copy(ORB_ID(vehicle_gps_position), mavlink_subs.gps_sub, &gps);
 
+	/* GPS COG is 0..2PI in degrees * 1e2 */
+	float cog_deg = gps.cog_rad;
+	if (cog_deg > M_PI_F)
+		cog_deg -= 2.0f * M_PI_F;
+	cog_deg *= M_RAD_TO_DEG_F;
+
+
 	/* GPS position */
 	mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
 				     gps.timestamp_position,
@@ -236,13 +243,14 @@ l_vehicle_gps_position(const struct listener *l)
 				     gps.lat,
 				     gps.lon,
 				     gps.alt,
-				     (uint16_t)(gps.eph_m * 1e2f), // from m to cm
-				     (uint16_t)(gps.epv_m * 1e2f), // from m to cm
-				     (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s
-				     (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100
+				     gps.eph_m * 1e2f, // from m to cm
+				     gps.epv_m * 1e2f, // from m to cm
+				     gps.vel_m_s * 1e2f, // from m/s to cm/s
+				     cog_deg * 1e2f, // from rad to deg * 100
 				     gps.satellites_visible);
 
-	if (gps.satellite_info_available && (gps_counter % 4 == 0)) {
+	/* update SAT info every 10 seconds */
+	if (gps.satellite_info_available && (gps_counter % 50 == 0)) {
 		mavlink_msg_gps_status_send(MAVLINK_COMM_0,
 					    gps.satellites_visible,
 					    gps.satellite_prn,
diff --git a/apps/nshlib/Makefile b/apps/nshlib/Makefile
index 76cdac40d..4256a1091 100644
--- a/apps/nshlib/Makefile
+++ b/apps/nshlib/Makefile
@@ -107,6 +107,8 @@ endif
 ROOTDEPPATH = --dep-path .
 VPATH = 
 
+MAXOPTIMIZATION = -Os
+
 # Build targets
 
 all: .built
diff --git a/apps/px4/tests/Makefile b/apps/px4/tests/Makefile
index cb1c3c618..34f058be4 100644
--- a/apps/px4/tests/Makefile
+++ b/apps/px4/tests/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 12000
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/system/i2c/Makefile b/apps/system/i2c/Makefile
index 1ed7a2fae..c98e2c0e2 100644
--- a/apps/system/i2c/Makefile
+++ b/apps/system/i2c/Makefile
@@ -64,6 +64,7 @@ VPATH		=
 APPNAME		= i2c
 PRIORITY	= SCHED_PRIORITY_DEFAULT
 STACKSIZE	= 2048
+MAXOPTIMIZATION = -Os
 
 # Build targets
 
diff --git a/apps/systemcmds/bl_update/Makefile b/apps/systemcmds/bl_update/Makefile
index 9d0e156f6..d05493577 100644
--- a/apps/systemcmds/bl_update/Makefile
+++ b/apps/systemcmds/bl_update/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/boardinfo/Makefile b/apps/systemcmds/boardinfo/Makefile
index 753a6843e..6f1be149c 100644
--- a/apps/systemcmds/boardinfo/Makefile
+++ b/apps/systemcmds/boardinfo/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile
index aa1aa7761..a1735962e 100644
--- a/apps/systemcmds/calibration/Makefile
+++ b/apps/systemcmds/calibration/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_MAX - 1
 STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/delay_test/Makefile b/apps/systemcmds/delay_test/Makefile
index d30fcba27..e54cf2f4e 100644
--- a/apps/systemcmds/delay_test/Makefile
+++ b/apps/systemcmds/delay_test/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/eeprom/Makefile b/apps/systemcmds/eeprom/Makefile
index 2f3db0fdc..79a05550e 100644
--- a/apps/systemcmds/eeprom/Makefile
+++ b/apps/systemcmds/eeprom/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/mixer/Makefile b/apps/systemcmds/mixer/Makefile
index b016ddc57..3d8ac38cb 100644
--- a/apps/systemcmds/mixer/Makefile
+++ b/apps/systemcmds/mixer/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/param/Makefile b/apps/systemcmds/param/Makefile
index 603746a20..f19cadbb6 100644
--- a/apps/systemcmds/param/Makefile
+++ b/apps/systemcmds/param/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/perf/Makefile b/apps/systemcmds/perf/Makefile
index 0134c9948..f8bab41b6 100644
--- a/apps/systemcmds/perf/Makefile
+++ b/apps/systemcmds/perf/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/preflight_check/Makefile b/apps/systemcmds/preflight_check/Makefile
index f138e2640..98aadaa86 100644
--- a/apps/systemcmds/preflight_check/Makefile
+++ b/apps/systemcmds/preflight_check/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/reboot/Makefile b/apps/systemcmds/reboot/Makefile
index 9609a24fd..15dd19982 100644
--- a/apps/systemcmds/reboot/Makefile
+++ b/apps/systemcmds/reboot/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemcmds/top/Makefile b/apps/systemcmds/top/Makefile
index c45775f4b..f58f9212e 100644
--- a/apps/systemcmds/top/Makefile
+++ b/apps/systemcmds/top/Makefile
@@ -40,3 +40,5 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT - 10
 STACKSIZE	 = 3000
 
 include $(APPDIR)/mk/app.mk
+
+MAXOPTIMIZATION	 = -Os
diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h
index 71386cba7..40d37fce2 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/apps/systemlib/mixer/mixer.h
@@ -418,6 +418,7 @@ public:
 	enum Geometry {
 		QUAD_X = 0,	/**< quad in X configuration */
 		QUAD_PLUS,	/**< quad in + configuration */
+		QUAD_V,		/**< quad in V configuration */
 		HEX_X,		/**< hex in X configuration */
 		HEX_PLUS,	/**< hex in + configuration */
 		OCTA_X,
diff --git a/apps/systemlib/mixer/mixer_multirotor.cpp b/apps/systemlib/mixer/mixer_multirotor.cpp
index 4b9cfc023..d79811c0f 100644
--- a/apps/systemlib/mixer/mixer_multirotor.cpp
+++ b/apps/systemlib/mixer/mixer_multirotor.cpp
@@ -82,6 +82,12 @@ const MultirotorMixer::Rotor _config_quad_plus[] = {
 	{  0.000000,  1.000000, -1.00 },
 	{ -0.000000, -1.000000, -1.00 },
 };
+const MultirotorMixer::Rotor _config_quad_v[] = {
+	{ -0.927184,  0.374607,  1.00 },
+	{  0.694658, -0.719340,  1.00 },
+	{  0.927184,  0.374607, -1.00 },
+	{ -0.694658, -0.719340, -1.00 },
+};
 const MultirotorMixer::Rotor _config_hex_x[] = {
 	{ -1.000000,  0.000000, -1.00 },
 	{  1.000000,  0.000000,  1.00 },
@@ -121,6 +127,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
+	&_config_quad_v[0],
 	&_config_hex_x[0],
 	&_config_hex_plus[0],
 	&_config_octa_x[0],
@@ -129,6 +136,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
+	4, /* quad_v */
 	6, /* hex_x */
 	6, /* hex_plus */
 	8, /* octa_x */
@@ -184,6 +192,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "4x")) {
 		geometry = MultirotorMixer::QUAD_X;
 
+	} else if (!strcmp(geomname, "4v")) {
+		geometry = MultirotorMixer::QUAD_V;
+
 	} else if (!strcmp(geomname, "6+")) {
 		geometry = MultirotorMixer::HEX_PLUS;
 
diff --git a/apps/systemlib/mixer/multi_tables b/apps/systemlib/mixer/multi_tables
index f17ae30ca..19a8239a6 100755
--- a/apps/systemlib/mixer/multi_tables
+++ b/apps/systemlib/mixer/multi_tables
@@ -20,6 +20,13 @@ set quad_plus {
 	 180 CW
 }
 
+set quad_v {
+	  68 CCW
+	-136 CCW
+	 -68 CW
+	 136 CW
+}
+
 set hex_x {
 	  90 CW
 	 -90 CCW
@@ -60,7 +67,7 @@ set octa_plus {
 	  90 CW
 }
 
-set tables {quad_x quad_plus hex_x hex_plus octa_x octa_plus}
+set tables {quad_x quad_plus quad_v hex_x hex_plus octa_x octa_plus}
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h
index 5463a460d..0a7fb4e9d 100644
--- a/apps/uORB/topics/vehicle_gps_position.h
+++ b/apps/uORB/topics/vehicle_gps_position.h
@@ -55,38 +55,39 @@
  */
 struct vehicle_gps_position_s
 {
-	uint64_t timestamp_position;		/**< Timestamp for position information */
-	int32_t lat;						/**< Latitude in 1E7 degrees */
-	int32_t lon;						/**< Longitude in 1E7 degrees */
-	int32_t alt;						/**< Altitude in 1E3 meters (millimeters) above MSL  */
+	uint64_t timestamp_position;			/**< Timestamp for position information */
+	int32_t lat;					/**< Latitude in 1E7 degrees */
+	int32_t lon;					/**< Longitude in 1E7 degrees */
+	int32_t alt;					/**< Altitude in 1E3 meters (millimeters) above MSL  */
 	
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
-	float p_variance_m;					/**< position accuracy estimate m */
-	uint8_t fix_type; 					/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
+	float p_variance_m;				/**< position accuracy estimate m */
+	float c_variance_rad;				/**< course accuracy estimate rad */
+	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
-	float eph_m;						/**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;						/**< GPS VDOP horizontal dilution of position in m */
+	float eph_m;					/**< GPS HDOP horizontal dilution of position in m */
+	float epv_m;					/**< GPS VDOP horizontal dilution of position in m */
 
-	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 */
-	float vel_e_m_s;					/**< GPS ground speed in m/s */
-	float vel_d_m_s;					/**< GPS ground speed in m/s */
-	float cog_rad;						/**< Course over ground (NOT heading, but direction of movement) in rad */
-	bool vel_ned_valid;					/**< Flag to indicate if NED speed is valid */
+	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 */
+	float vel_e_m_s;				/**< GPS ground speed in m/s */
+	float vel_d_m_s;				/**< GPS ground speed in m/s */
+	float cog_rad;					/**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */
 
 	uint64_t timestamp_time;			/**< Timestamp for time information */
 	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
 
-	uint64_t timestamp_satellites;		/**< Timestamp for sattelite information */
+	uint64_t timestamp_satellites;			/**< Timestamp for sattelite information */
 	uint8_t satellites_visible;			/**< Number of satellites visible. If unknown, set to 255 */
 	uint8_t satellite_prn[20]; 			/**< Global satellite ID */
 	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
-	uint8_t satellite_elevation[20]; 	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
-	uint8_t satellite_azimuth[20];		/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t satellite_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   */
-	bool satellite_info_available;	/**< 0 for no info, 1 for info available */
+	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
 };
 
 /**
diff --git a/nuttx/configs/px4fmu/include/board.h b/nuttx/configs/px4fmu/include/board.h
index 8ad56a4c6..0bc0b3021 100755
--- a/nuttx/configs/px4fmu/include/board.h
+++ b/nuttx/configs/px4fmu/include/board.h
@@ -326,6 +326,7 @@
  */
 #define TONE_ALARM_TIMER	3	/* timer 3 */
 #define TONE_ALARM_CHANNEL	3	/* channel 3 */
+#define GPIO_TONE_ALARM_IDLE	(GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN8)
 #define GPIO_TONE_ALARM		(GPIO_ALT|GPIO_AF2|GPIO_SPEED_2MHz|GPIO_FLOAT|GPIO_PUSHPULL|GPIO_PORTC|GPIO_PIN8)
 
 /************************************************************************************
diff --git a/nuttx/configs/px4io/common/Make.defs b/nuttx/configs/px4io/common/Make.defs
index 4958f7092..7f782b5b2 100644
--- a/nuttx/configs/px4io/common/Make.defs
+++ b/nuttx/configs/px4io/common/Make.defs
@@ -112,7 +112,6 @@ ARCHOPTIMIZATION	 = $(MAXOPTIMIZATION) \
 
 ifeq ("${CONFIG_DEBUG_SYMBOLS}","y")
 ARCHOPTIMIZATION	+= -g
-ARCHSCRIPT		+= -g
 endif
 
 ARCHCFLAGS		 = -std=gnu99
@@ -145,7 +144,7 @@ ARCHDEFINES		 =
 ARCHPICFLAGS		 = -fpic -msingle-pic-base -mpic-register=r10
 
 # this seems to be the only way to add linker flags
-ARCHSCRIPT		+= --warn-common \
+EXTRA_LIBS		+= --warn-common \
 			   --gc-sections
 
 CFLAGS			 = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common