Merge branch 'master' of github.com:PX4/Firmware

140 files changed, 8518 insertions, 21803 deletions

diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 191d20f30..33879892e 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -325,7 +325,7 @@ void KalmanNav::updatePublications()
 	_pos.vx = vN;
 	_pos.vy = vE;
 	_pos.vz = vD;
-	_pos.hdg = psi;
+	_pos.yaw = psi;
 
 	// attitude publication
 	_att.timestamp = _pubTimeStamp;
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 9e533ccdf..a70a14fe4 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -57,7 +57,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -216,8 +216,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	memset(&raw, 0, sizeof(raw));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -230,8 +230,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode*/
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -282,9 +282,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
 			}
@@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
 				if (!initialized) {
-
-					gyro_offsets[0] += raw.gyro_rad_s[0];
-					gyro_offsets[1] += raw.gyro_rad_s[1];
-					gyro_offsets[2] += raw.gyro_rad_s[2];
-					offset_count++;
-
-					if (hrt_absolute_time() - start_time > 3000000LL) {
-						initialized = true;
-						gyro_offsets[0] /= offset_count;
-						gyro_offsets[1] /= offset_count;
-						gyro_offsets[2] /= offset_count;
-					}
+					// XXX disabling init for now
+					initialized = true;
+
+					// gyro_offsets[0] += raw.gyro_rad_s[0];
+					// gyro_offsets[1] += raw.gyro_rad_s[1];
+					// gyro_offsets[2] += raw.gyro_rad_s[2];
+					// offset_count++;
+
+					// if (hrt_absolute_time() - start_time > 3000000LL) {
+					// 	initialized = true;
+					// 	gyro_offsets[0] /= offset_count;
+					// 	gyro_offsets[1] /= offset_count;
+					// 	gyro_offsets[2] /= offset_count;
+					// }
 
 				} else {
 
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 107c2dfb1..236052b56 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -35,7 +35,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
@@ -547,8 +547,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 
-	struct vehicle_status_s state;
-	memset(&state, 0, sizeof(state));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 
 	uint64_t last_data = 0;
 	uint64_t last_measurement = 0;
@@ -561,8 +561,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* subscribe to param changes */
 	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
-	/* subscribe to system state*/
-	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+	/* subscribe to control mode */
+	int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -612,9 +612,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			/* XXX this is seriously bad - should be an emergency */
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
-			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+			orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode);
 
-			if (!state.flag_hil_enabled) {
+			if (!control_mode.flag_system_hil_enabled) {
 				fprintf(stderr,
 					"[att so3_comp] WARNING: Not getting sensors - sensor app running?\n");
 			}
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp
index fbb73d997..ed6707f04 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file accelerometer_calibration.c
+ * @file accelerometer_calibration.cpp
  *
  * Implementation of accelerometer calibration.
  *
@@ -104,32 +104,44 @@
  */
 
 #include "accelerometer_calibration.h"
+#include "commander_helper.h"
 
+#include <unistd.h>
+#include <stdio.h>
 #include <poll.h>
+#include <fcntl.h>
+#include <sys/prctl.h>
+#include <math.h>
+#include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
-#include <systemlib/conversions.h>
+#include <geo/geo.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.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]);
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_accel_calibration_measurements(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) {
+int do_accel_calibration(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);
+	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
 
 	/* 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);
+	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
 
 	if (res == OK) {
 		/* measurements complete successfully, set parameters */
@@ -165,24 +177,17 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
 		}
 
 		mavlink_log_info(mavlink_fd, "accel calibration done");
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
+		return OK;
 	} else {
 		/* measurements error */
 		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		tune_error();
-		sleep(2);
+		return ERROR;
 	}
 
 	/* 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]) {
+int do_accel_calibration_measurements(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 };
@@ -207,38 +212,52 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
 	}
 
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+
+	unsigned done_count = 0;
+
 	while (true) {
 		bool done = true;
-		char str[80];
-		int str_ptr;
-		str_ptr = sprintf(str, "keep vehicle still:");
+		unsigned old_done_count = done_count;
+		done_count = 0;
+
 		for (int i = 0; i < 6; i++) {
 			if (!data_collected[i]) {
-				str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]);
 				done = false;
 			}
 		}
+
+		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
+			(!data_collected[0]) ? "x+ " : "",
+			(!data_collected[1]) ? "x- " : "",
+			(!data_collected[2]) ? "y+ " : "",
+			(!data_collected[3]) ? "y- " : "",
+			(!data_collected[4]) ? "z+ " : "",
+			(!data_collected[5]) ? "z- " : "");
+
+		if (old_done_count != done_count)
+			mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
+
 		if (done)
 			break;
-		mavlink_log_info(mavlink_fd, str);
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
 		if (orient < 0)
 			return ERROR;
 
 		if (data_collected[orient]) {
-			sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]);
-			mavlink_log_info(mavlink_fd, str);
+			mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
 			continue;
 		}
 
-		sprintf(str, "meas started: %s", orientation_strs[orient]);
-		mavlink_log_info(mavlink_fd, str);
+		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
 		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);
+		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
+			(double)accel_ref[orient][0],
+			(double)accel_ref[orient][1],
+			(double)accel_ref[orient][2]);
+
 		data_collected[orient] = true;
-		tune_confirm();
+		tune_neutral();
 	}
 	close(sensor_combined_sub);
 
@@ -246,7 +265,7 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float
 	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");
+		mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
 		return ERROR;
 	}
 
@@ -279,8 +298,10 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	/* set accel error threshold to 5m/s^2 */
 	float accel_err_thr = 5.0f;
 	/* still time required in us */
-	int64_t still_time = 2000000;
-	struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+	hrt_abstime still_time = 2000000;
+	struct pollfd fds[1];
+	fds[0].fd = sub_sensor_combined;
+	fds[0].events = POLLIN;
 
 	hrt_abstime t_start = hrt_absolute_time();
 	/* set timeout to 30s */
@@ -316,12 +337,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
-					mavlink_log_info(mavlink_fd, "still...");
+					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
 					t_still = t;
 					t_timeout = t + timeout;
 				} else {
 					/* still since t_still */
-					if ((int64_t) t - (int64_t) t_still > still_time) {
+					if (t > t_still + still_time) {
 						/* vehicle is still, exit from the loop to detection of its orientation */
 						break;
 					}
@@ -331,7 +352,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 					    accel_disp[2] > still_thr2 * 2.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_log_info(mavlink_fd, "moving...");
+					mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
 					t_still = 0;
 				}
 			}
@@ -343,34 +364,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 		}
 
 		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: failed reading accel");
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
 			return -1;
 		}
 	}
 
-	if (  fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 0;	// [ g, 0, 0 ]
-	if (  fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 1;	// [ -g, 0, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 2;	// [ 0, g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabs(accel_ema[2]) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+		  fabsf(accel_ema[2]) < accel_err_thr  )
 		return 3;	// [ 0, -g, 0 ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
 		return 4;	// [ 0, 0, g ]
-	if (  fabs(accel_ema[0]) < accel_err_thr &&
-		  fabs(accel_ema[1]) < accel_err_thr &&
-		  fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+	if (  fabsf(accel_ema[0]) < accel_err_thr &&
+		  fabsf(accel_ema[1]) < accel_err_thr &&
+		  fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
 		return 5;	// [ 0, 0, -g ]
 
 	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -382,7 +403,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
  * 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 } };
+	struct pollfd fds[1];
+	fds[0].fd = sensor_combined_sub;
+	fds[0].events = POLLIN;
 	int count = 0;
 	float accel_sum[3] = { 0.0f, 0.0f, 0.0f };
 
@@ -416,7 +439,7 @@ 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)
+	if (det == 0.0f)
 		return ERROR;	// Singular matrix
 
 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index f93a867ba..1cf9c0977 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -44,8 +44,7 @@
 #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);
+int do_accel_calibration(int mavlink_fd);
 
 #endif /* ACCELEROMETER_CALIBRATION_H_ */
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
new file mode 100644
index 000000000..e414e5f70
--- /dev/null
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file airspeed_calibration.cpp
+ * Airspeed sensor calibration routine
+ */
+
+#include "airspeed_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/differential_pressure.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_airspeed_calibration(int mavlink_fd)
+{
+	/* give directions */
+	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+
+	const int calibration_count = 2500;
+
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+
+	int calibration_counter = 0;
+	float diff_pres_offset = 0.0f;
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = diff_pres_sub;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+			diff_pres_offset += diff_pres.differential_pressure_pa;
+			calibration_counter++;
+
+		} else if (poll_ret == 0) {
+			/* any poll failure for 1s is a reason to abort */
+			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			return ERROR;
+		}
+	}
+
+	diff_pres_offset = diff_pres_offset / calibration_count;
+
+	if (isfinite(diff_pres_offset)) {
+
+		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
+			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			return ERROR;
+		}
+
+		/* 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, "FAILED storing calibration");
+			return ERROR;
+		}
+
+		//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, "airspeed calibration done");
+
+		return OK;
+
+	} else {
+		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		return ERROR;
+	}
+}
diff --git a/src/modules/mathlib/math/test/test.hpp b/src/modules/commander/airspeed_calibration.h
index 2027bb827..71c701fc2 100644
--- a/src/modules/mathlib/math/test/test.hpp
+++ b/src/modules/commander/airspeed_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,19 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file test.hpp
- *
- * Controller library code
+ * @file gyro_calibration.h
+ * Airspeed sensor calibration routine
  */
 
-#pragma once
+#ifndef AIRSPEED_CALIBRATION_H_
+#define AIRSPEED_CALIBRATION_H_
+
+#include <stdint.h>
 
-//#include <assert.h>
-//#include <time.h>
-//#include <stdlib.h>
+int do_airspeed_calibration(int mavlink_fd);
 
-bool equal(float a, float b, float eps = 1e-5);
-void float2SigExp(
-	const float &num,
-	float &sig,
-	int &exp);
+#endif /* AIRSPEED_CALIBRATION_H_ */
diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/modules/commander/baro_calibration.cpp
index f19db15ec..3123c4087 100644
--- a/src/modules/mathlib/math/Matrix.hpp
+++ b/src/modules/commander/baro_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,30 +32,29 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.h
- *
- * matrix code
+ * @file baro_calibration.cpp
+ * Barometer calibration routine
  */
 
-#pragma once
+#include "baro_calibration.h"
 
-#include <nuttx/config.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_baro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
 
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Matrix.hpp"
-#else
-#include "generic/Matrix.hpp"
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
 #endif
+static const int ERROR = -1;
 
-namespace math
+int do_baro_calibration(int mavlink_fd)
 {
-class Matrix;
-int matrixTest();
-int matrixAddTest();
-int matrixSubTest();
-int matrixMultTest();
-int matrixInvTest();
-int matrixDivTest();
-int matrixArmTest();
-bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
-} // namespace math
+	// TODO implement this
+	return ERROR;
+}
diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/modules/commander/baro_calibration.h
index 21661622a..bc42bc6cf 100644
--- a/src/modules/mathlib/math/generic/Matrix.cpp
+++ b/src/modules/commander/baro_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,9 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.cpp
- *
- * matrix code
+ * @file mag_calibration.h
+ * Barometer calibration routine
  */
 
-#include "Matrix.hpp"
+#ifndef BARO_CALIBRATION_H_
+#define BARO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_baro_calibration(int mavlink_fd);
+
+#endif /* BARO_CALIBRATION_H_ */
diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp
index a26938637..be38ea104 100644
--- a/src/modules/commander/calibration_routines.c
+++ b/src/modules/commander/calibration_routines.cpp
@@ -33,7 +33,7 @@
  ****************************************************************************/
 
 /**
- * @file calibration_routines.c
+ * @file calibration_routines.cpp
  * Calibration routines implementations.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
 
 	return 0;
 }
+
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
deleted file mode 100644
index e9d1f3954..000000000
--- a/src/modules/commander/commander.c
+++ /dev/null
@@ -1,2097 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 commander.c
- * Main system state machine implementation.
- *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- *
- */
-
-#include "commander.h"
-
-#include <nuttx/config.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <sys/prctl.h>
-#include <string.h>
-#include <drivers/drv_led.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_tone_alarm.h>
-#include "state_machine_helper.h"
-#include "systemlib/systemlib.h"
-#include <math.h>
-#include <poll.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/subsystem_info.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/differential_pressure.h>
-#include <mavlink/mavlink_log.h>
-
-#include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-
-/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_mag.h>
-#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 */
-PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
-PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
-
-#include <systemlib/cpuload.h>
-extern struct system_load_s system_load;
-
-/* Decouple update interval and hysteris counters, all depends on intervals */
-#define COMMANDER_MONITORING_INTERVAL 50000
-#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
-#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define STICK_ON_OFF_LIMIT 0.75f
-#define STICK_THRUST_RANGE 1.0f
-#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
-#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-#define GPS_FIX_TYPE_2D 2
-#define GPS_FIX_TYPE_3D 3
-#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000
-#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
-
-/* File descriptors */
-static int leds;
-static int buzzer;
-static int mavlink_fd;
-static bool commander_initialized = false;
-static struct vehicle_status_s current_status; /**< Main state machine */
-static orb_advert_t stat_pub;
-
-// static uint16_t nofix_counter = 0;
-// static uint16_t gotfix_counter = 0;
-
-static unsigned int failsafe_lowlevel_timeout_ms;
-
-static bool thread_should_exit = false;		/**< daemon exit flag */
-static bool thread_running = false;		/**< daemon status flag */
-static int daemon_task;				/**< Handle of daemon task / thread */
-
-/* pthread loops */
-static void *orb_receive_loop(void *arg);
-
-__EXPORT int commander_main(int argc, char *argv[]);
-
-/**
- * Mainloop of commander.
- */
-int commander_thread_main(int argc, char *argv[]);
-
-static int buzzer_init(void);
-static void buzzer_deinit(void);
-static int led_init(void);
-static void led_deinit(void);
-static int led_toggle(int led);
-static int led_on(int led);
-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 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);
-
-
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/**
- * Sort calibration values.
- *
- * Sorts the calibration values with bubble sort.
- *
- * @param a 	The array to sort
- * @param n 	The number of entries in the array
- */
-// static void cal_bsort(float a[], int n);
-
-static int buzzer_init()
-{
-	buzzer = open("/dev/tone_alarm", O_WRONLY);
-
-	if (buzzer < 0) {
-		warnx("Buzzer: open fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void buzzer_deinit()
-{
-	close(buzzer);
-}
-
-
-static int led_init()
-{
-	leds = open(LED_DEVICE_PATH, 0);
-
-	if (leds < 0) {
-		warnx("LED: open fail\n");
-		return ERROR;
-	}
-
-	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
-		warnx("LED: ioctl fail\n");
-		return ERROR;
-	}
-
-	return 0;
-}
-
-static void led_deinit()
-{
-	close(leds);
-}
-
-static int led_toggle(int led)
-{
-	static int last_blue = LED_ON;
-	static int last_amber = LED_ON;
-
-	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
-
-	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
-
-	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
-}
-
-static int led_on(int led)
-{
-	return ioctl(leds, LED_ON, led);
-}
-
-static int led_off(int led)
-{
-	return ioctl(leds, LED_OFF, led);
-}
-
-enum AUDIO_PATTERN {
-	AUDIO_PATTERN_ERROR = 2,
-	AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
-	AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
-	AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
-	AUDIO_PATTERN_NOTIFY_CHARGE = 6
-};
-
-int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
-{
-
-	/* Trigger alarm if going into any error state */
-	if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
-	    ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
-	}
-
-	/* Trigger neutral on arming / disarming */
-	if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
-		ioctl(buzzer, TONE_SET_ALARM, 0);
-		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
-	}
-
-	/* Trigger Tetris on being bored */
-
-	return 0;
-}
-
-void tune_confirm(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 3);
-}
-
-void tune_error(void)
-{
-	ioctl(buzzer, TONE_SET_ALARM, 4);
-}
-
-void do_rc_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	if (current_status.rc_signal_lost) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-		return;
-	}
-
-	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp;
-	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
-
-	/* set parameters */
-
-	float p = sp.roll;
-	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
-	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
-	param_set(param_find("TRIM_YAW"), &p);
-
-	/* store to permanent storage */
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
-
-	if (save_ret != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
-	}
-
-	mavlink_log_info(mavlink_fd, "trim calibration done");
-}
-
-void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
-{
-
-	/* set to mag calibration mode */
-	status->flag_preflight_mag_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
-
-	/* 45 seconds */
-	uint64_t calibration_interval = 45 * 1000 * 1000;
-
-	/* maximum 2000 values */
-	const unsigned int calibration_maxcount = 500;
-	unsigned int calibration_counter = 0;
-
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	// XXX old cal
-	//  * FLT_MIN is not the most negative float number,
-	//  * but the smallest number by magnitude float can
-	//  * represent. Use -FLT_MAX to initialize the most
-	//  * negative number
-
-	// float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
-	// float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
-	struct mag_scale mscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
-	}
-
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
-
-	close(fd);
-
-	/* calibrate offsets */
-
-	// uint64_t calibration_start = hrt_absolute_time();
-
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
-
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return;
-	}
-
-	tune_confirm();
-	sleep(2);
-	tune_confirm();
-
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
-
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
-
-			axis_index++;
-
-			char buf[50];
-			sprintf(buf, "Please rotate around %c", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, buf);
-			tune_confirm();
-
-			axis_deadline += calibration_interval / 3;
-		}
-
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
-
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
-			break;
-		}
-	}
-
-	float sphere_x;
-	float sphere_y;
-	float sphere_z;
-	float sphere_radius;
-
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-
-	free(x);
-	free(y);
-	free(z);
-
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
-
-		fd = open(MAG_DEVICE_PATH, 0);
-
-		struct mag_scale mscale;
-
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
-
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
-
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
-
-		close(fd);
-
-		/* announce and set new offset */
-
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
-
-		/* 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, "FAILED storing calibration");
-		}
-
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
-
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
-
-		mavlink_log_info(mavlink_fd, "mag calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-	}
-
-	/* disable calibration mode */
-	status->flag_preflight_mag_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(sub_mag);
-}
-
-void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* set to gyro calibration mode */
-	status->flag_preflight_gyro_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	int calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
-
-	close(fd);
-
-	int errcount = 0;
-
-	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);
-			gyro_offset[0] += raw.gyro_rad_s[0];
-			gyro_offset[1] += raw.gyro_rad_s[1];
-			gyro_offset[2] += raw.gyro_rad_s[2];
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			errcount++;
-		}
-
-		if (errcount > 1000) {
-			/* any persisting poll error is a reason to abort */
-			mavlink_log_info(mavlink_fd, "permanent gyro error, aborted.");
-			return;
-		}
-	}
-
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
-
-	/* exit gyro calibration mode */
-	status->flag_preflight_gyro_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		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, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
-		// mavlink_log_info(mavlink_fd, buf);
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-	}
-
-	close(sub_sensor_combined);
-}
-
-void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
-{
-	/* announce change */
-
-	mavlink_log_info(mavlink_fd, "keep it still");
-	/* set to accel calibration mode */
-	status->flag_preflight_airspeed_calibration = true;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	const int calibration_count = 2500;
-
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s diff_pres;
-
-	int calibration_counter = 0;
-	float diff_pres_offset = 0.0f;
-
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			diff_pres_offset += diff_pres.differential_pressure_pa;
-			calibration_counter++;
-
-		} else if (poll_ret == 0) {
-			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
-			return;
-		}
-	}
-
-	diff_pres_offset = diff_pres_offset / calibration_count;
-
-	if (isfinite(diff_pres_offset)) {
-
-		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
-		}
-
-		/* 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, "airspeed calibration done");
-
-		tune_confirm();
-		sleep(2);
-		tune_confirm();
-		sleep(2);
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
-	}
-
-	/* exit airspeed calibration mode */
-	status->flag_preflight_airspeed_calibration = false;
-	state_machine_publish(status_pub, status, mavlink_fd);
-
-	close(diff_pres_sub);
-}
-
-
-
-void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
-{
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-	/* announce command handling */
-	tune_confirm();
-
-
-	/* supported command handling start */
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-	case VEHICLE_CMD_DO_SET_MODE: {
-			if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) {
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-			} else {
-				result = VEHICLE_CMD_RESULT_DENIED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-			/* request to arm */
-			if ((int)cmd->param1 == 1) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				/* request to disarm */
-
-			} else if ((int)cmd->param1 == 0) {
-				if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-		/* request for an autopilot reboot */
-	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: {
-			if ((int)cmd->param1 == 1) {
-				if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) {
-					/* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					/* system may return here */
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-			}
-		}
-		break;
-
-//		/* request to land */
-//		case VEHICLE_CMD_NAV_LAND:
-//		 {
-//				//TODO: add check if landing possible
-//				//TODO: add landing maneuver
-//
-//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
-//					result = VEHICLE_CMD_RESULT_ACCEPTED;
-//		}		}
-//		break;
-//
-//		/* request to takeoff */
-//		case VEHICLE_CMD_NAV_TAKEOFF:
-//		{
-//			//TODO: add check if takeoff possible
-//			//TODO: add takeoff maneuver
-//
-//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
-//				result = VEHICLE_CMD_RESULT_ACCEPTED;
-//			}
-//		}
-//		break;
-//
-		/* preflight calibration */
-	case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
-			bool handled = false;
-
-			/* gyro calibration */
-			if ((int)(cmd->param1) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting gyro cal");
-					tune_confirm();
-					do_gyro_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished gyro cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING gyro cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* magnetometer calibration */
-			if ((int)(cmd->param2) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting mag cal");
-					tune_confirm();
-					do_mag_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished mag cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING mag cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* zero-altitude pressure calibration */
-			if ((int)(cmd->param3) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented");
-					tune_confirm();
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* trim calibration */
-			if ((int)(cmd->param4) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "starting trim cal");
-					tune_confirm();
-					do_rc_calibration(status_pub, &current_status);
-					mavlink_log_info(mavlink_fd, "finished trim cal");
-					tune_confirm();
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING trim cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* accel calibration */
-			if ((int)(cmd->param5) == 1) {
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				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, 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);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING accel cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* airspeed calibration */
-			if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
-				/* transition to calibration state */
-				do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
-
-				if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
-					mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
-					tune_confirm();
-					do_airspeed_calibration(status_pub, &current_status);
-					tune_confirm();
-					mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
-					do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-					result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-				} else {
-					mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
-					result = VEHICLE_CMD_RESULT_DENIED;
-				}
-
-				handled = true;
-			}
-
-			/* none found */
-			if (!handled) {
-				//warnx("refusing unsupported calibration request\n");
-				mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request");
-				result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			}
-		}
-		break;
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE: {
-			if (current_status.flag_system_armed &&
-			    ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-			     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) {
-				/* do not perform expensive memory tasks on multirotors in flight */
-				// XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed
-				mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed");
-
-			} else {
-
-				// XXX move this to LOW PRIO THREAD of commander app
-				/* Read all parameters from EEPROM to RAM */
-
-				if (((int)(cmd->param1)) == 0)	{
-
-					/* read all parameters from EEPROM to RAM */
-					int read_ret = param_load_default();
-
-					if (read_ret == OK) {
-						//warnx("[mavlink pm] Loaded EEPROM params in RAM\n");
-						mavlink_log_info(mavlink_fd, "OK loading params from");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else if (read_ret == 1) {
-						mavlink_log_info(mavlink_fd, "OK no changes in");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (read_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR loading params from");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR no param file named");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else if (((int)(cmd->param1)) == 1)	{
-
-					/* write all parameters from RAM to EEPROM */
-					int write_ret = param_save_default();
-
-					if (write_ret == OK) {
-						mavlink_log_info(mavlink_fd, "OK saved param file");
-						mavlink_log_info(mavlink_fd, param_get_default_file());
-						result = VEHICLE_CMD_RESULT_ACCEPTED;
-
-					} else {
-						if (write_ret < -1) {
-							mavlink_log_info(mavlink_fd, "ERR params file does not exit:");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-
-						} else {
-							mavlink_log_info(mavlink_fd, "ERR writing params to");
-							mavlink_log_info(mavlink_fd, param_get_default_file());
-						}
-
-						result = VEHICLE_CMD_RESULT_FAILED;
-					}
-
-				} else {
-					mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request");
-					result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-				}
-			}
-		}
-		break;
-
-	default: {
-			mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command");
-			result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-			/* announce command rejection */
-			ioctl(buzzer, TONE_SET_ALARM, 4);
-		}
-		break;
-	}
-
-	/* supported command handling stop */
-	if (result == VEHICLE_CMD_RESULT_FAILED ||
-	    result == VEHICLE_CMD_RESULT_DENIED ||
-	    result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
-		ioctl(buzzer, TONE_SET_ALARM, 5);
-
-	} else if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
-		tune_confirm();
-	}
-
-	/* send any requested ACKs */
-	if (cmd->confirmation > 0) {
-		/* send acknowledge command */
-		// XXX TODO
-	}
-
-}
-
-static void *orb_receive_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
-{
-	/* Set thread name */
-	prctl(PR_SET_NAME, "commander orb rcv", getpid());
-
-	/* Subscribe to command topic */
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	struct subsystem_info_s info;
-
-	struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg;
-
-	while (!thread_should_exit) {
-		struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } };
-
-		if (poll(fds, 1, 5000) == 0) {
-			/* timeout, but this is no problem, silently ignore */
-		} else {
-			/* got command */
-			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
-
-			warnx("Subsys changed: %d\n", (int)info.subsystem_type);
-
-			/* mark / unmark as present */
-			if (info.present) {
-				vstatus->onboard_control_sensors_present |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_present &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as enabled */
-			if (info.enabled) {
-				vstatus->onboard_control_sensors_enabled |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type;
-			}
-
-			/* mark / unmark as ok */
-			if (info.ok) {
-				vstatus->onboard_control_sensors_health |= info.subsystem_type;
-
-			} else {
-				vstatus->onboard_control_sensors_health &= ~info.subsystem_type;
-			}
-		}
-	}
-
-	close(subsys_sub);
-
-	return NULL;
-}
-
-/*
- * Provides a coarse estimate of remaining battery power.
- *
- * The estimate is very basic and based on decharging voltage curves.
- *
- * @return the estimated remaining capacity in 0..1
- */
-float battery_remaining_estimate_voltage(float voltage);
-
-PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
-PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
-
-float battery_remaining_estimate_voltage(float voltage)
-{
-	float ret = 0;
-	static param_t bat_volt_empty;
-	static param_t bat_volt_full;
-	static param_t bat_n_cells;
-	static bool initialized = false;
-	static unsigned int counter = 0;
-	static float ncells = 3;
-	// XXX change cells to int (and param to INT32)
-
-	if (!initialized) {
-		bat_volt_empty = param_find("BAT_V_EMPTY");
-		bat_volt_full = param_find("BAT_V_FULL");
-		bat_n_cells = param_find("BAT_N_CELLS");
-		initialized = true;
-	}
-
-	static float chemistry_voltage_empty = 3.2f;
-	static float chemistry_voltage_full = 4.05f;
-
-	if (counter % 100 == 0) {
-		param_get(bat_volt_empty, &chemistry_voltage_empty);
-		param_get(bat_volt_full, &chemistry_voltage_full);
-		param_get(bat_n_cells, &ncells);
-	}
-
-	counter++;
-
-	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
-
-	/* limit to sane values */
-	ret = (ret < 0) ? 0 : ret;
-	ret = (ret > 1) ? 1 : ret;
-	return ret;
-}
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The daemon 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_create().
- */
-int commander_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			warnx("commander already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		daemon_task = task_spawn_cmd("commander",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 40,
-					 3000,
-					 commander_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) {
-			warnx("\tcommander is running\n");
-
-		} else {
-			warnx("\tcommander not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int commander_thread_main(int argc, char *argv[])
-{
-	/* not yet initialized */
-	commander_initialized = false;
-	bool home_position_set = false;
-
-	/* set parameters */
-	failsafe_lowlevel_timeout_ms = 0;
-	param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
-
-	param_t _param_sys_type = param_find("MAV_TYPE");
-	param_t _param_system_id = param_find("MAV_SYS_ID");
-	param_t _param_component_id = param_find("MAV_COMP_ID");
-
-	/* welcome user */
-	warnx("I am in command now!\n");
-
-	/* pthreads for command and subsystem info handling */
-	// pthread_t command_handling_thread;
-	pthread_t subsystem_info_thread;
-
-	/* initialize */
-	if (led_init() != 0) {
-		warnx("ERROR: Failed to initialize leds\n");
-	}
-
-	if (buzzer_init() != 0) {
-		warnx("ERROR: Failed to initialize buzzer\n");
-	}
-
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* make sure we are in preflight state */
-	memset(&current_status, 0, sizeof(current_status));
-	current_status.state_machine = SYSTEM_STATE_PREFLIGHT;
-	current_status.flag_system_armed = false;
-	/* neither manual nor offboard control commands have been received */
-	current_status.offboard_control_signal_found_once = false;
-	current_status.rc_signal_found_once = false;
-	/* mark all signals lost as long as they haven't been found */
-	current_status.rc_signal_lost = true;
-	current_status.offboard_control_signal_lost = true;
-	/* allow manual override initially */
-	current_status.flag_external_manual_override_ok = true;
-	/* flag position info as bad, do not allow auto mode */
-	current_status.flag_vector_flight_mode_ok = false;
-	/* set battery warning flag */
-	current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
-
-	/* advertise to ORB */
-	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
-	/* publish current state machine */
-	state_machine_publish(stat_pub, &current_status, mavlink_fd);
-
-	/* home position */
-	orb_advert_t home_pub = -1;
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
-
-	if (stat_pub < 0) {
-		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
-		warnx("exiting.");
-		exit(ERROR);
-	}
-
-	mavlink_log_info(mavlink_fd, "system is running");
-
-	/* create pthreads */
-	pthread_attr_t subsystem_info_attr;
-	pthread_attr_init(&subsystem_info_attr);
-	pthread_attr_setstacksize(&subsystem_info_attr, 2048);
-	pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, &current_status);
-
-	/* Start monitoring loop */
-	uint16_t counter = 0;
-	uint8_t flight_env;
-
-	/* Initialize to 0.0V */
-	float battery_voltage = 0.0f;
-	bool battery_voltage_valid = false;
-	bool low_battery_voltage_actions_done = false;
-	bool critical_battery_voltage_actions_done = false;
-	uint8_t low_voltage_counter = 0;
-	uint16_t critical_voltage_counter = 0;
-	int16_t mode_switch_rc_value;
-	float bat_remain = 1.0f;
-
-	uint16_t stick_off_counter = 0;
-	uint16_t stick_on_counter = 0;
-
-	/* Subscribe to manual control data */
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	struct manual_control_setpoint_s sp_man;
-	memset(&sp_man, 0, sizeof(sp_man));
-
-	/* Subscribe to offboard control data */
-	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	struct offboard_control_setpoint_s sp_offboard;
-	memset(&sp_offboard, 0, sizeof(sp_offboard));
-
-	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	struct vehicle_global_position_s global_position;
-	memset(&global_position, 0, sizeof(global_position));
-	uint64_t last_global_position_time = 0;
-
-	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	struct vehicle_local_position_s local_position;
-	memset(&local_position, 0, sizeof(local_position));
-	uint64_t last_local_position_time = 0;
-
-	/* 
-	 * The home position is set based on GPS only, to prevent a dependency between
-	 * position estimator and commander. RAW GPS is more than good enough for a
-	 * non-flying vehicle.
-	 */
-	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	struct vehicle_gps_position_s gps_position;
-	memset(&gps_position, 0, sizeof(gps_position));
-
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s sensors;
-	memset(&sensors, 0, sizeof(sensors));
-
-	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	struct differential_pressure_s diff_pres;
-	memset(&diff_pres, 0, sizeof(diff_pres));
-	uint64_t last_diff_pres_time = 0;
-
-	/* Subscribe to command topic */
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	struct vehicle_command_s cmd;
-	memset(&cmd, 0, sizeof(cmd));
-
-	/* Subscribe to parameters changed topic */
-	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-	struct parameter_update_s param_changed;
-	memset(&param_changed, 0, sizeof(param_changed));
-
-	/* subscribe to battery topic */
-	int battery_sub = orb_subscribe(ORB_ID(battery_status));
-	struct battery_status_s battery;
-	memset(&battery, 0, sizeof(battery));
-	battery.voltage_v = 0.0f;
-
-	// uint8_t vehicle_state_previous = current_status.state_machine;
-	float voltage_previous = 0.0f;
-
-	uint64_t last_idle_time = 0;
-
-	/* now initialized */
-	commander_initialized = true;
-	thread_running = true;
-
-	uint64_t start_time = hrt_absolute_time();
-	uint64_t failsave_ll_start_time = 0;
-
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;
-	bool state_changed = true;
-	bool param_init_forced = true;
-
-	while (!thread_should_exit) {
-
-		/* Get current values */
-		bool new_data;
-		orb_check(sp_man_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-		}
-
-		orb_check(sp_offboard_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
-		}
-
-		orb_check(sensor_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
-		}
-
-		orb_check(diff_pres_sub, &new_data);
-
-		if (new_data) {
-			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
-			last_diff_pres_time = diff_pres.timestamp;
-		}
-
-		orb_check(cmd_sub, &new_data);
-
-		if (new_data) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(stat_pub, &current_status, &cmd);
-		}
-
-		/* update parameters */
-		orb_check(param_changed_sub, &new_data);
-
-		if (new_data || param_init_forced) {
-			param_init_forced = false;
-			/* parameters changed */
-			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
-
-
-			/* update parameters */
-			if (!current_status.flag_system_armed) {
-				if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
-					warnx("failed setting new system type");
-				}
-
-				/* disable manual override for all systems that rely on electronic stabilization */
-				if (current_status.system_type == VEHICLE_TYPE_QUADROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_HEXAROTOR ||
-				    current_status.system_type == VEHICLE_TYPE_OCTOROTOR) {
-					current_status.flag_external_manual_override_ok = false;
-
-				} else {
-					current_status.flag_external_manual_override_ok = true;
-				}
-
-				/* check and update system / component ID */
-				param_get(_param_system_id, &(current_status.system_id));
-				param_get(_param_component_id, &(current_status.component_id));
-
-			}
-		}
-
-		/* update global position estimate */
-		orb_check(global_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
-			last_global_position_time = global_position.timestamp;
-		}
-
-		/* update local position estimate */
-		orb_check(local_position_sub, &new_data);
-
-		if (new_data) {
-			/* position changed */
-			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
-			last_local_position_time = local_position.timestamp;
-		}
-
-		/* update battery status */
-		orb_check(battery_sub, &new_data);
-		if (new_data) {
-			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
-			battery_voltage = battery.voltage_v;
-			battery_voltage_valid = true;
-
-			/*
-			 * Only update battery voltage estimate if system has
-			 * been running for two and a half seconds.
-			 */
-			if (hrt_absolute_time() - start_time > 2500000) {
-				bat_remain = battery_remaining_estimate_voltage(battery_voltage);
-			}
-		}
-
-		/* Slow but important 8 Hz checks */
-		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
-			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
-			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
-			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
-			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed, solid */
-				led_on(LED_AMBER);
-
-			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* not armed */
-				led_toggle(LED_AMBER);
-			}
-
-			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
-
-				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
-				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
-					/* GPS lock */
-					led_on(LED_BLUE);
-
-				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-					/* no GPS lock, but GPS module is aquiring lock */
-					led_toggle(LED_BLUE);
-				}
-
-			} else {
-				/* no GPS info, don't light the blue led */
-				led_off(LED_BLUE);
-			}
-
-			/* toggle GPS led at 5 Hz in HIL mode */
-			if (current_status.flag_hil_enabled) {
-				/* hil enabled */
-				led_toggle(LED_BLUE);
-
-			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle arming (red) at 5 Hz on low battery or error */
-				led_toggle(LED_AMBER);
-
-			} else {
-				// /* Constant error indication in standby mode without GPS */
-				// if (!current_status.gps_valid) {
-				// 	led_on(LED_AMBER);
-
-				// } else {
-				// 	led_off(LED_AMBER);
-				// }
-			}
-
-			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				/* compute system load */
-				uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
-
-				if (last_idle_time > 0)
-					current_status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
-
-				last_idle_time = system_load.tasks[0].total_runtime;
-			}
-		}
-
-		// // XXX Export patterns and threshold to parameters
-		/* Trigger audio event for low battery */
-		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
-			/* For less than 10%, start be really annoying at 5 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			ioctl(buzzer, TONE_SET_ALARM, 3);
-
-		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
-			/* For less than 20%, start be slightly annoying at 1 Hz */
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-			tune_confirm();
-
-		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
-			ioctl(buzzer, TONE_SET_ALARM, 0);
-		}
-
-		/* Check battery voltage */
-		/* write to sys_status */
-		if (battery_voltage_valid) {
-			current_status.voltage_battery = battery_voltage;
-
-		} else {
-			current_status.voltage_battery = 0.0f;
-		}
-
-		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
-
-			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
-			}
-
-			low_voltage_counter++;
-		}
-
-		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
-		else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
-			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
-				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!");
-				current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
-				state_machine_emergency(stat_pub, &current_status, mavlink_fd);
-			}
-
-			critical_voltage_counter++;
-
-		} else {
-			low_voltage_counter = 0;
-			critical_voltage_counter = 0;
-		}
-
-		/* End battery voltage check */
-
-
-		/*
-		 * Check for valid position information.
-		 *
-		 * If the system has a valid position source from an onboard
-		 * position estimator, it is safe to operate it autonomously.
-		 * The flag_vector_flight_mode_ok flag indicates that a minimum
-		 * set of position measurements is available.
-		 */
-
-		/* store current state to reason later about a state change */
-		bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = current_status.flag_global_position_valid;
-		bool local_pos_valid = current_status.flag_local_position_valid;
-		bool airspeed_valid = current_status.flag_airspeed_valid;
-
-		/* check for global or local position updates, set a timeout of 2s */
-		if (hrt_absolute_time() - last_global_position_time < 2000000) {
-			current_status.flag_global_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_global_position_valid = false;
-		}
-
-		if (hrt_absolute_time() - last_local_position_time < 2000000) {
-			current_status.flag_local_position_valid = true;
-			// XXX check for controller status and home position as well
-
-		} else {
-			current_status.flag_local_position_valid = false;
-		}
-
-		/* Check for valid airspeed/differential pressure measurements */
-		if (hrt_absolute_time() - last_diff_pres_time < 2000000) {
-			current_status.flag_airspeed_valid = true;
-
-		} else {
-			current_status.flag_airspeed_valid = false;
-		}
-
-		/*
-		 * Consolidate global position and local position valid flags
-		 * for vector flight mode.
-		 */
-		if (current_status.flag_local_position_valid ||
-		    current_status.flag_global_position_valid) {
-			current_status.flag_vector_flight_mode_ok = true;
-
-		} else {
-			current_status.flag_vector_flight_mode_ok = false;
-		}
-
-		/* consolidate state change, flag as changed if required */
-		if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
-		    global_pos_valid != current_status.flag_global_position_valid ||
-		    local_pos_valid != current_status.flag_local_position_valid ||
-		    airspeed_valid != current_status.flag_airspeed_valid) {
-			state_changed = true;
-		}
-
-		/*
-		 * Mark the position of the first position lock as return to launch (RTL)
-		 * position. The MAV will return here on command or emergency.
-		 *
-		 * Conditions:
-		 *
-		 * 	1) The system aquired position lock just now
-		 *	2) The system has not aquired position lock before
-		 *	3) The system is not armed (on the ground)
-		 */
-		if (!current_status.flag_valid_launch_position &&
-		    !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok &&
-		    !current_status.flag_system_armed) {
-			/* first time a valid position, store it and emit it */
-
-			// XXX implement storage and publication of RTL position
-			current_status.flag_valid_launch_position = true;
-			current_status.flag_auto_flight_mode_ok = true;
-			state_changed = true;
-		}
-
-		if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) {
-
-			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
-
-			/* check for first, long-term and valid GPS lock -> set home position */
-			float hdop_m = gps_position.eph_m;
-			float vdop_m = gps_position.epv_m;
-
-			/* check if gps fix is ok */
-			// XXX magic number
-			float hdop_threshold_m = 4.0f;
-			float vdop_threshold_m = 8.0f;
-
-			/*
-			 * If horizontal dilution of precision (hdop / eph)
-			 * and vertical diluation of precision (vdop / epv)
-			 * are below a certain threshold (e.g. 4 m), AND
-			 * home position is not yet set AND the last GPS
-			 * GPS measurement is not older than two seconds AND
-			 * the system is currently not armed, set home
-			 * position to the current position.
-			 */
-
-			if (gps_position.fix_type == GPS_FIX_TYPE_3D
-				&& (hdop_m < hdop_threshold_m)
-				&& (vdop_m < vdop_threshold_m)
-				&& !home_position_set
-				&& (hrt_absolute_time() - gps_position.timestamp_position < 2000000)
-				&& !current_status.flag_system_armed) {
-				warnx("setting home position");
-
-				/* copy position data to uORB home message, store it locally as well */
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
-
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
-
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				home_position_set = true;
-				tune_confirm();
-			}
-		}
-
-		/* ignore RC signals if in offboard control mode */
-		if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
-			/* Start RC state check */
-
-			if ((hrt_absolute_time() - sp_man.timestamp) < 100000) {
-
-				// /*
-				//  * Check if manual control modes have to be switched
-				//  */
-				// if (!isfinite(sp_man.manual_mode_switch)) {
-				// 	warnx("man mode sw not finite\n");
-
-				// 	/* this switch is not properly mapped, set default */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, fall back to direct pass-through */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) {
-
-				// 	/* bottom stick position, set direct mode for vehicles supporting it */
-				// 	if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				// 		(current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-				// 		/* assuming a rotary wing, fall back to SAS */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 		current_status.flag_control_attitude_enabled = true;
-				// 		current_status.flag_control_rates_enabled = true;
-				// 	} else {
-
-				// 		/* assuming a fixed wing, set to direct pass-through as requested */
-				// 		current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-				// 		current_status.flag_control_attitude_enabled = false;
-				// 		current_status.flag_control_rates_enabled = false;
-				// 	}
-
-				// } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) {
-
-				// 	/* top stick position, set SAS for all vehicle types */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-				// 	current_status.flag_control_attitude_enabled = true;
-				// 	current_status.flag_control_rates_enabled = true;
-
-				// } else {
-				// 	/* center stick position, set rate control */
-				// 	current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES;
-				// 	current_status.flag_control_attitude_enabled = false;
-				// 	current_status.flag_control_rates_enabled = true;
-				// }
-
-				// warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode);
-
-				/*
-				 * Check if manual stability control modes have to be switched
-				 */
-				if (!isfinite(sp_man.manual_sas_switch)) {
-
-					/* this switch is not properly mapped, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
-
-					/* bottom stick position, set default */
-					/* this MUST be mapped to extremal position to switch easy in case of emergency */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
-
-				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
-
-					/* top stick position */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
-
-				} else {
-					/* center stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
-				}
-
-				if (current_status.manual_sas_mode != manual_sas_mode) {
-					/* publish SAS mode changes immediately */
-					manual_sas_mode = current_status.manual_sas_mode;
-					state_changed = true;
-				}
-
-				/*
-				 * Check if left stick is in lower left position --> switch to standby state.
-				 * Do this only for multirotors, not for fixed wing aircraft.
-				 */
-				if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
-				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
-				    ) &&
-					current_status.flag_control_manual_enabled &&
-					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
-				    sp_man.yaw < -STICK_ON_OFF_LIMIT &&
-				    sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_off_counter++;
-						stick_on_counter = 0;
-					}
-				}
-
-				/* check if left stick is in lower right position --> arm */
-				if (current_status.flag_control_manual_enabled &&
-					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
-					sp_man.yaw > STICK_ON_OFF_LIMIT &&
-					sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
-					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-						stick_on_counter = 0;
-
-					} else {
-						stick_on_counter++;
-						stick_off_counter = 0;
-					}
-				}
-
-				/* check manual override switch - switch to manual or auto mode */
-				if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) {
-					/* enable manual override */
-					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
-
-				} else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) {
-					// /* check auto mode switch for correct mode */
-					// if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) {
-					// 	/* enable guided mode */
-					// 	update_state_machine_mode_guided(stat_pub, &current_status, mavlink_fd);
-
-					// } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) {
-						// XXX hardcode to auto for now
-						update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
-
-					// }
-
-				} else {
-					/* center stick position, set SAS for all vehicle types */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-				}
-
-				/* handle the case where RC signal was regained */
-				if (!current_status.rc_signal_found_once) {
-					current_status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME.");
-
-				} else {
-					if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!");
-				}
-
-				current_status.rc_signal_cutting_off = false;
-				current_status.rc_signal_lost = false;
-				current_status.rc_signal_lost_interval = 0;
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					/* only complain if the offboard control is NOT active */
-					current_status.rc_signal_cutting_off = true;
-					mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp;
-
-				/* if the RC signal is gone for a full second, consider it lost */
-				if (current_status.rc_signal_lost_interval > 1000000) {
-					current_status.rc_signal_lost = true;
-					current_status.failsave_lowlevel = true;
-					state_changed = true;
-				}
-
-				// if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) {
-				// 	publish_armed_status(&current_status);
-				// }
-			}
-		}
-
-
-
-
-		/* End mode switch */
-
-		/* END RC state check */
-
-
-		/* State machine update for offboard control */
-		if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) {
-			if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) {
-
-				/* decide about attitude control flag, enable in att/pos/vel */
-				bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY ||
-							      sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION);
-
-				/* decide about rate control flag, enable it always XXX (for now) */
-				bool rates_ctrl_enabled = true;
-
-				/* set up control mode */
-				if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) {
-					current_status.flag_control_attitude_enabled = attitude_ctrl_enabled;
-					state_changed = true;
-				}
-
-				if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) {
-					current_status.flag_control_rates_enabled = rates_ctrl_enabled;
-					state_changed = true;
-				}
-
-				/* handle the case where offboard control signal was regained */
-				if (!current_status.offboard_control_signal_found_once) {
-					current_status.offboard_control_signal_found_once = true;
-					/* enable offboard control, disable manual input */
-					current_status.flag_control_manual_enabled = false;
-					current_status.flag_control_offboard_enabled = true;
-					state_changed = true;
-					tune_confirm();
-
-					mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST");
-
-				} else {
-					if (current_status.offboard_control_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL");
-						state_changed = true;
-						tune_confirm();
-					}
-				}
-
-				current_status.offboard_control_signal_weak = false;
-				current_status.offboard_control_signal_lost = false;
-				current_status.offboard_control_signal_lost_interval = 0;
-
-				/* arm / disarm on request */
-				if (sp_offboard.armed && !current_status.flag_system_armed) {
-					update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
-					/* switch to stabilized mode = takeoff */
-					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
-
-				} else if (!sp_offboard.armed && current_status.flag_system_armed) {
-					update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
-				}
-
-			} else {
-				static uint64_t last_print_time = 0;
-
-				/* print error message for first RC glitch and then every 5 s / 5000 ms) */
-				if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) {
-					current_status.offboard_control_signal_weak = true;
-					mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!");
-					last_print_time = hrt_absolute_time();
-				}
-
-				/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-				current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp;
-
-				/* if the signal is gone for 0.1 seconds, consider it lost */
-				if (current_status.offboard_control_signal_lost_interval > 100000) {
-					current_status.offboard_control_signal_lost = true;
-					current_status.failsave_lowlevel_start_time = hrt_absolute_time();
-					tune_confirm();
-
-					/* kill motors after timeout */
-					if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) {
-						current_status.failsave_lowlevel = true;
-						state_changed = true;
-					}
-				}
-			}
-		}
-
-
-		current_status.counter++;
-		current_status.timestamp = hrt_absolute_time();
-
-
-		/* If full run came back clean, transition to standby */
-		if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT &&
-		    current_status.flag_preflight_gyro_calibration == false &&
-		    current_status.flag_preflight_mag_calibration == false &&
-		    current_status.flag_preflight_accel_calibration == false) {
-			/* All ok, no calibration going on, go to standby */
-			do_state_update(stat_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
-		}
-
-		/* publish at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) {
-			publish_armed_status(&current_status);
-			orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
-			state_changed = false;
-		}
-
-		/* Store old modes to detect and act on state transitions */
-		voltage_previous = current_status.voltage_battery;
-
-		fflush(stdout);
-		counter++;
-		usleep(COMMANDER_MONITORING_INTERVAL);
-	}
-
-	/* wait for threads to complete */
-	// pthread_join(command_handling_thread, NULL);
-	pthread_join(subsystem_info_thread, NULL);
-
-	/* close fds */
-	led_deinit();
-	buzzer_deinit();
-	close(sp_man_sub);
-	close(sp_offboard_sub);
-	close(global_position_sub);
-	close(sensor_sub);
-	close(cmd_sub);
-
-	warnx("exiting..\n");
-	fflush(stdout);
-
-	thread_running = false;
-
-	return 0;
-}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
new file mode 100644
index 000000000..45e0307e6
--- /dev/null
+++ b/src/modules/commander/commander.cpp
@@ -0,0 +1,1768 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *           Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.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 commander.cpp
+ * Main system state machine implementation.
+ *
+ */
+
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <sys/prctl.h>
+#include <sys/stat.h>
+#include <string.h>
+#include <math.h>
+#include <poll.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/subsystem_info.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/safety.h>
+
+#include <drivers/drv_led.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/cpuload.h>
+#include <systemlib/rc_check.h>
+
+#include "px4_custom_mode.h"
+#include "commander_helper.h"
+#include "state_machine_helper.h"
+#include "calibration_routines.h"
+#include "accelerometer_calibration.h"
+#include "gyro_calibration.h"
+#include "mag_calibration.h"
+#include "baro_calibration.h"
+#include "rc_calibration.h"
+#include "airspeed_calibration.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+extern struct system_load_s system_load;
+
+#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
+#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
+
+/* Decouple update interval and hysteris counters, all depends on intervals */
+#define COMMANDER_MONITORING_INTERVAL 50000
+#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
+#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define STICK_ON_OFF_LIMIT 0.75f
+#define STICK_THRUST_RANGE 1.0f
+#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
+#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
+#define RC_TIMEOUT 100000
+#define DIFFPRESS_TIMEOUT 2000000
+
+#define PRINT_INTERVAL	5000000
+#define PRINT_MODE_REJECT_INTERVAL	2000000
+
+enum MAV_MODE_FLAG {
+	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
+	MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */
+	MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */
+	MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */
+	MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */
+	MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */
+	MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */
+	MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */
+	MAV_MODE_FLAG_ENUM_END = 129, /*  | */
+};
+
+/* Mavlink file descriptors */
+static int mavlink_fd;
+
+/* flags */
+static bool commander_initialized = false;
+static bool thread_should_exit = false;		/**< daemon exit flag */
+static bool thread_running = false;		/**< daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+static unsigned int leds_counter;
+/* To remember when last notification was sent */
+static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
+
+static float takeoff_alt = 5.0f;
+
+/* tasks waiting for low prio thread */
+typedef enum {
+	LOW_PRIO_TASK_NONE = 0,
+	LOW_PRIO_TASK_PARAM_SAVE,
+	LOW_PRIO_TASK_PARAM_LOAD,
+	LOW_PRIO_TASK_GYRO_CALIBRATION,
+	LOW_PRIO_TASK_MAG_CALIBRATION,
+	LOW_PRIO_TASK_ALTITUDE_CALIBRATION,
+	LOW_PRIO_TASK_RC_CALIBRATION,
+	LOW_PRIO_TASK_ACCEL_CALIBRATION,
+	LOW_PRIO_TASK_AIRSPEED_CALIBRATION
+} low_prio_task_t;
+
+static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE;
+
+/**
+ * The daemon 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_create().
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int commander_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+void usage(const char *reason);
+
+/**
+ * React to commands that are sent e.g. from the mavlink module.
+ */
+void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+
+/**
+ * Mainloop of commander.
+ */
+int commander_thread_main(int argc, char *argv[]);
+
+void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+
+void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
+
+void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status);
+
+transition_result_t check_main_state_machine(struct vehicle_status_s *current_status);
+
+void print_reject_mode(const char *msg);
+
+void print_reject_arm(const char *msg);
+
+void print_status();
+
+transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);
+
+/**
+ * Loop that runs at a lower rate and priority for calibration and parameter tasks.
+ */
+void *commander_low_prio_loop(void *arg);
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result);
+
+
+int commander_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("commander already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn_cmd("commander",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 40,
+					     3000,
+					     commander_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) {
+			warnx("\tcommander is running\n");
+			print_status();
+
+		} else {
+			warnx("\tcommander not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+void print_status()
+{
+	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char *armed_str;
+
+	switch (state.arming_state) {
+	case ARMING_STATE_INIT:
+		armed_str = "INIT";
+		break;
+
+	case ARMING_STATE_STANDBY:
+		armed_str = "STANDBY";
+		break;
+
+	case ARMING_STATE_ARMED:
+		armed_str = "ARMED";
+		break;
+
+	case ARMING_STATE_ARMED_ERROR:
+		armed_str = "ARMED_ERROR";
+		break;
+
+	case ARMING_STATE_STANDBY_ERROR:
+		armed_str = "STANDBY_ERROR";
+		break;
+
+	case ARMING_STATE_REBOOT:
+		armed_str = "REBOOT";
+		break;
+
+	case ARMING_STATE_IN_AIR_RESTORE:
+		armed_str = "IN_AIR_RESTORE";
+		break;
+
+	default:
+		armed_str = "ERR: UNKNOWN STATE";
+		break;
+	}
+
+
+	warnx("arming: %s", armed_str);
+}
+
+void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+{
+	/* result of the command */
+	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+
+	/* only handle high-priority commands here */
+
+	/* request to set different system mode */
+	switch (cmd->command) {
+	case VEHICLE_CMD_DO_SET_MODE: {
+			uint8_t base_mode = (uint8_t) cmd->param1;
+			uint8_t custom_main_mode = (uint8_t) cmd->param2;
+
+			// TODO remove debug code
+			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
+			/* set arming state */
+			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+
+				if (arming_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
+				}
+
+			} else {
+				if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+					arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+					arming_res = arming_state_transition(status, safety, new_arming_state, armed);
+
+					if (arming_res == TRANSITION_CHANGED) {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command");
+					}
+
+				} else {
+					arming_res = TRANSITION_NOT_CHANGED;
+				}
+			}
+
+			/* set main state */
+			transition_result_t main_res = TRANSITION_DENIED;
+
+			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+				/* use autopilot-specific mode */
+				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+					/* MANUAL */
+					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+					/* SEATBELT */
+					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
+					/* EASY */
+					main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				}
+
+			} else {
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* EASY */
+						main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
+				}
+			}
+
+			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_NAV_TAKEOFF: {
+			if (armed->armed) {
+				transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+
+				if (nav_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command");
+				}
+
+				if (nav_res != TRANSITION_DENIED) {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
+
+			} else {
+				/* reject TAKEOFF not armed */
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+
+			break;
+		}
+
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
+		// XXX implement later
+		result = VEHICLE_CMD_RESULT_DENIED;
+		break;
+
+	default:
+		break;
+	}
+
+	/* supported command handling stop */
+	if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
+		tune_positive();
+
+	} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* we do not care in the high prio loop about commands we don't know */
+	} else {
+		tune_negative();
+
+		if (result == VEHICLE_CMD_RESULT_DENIED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+
+		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
+			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
+
+		}
+	}
+
+	/* send any requested ACKs */
+	if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
+		/* send acknowledge command */
+		// XXX TODO
+	}
+
+}
+
+static struct vehicle_status_s status;
+
+/* armed topic */
+static struct actuator_armed_s armed;
+
+static struct safety_s safety;
+
+int commander_thread_main(int argc, char *argv[])
+{
+	/* not yet initialized */
+	commander_initialized = false;
+	bool home_position_set = false;
+
+	bool battery_tune_played = false;
+	bool arm_tune_played = false;
+
+	/* set parameters */
+	param_t _param_sys_type = param_find("MAV_TYPE");
+	param_t _param_system_id = param_find("MAV_SYS_ID");
+	param_t _param_component_id = param_find("MAV_COMP_ID");
+	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+
+	/* welcome user */
+	warnx("starting");
+
+	/* pthread for slow low prio thread */
+	pthread_t commander_low_prio_thread;
+
+	/* initialize */
+	if (led_init() != 0) {
+		warnx("ERROR: Failed to initialize leds");
+	}
+
+	if (buzzer_init() != OK) {
+		warnx("ERROR: Failed to initialize buzzer");
+	}
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	if (mavlink_fd < 0) {
+		/* try again later */
+		usleep(20000);
+		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+		if (mavlink_fd < 0) {
+			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
+		}
+	}
+
+	/* Main state machine */
+	orb_advert_t status_pub;
+	/* make sure we are in preflight state */
+	memset(&status, 0, sizeof(status));
+	status.condition_landed = true;	// initialize to safe value
+
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
+
+	/* flags for control apps */
+	struct vehicle_control_mode_s control_mode;
+	orb_advert_t control_mode_pub;
+
+	/* Initialize all flags to false */
+	memset(&control_mode, 0, sizeof(control_mode));
+
+	status.main_state = MAIN_STATE_MANUAL;
+	status.navigation_state = NAVIGATION_STATE_DIRECT;
+	status.arming_state = ARMING_STATE_INIT;
+	status.hil_state = HIL_STATE_OFF;
+
+	/* neither manual nor offboard control commands have been received */
+	status.offboard_control_signal_found_once = false;
+	status.rc_signal_found_once = false;
+
+	/* mark all signals lost as long as they haven't been found */
+	status.rc_signal_lost = true;
+	status.offboard_control_signal_lost = true;
+
+	/* allow manual override initially */
+	control_mode.flag_external_manual_override_ok = true;
+
+	/* set battery warning flag */
+	status.battery_warning = VEHICLE_BATTERY_WARNING_NONE;
+	status.condition_battery_voltage_valid = false;
+
+	// XXX for now just set sensors as initialized
+	status.condition_system_sensors_initialized = true;
+
+	// XXX just disable offboard control for now
+	control_mode.flag_control_offboard_enabled = false;
+
+	/* advertise to ORB */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+	/* publish current state machine */
+
+	/* publish initial state */
+	status.counter++;
+	status.timestamp = hrt_absolute_time();
+	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+
+	control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
+
+	/* home position */
+	orb_advert_t home_pub = -1;
+	struct home_position_s home;
+	memset(&home, 0, sizeof(home));
+
+	if (status_pub < 0) {
+		warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
+		warnx("exiting.");
+		exit(ERROR);
+	}
+
+	mavlink_log_info(mavlink_fd, "[cmd] started");
+
+	pthread_attr_t commander_low_prio_attr;
+	pthread_attr_init(&commander_low_prio_attr);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
+
+	struct sched_param param;
+	/* low priority */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
+	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
+	pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL);
+
+	/* Start monitoring loop */
+	unsigned counter = 0;
+	unsigned low_voltage_counter = 0;
+	unsigned critical_voltage_counter = 0;
+	unsigned stick_off_counter = 0;
+	unsigned stick_on_counter = 0;
+
+	bool low_battery_voltage_actions_done = false;
+	bool critical_battery_voltage_actions_done = false;
+
+	uint64_t last_idle_time = 0;
+
+	uint64_t start_time = 0;
+
+	bool status_changed = true;
+	bool param_init_forced = true;
+
+	bool updated = false;
+
+	bool rc_calibration_ok = (OK == rc_calibration_check());
+
+	/* Subscribe to safety topic */
+	int safety_sub = orb_subscribe(ORB_ID(safety));
+	memset(&safety, 0, sizeof(safety));
+	safety.safety_switch_available = false;
+	safety.safety_off = false;
+
+	/* Subscribe to manual control data */
+	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	struct manual_control_setpoint_s sp_man;
+	memset(&sp_man, 0, sizeof(sp_man));
+
+	/* Subscribe to offboard control data */
+	int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
+	struct offboard_control_setpoint_s sp_offboard;
+	memset(&sp_offboard, 0, sizeof(sp_offboard));
+
+	/* Subscribe to global position */
+	int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	struct vehicle_global_position_s global_position;
+	memset(&global_position, 0, sizeof(global_position));
+
+	/* Subscribe to local position data */
+	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	struct vehicle_local_position_s local_position;
+	memset(&local_position, 0, sizeof(local_position));
+
+	/*
+	 * The home position is set based on GPS only, to prevent a dependency between
+	 * position estimator and commander. RAW GPS is more than good enough for a
+	 * non-flying vehicle.
+	 */
+
+	/* Subscribe to GPS topic */
+	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	struct vehicle_gps_position_s gps_position;
+	memset(&gps_position, 0, sizeof(gps_position));
+
+	/* Subscribe to sensor topic */
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s sensors;
+	memset(&sensors, 0, sizeof(sensors));
+
+	/* Subscribe to differential pressure topic */
+	int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
+	struct differential_pressure_s diff_pres;
+	memset(&diff_pres, 0, sizeof(diff_pres));
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* Subscribe to parameters changed topic */
+	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
+	struct parameter_update_s param_changed;
+	memset(&param_changed, 0, sizeof(param_changed));
+
+	/* Subscribe to battery topic */
+	int battery_sub = orb_subscribe(ORB_ID(battery_status));
+	struct battery_status_s battery;
+	memset(&battery, 0, sizeof(battery));
+	battery.voltage_v = 0.0f;
+
+	/* Subscribe to subsystem info topic */
+	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
+	struct subsystem_info_s info;
+	memset(&info, 0, sizeof(info));
+
+	toggle_status_leds(&status, &armed, true);
+
+	/* now initialized */
+	commander_initialized = true;
+	thread_running = true;
+
+	start_time = hrt_absolute_time();
+
+	while (!thread_should_exit) {
+
+		/* update parameters */
+		orb_check(param_changed_sub, &updated);
+
+		if (updated || param_init_forced) {
+			param_init_forced = false;
+			/* parameters changed */
+			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
+			/* update parameters */
+			if (!armed.armed) {
+				if (param_get(_param_sys_type, &(status.system_type)) != OK) {
+					warnx("failed getting new system type");
+				}
+
+				/* disable manual override for all systems that rely on electronic stabilization */
+				if (status.system_type == VEHICLE_TYPE_COAXIAL ||
+				    status.system_type == VEHICLE_TYPE_HELICOPTER ||
+				    status.system_type == VEHICLE_TYPE_TRICOPTER ||
+				    status.system_type == VEHICLE_TYPE_QUADROTOR ||
+				    status.system_type == VEHICLE_TYPE_HEXAROTOR ||
+				    status.system_type == VEHICLE_TYPE_OCTOROTOR) {
+					control_mode.flag_external_manual_override_ok = false;
+					status.is_rotary_wing = true;
+
+				} else {
+					control_mode.flag_external_manual_override_ok = true;
+					status.is_rotary_wing = false;
+				}
+
+				/* check and update system / component ID */
+				param_get(_param_system_id, &(status.system_id));
+				param_get(_param_component_id, &(status.component_id));
+				status_changed = true;
+
+				/* re-check RC calibration */
+				rc_calibration_ok = (OK == rc_calibration_check());
+
+				/* navigation parameters */
+				param_get(_param_takeoff_alt, &takeoff_alt);
+			}
+		}
+
+		orb_check(sp_man_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
+		}
+
+		orb_check(sp_offboard_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard);
+		}
+
+		orb_check(sensor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+		}
+
+		orb_check(diff_pres_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
+		}
+
+		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
+
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
+		/* update safety topic */
+		orb_check(safety_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(safety), safety_sub, &safety);
+		}
+
+		/* update global position estimate */
+		orb_check(global_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position);
+		}
+
+		/* update condition_global_position_valid */
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+
+		/* update local position estimate */
+		orb_check(local_position_sub, &updated);
+
+		if (updated) {
+			/* position changed */
+			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
+		}
+
+		/* update condition_local_position_valid and condition_local_altitude_valid */
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
+
+		if (status.condition_local_altitude_valid) {
+			if (status.condition_landed != local_position.landed) {
+				status.condition_landed = local_position.landed;
+				status_changed = true;
+
+				if (status.condition_landed) {
+					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+
+				} else {
+					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
+				}
+			}
+		}
+
+		/* update battery status */
+		orb_check(battery_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+
+			// warnx("bat v: %2.2f", battery.voltage_v);
+
+			/* only consider battery voltage if system has been running 2s and battery voltage is not 0 */
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) {
+				status.battery_voltage = battery.voltage_v;
+				status.condition_battery_voltage_valid = true;
+				status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage);
+			}
+		}
+
+		/* update subsystem */
+		orb_check(subsys_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
+
+			warnx("subsystem changed: %d\n", (int)info.subsystem_type);
+
+			/* mark / unmark as present */
+			if (info.present) {
+				status.onboard_control_sensors_present |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_present &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as enabled */
+			if (info.enabled) {
+				status.onboard_control_sensors_enabled |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_enabled &= ~info.subsystem_type;
+			}
+
+			/* mark / unmark as ok */
+			if (info.ok) {
+				status.onboard_control_sensors_health |= info.subsystem_type;
+
+			} else {
+				status.onboard_control_sensors_health &= ~info.subsystem_type;
+			}
+
+			status_changed = true;
+		}
+
+		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+			/* compute system load */
+			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
+
+			if (last_idle_time > 0)
+				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
+
+			last_idle_time = system_load.tasks[0].total_runtime;
+
+			/* check if board is connected via USB */
+			struct stat statbuf;
+			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
+		}
+
+		// XXX remove later
+		//warnx("bat remaining: %2.2f", status.battery_remaining);
+
+		/* if battery voltage is getting lower, warn using buzzer, etc. */
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
+			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				low_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
+				status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
+				status_changed = true;
+				battery_tune_played = false;
+			}
+
+			low_voltage_counter++;
+
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+			/* critical battery voltage, this is rather an emergency, change state machine */
+			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				critical_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+				status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
+				battery_tune_played = false;
+
+				if (armed.armed) {
+					// XXX not sure what should happen when voltage is low in flight
+					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+				} else {
+					// XXX should we still allow to arm with critical battery?
+					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+				}
+
+				status_changed = true;
+			}
+
+			critical_voltage_counter++;
+
+		} else {
+			low_voltage_counter = 0;
+			critical_voltage_counter = 0;
+		}
+
+		/* End battery voltage check */
+
+		/* If in INIT state, try to proceed to STANDBY state */
+		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
+			// XXX check for sensors
+			arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+		} else {
+			// XXX: Add emergency stuff if sensors are lost
+		}
+
+
+		/*
+		 * Check for valid position information.
+		 *
+		 * If the system has a valid position source from an onboard
+		 * position estimator, it is safe to operate it autonomously.
+		 * The flag_vector_flight_mode_ok flag indicates that a minimum
+		 * set of position measurements is available.
+		 */
+
+		orb_check(gps_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position);
+			/* check if GPS fix is ok */
+			float hdop_threshold_m = 4.0f;
+			float vdop_threshold_m = 8.0f;
+
+			/*
+			 * If horizontal dilution of precision (hdop / eph)
+			 * and vertical diluation of precision (vdop / epv)
+			 * are below a certain threshold (e.g. 4 m), AND
+			 * home position is not yet set AND the last GPS
+			 * GPS measurement is not older than two seconds AND
+			 * the system is currently not armed, set home
+			 * position to the current position.
+			 */
+
+			if (!home_position_set && gps_position.fix_type >= 3 &&
+			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
+			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
+				/* copy position data to uORB home message, store it locally as well */
+				// TODO use global position estimate
+				home.lat = gps_position.lat;
+				home.lon = gps_position.lon;
+				home.alt = gps_position.alt;
+
+				home.eph_m = gps_position.eph_m;
+				home.epv_m = gps_position.epv_m;
+
+				home.s_variance_m_s = gps_position.s_variance_m_s;
+				home.p_variance_m = gps_position.p_variance_m;
+
+				double home_lat_d = home.lat * 1e-7;
+				double home_lon_d = home.lon * 1e-7;
+				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+
+				/* announce new home position */
+				if (home_pub > 0) {
+					orb_publish(ORB_ID(home_position), home_pub, &home);
+
+				} else {
+					home_pub = orb_advertise(ORB_ID(home_position), &home);
+				}
+
+				/* mark home position as set */
+				home_position_set = true;
+				tune_positive();
+			}
+		}
+
+		/* ignore RC signals if in offboard control mode */
+		if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) {
+			/* start RC input check */
+			if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+				/* handle the case where RC signal was regained */
+				if (!status.rc_signal_found_once) {
+					status.rc_signal_found_once = true;
+					mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+					status_changed = true;
+
+				} else {
+					if (status.rc_signal_lost) {
+						mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+						status_changed = true;
+					}
+				}
+
+				status.rc_signal_lost = false;
+
+				transition_result_t res;	// store all transitions results here
+
+				/* arm/disarm by RC */
+				res = TRANSITION_NOT_CHANGED;
+
+				/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+				 * do it only for rotary wings */
+				if (status.is_rotary_wing &&
+				    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
+				    (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY ||
+				     (status.condition_landed && (
+					      status.navigation_state == NAVIGATION_STATE_ALTHOLD ||
+					      status.navigation_state == NAVIGATION_STATE_VECTOR
+				      ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+						/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
+						arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+						res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+						stick_off_counter = 0;
+
+					} else {
+						stick_off_counter++;
+					}
+
+				} else {
+					stick_off_counter = 0;
+				}
+
+				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
+				if (status.arming_state == ARMING_STATE_STANDBY &&
+				    status.main_state == MAIN_STATE_MANUAL &&
+				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
+					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						stick_on_counter = 0;
+
+					} else {
+						stick_on_counter++;
+					}
+
+				} else {
+					stick_on_counter = 0;
+				}
+
+				if (res == TRANSITION_CHANGED) {
+					if (status.arming_state == ARMING_STATE_ARMED) {
+						mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
+
+					} else {
+						mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
+					}
+
+				} else if (res == TRANSITION_DENIED) {
+					/* DENIED here indicates safety switch not pressed */
+
+					if (!(!safety.safety_switch_available || safety.safety_off)) {
+						print_reject_arm("NOT ARMING: Press safety switch first.");
+
+					} else {
+						warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+						mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					}
+				}
+
+				/* fill current_status according to mode switches */
+				check_mode_switches(&sp_man, &status);
+
+				/* evaluate the main state machine */
+				res = check_main_state_machine(&status);
+
+				if (res == TRANSITION_CHANGED) {
+					//mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state);
+					tune_positive();
+
+				} else if (res == TRANSITION_DENIED) {
+					/* DENIED here indicates bug in the commander */
+					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+				}
+
+			} else {
+				if (!status.rc_signal_lost) {
+					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
+					status.rc_signal_lost = true;
+					status_changed = true;
+				}
+			}
+		}
+
+		/* evaluate the navigation state machine */
+		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
+
+		if (res == TRANSITION_DENIED) {
+			/* DENIED here indicates bug in the commander */
+			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+		}
+
+		/* check which state machines for changes, clear "changed" flag */
+		bool arming_state_changed = check_arming_state_changed();
+		bool main_state_changed = check_main_state_changed();
+		bool navigation_state_changed = check_navigation_state_changed();
+
+		if (arming_state_changed || main_state_changed || navigation_state_changed) {
+			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
+			status_changed = true;
+
+		} else {
+			status_changed = false;
+		}
+
+		hrt_abstime t1 = hrt_absolute_time();
+
+		/* publish arming state */
+		if (arming_state_changed) {
+			armed.timestamp = t1;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		}
+
+		/* publish control mode */
+		if (navigation_state_changed || arming_state_changed) {
+			/* publish new navigation state */
+			control_mode.flag_armed = armed.armed;	// copy armed state to vehicle_control_mode topic
+			control_mode.counter++;
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+		}
+
+		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
+		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+			status.counter++;
+			status.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t1;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+		}
+
+		/* play arming and battery warning tunes */
+		if (!arm_tune_played && armed.armed) {
+			/* play tune when armed */
+			if (tune_arm() == OK)
+				arm_tune_played = true;
+
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+			/* play tune on battery warning */
+			if (tune_low_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
+			/* play tune on battery critical */
+			if (tune_critical_bat() == OK)
+				battery_tune_played = true;
+
+		} else if (battery_tune_played) {
+			tune_stop();
+			battery_tune_played = false;
+		}
+
+		/* reset arm_tune_played when disarmed */
+		if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
+			arm_tune_played = false;
+		}
+
+		fflush(stdout);
+		counter++;
+
+		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+
+		usleep(COMMANDER_MONITORING_INTERVAL);
+	}
+
+	/* wait for threads to complete */
+	pthread_join(commander_low_prio_thread, NULL);
+
+	/* close fds */
+	led_deinit();
+	buzzer_deinit();
+	close(sp_man_sub);
+	close(sp_offboard_sub);
+	close(local_position_sub);
+	close(global_position_sub);
+	close(gps_sub);
+	close(sensor_sub);
+	close(safety_sub);
+	close(cmd_sub);
+	close(subsys_sub);
+	close(diff_pres_sub);
+	close(param_changed_sub);
+	close(battery_sub);
+
+	warnx("exiting");
+	fflush(stdout);
+
+	thread_running = false;
+
+	return 0;
+}
+
+void
+check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed)
+{
+	hrt_abstime t = hrt_absolute_time();
+	bool valid_new = (t < timestamp + timeout && t > timeout && valid_in);
+
+	if (*valid_out != valid_new) {
+		*valid_out = valid_new;
+		*changed = true;
+	}
+}
+
+void
+toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+{
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
+	if (armed->armed) {
+		/* armed, solid */
+		led_on(LED_BLUE);
+
+	} else if (armed->ready_to_arm) {
+		/* ready to arm, blink at 1Hz */
+		if (leds_counter % 20 == 0)
+			led_toggle(LED_BLUE);
+
+	} else {
+		/* not ready to arm, blink at 10Hz */
+		if (leds_counter % 2 == 0)
+			led_toggle(LED_BLUE);
+	}
+
+#endif
+
+	if (changed) {
+		/* XXX TODO blink fast when armed and serious error occurs */
+
+		if (armed->armed) {
+			rgbled_set_mode(RGBLED_MODE_ON);
+		} else if (armed->ready_to_arm) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+		} else {
+			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+		}
+	}
+
+	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+		switch (status->battery_warning) {
+			case VEHICLE_BATTERY_WARNING_LOW:
+				rgbled_set_color(RGBLED_COLOR_YELLOW);
+				break;
+			case VEHICLE_BATTERY_WARNING_CRITICAL:
+				rgbled_set_color(RGBLED_COLOR_AMBER);
+				break;
+			default:
+				break;
+		}
+	} else {
+		switch (status->main_state) {
+			case MAIN_STATE_MANUAL:
+				rgbled_set_color(RGBLED_COLOR_WHITE);
+				break;
+			case MAIN_STATE_SEATBELT:
+			case MAIN_STATE_EASY:
+				rgbled_set_color(RGBLED_COLOR_GREEN);
+				break;
+			case MAIN_STATE_AUTO:
+				rgbled_set_color(RGBLED_COLOR_BLUE);
+				break;
+			default:
+				break;
+		}
+	}
+
+	/* give system warnings on error LED, XXX maybe add memory usage warning too */
+	if (status->load > 0.95f) {
+		if (leds_counter % 2 == 0)
+			led_toggle(LED_AMBER);
+
+	} else {
+		led_off(LED_AMBER);
+	}
+
+	leds_counter++;
+}
+
+void
+check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status)
+{
+	/* main mode switch */
+	if (!isfinite(sp_man->mode_switch)) {
+		warnx("mode sw not finite");
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_AUTO;
+
+	} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
+		current_status->mode_switch = MODE_SWITCH_MANUAL;
+
+	} else {
+		current_status->mode_switch = MODE_SWITCH_ASSISTED;
+	}
+
+	/* land switch */
+	if (!isfinite(sp_man->return_switch)) {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+
+	} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
+		current_status->return_switch = RETURN_SWITCH_RETURN;
+
+	} else {
+		current_status->return_switch = RETURN_SWITCH_NONE;
+	}
+
+	/* assisted switch */
+	if (!isfinite(sp_man->assisted_switch)) {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+
+	} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
+		current_status->assisted_switch = ASSISTED_SWITCH_EASY;
+
+	} else {
+		current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
+	}
+
+	/* mission switch  */
+	if (!isfinite(sp_man->mission_switch)) {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+
+	} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
+		current_status->mission_switch = MISSION_SWITCH_NONE;
+
+	} else {
+		current_status->mission_switch = MISSION_SWITCH_MISSION;
+	}
+}
+
+transition_result_t
+check_main_state_machine(struct vehicle_status_s *current_status)
+{
+	/* evaluate the main state machine */
+	transition_result_t res = TRANSITION_DENIED;
+
+	switch (current_status->mode_switch) {
+	case MODE_SWITCH_MANUAL:
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_ASSISTED:
+		if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) {
+			res = main_state_transition(current_status, MAIN_STATE_EASY);
+
+			if (res != TRANSITION_DENIED)
+				break;	// changed successfully or already in this state
+
+			// else fallback to SEATBELT
+			print_reject_mode("EASY");
+		}
+
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this mode
+
+		if (current_status->assisted_switch != ASSISTED_SWITCH_EASY)	// don't print both messages
+			print_reject_mode("SEATBELT");
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	case MODE_SWITCH_AUTO:
+		res = main_state_transition(current_status, MAIN_STATE_AUTO);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to SEATBELT (EASY likely will not work too)
+		print_reject_mode("AUTO");
+		res = main_state_transition(current_status, MAIN_STATE_SEATBELT);
+
+		if (res != TRANSITION_DENIED)
+			break;	// changed successfully or already in this state
+
+		// else fallback to MANUAL
+		res = main_state_transition(current_status, MAIN_STATE_MANUAL);
+		// TRANSITION_DENIED is not possible here
+		break;
+
+	default:
+		break;
+	}
+
+	return res;
+}
+
+void
+print_reject_mode(const char *msg)
+{
+	hrt_abstime t = hrt_absolute_time();
+
+	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		last_print_mode_reject_time = t;
+		char s[80];
+		sprintf(s, "[cmd] WARNING: reject %s", msg);
+		mavlink_log_critical(mavlink_fd, s);
+		tune_negative();
+	}
+}
+
+void
+print_reject_arm(const char *msg)
+{
+	hrt_abstime t = hrt_absolute_time();
+
+	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
+		last_print_mode_reject_time = t;
+		char s[80];
+		sprintf(s, "[cmd] %s", msg);
+		mavlink_log_critical(mavlink_fd, s);
+		tune_negative();
+	}
+}
+
+transition_result_t
+check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos)
+{
+	transition_result_t res = TRANSITION_DENIED;
+
+	if (status->main_state == MAIN_STATE_AUTO) {
+		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			// TODO AUTO_LAND handling
+			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+				/* don't switch to other states until takeoff not completed */
+				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+					return TRANSITION_NOT_CHANGED;
+				}
+			}
+			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
+				status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
+				status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
+				status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
+				/* possibly on ground, switch to TAKEOFF if needed */
+				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+					return res;
+				}
+			}
+			/* switch to AUTO mode */
+			if (status->rc_signal_found_once && !status->rc_signal_lost) {
+				/* act depending on switches when manual control enabled */
+				if (status->return_switch == RETURN_SWITCH_RETURN) {
+					/* RTL */
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+
+				} else {
+					if (status->mission_switch == MISSION_SWITCH_MISSION) {
+						/* MISSION */
+						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+
+					} else {
+						/* LOITER */
+						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
+					}
+				}
+			} else {
+				/* switch to MISSION when no RC control and first time in some AUTO mode  */
+				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
+					status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+					status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+					status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+					res = TRANSITION_NOT_CHANGED;
+
+				} else {
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+				}
+			}
+		} else {
+			/* disarmed, always switch to AUTO_READY */
+			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
+		}
+
+	} else {
+		/* manual control modes */
+		if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) {
+			/* switch to failsafe mode */
+			bool manual_control_old = control_mode->flag_control_manual_enabled;
+
+			if (!status->condition_landed) {
+				/* in air: try to hold position */
+				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+
+			} else {
+				/* landed: don't try to hold position but land (if taking off) */
+				res = TRANSITION_DENIED;
+			}
+
+			if (res == TRANSITION_DENIED) {
+				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+			}
+
+			control_mode->flag_control_manual_enabled = false;
+
+			if (res == TRANSITION_NOT_CHANGED && manual_control_old) {
+				/* mark navigation state as changed to force immediate flag publishing */
+				set_navigation_state_changed();
+				res = TRANSITION_CHANGED;
+			}
+
+			if (res == TRANSITION_CHANGED) {
+				if (control_mode->flag_control_position_enabled) {
+					mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+
+				} else {
+					if (status->condition_landed) {
+						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+					}
+				}
+			}
+
+		} else {
+			switch (status->main_state) {
+			case MAIN_STATE_MANUAL:
+				res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+				break;
+
+			case MAIN_STATE_SEATBELT:
+				res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode);
+				break;
+
+			case MAIN_STATE_EASY:
+				res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode);
+				break;
+
+			default:
+				break;
+			}
+		}
+	}
+
+	return res;
+}
+
+void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+	case VEHICLE_CMD_RESULT_ACCEPTED:
+			tune_positive();
+		break;
+
+	case VEHICLE_CMD_RESULT_DENIED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_FAILED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+		tune_negative();
+		break;
+
+	case VEHICLE_CMD_RESULT_UNSUPPORTED:
+		mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+		tune_negative();
+		break;
+
+	default:
+		break;
+	}
+}
+
+void *commander_low_prio_loop(void *arg)
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander_low_prio", getpid());
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+	memset(&cmd, 0, sizeof(cmd));
+
+	/* wakeup source(s) */
+	struct pollfd fds[1];
+
+	/* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */
+	fds[0].fd = cmd_sub;
+	fds[0].events = POLLIN;
+
+	while (!thread_should_exit) {
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);
+
+		/* timed out - periodic check for _task_should_exit, etc. */
+		if (pret == 0)
+			continue;
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		/* if we reach here, we have a valid command */
+		orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+		/* ignore commands the high-prio loop handles */
+		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
+		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
+		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF)
+			continue;
+
+		/* only handle low-priority commands here */
+		switch (cmd.command) {
+
+		case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+			if (is_safe(&status, &safety, &armed)) {
+
+				if (((int)(cmd.param1)) == 1) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot */
+					systemreset(false);
+
+				} else if (((int)(cmd.param1)) == 3) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					usleep(1000000);
+					/* reboot to bootloader */
+					systemreset(true);
+
+				} else {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+				}
+
+			} else {
+				answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+			}
+
+			break;
+
+		case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
+
+				int calib_ret = ERROR;
+
+				/* try to go to INIT/PREFLIGHT arming state */
+
+				// XXX disable interrupts in arming_state_transition
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					break;
+				}
+
+				if ((int)(cmd.param1) == 1) {
+					/* gyro calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_gyro_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param2) == 1) {
+					/* magnetometer calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_mag_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param3) == 1) {
+					/* zero-altitude pressure calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+				} else if ((int)(cmd.param4) == 1) {
+					/* RC calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+
+				} else if ((int)(cmd.param5) == 1) {
+					/* accelerometer calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_accel_calibration(mavlink_fd);
+
+				} else if ((int)(cmd.param6) == 1) {
+					/* airspeed calibration */
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+					calib_ret = do_airspeed_calibration(mavlink_fd);
+				}
+
+				if (calib_ret == OK)
+					tune_positive();
+				else
+					tune_negative();
+
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+
+				break;
+			}
+
+		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
+
+				if (((int)(cmd.param1)) == 0) {
+					if (0 == param_load_default()) {
+						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
+
+				} else if (((int)(cmd.param1)) == 1) {
+					if (0 == param_save_default()) {
+						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
+						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+
+					} else {
+						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
+					}
+				}
+
+				break;
+			}
+
+		default:
+			answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
+			break;
+		}
+
+		/* send any requested ACKs */
+		if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE
+		    && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) {
+			/* send acknowledge command */
+			// XXX TODO
+		}
+
+	}
+
+	return 0;
+}
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
new file mode 100644
index 000000000..7feace2b4
--- /dev/null
+++ b/src/modules/commander/commander_helper.cpp
@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.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 commander_helper.cpp
+ * Commander helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <fcntl.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_tone_alarm.h>
+#include <drivers/drv_led.h>
+#include <drivers/drv_rgbled.h>
+
+
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+bool is_multirotor(const struct vehicle_status_s *current_status)
+{
+	return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_TRICOPTER));
+}
+
+bool is_rotary_wing(const struct vehicle_status_s *current_status)
+{
+	return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
+	|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
+}
+
+static int buzzer;
+
+int buzzer_init()
+{
+	buzzer = open("/dev/tone_alarm", O_WRONLY);
+
+	if (buzzer < 0) {
+		warnx("Buzzer: open fail\n");
+		return ERROR;
+	}
+
+	return OK;
+}
+
+void buzzer_deinit()
+{
+	close(buzzer);
+}
+
+void tune_error()
+{
+	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+}
+
+void tune_positive()
+{
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
+}
+
+void tune_neutral()
+{
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
+}
+
+void tune_negative()
+{
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+}
+
+int tune_arm()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
+}
+
+int tune_low_bat()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
+}
+
+int tune_critical_bat()
+{
+	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
+}
+
+
+
+void tune_stop()
+{
+	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
+}
+
+static int leds;
+static int rgbleds;
+
+int led_init()
+{
+	/* first open normal LEDs */
+	leds = open(LED_DEVICE_PATH, 0);
+
+	if (leds < 0) {
+		warnx("LED: open fail\n");
+		return ERROR;
+	}
+
+	/* the blue LED is only available on FMUv1 but not FMUv2 */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
+	if (ioctl(leds, LED_ON, LED_BLUE)) {
+		warnx("Blue LED: ioctl fail\n");
+		return ERROR;
+	}
+#endif
+
+	if (ioctl(leds, LED_ON, LED_AMBER)) {
+		warnx("Amber LED: ioctl fail\n");
+		return ERROR;
+	}
+
+	/* then try RGB LEDs, this can fail on FMUv1*/
+	rgbleds = open(RGBLED_DEVICE_PATH, 0);
+	if (rgbleds == -1) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		errx(1, "Unable to open " RGBLED_DEVICE_PATH);
+#else
+		warnx("No RGB LED found");
+#endif
+	}
+
+	return 0;
+}
+
+void led_deinit()
+{
+	close(leds);
+
+	if (rgbleds != -1) {
+		close(rgbleds);
+	}
+}
+
+int led_toggle(int led)
+{
+	return ioctl(leds, LED_TOGGLE, led);
+}
+
+int led_on(int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
+
+int led_off(int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+void rgbled_set_color(rgbled_color_t color) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+}
+
+void rgbled_set_mode(rgbled_mode_t mode) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+}
+
+void rgbled_set_pattern(rgbled_pattern_t *pattern) {
+
+	if (rgbleds != -1)
+		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+}
+
+float battery_remaining_estimate_voltage(float voltage)
+{
+	float ret = 0;
+	static param_t bat_volt_empty;
+	static param_t bat_volt_full;
+	static param_t bat_n_cells;
+	static bool initialized = false;
+	static unsigned int counter = 0;
+	static float ncells = 3;
+	// XXX change cells to int (and param to INT32)
+
+	if (!initialized) {
+		bat_volt_empty = param_find("BAT_V_EMPTY");
+		bat_volt_full = param_find("BAT_V_FULL");
+		bat_n_cells = param_find("BAT_N_CELLS");
+		initialized = true;
+	}
+
+	static float chemistry_voltage_empty = 3.2f;
+	static float chemistry_voltage_full = 4.05f;
+
+	if (counter % 100 == 0) {
+		param_get(bat_volt_empty, &chemistry_voltage_empty);
+		param_get(bat_volt_full, &chemistry_voltage_full);
+		param_get(bat_n_cells, &ncells);
+	}
+
+	counter++;
+
+	ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty));
+
+	/* limit to sane values */
+	ret = (ret < 0.0f) ? 0.0f : ret;
+	ret = (ret > 1.0f) ? 1.0f : ret;
+	return ret;
+}
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/commander/commander_helper.h
index 048a55d33..027d0535f 100644
--- a/src/modules/mathlib/math/Quaternion.hpp
+++ b/src/modules/commander/commander_helper.h
@@ -1,6 +1,8 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,84 +34,54 @@
  ****************************************************************************/
 
 /**
- * @file Quaternion.hpp
- *
- * math quaternion lib
+ * @file commander_helper.h
+ * Commander helper functions definitions
  */
 
-#pragma once
-
-#include "Vector.hpp"
-#include "Matrix.hpp"
-
-namespace math
-{
-
-class Dcm;
-class EulerAngles;
-
-class __EXPORT Quaternion : public Vector
-{
-public:
+#ifndef COMMANDER_HELPER_H_
+#define COMMANDER_HELPER_H_
 
-	/**
-	 * default ctor
-	 */
-	Quaternion();
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <drivers/drv_rgbled.h>
 
-	/**
-	 * ctor from floats
-	 */
-	Quaternion(float a, float b, float c, float d);
 
-	/**
-	 * ctor from data
-	 */
-	Quaternion(const float *data);
+bool is_multirotor(const struct vehicle_status_s *current_status);
+bool is_rotary_wing(const struct vehicle_status_s *current_status);
 
-	/**
-	 * ctor from Vector
-	 */
-	Quaternion(const Vector &v);
+int buzzer_init(void);
+void buzzer_deinit(void);
 
-	/**
-	 * ctor from EulerAngles
-	 */
-	Quaternion(const EulerAngles &euler);
+void tune_error(void);
+void tune_positive(void);
+void tune_neutral(void);
+void tune_negative(void);
+int tune_arm(void);
+int tune_low_bat(void);
+int tune_critical_bat(void);
+void tune_stop(void);
 
-	/**
-	 * ctor from Dcm
-	 */
-	Quaternion(const Dcm &dcm);
+int led_init(void);
+void led_deinit(void);
+int led_toggle(int led);
+int led_on(int led);
+int led_off(int led);
 
-	/**
-	 * deep copy ctor
-	 */
-	Quaternion(const Quaternion &right);
+void rgbled_set_color(rgbled_color_t color);
 
-	/**
-	 * dtor
-	 */
-	virtual ~Quaternion();
+void rgbled_set_mode(rgbled_mode_t mode);
 
-	/**
-	 * derivative
-	 */
-	Vector derivative(const Vector &w);
+void rgbled_set_pattern(rgbled_pattern_t *pattern);
 
-	/**
-	 * accessors
-	 */
-	void setA(float a) { (*this)(0) = a; }
-	void setB(float b) { (*this)(1) = b; }
-	void setC(float c) { (*this)(2) = c; }
-	void setD(float d) { (*this)(3) = d; }
-	const float &getA() const { return (*this)(0); }
-	const float &getB() const { return (*this)(1); }
-	const float &getC() const { return (*this)(2); }
-	const float &getD() const { return (*this)(3); }
-};
-
-int __EXPORT quaternionTest();
-} // math
+/**
+ * Provides a coarse estimate of remaining battery power.
+ *
+ * The estimate is very basic and based on decharging voltage curves.
+ *
+ * @return the estimated remaining capacity in 0..1
+ */
+float battery_remaining_estimate_voltage(float voltage);
 
+#endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c
index 04b4e72ab..f22dac0c1 100644
--- a/src/modules/commander/commander.h
+++ b/src/modules/commander/commander_params.c
@@ -1,10 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *           Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,23 +33,22 @@
  ****************************************************************************/
 
 /**
- * @file commander.h
- * Main system state machine definition.
+ * @file commander_params.c
+ *
+ * Parameters defined by the sensors task.
  *
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
- *
  */
 
-#ifndef COMMANDER_H_
-#define COMMANDER_H_
-
-#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
-#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
-
-void tune_confirm(void);
-void tune_error(void);
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
 
-#endif /* COMMANDER_H_ */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f);
+PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
+PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
+PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
+PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
+PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
diff --git a/src/modules/mathlib/math/Vector.hpp b/src/modules/commander/commander_tests/commander_tests.cpp
index 73de793d5..6e72cf0d9 100644
--- a/src/modules/mathlib/math/Vector.hpp
+++ b/src/modules/commander/commander_tests/commander_tests.cpp
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,26 +33,23 @@
  ****************************************************************************/
 
 /**
- * @file Vector.h
+ * @file commander_tests.cpp
+ * Commander unit tests. Run the tests as follows:
+ *   nsh> commander_tests
  *
- * math vector
  */
 
-#pragma once
+#include <systemlib/err.h>
 
-#include <nuttx/config.h>
+#include "state_machine_helper_test.h"
 
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Vector.hpp"
-#else
-#include "generic/Vector.hpp"
-#endif
+extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);
 
-namespace math
+
+int commander_tests_main(int argc, char *argv[])
 {
-class Vector;
-int __EXPORT vectorTest();
-int __EXPORT vectorAddTest();
-int __EXPORT vectorSubTest();
-bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
-} // math
+	state_machine_helper_test();
+	//state_machine_test();
+
+	return 0;
+}
diff --git a/src/modules/mathlib/math/filter/module.mk b/src/modules/commander/commander_tests/module.mk
index fe92c8c70..4d10275d1 100644
--- a/src/modules/mathlib/math/filter/module.mk
+++ b/src/modules/commander/commander_tests/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 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,13 +32,10 @@
 ############################################################################
 
 #
-# filter library
+# System state machine tests.
 #
-SRCS		 = LowPassFilter2p.cpp
 
-#
-# In order to include .config we first have to save off the
-# current makefile name, since app.mk needs it.
-#
-APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
+MODULE_COMMAND		= commander_tests
+SRCS			= commander_tests.cpp \
+			state_machine_helper_test.cpp \
+			../state_machine_helper.cpp
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
new file mode 100644
index 000000000..40bedd9f3
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -0,0 +1,247 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper_test.cpp
+ * System state machine unit test.
+ *
+ */
+
+#include "state_machine_helper_test.h"
+
+#include "../state_machine_helper.h"
+#include <unit_test/unit_test.h>
+
+class StateMachineHelperTest : public UnitTest
+{
+public:
+	StateMachineHelperTest();
+	virtual ~StateMachineHelperTest();
+
+	virtual const char*	run_tests();
+
+private:
+	const char*	arming_state_transition_test();
+	const char*	arming_state_transition_arm_disarm_test();
+	const char*	main_state_transition_test();
+	const char*	is_safe_test();
+};
+
+StateMachineHelperTest::StateMachineHelperTest() {
+}
+
+StateMachineHelperTest::~StateMachineHelperTest() {
+}
+
+const char*
+StateMachineHelperTest::arming_state_transition_test()
+{
+	struct vehicle_status_s status;
+	struct safety_s safety;
+	arming_state_t new_arming_state;
+	struct actuator_armed_s armed;
+
+	// Identical states.
+	status.arming_state = ARMING_STATE_INIT;
+	new_arming_state = ARMING_STATE_INIT;
+	mu_assert("no transition: identical states",
+		  TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
+
+	// INIT to STANDBY.
+	armed.armed = false;
+	armed.ready_to_arm = false;
+	status.arming_state = ARMING_STATE_INIT;
+	status.condition_system_sensors_initialized = true;
+	new_arming_state = ARMING_STATE_STANDBY;
+	mu_assert("transition: init to standby",
+		  TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed));
+	mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state);
+	mu_assert("not armed", !armed.armed);
+	mu_assert("ready to arm", armed.ready_to_arm);
+
+	// INIT to STANDBY, sensors not initialized.
+	armed.armed = false;
+	armed.ready_to_arm = false;
+	status.arming_state = ARMING_STATE_INIT;
+	status.condition_system_sensors_initialized = false;
+	new_arming_state = ARMING_STATE_STANDBY;
+	mu_assert("no transition: sensors not initialized",
+		  TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed));
+	mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state);
+	mu_assert("not armed", !armed.armed);
+	mu_assert("not ready to arm", !armed.ready_to_arm);
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::arming_state_transition_arm_disarm_test()
+{
+	struct vehicle_status_s status;
+	struct safety_s safety;
+	arming_state_t new_arming_state;
+	struct actuator_armed_s armed;
+
+	// TODO(sjwilks): ARM then DISARM.
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::main_state_transition_test()
+{
+	struct vehicle_status_s current_state;
+	main_state_t new_main_state;
+	
+	// Identical states.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	new_main_state = MAIN_STATE_MANUAL;
+	mu_assert("no transition: identical states",
+		  TRANSITION_NOT_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);	
+
+	// AUTO to MANUAL.
+	current_state.main_state = MAIN_STATE_AUTO;
+	new_main_state = MAIN_STATE_MANUAL;
+	mu_assert("transition changed: auto to manual",
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	// MANUAL to SEATBELT.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_altitude_valid = true;
+	new_main_state = MAIN_STATE_SEATBELT;
+	mu_assert("tranisition: manual to seatbelt", 
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+
+	// MANUAL to SEATBELT, invalid local altitude.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_altitude_valid = false;
+	new_main_state = MAIN_STATE_SEATBELT;
+	mu_assert("no transition: invalid local altitude",
+		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	// MANUAL to EASY.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_position_valid = true;
+	new_main_state = MAIN_STATE_EASY;
+	mu_assert("transition: manual to easy",
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+
+	// MANUAL to EASY, invalid local position.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_local_position_valid = false;
+	new_main_state = MAIN_STATE_EASY;
+	mu_assert("no transition: invalid position",
+		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	// MANUAL to AUTO.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_global_position_valid = true;
+	new_main_state = MAIN_STATE_AUTO;
+	mu_assert("transition: manual to auto",
+		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state);
+
+	// MANUAL to AUTO, invalid global position.
+	current_state.main_state = MAIN_STATE_MANUAL;
+	current_state.condition_global_position_valid = false;
+	new_main_state = MAIN_STATE_AUTO;
+	mu_assert("no transition: invalid global position",
+		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
+	mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::is_safe_test()
+{
+	struct vehicle_status_s current_state;
+	struct safety_s safety;
+	struct actuator_armed_s armed;
+
+	armed.armed = false;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = false;
+	mu_assert("is safe: not armed", is_safe(&current_state, &safety, &armed));
+
+	armed.armed = false;
+	armed.lockdown = true;
+	safety.safety_switch_available = true;
+	safety.safety_off = true;
+	mu_assert("is safe: software lockdown", is_safe(&current_state, &safety, &armed));
+
+	armed.armed = true;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = true;
+	mu_assert("not safe: safety off", !is_safe(&current_state, &safety, &armed));
+
+	armed.armed = true;
+	armed.lockdown = false;
+	safety.safety_switch_available = true;
+	safety.safety_off = false;
+	mu_assert("is safe: safety off", is_safe(&current_state, &safety, &armed));
+
+	armed.armed = true;
+	armed.lockdown = false;
+	safety.safety_switch_available = false;
+	safety.safety_off = false;
+	mu_assert("not safe: no safety switch", !is_safe(&current_state, &safety, &armed));
+
+	return 0;
+}
+
+const char*
+StateMachineHelperTest::run_tests()
+{
+	mu_run_test(arming_state_transition_test);
+	mu_run_test(arming_state_transition_arm_disarm_test);
+	mu_run_test(main_state_transition_test);
+	mu_run_test(is_safe_test);
+
+	return 0;
+}
+
+void
+state_machine_helper_test()
+{
+	StateMachineHelperTest* test = new StateMachineHelperTest();
+	test->UnitTest::print_results(test->run_tests());
+}
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h
new file mode 100644
index 000000000..10a68e602
--- /dev/null
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.h
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper_test.h
+ */
+
+#ifndef STATE_MACHINE_HELPER_TEST_H_
+#define STATE_MACHINE_HELPER_TEST_
+
+void state_machine_helper_test();
+
+#endif /* STATE_MACHINE_HELPER_TEST_H_ */
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
new file mode 100644
index 000000000..33566d4e5
--- /dev/null
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -0,0 +1,289 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file gyro_calibration.cpp
+ * Gyroscope calibration routine
+ */
+
+#include "gyro_calibration.h"
+#include "commander_helper.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_gyro.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_gyro_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
+
+	const int calibration_count = 5000;
+
+	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s raw;
+
+	int calibration_counter = 0;
+	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
+
+	/* set offsets to zero */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+	struct gyro_scale gscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
+		warn("WARNING: failed to set scale / offsets for gyro");
+
+	close(fd);
+
+	unsigned poll_errcount = 0;
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret > 0) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+			gyro_offset[0] += raw.gyro_rad_s[0];
+			gyro_offset[1] += raw.gyro_rad_s[1];
+			gyro_offset[2] += raw.gyro_rad_s[2];
+			calibration_counter++;
+
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
+			return ERROR;
+		}
+	}
+
+	gyro_offset[0] = gyro_offset[0] / calibration_count;
+	gyro_offset[1] = gyro_offset[1] / calibration_count;
+	gyro_offset[2] = gyro_offset[2] / calibration_count;
+
+
+	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
+
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
+			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
+			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
+		}
+
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = {
+			gyro_offset[0],
+			1.0f,
+			gyro_offset[1],
+			1.0f,
+			gyro_offset[2],
+			1.0f,
+		};
+
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+			warn("WARNING: failed to set scale / offsets for gyro");
+
+		close(fd);
+
+		/* auto-save to EEPROM */
+		int save_ret = param_save_default();
+
+		if (save_ret != 0) {
+			warnx("WARNING: auto-save of params to storage failed");
+			mavlink_log_critical(mavlink_fd, "gyro store failed");
+			return ERROR;
+		}
+
+		mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+		tune_neutral();
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+		return ERROR;
+	}
+
+
+	/*** --- SCALING --- ***/
+
+	mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
+	mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
+	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
+
+	unsigned rotations_count = 30;
+	float gyro_integral = 0.0f;
+	float baseline_integral = 0.0f;
+
+	// XXX change to mag topic
+	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+	float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+	if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
+	if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
+
+
+	uint64_t last_time = hrt_absolute_time();
+	uint64_t start_time = hrt_absolute_time();
+
+	while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) {
+
+		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
+		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
+			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
+			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
+			mavlink_log_info(mavlink_fd, "gyro calibration done");
+			return OK;
+		}
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_sensor_combined;
+		fds[0].events = POLLIN;
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret) {
+
+			float dt_ms = (hrt_absolute_time() - last_time) / 1e3f;
+			last_time = hrt_absolute_time();
+
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+
+			// XXX this is just a proof of concept and needs world / body
+			// transformation and more
+
+			//math::Vector2f magNav(raw.magnetometer_ga);
+
+			// calculate error between estimate and measurement
+			// apply declination correction for true heading as well.
+			//float mag = -atan2f(magNav(1),magNav(0));
+			float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
+			if (mag > M_PI_F) mag -= 2*M_PI_F;
+			if (mag < -M_PI_F) mag += 2*M_PI_F;
+
+			float diff = mag - mag_last;
+
+			if (diff > M_PI_F) diff -= 2*M_PI_F;
+			if (diff < -M_PI_F) diff += 2*M_PI_F;
+
+			baseline_integral += diff;
+			mag_last = mag;
+			// Jump through some timing scale hoops to avoid
+			// operating near the 1e6/1e8 max sane resolution of float.
+			gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
+
+//			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
+
+		// } else if (poll_ret == 0) {
+		// 	/* any poll failure for 1s is a reason to abort */
+		// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
+		// 	return;
+		}
+	}
+
+	float gyro_scale = baseline_integral / gyro_integral;
+	float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
+	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
+
+
+	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
+
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
+			|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
+			|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
+			mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
+		}
+
+		/* set offsets to actual value */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		struct gyro_scale gscale = {
+			gyro_offset[0],
+			gyro_scales[0],
+			gyro_offset[1],
+			gyro_scales[1],
+			gyro_offset[2],
+			gyro_scales[2],
+		};
+
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+			warn("WARNING: failed to set scale / offsets for gyro");
+
+		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, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
+		// mavlink_log_info(mavlink_fd, buf);
+		mavlink_log_info(mavlink_fd, "gyro calibration done");
+
+		/* third beep by cal end routine */
+		return OK;
+	} else {
+		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		return ERROR;
+	}
+}
diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/modules/commander/gyro_calibration.h
index 21661622a..59c32a15e 100644
--- a/src/modules/mathlib/math/arm/Matrix.cpp
+++ b/src/modules/commander/gyro_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,9 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.cpp
- *
- * matrix code
+ * @file gyro_calibration.h
+ * Gyroscope calibration routine
  */
 
-#include "Matrix.hpp"
+#ifndef GYRO_CALIBRATION_H_
+#define GYRO_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_gyro_calibration(int mavlink_fd);
+
+#endif /* GYRO_CALIBRATION_H_ */
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
new file mode 100644
index 000000000..e38616027
--- /dev/null
+++ b/src/modules/commander/mag_calibration.cpp
@@ -0,0 +1,298 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag_calibration.cpp
+ * Magnetometer calibration routine
+ */
+
+#include "mag_calibration.h"
+#include "commander_helper.h"
+#include "calibration_routines.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <poll.h>
+#include <math.h>
+#include <fcntl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/sensor_combined.h>
+#include <drivers/drv_mag.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+int do_mag_calibration(int mavlink_fd)
+{
+	mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
+
+	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+	struct mag_report mag;
+
+	/* 45 seconds */
+	uint64_t calibration_interval = 45 * 1000 * 1000;
+
+	/* maximum 2000 values */
+	const unsigned int calibration_maxcount = 500;
+	unsigned int calibration_counter = 0;
+
+	/* limit update rate to get equally spaced measurements over time (in ms) */
+	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
+
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	/* erase old calibration */
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
+		warn("WARNING: failed to set scale / offsets for mag");
+		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
+	}
+
+	/* calibrate range */
+	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
+		warnx("failed to calibrate scale");
+	}
+
+	close(fd);
+
+	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+
+	/* calibrate offsets */
+
+	// uint64_t calibration_start = hrt_absolute_time();
+
+	uint64_t axis_deadline = hrt_absolute_time();
+	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+
+	const char axislabels[3] = { 'X', 'Y', 'Z'};
+	int axis_index = -1;
+
+	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
+	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
+	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
+
+	if (x == NULL || y == NULL || z == NULL) {
+		warnx("mag cal failed: out of memory");
+		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
+		warnx("x:%p y:%p z:%p\n", x, y, z);
+		return ERROR;
+	}
+
+	mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
+
+	unsigned poll_errcount = 0;
+
+	while (hrt_absolute_time() < calibration_deadline &&
+	       calibration_counter < calibration_maxcount) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1];
+		fds[0].fd = sub_mag;
+		fds[0].events = POLLIN;
+
+		/* user guidance */
+		if (hrt_absolute_time() >= axis_deadline &&
+		    axis_index < 3) {
+
+			axis_index++;
+
+			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
+			mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
+			tune_neutral();
+
+			axis_deadline += calibration_interval / 3;
+		}
+
+		if (!(axis_index < 3)) {
+			break;
+		}
+
+		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
+
+		// if ((axis_left / 1000) == 0 && axis_left > 0) {
+		// 	char buf[50];
+		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
+		// 	mavlink_log_info(mavlink_fd, buf);
+		// }
+
+		int poll_ret = poll(fds, 1, 1000);
+
+		if (poll_ret > 0) {
+			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+
+			x[calibration_counter] = mag.x;
+			y[calibration_counter] = mag.y;
+			z[calibration_counter] = mag.z;
+
+			/* get min/max values */
+
+			// if (mag.x < mag_min[0]) {
+			// 	mag_min[0] = mag.x;
+			// }
+			// else if (mag.x > mag_max[0]) {
+			// 	mag_max[0] = mag.x;
+			// }
+
+			// if (raw.magnetometer_ga[1] < mag_min[1]) {
+			// 	mag_min[1] = raw.magnetometer_ga[1];
+			// }
+			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
+			// 	mag_max[1] = raw.magnetometer_ga[1];
+			// }
+
+			// if (raw.magnetometer_ga[2] < mag_min[2]) {
+			// 	mag_min[2] = raw.magnetometer_ga[2];
+			// }
+			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
+			// 	mag_max[2] = raw.magnetometer_ga[2];
+			// }
+
+			calibration_counter++;
+
+		} else {
+			poll_errcount++;
+		}
+
+		if (poll_errcount > 1000) {
+			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
+			return ERROR;
+		}
+
+
+	}
+
+	float sphere_x;
+	float sphere_y;
+	float sphere_z;
+	float sphere_radius;
+
+	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+
+	free(x);
+	free(y);
+	free(z);
+
+	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
+
+		fd = open(MAG_DEVICE_PATH, 0);
+
+		struct mag_scale mscale;
+
+		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
+			warn("WARNING: failed to get scale / offsets for mag");
+
+		mscale.x_offset = sphere_x;
+		mscale.y_offset = sphere_y;
+		mscale.z_offset = sphere_z;
+
+		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+			warn("WARNING: failed to set scale / offsets for mag");
+
+		close(fd);
+
+		/* announce and set new offset */
+
+		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
+			warnx("Setting X mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
+			warnx("Setting Y mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
+			warnx("Setting Z mag offset failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
+			warnx("Setting X mag scale failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
+			warnx("Setting Y mag scale failed!\n");
+		}
+
+		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+			warnx("Setting Z mag scale failed!\n");
+		}
+
+		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
+
+		/* 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, "FAILED storing calibration");
+			return ERROR;
+		}
+
+		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
+		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
+		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
+
+		char buf[52];
+		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+			(double)mscale.y_offset, (double)mscale.z_offset);
+		mavlink_log_info(mavlink_fd, buf);
+
+		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+			(double)mscale.y_scale, (double)mscale.z_scale);
+		mavlink_log_info(mavlink_fd, buf);
+
+		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
+		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
+
+		return OK;
+		/* third beep by cal end routine */
+
+	} else {
+		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+		return ERROR;
+	}
+}
diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/modules/commander/mag_calibration.h
index 7ea6496bb..a101cd7b1 100644
--- a/src/modules/mathlib/math/generic/Vector.cpp
+++ b/src/modules/commander/mag_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,9 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
- *
- * math vector
+ * @file mag_calibration.h
+ * Magnetometer calibration routine
  */
 
-#include "Vector.hpp"
+#ifndef MAG_CALIBRATION_H_
+#define MAG_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_mag_calibration(int mavlink_fd);
+
+#endif /* MAG_CALIBRATION_H_ */
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index fe44e955a..91d75121e 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -36,8 +36,15 @@
 #
 
 MODULE_COMMAND	 	= commander
-SRCS		 	= commander.c \
-			state_machine_helper.c \
-			calibration_routines.c \
-			accelerometer_calibration.c
+SRCS		 	= commander.cpp \
+			commander_params.c \
+			state_machine_helper.cpp \
+			commander_helper.cpp \
+			calibration_routines.cpp \
+			accelerometer_calibration.cpp \
+			gyro_calibration.cpp \
+			mag_calibration.cpp \
+			baro_calibration.cpp \
+			rc_calibration.cpp \
+			airspeed_calibration.cpp
 
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
new file mode 100644
index 000000000..b60a7e0b9
--- /dev/null
+++ b/src/modules/commander/px4_custom_mode.h
@@ -0,0 +1,37 @@
+/*
+ * px4_custom_mode.h
+ *
+ *  Created on: 09.08.2013
+ *      Author: ton
+ */
+
+#ifndef PX4_CUSTOM_MODE_H_
+#define PX4_CUSTOM_MODE_H_
+
+enum PX4_CUSTOM_MAIN_MODE {
+	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
+	PX4_CUSTOM_MAIN_MODE_SEATBELT,
+	PX4_CUSTOM_MAIN_MODE_EASY,
+	PX4_CUSTOM_MAIN_MODE_AUTO,
+};
+
+enum PX4_CUSTOM_SUB_MODE_AUTO {
+	PX4_CUSTOM_SUB_MODE_AUTO_READY = 1,
+	PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF,
+	PX4_CUSTOM_SUB_MODE_AUTO_LOITER,
+	PX4_CUSTOM_SUB_MODE_AUTO_MISSION,
+	PX4_CUSTOM_SUB_MODE_AUTO_RTL,
+	PX4_CUSTOM_SUB_MODE_AUTO_LAND,
+};
+
+union px4_custom_mode {
+	struct {
+		uint16_t reserved;
+		uint8_t main_mode;
+		uint8_t sub_mode;
+	};
+	uint32_t data;
+	float data_float;
+};
+
+#endif /* PX4_CUSTOM_MODE_H_ */
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/commander/rc_calibration.cpp
index dcb85600e..fe87a3323 100644
--- a/src/modules/mathlib/math/Vector3.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,68 +32,57 @@
  ****************************************************************************/
 
 /**
- * @file Vector3.cpp
- *
- * math vector
+ * @file rc_calibration.cpp
+ * Remote Control calibration routine
  */
 
-#include "test/test.hpp"
+#include "rc_calibration.h"
+#include "commander_helper.h"
 
-#include "Vector3.hpp"
+#include <poll.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
 
-namespace math
-{
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
-Vector3::Vector3() :
-	Vector(3)
+int do_rc_calibration(int mavlink_fd)
 {
-}
+	mavlink_log_info(mavlink_fd, "trim calibration starting");
 
-Vector3::Vector3(const Vector &right) :
-	Vector(right)
-{
-#ifdef VECTOR_ASSERT
-	ASSERT(right.getRows() == 3);
-#endif
-}
+	/* XXX fix this */
+	// if (current_status.rc_signal) {
+	// 	mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
+	// 	return;
+	// }
 
-Vector3::Vector3(float x, float y, float z) :
-	Vector(3)
-{
-	setX(x);
-	setY(y);
-	setZ(z);
-}
+	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
+	struct manual_control_setpoint_s sp;
+	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
-Vector3::Vector3(const float *data) :
-	Vector(3, data)
-{
-}
+	/* set parameters */
+	float p = sp.roll;
+	param_set(param_find("TRIM_ROLL"), &p);
+	p = sp.pitch;
+	param_set(param_find("TRIM_PITCH"), &p);
+	p = sp.yaw;
+	param_set(param_find("TRIM_YAW"), &p);
 
-Vector3::~Vector3()
-{
-}
+	/* store to permanent storage */
+	/* auto-save */
+	int save_ret = param_save_default();
 
-Vector3 Vector3::cross(const Vector3 &b) const
-{
-	const Vector3 &a = *this;
-	Vector3 result;
-	result(0) = a(1) * b(2) - a(2) * b(1);
-	result(1) = a(2) * b(0) - a(0) * b(2);
-	result(2) = a(0) * b(1) - a(1) * b(0);
-	return result;
-}
+	if (save_ret != 0) {
+		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		return ERROR;
+	}
 
-int __EXPORT vector3Test()
-{
-	printf("Test Vector3\t\t: ");
-	// test float ctor
-	Vector3 v(1, 2, 3);
-	ASSERT(equal(v(0), 1));
-	ASSERT(equal(v(1), 2));
-	ASSERT(equal(v(2), 3));
-	printf("PASS\n");
-	return 0;
+	mavlink_log_info(mavlink_fd, "trim calibration done");
+	return OK;
 }
-
-} // namespace math
diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/modules/commander/rc_calibration.h
index 7ea6496bb..9aa6faafa 100644
--- a/src/modules/mathlib/math/arm/Vector.cpp
+++ b/src/modules/commander/rc_calibration.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,9 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
- *
- * math vector
+ * @file rc_calibration.h
+ * Remote Control calibration routine
  */
 
-#include "Vector.hpp"
+#ifndef RC_CALIBRATION_H_
+#define RC_CALIBRATION_H_
+
+#include <stdint.h>
+
+int do_rc_calibration(int mavlink_fd);
+
+#endif /* RC_CALIBRATION_H_ */
diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c
deleted file mode 100644
index ab728c7bb..000000000
--- a/src/modules/commander/state_machine_helper.c
+++ /dev/null
@@ -1,757 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 state_machine_helper.c
- * State machine helper functions implementations
- */
-
-#include <stdio.h>
-#include <unistd.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <systemlib/systemlib.h>
-#include <drivers/drv_hrt.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-
-static const char *system_state_txt[] = {
-	"SYSTEM_STATE_PREFLIGHT",
-	"SYSTEM_STATE_STANDBY",
-	"SYSTEM_STATE_GROUND_READY",
-	"SYSTEM_STATE_MANUAL",
-	"SYSTEM_STATE_STABILIZED",
-	"SYSTEM_STATE_AUTO",
-	"SYSTEM_STATE_MISSION_ABORT",
-	"SYSTEM_STATE_EMCY_LANDING",
-	"SYSTEM_STATE_EMCY_CUTOFF",
-	"SYSTEM_STATE_GROUND_ERROR",
-	"SYSTEM_STATE_REBOOT",
-
-};
-
-/**
- * Transition from one state to another
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
-{
-	int invalid_state = false;
-	int ret = ERROR;
-
-	commander_state_machine_t old_state = current_status->state_machine;
-
-	switch (new_state) {
-	case SYSTEM_STATE_MISSION_ABORT: {
-			/* Indoor or outdoor */
-			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
-
-			// } else {
-			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
-			// }
-		}
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		/* Tell the controller to land */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		warnx("EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		/* Tell the controller to cutoff the motors (thrust = 0) */
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = false;
-
-		warnx("EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-
-		/* set system flags according to state */
-
-		/* prevent actuators from arming */
-		current_status->flag_system_armed = false;
-
-		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
-		break;
-
-	case SYSTEM_STATE_PREFLIGHT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
-		}
-
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
-			|| current_status->flag_hil_enabled) {
-			invalid_state = false;
-			/* set system flags according to state */
-			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
-			usleep(500000);
-			up_systemreset();
-			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
-
-		} else {
-			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
-		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		/* set system flags according to state */
-
-		/* standby enforces disarmed */
-		current_status->flag_system_armed = false;
-
-		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-
-		/* set system flags according to state */
-
-		/* ground ready has motors / actuators armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
-		break;
-
-	case SYSTEM_STATE_AUTO:
-
-		/* set system flags according to state */
-
-		/* auto is airborne and in auto mode, motors armed */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-
-		/* set system flags according to state */
-		current_status->flag_system_armed = true;
-
-		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
-		break;
-
-	default:
-		invalid_state = true;
-		break;
-	}
-
-	if (invalid_state == false || old_state != new_state) {
-		current_status->state_machine = new_state;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		ret = OK;
-	}
-
-	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* publish the new state */
-	current_status->counter++;
-	current_status->timestamp = hrt_absolute_time();
-
-	/* assemble state vector based on flag values */
-	if (current_status->flag_control_rates_enabled) {
-		current_status->onboard_control_sensors_present |= 0x400;
-
-	} else {
-		current_status->onboard_control_sensors_present &= ~0x400;
-	}
-
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
-	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
-
-	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
-}
-
-void publish_armed_status(const struct vehicle_status_s *current_status)
-{
-	struct actuator_armed_s armed;
-	armed.armed = current_status->flag_system_armed;
-
-	/* XXX allow arming by external components on multicopters only if not yet armed by RC */
-	/* XXX allow arming only if core sensors are ok */
-	armed.ready_to_arm = true;
-
-	/* lock down actuators if required, only in HIL */
-	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
-	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
-	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
-}
-
-
-/*
- * Private functions, update the state machine
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	warnx("EMERGENCY HANDLER\n");
-	/* Depending on the current state go to one of the error states */
-
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
-
-		// DO NOT abort mission
-		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
-
-	} else {
-		warnx("Unknown system state: #%d\n", current_status->state_machine);
-	}
-}
-
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
-{
-	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
-		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
-
-	} else {
-		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
-	}
-
-}
-
-
-
-// /*
-//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
-//  */
-
-// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
-//  *
-//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
-//  * */
-
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was removed something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-// }
-
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if a subsystem was disabled something went completely wrong */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
-// 		state_machine_emergency_always_critical(status_pub, current_status);
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		{
-// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
-
-// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
-// 				state_machine_emergency(status_pub, current_status);
-// 			}
-// 		}
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//TODO state machine change (recovering)
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//TODO state machine change
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// 		//TODO state machine change
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-
-// }
-
-
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
-// {
-// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
-// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
-// 	current_status->counter++;
-// 	current_status->timestamp = hrt_absolute_time();
-// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-
-// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
-
-// 	switch (*subsystem_type) {
-// 	case SUBSYSTEM_TYPE_GYRO:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_ACC:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_MAG:
-// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
-
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency_always_critical(status_pub, current_status);
-
-// 		break;
-
-// 	case SUBSYSTEM_TYPE_GPS:
-// //			//TODO: remove this block
-// //			break;
-// //			///////////////////
-// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
-
-// //				printf("previosly_healthy = %u\n", previosly_healthy);
-// 		if (previosly_healthy) 	//only throw emergency if previously healthy
-// 			state_machine_emergency(status_pub, current_status);
-
-// 		break;
-
-// 	default:
-// 		break;
-// 	}
-
-// }
-
-
-/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
-
-
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	/* Depending on the current state switch state */
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		state_machine_emergency(status_pub, current_status, mavlink_fd);
-	}
-}
-
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[cmd] arming\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-	}
-}
-
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-		printf("[cmd] going standby\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-
-	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] MISSION ABORT!\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-	}
-}
-
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	int old_mode = current_status->flight_mode;
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-
-	current_status->flag_control_manual_enabled = true;
-
-	/* set behaviour based on airframe */
-	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
-	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
-
-		/* assuming a rotary wing, set to SAS */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-
-	} else {
-
-		/* assuming a fixed wing, set to direct pass-through */
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
-		current_status->flag_control_attitude_enabled = false;
-		current_status->flag_control_rates_enabled = false;
-	}
-
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] manual mode\n");
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-	}
-}
-
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		int old_mode = current_status->flight_mode;
-		int old_manual_control_mode = current_status->manual_control_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		current_status->flag_control_manual_enabled = true;
-
-		if (old_mode != current_status->flight_mode ||
-		    old_manual_control_mode != current_status->manual_control_mode) {
-			printf("[cmd] att stabilized mode\n");
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-		}
-
-	}
-}
-
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
-		tune_error();
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-		printf("[cmd] position guided mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-
-	}
-}
-
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	if (!current_status->flag_vector_flight_mode_ok) {
-		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
-		return;
-	}
-
-	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
-		printf("[cmd] auto mode\n");
-		int old_mode = current_status->flight_mode;
-		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
-		current_status->flag_control_manual_enabled = false;
-		current_status->flag_control_attitude_enabled = true;
-		current_status->flag_control_rates_enabled = true;
-		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-
-		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
-	}
-}
-
-
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
-{
-	uint8_t ret = 1;
-
-	/* Switch on HIL if in standby and not already in HIL mode */
-	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-	    && !current_status->flag_hil_enabled) {
-		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
-			/* Enable HIL on request */
-			current_status->flag_hil_enabled = true;
-			ret = OK;
-			state_machine_publish(status_pub, current_status, mavlink_fd);
-			publish_armed_status(current_status);
-			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-
-		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
-			   current_status->flag_system_armed) {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
-
-		} else {
-
-			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
-		}
-	}
-
-	/* switch manual / auto */
-	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
-		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
-		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
-		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
-
-	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
-	}
-
-	/* vehicle is disarmed, mode requests arming */
-	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only arm in standby state */
-		// XXX REMOVE
-		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
-			ret = OK;
-			printf("[cmd] arming due to command request\n");
-		}
-	}
-
-	/* vehicle is armed, mode requests disarming */
-	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
-		/* only disarm in ground ready */
-		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
-			ret = OK;
-			printf("[cmd] disarming due to command request\n");
-		}
-	}
-
-	/* NEVER actually switch off HIL without reboot */
-	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
-		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
-{
-	commander_state_machine_t current_system_state = current_status->state_machine;
-
-	uint8_t ret = 1;
-
-	switch (custom_mode) {
-	case SYSTEM_STATE_GROUND_READY:
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		printf("try to reboot\n");
-
-		if (current_system_state == SYSTEM_STATE_STANDBY
-				|| current_system_state == SYSTEM_STATE_PREFLIGHT
-				|| current_status->flag_hil_enabled) {
-			printf("system will reboot\n");
-			mavlink_log_critical(mavlink_fd, "Rebooting..");
-			usleep(200000);
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		printf("try to switch to auto/takeoff\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
-			printf("state: auto\n");
-			ret = 0;
-		}
-
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		printf("try to switch to manual\n");
-
-		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
-			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
-			printf("state: manual\n");
-			ret = 0;
-		}
-
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
new file mode 100644
index 000000000..3cef10185
--- /dev/null
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -0,0 +1,698 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.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 state_machine_helper.cpp
+ * State machine helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+#include "commander_helper.h"
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static bool arming_state_changed = true;
+static bool main_state_changed = true;
+static bool navigation_state_changed = true;
+
+transition_result_t
+arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
+{
+	/*
+	 * Perform an atomic state update
+	 */
+	irqstate_t flags = irqsave();
+
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_arming_state == status->arming_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_arming_state) {
+		case ARMING_STATE_INIT:
+
+			/* allow going back from INIT for calibration */
+			if (status->arming_state == ARMING_STATE_STANDBY) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY:
+
+			/* allow coming from INIT and disarming from ARMED */
+			if (status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_ARMED) {
+
+				/* sensors need to be initialized for STANDBY state */
+				if (status->condition_system_sensors_initialized) {
+					ret = TRANSITION_CHANGED;
+					armed->armed = false;
+					armed->ready_to_arm = true;
+				}
+			}
+
+			break;
+
+		case ARMING_STATE_ARMED:
+
+			/* allow arming from STANDBY and IN-AIR-RESTORE */
+			if ((status->arming_state == ARMING_STATE_STANDBY
+			     || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)
+			    && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */
+				ret = TRANSITION_CHANGED;
+				armed->armed = true;
+				armed->ready_to_arm = true;
+			}
+
+			break;
+
+		case ARMING_STATE_ARMED_ERROR:
+
+			/* an armed error happens when ARMED obviously */
+			if (status->arming_state == ARMING_STATE_ARMED) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = true;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_STANDBY_ERROR:
+
+			/* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */
+			if (status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_REBOOT:
+
+			/* an armed error happens when ARMED obviously */
+			if (status->arming_state == ARMING_STATE_INIT
+			    || status->arming_state == ARMING_STATE_STANDBY
+			    || status->arming_state == ARMING_STATE_STANDBY_ERROR) {
+				ret = TRANSITION_CHANGED;
+				armed->armed = false;
+				armed->ready_to_arm = false;
+			}
+
+			break;
+
+		case ARMING_STATE_IN_AIR_RESTORE:
+
+			/* XXX implement */
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			status->arming_state = new_arming_state;
+			arming_state_changed = true;
+		}
+	}
+
+	/* end of atomic state update */
+	irqrestore(flags);
+
+	if (ret == TRANSITION_DENIED)
+		warnx("arming transition rejected");
+
+	return ret;
+}
+
+bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
+{
+	// System is safe if:
+	// 1) Not armed
+	// 2) Armed, but in software lockdown (HIL)
+	// 3) Safety switch is present AND engaged -> actuators locked
+	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+bool
+check_arming_state_changed()
+{
+	if (arming_state_changed) {
+		arming_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_main_state == current_state->main_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_main_state) {
+		case MAIN_STATE_MANUAL:
+			ret = TRANSITION_CHANGED;
+			break;
+
+		case MAIN_STATE_SEATBELT:
+
+			/* need altitude estimate */
+			if (current_state->condition_local_altitude_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case MAIN_STATE_EASY:
+
+			/* need local position estimate */
+			if (current_state->condition_local_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+
+		case MAIN_STATE_AUTO:
+
+			/* need global position estimate */
+			if (current_state->condition_global_position_valid) {
+				ret = TRANSITION_CHANGED;
+			}
+
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			current_state->main_state = new_main_state;
+			main_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool
+check_main_state_changed()
+{
+	if (main_state_changed) {
+		main_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+transition_result_t
+navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode)
+{
+	transition_result_t ret = TRANSITION_DENIED;
+
+	/* only check transition if the new state is actually different from the current one */
+	if (new_navigation_state == status->navigation_state) {
+		ret = TRANSITION_NOT_CHANGED;
+
+	} else {
+
+		switch (new_navigation_state) {
+		case NAVIGATION_STATE_DIRECT:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_STABILIZE:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_ALTHOLD:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_VECTOR:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = true;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_AUTO_READY:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = false;
+			control_mode->flag_control_attitude_enabled = false;
+			control_mode->flag_control_velocity_enabled = false;
+			control_mode->flag_control_position_enabled = false;
+			control_mode->flag_control_altitude_enabled = false;
+			control_mode->flag_control_climb_rate_enabled = false;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_TAKEOFF:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_LOITER:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = false;
+			break;
+
+		case NAVIGATION_STATE_AUTO_MISSION:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_RTL:
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
+			break;
+
+		case NAVIGATION_STATE_AUTO_LAND:
+
+			/* deny transitions from landed state */
+			if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+				ret = TRANSITION_CHANGED;
+				control_mode->flag_control_rates_enabled = true;
+				control_mode->flag_control_attitude_enabled = true;
+				control_mode->flag_control_velocity_enabled = true;
+				control_mode->flag_control_position_enabled = true;
+				control_mode->flag_control_altitude_enabled = true;
+				control_mode->flag_control_climb_rate_enabled = true;
+				control_mode->flag_control_manual_enabled = false;
+				control_mode->flag_control_auto_enabled = true;
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+		if (ret == TRANSITION_CHANGED) {
+			status->navigation_state = new_navigation_state;
+			control_mode->auto_state = status->navigation_state;
+			navigation_state_changed = true;
+		}
+	}
+
+	return ret;
+}
+
+bool
+check_navigation_state_changed()
+{
+	if (navigation_state_changed) {
+		navigation_state_changed = false;
+		return true;
+
+	} else {
+		return false;
+	}
+}
+
+void
+set_navigation_state_changed()
+{
+	navigation_state_changed = true;
+}
+
+/**
+* Transition from one hil state to another
+*/
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd)
+{
+	bool valid_transition = false;
+	int ret = ERROR;
+
+	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
+
+	if (current_status->hil_state == new_state) {
+		warnx("Hil state not changed");
+		valid_transition = true;
+
+	} else {
+
+		switch (new_state) {
+
+		case HIL_STATE_OFF:
+
+			if (current_status->arming_state == ARMING_STATE_INIT
+			    || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+				current_control_mode->flag_system_hil_enabled = false;
+				mavlink_log_critical(mavlink_fd, "Switched to OFF hil state");
+				valid_transition = true;
+			}
+
+			break;
+
+		case HIL_STATE_ON:
+
+			if (current_status->arming_state == ARMING_STATE_INIT
+			    || current_status->arming_state == ARMING_STATE_STANDBY) {
+
+				current_control_mode->flag_system_hil_enabled = true;
+				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
+				valid_transition = true;
+			}
+
+			break;
+
+		default:
+			warnx("Unknown hil state");
+			break;
+		}
+	}
+
+	if (valid_transition) {
+		current_status->hil_state = new_state;
+
+		current_status->counter++;
+		current_status->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+		current_control_mode->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode);
+
+		ret = OK;
+
+	} else {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
+	}
+
+	return ret;
+}
+
+
+
+// /*
+//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
+//  */
+
+// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
+//  *
+//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
+//  * */
+
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was removed something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was disabled something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+//
+//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+//{
+//	int ret = 1;
+//
+////	/* Switch on HIL if in standby and not already in HIL mode */
+////	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+////	    && !current_status->flag_hil_enabled) {
+////		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+////			/* Enable HIL on request */
+////			current_status->flag_hil_enabled = true;
+////			ret = OK;
+////			state_machine_publish(status_pub, current_status, mavlink_fd);
+////			publish_armed_status(current_status);
+////			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+////
+////		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+////			   current_status->flag_fmu_armed) {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+////
+////		} else {
+////
+////			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+////		}
+////	}
+//
+//	/* switch manual / auto */
+//	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+//		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+//		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+//		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+//
+//	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+//		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+//	}
+//
+//	/* vehicle is disarmed, mode requests arming */
+//	if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only arm in standby state */
+//		// XXX REMOVE
+//		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+//			ret = OK;
+//			printf("[cmd] arming due to command request\n");
+//		}
+//	}
+//
+//	/* vehicle is armed, mode requests disarming */
+//	if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+//		/* only disarm in ground ready */
+//		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+//			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+//			ret = OK;
+//			printf("[cmd] disarming due to command request\n");
+//		}
+//	}
+//
+//	/* NEVER actually switch off HIL without reboot */
+//	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+//		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+//		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
+//		ret = ERROR;
+//	}
+//
+//	return ret;
+//}
+
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index 2f2ccc729..1641b6f60 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
  *
@@ -46,164 +46,34 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/safety.h>
+#include <uORB/topics/vehicle_control_mode.h>
 
-/**
- * Switch to new state with no checking.
- *
- * do_state_update: this is the functions that all other functions have to call in order to update the state.
- * the function does not question the state change, this must be done before
- * The function performs actions that are connected with the new state (buzzer, reboot, ...)
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- *
- * @return 0 (macro OK) or 1 on error (macro ERROR)
- */
-int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
+typedef enum {
+	TRANSITION_DENIED = -1,
+	TRANSITION_NOT_CHANGED = 0,
+	TRANSITION_CHANGED
 
-/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
-// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+} transition_result_t;
 
-// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
+		arming_state_t new_arming_state, struct actuator_armed_s *armed);
 
-// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
-// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
+bool check_arming_state_changed();
 
-/**
- * Handle state machine if got position fix
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 
-/**
- * Handle state machine if position fix lost
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+bool check_main_state_changed();
 
-/**
- * Handle state machine if user wants to arm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode);
 
-/**
- * Handle state machine if user wants to disarm
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is manual
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is stabilized
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is guided
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Handle state machine if mode switch is auto
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish current state information
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-
-/*
- *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
- *  If the request is obeyed the functions return 0
- *
- */
-
-/**
- * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
-
-/**
- * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
-
-/**
- * Always switches to critical mode under any circumstances.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Switches to emergency if required.
- *
- * @param status_pub file descriptor for state update topic publication
- * @param current_status pointer to the current state machine to operate on
- * @param mavlink_fd file descriptor for MAVLink statustext messages
- */
-void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
-
-/**
- * Publish the armed state depending on the current system state
- *
- * @param current_status the current system status
- */
-void publish_armed_status(const struct vehicle_status_s *current_status);
+bool check_navigation_state_changed();
 
+void set_navigation_state_changed();
 
+int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd);
 
 #endif /* STATE_MACHINE_HELPER_H_ */
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 9c0720aa5..46dc1bec2 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -58,7 +58,7 @@
 #include <poll.h>
 
 extern "C" {
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 }
 
 #include "../blocks.hpp"
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 6c9c137bb..b6b4546c2 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -53,6 +53,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -116,6 +117,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 	memset(&global_pos, 0, sizeof(global_pos));
 	struct manual_control_setpoint_s manual_sp;
 	memset(&manual_sp, 0, sizeof(manual_sp));
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_status_s vstatus;
 	memset(&vstatus, 0, sizeof(vstatus));
 
@@ -137,7 +140,8 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* Setup of loop */
 	float gyro[3] = {0.0f, 0.0f, 0.0f};
@@ -178,30 +182,16 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 		}
 
 		orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
-		orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+		orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+		orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
 
 		gyro[0] = att.rollspeed;
 		gyro[1] = att.pitchspeed;
 		gyro[2] = att.yawspeed;
 
-		/* control */
-
-		if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
-		    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* attitude control */
-			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-			/* angular rate control */
-			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-			/* pass through throttle */
-			actuators.control[3] = att_sp.thrust;
-
-			/* set flaps to zero */
-			actuators.control[4] = 0.0f;
-
-		} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-			if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+		/* set manual setpoints if required */
+		if (control_mode.flag_control_manual_enabled) {
+			if (control_mode.flag_control_attitude_enabled) {
 
 				/* if the RC signal is lost, try to stay level and go slowly back down to ground */
 				if (vstatus.rc_signal_lost) {
@@ -234,15 +224,6 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 
 				att_sp.timestamp = hrt_absolute_time();
 
-				/* attitude control */
-				fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
-
-				/* angular rate control */
-				fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
-
-				/* pass through throttle */
-				actuators.control[3] = att_sp.thrust;
-
 				/* pass through flaps */
 				if (isfinite(manual_sp.flaps)) {
 					actuators.control[4] = manual_sp.flaps;
@@ -251,7 +232,7 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 					actuators.control[4] = 0.0f;
 				}
 
-			} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+			} else {
 				/* directly pass through values */
 				actuators.control[0] = manual_sp.roll;
 				/* positive pitch means negative actuator -> pull up */
@@ -267,6 +248,22 @@ int fixedwing_att_control_thread_main(int argc, char *argv[])
 				}
 			}
 		}
+		
+		/* execute attitude control if requested */
+		if (control_mode.flag_control_attitude_enabled) {
+			/* attitude control */
+			fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp);
+
+			/* angular rate control */
+			fixedwing_att_control_rates(&rates_sp, gyro, &actuators);
+
+			/* pass through throttle */
+			actuators.control[3] = att_sp.thrust;
+
+			/* set flaps to zero */
+			actuators.control[4] = 0.0f;
+
+		}
 
 		/* publish rates */
 		orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index f655a13bd..d65045d68 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -156,7 +156,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {	// TODO use vehicle_control_mode here?
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
@@ -166,8 +166,8 @@ void BlockMultiModeBacksideAutopilot::update()
 	// the setpoint should update to loitering around this position
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+	    _status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
 
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
@@ -219,89 +219,83 @@ void BlockMultiModeBacksideAutopilot::update()
 		// This is not a hack, but a design choice.
 
 		/* do not limit in HIL */
-		if (!_status.flag_hil_enabled) {
+		if (_status.hil_state != HIL_STATE_ON) {
 			/* limit to value of manual throttle */
 			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
 						     _actuators.control[CH_THR] : _manual.throttle;
 		}
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-			_actuators.control[CH_AIL] = _manual.roll;
-			_actuators.control[CH_ELV] = _manual.pitch;
-			_actuators.control[CH_RDR] = _manual.yaw;
-			_actuators.control[CH_THR] = _manual.throttle;
-
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-			// calculate velocity, XXX should be airspeed, but using ground speed for now
-			// for the purpose of control we will limit the velocity feedback between
-			// the min/max velocity
-			float v = _vLimit.update(sqrtf(
-						_pos.vx * _pos.vx +
-						_pos.vy * _pos.vy +
-						_pos.vz * _pos.vz));
-
-			// pitch channel -> rate of climb
-			// TODO, might want to put a gain on this, otherwise commanding
-			// from +1 -> -1 m/s for rate of climb
-			//float dThrottle = _cr2Thr.update(
-			//_crMax.get()*_manual.pitch - _pos.vz);
-
-			// roll channel -> bank angle
-			float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
-			float pCmd = _phi2P.update(phiCmd - _att.roll);
-
-			// throttle channel -> velocity
-			// negative sign because nose over to increase speed
-			float vCmd = _vLimit.update(_manual.throttle *
-					(_vLimit.getMax() - _vLimit.getMin()) +
-					_vLimit.getMin());
-			float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
-			float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
-
-			// yaw rate cmd
-			float rCmd = 0;
-
-			// stabilization
-			_stabilization.update(pCmd, qCmd, rCmd,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
-
-			// output
-			_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
-			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
-			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
-
-			// currently using manual throttle
-			// XXX if you enable this watch out, vz might be very noisy
-			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
-			_actuators.control[CH_THR] = _manual.throttle;
-
-			// XXX limit throttle to manual setting (safety) for now.
-			// If it turns out to be confusing, it can be removed later once
-			// a first binary release can be targeted.
-			// This is not a hack, but a design choice.
-
-			/* do not limit in HIL */
-			if (!_status.flag_hil_enabled) {
-				/* limit to value of manual throttle */
-				_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
-							     _actuators.control[CH_THR] : _manual.throttle;
-			}
-		}
+	} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {	// TODO use vehicle_control_mode here?
+		_actuators.control[CH_AIL] = _manual.roll;
+		_actuators.control[CH_ELV] = _manual.pitch;
+		_actuators.control[CH_RDR] = _manual.yaw;
+		_actuators.control[CH_THR] = _manual.throttle;
+	} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		// for the purpose of control we will limit the velocity feedback between
+		// the min/max velocity
+		float v = _vLimit.update(sqrtf(
+					_pos.vx * _pos.vx +
+					_pos.vy * _pos.vy +
+					_pos.vz * _pos.vz));
+
+		// pitch channel -> rate of climb
+		// TODO, might want to put a gain on this, otherwise commanding
+		// from +1 -> -1 m/s for rate of climb
+		//float dThrottle = _cr2Thr.update(
+		//_crMax.get()*_manual.pitch - _pos.vz);
 
-		// body rates controller, disabled for now
-		else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+		// roll channel -> bank angle
+		float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+		float pCmd = _phi2P.update(phiCmd - _att.roll);
 
-			_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
-					      _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+		// throttle channel -> velocity
+		// negative sign because nose over to increase speed
+		float vCmd = _vLimit.update(_manual.throttle *
+				(_vLimit.getMax() - _vLimit.getMin()) +
+				_vLimit.getMin());
+		float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
+		float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
 
-			_actuators.control[CH_AIL] = _stabilization.getAileron();
-			_actuators.control[CH_ELV] = _stabilization.getElevator();
-			_actuators.control[CH_RDR] = _stabilization.getRudder();
-			_actuators.control[CH_THR] = _manual.throttle;
+		// yaw rate cmd
+		float rCmd = 0;
+
+		// stabilization
+		_stabilization.update(pCmd, qCmd, rCmd,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		// output
+		_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+		_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+		_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+		// currently using manual throttle
+		// XXX if you enable this watch out, vz might be very noisy
+		//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+		_actuators.control[CH_THR] = _manual.throttle;
+
+		// XXX limit throttle to manual setting (safety) for now.
+		// If it turns out to be confusing, it can be removed later once
+		// a first binary release can be targeted.
+		// This is not a hack, but a design choice.
+
+		/* do not limit in HIL */
+		if (_status.hil_state != HIL_STATE_ON) {
+			/* limit to value of manual throttle */
+			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+							 _actuators.control[CH_THR] : _manual.throttle;
 		}
+	// body rates controller, disabled for now
+	// TODO
+	} else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {	// TODO use vehicle_control_mode here?
+
+		_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
+					  _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+		_actuators.control[CH_AIL] = _stabilization.getAileron();
+		_actuators.control[CH_ELV] = _stabilization.getElevator();
+		_actuators.control[CH_RDR] = _stabilization.getRudder();
+		_actuators.control[CH_THR] = _manual.throttle;
 	}
 
 	// update all publications
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 1aef739c7..d383146f9 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -51,6 +51,7 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -62,6 +63,8 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
+	struct actuator_armed_s armed;
+	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -109,19 +112,19 @@ int gpio_led_main(int argc, char *argv[])
 
 					} else if (!strcmp(argv[3], "a1")) {
 						use_io = true;
-						pin = PX4IO_ACC1;
+						pin = PX4IO_P_SETUP_RELAYS_ACC1;
 
 					} else if (!strcmp(argv[3], "a2")) {
 						use_io = true;
-						pin = PX4IO_ACC2;
+						pin = PX4IO_P_SETUP_RELAYS_ACC2;
 
 					} else if (!strcmp(argv[3], "r1")) {
 						use_io = true;
-						pin = PX4IO_RELAY1;
+						pin = PX4IO_P_SETUP_RELAYS_POWER1;
 
 					} else if (!strcmp(argv[3], "r2")) {
 						use_io = true;
-						pin = PX4IO_RELAY2;
+						pin = PX4IO_P_SETUP_RELAYS_POWER2;
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
@@ -142,7 +145,7 @@ int gpio_led_main(int argc, char *argv[])
 				char pin_name[24];
 
 				if (use_io) {
-					if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+					if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
 						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
 
 					} else {
@@ -227,10 +230,15 @@ void gpio_led_cycle(FAR void *arg)
 	if (status_updated)
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
 
+	orb_check(priv->vehicle_status_sub, &status_updated);
+
+	if (status_updated)
+		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
+
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->status.flag_system_armed) {
+	if (priv->armed.armed) {
 		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x3f;	// ****** solid (armed)
 
@@ -239,11 +247,10 @@ void gpio_led_cycle(FAR void *arg)
 		}
 
 	} else {
-		if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) {
+		if (priv->armed.ready_to_arm) {
 			pattern = 0x00;	// ______ off (disarmed, preflight check)
 
-		} else if (priv->status.state_machine == SYSTEM_STATE_STANDBY &&
-			   priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
 			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
 		} else {
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
deleted file mode 100644
index 8f39acd9d..000000000
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
+++ /dev/null
@@ -1,264 +0,0 @@
-/**************************************************************************//**

- * @file     ARMCM3.h

- * @brief    CMSIS Core Peripheral Access Layer Header File for

- *           ARMCM3 Device Series

- * @version  V1.07

- * @date     30. January 2012

- *

- * @note

- * Copyright (C) 2012 ARM Limited. All rights reserved.

- *

- * @par

- * ARM Limited (ARM) is supplying this software for use with Cortex-M 

- * processor based microcontrollers.  This file can be freely distributed 

- * within development tools that are supporting such ARM based processors. 

- *

- * @par

- * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED

- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF

- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.

- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR

- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.

- *

- ******************************************************************************/

-

-#ifndef ARMCM3_H

-#define ARMCM3_H

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-

-

-/* -------------------------  Interrupt Number Definition  ------------------------ */

-

-typedef enum IRQn

-{

-/* -------------------  Cortex-M3 Processor Exceptions Numbers  ------------------- */

-  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */

-  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */

-  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */

-  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */

-  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */

-  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */

-  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */

-  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */

-  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */

-

-/* ----------------------  ARMCM3 Specific Interrupt Numbers  --------------------- */

-  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */

-  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */

-  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */

-  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */

-  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */

-  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */

-  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */

-  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */

-  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */

-  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */

-  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */

-  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */

-  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */

-  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */

-  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */

-  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */

-  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */

-  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */

-  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */

-  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */

-  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */

-  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */

-  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */

-} IRQn_Type;

-

-

-/* ================================================================================ */

-/* ================      Processor and Core Peripheral Section     ================ */

-/* ================================================================================ */

-

-/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */

-#define __CM3_REV                 0x0201    /*!< Core revision r2p1                              */

-#define __MPU_PRESENT             1         /*!< MPU present or not                              */

-#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */

-#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */

-

-#include <core_cm3.h>                       /* Processor and core peripherals                    */

-/* NuttX */

-//#include "system_ARMCM3.h"                  /* System Header                                     */

-

-

-/* ================================================================================ */

-/* ================       Device Specific Peripheral Section       ================ */

-/* ================================================================================ */

-

-/* -------------------  Start of section using anonymous unions  ------------------ */

-#if defined(__CC_ARM)

-  #pragma push

-  #pragma anon_unions

-#elif defined(__ICCARM__)

-  #pragma language=extended

-#elif defined(__GNUC__)

-  /* anonymous unions are enabled by default */

-#elif defined(__TMS470__)

-/* anonymous unions are enabled by default */

-#elif defined(__TASKING__)

-  #pragma warning 586

-#else

-  #warning Not supported compiler type

-#endif

-

-

-

-/* ================================================================================ */

-/* ================            CPU FPGA System (CPU_SYS)           ================ */

-/* ================================================================================ */

-typedef struct

-{

-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */

-  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */

-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */

-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */

-  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */

-  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */

-       uint32_t RESERVED0[2];

-  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */

-  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */

-  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */

-       uint32_t RESERVED1[3];

-  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */

-  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */

-} ARM_CPU_SYS_TypeDef;

-

-

-/* ================================================================================ */

-/* ================            DUT FPGA System (DUT_SYS)           ================ */

-/* ================================================================================ */

-typedef struct

-{

-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */

-  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */

-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */

-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */

-  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */

-  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */

-  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */

-} ARM_DUT_SYS_TypeDef;

-

-

-/* ================================================================================ */

-/* ================                   Timer (TIM)                  ================ */

-/* ================================================================================ */

-typedef struct

-{

-  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */

-  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */

-  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */

-  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */

-  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */

-  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */

-  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */

-       uint32_t RESERVED0[1];

-  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */

-  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */

-  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */

-  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */

-  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */

-  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */

-  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */

-} ARM_TIM_TypeDef;

-

-

-/* ================================================================================ */

-/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */

-/* ================================================================================ */

-typedef struct

-{

-  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 

-  union {

-  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */

-  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */

-  };

-       uint32_t RESERVED0[4];

-  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */

-       uint32_t RESERVED1[1];

-  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */

-  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */

-  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */

-  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */

-  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */

-  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */

-  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */

-  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */

-  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */

-  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */

-  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */

-} ARM_UART_TypeDef;

-

-

-/* --------------------  End of section using anonymous unions  ------------------- */

-#if defined(__CC_ARM)

-  #pragma pop

-#elif defined(__ICCARM__)

-  /* leave anonymous unions enabled */

-#elif defined(__GNUC__)

-  /* anonymous unions are enabled by default */

-#elif defined(__TMS470__)

-  /* anonymous unions are enabled by default */

-#elif defined(__TASKING__)

-  #pragma warning restore

-#else

-  #warning Not supported compiler type

-#endif

-

-

-

-

-/* ================================================================================ */

-/* ================              Peripheral memory map             ================ */

-/* ================================================================================ */

-/* --------------------------  CPU FPGA memory map  ------------------------------- */

-#define ARM_FLASH_BASE            (0x00000000UL)

-#define ARM_RAM_BASE              (0x20000000UL)

-#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)

-#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)

-

-#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)

-#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)

-

-/* --------------------------  DUT FPGA memory map  ------------------------------- */

-#define ARM_APB_BASE              (0x40000000UL)

-#define ARM_AHB_BASE              (0x4FF00000UL)

-#define ARM_DMC_BASE              (0x60000000UL)

-#define ARM_SMC_BASE              (0xA0000000UL)

-

-#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)

-#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)

-#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)

-#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)

-#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)

-#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)

-#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)

-

-

-/* ================================================================================ */

-/* ================             Peripheral declaration             ================ */

-/* ================================================================================ */

-/* --------------------------  CPU FPGA Peripherals  ------------------------------ */

-#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)

-#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)

-

-/* --------------------------  DUT FPGA Peripherals  ------------------------------ */

-#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)

-#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)

-#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)

-#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)

-#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)

-#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)

-#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)

-

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif  /* ARMCM3_H */

diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
deleted file mode 100644
index 181b7e433..000000000
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
+++ /dev/null
@@ -1,265 +0,0 @@
-/**************************************************************************//**

- * @file     ARMCM4.h

- * @brief    CMSIS Core Peripheral Access Layer Header File for

- *           ARMCM4 Device Series

- * @version  V1.07

- * @date     30. January 2012

- *

- * @note

- * Copyright (C) 2012 ARM Limited. All rights reserved.

- *

- * @par

- * ARM Limited (ARM) is supplying this software for use with Cortex-M 

- * processor based microcontrollers.  This file can be freely distributed 

- * within development tools that are supporting such ARM based processors. 

- *

- * @par

- * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED

- * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF

- * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.

- * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR

- * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.

- *

- ******************************************************************************/

-

-#ifndef ARMCM4_H

-#define ARMCM4_H

-

-#ifdef __cplusplus

-extern "C" {

-#endif

-

-

-/* -------------------------  Interrupt Number Definition  ------------------------ */

-

-typedef enum IRQn

-{

-/* -------------------  Cortex-M4 Processor Exceptions Numbers  ------------------- */

-  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */

-  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */

-  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */

-  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */

-  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */

-  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */

-  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */

-  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */

-  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */

-

-/* ----------------------  ARMCM4 Specific Interrupt Numbers  --------------------- */

-  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */

-  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */

-  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */

-  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */

-  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */

-  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */

-  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */

-  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */

-  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */

-  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */

-  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */

-  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */

-  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */

-  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */

-  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */

-  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */

-  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */

-  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */

-  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */

-  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */

-  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */

-  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */

-  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */

-} IRQn_Type;

-

-

-/* ================================================================================ */

-/* ================      Processor and Core Peripheral Section     ================ */

-/* ================================================================================ */

-

-/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */

-#define __CM4_REV                 0x0001    /*!< Core revision r0p1                              */

-#define __MPU_PRESENT             1         /*!< MPU present or not                              */

-#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */

-#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */

-#define __FPU_PRESENT             1         /*!< FPU present or not                              */

-

-#include <core_cm4.h>                       /* Processor and core peripherals                    */

-/* NuttX */

-//#include "system_ARMCM4.h"                  /* System Header                                     */

-

-

-/* ================================================================================ */

-/* ================       Device Specific Peripheral Section       ================ */

-/* ================================================================================ */

-

-/* -------------------  Start of section using anonymous unions  ------------------ */

-#if defined(__CC_ARM)

-  #pragma push

-  #pragma anon_unions

-#elif defined(__ICCARM__)

-  #pragma language=extended

-#elif defined(__GNUC__)

-  /* anonymous unions are enabled by default */

-#elif defined(__TMS470__)

-/* anonymous unions are enabled by default */

-#elif defined(__TASKING__)

-  #pragma warning 586

-#else

-  #warning Not supported compiler type

-#endif

-

-

-

-/* ================================================================================ */

-/* ================            CPU FPGA System (CPU_SYS)           ================ */

-/* ================================================================================ */

-typedef struct

-{

-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */

-  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */

-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */

-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */

-  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */

-  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */

-       uint32_t RESERVED0[2];

-  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */

-  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */

-  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */

-       uint32_t RESERVED1[3];

-  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */

-  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */

-} ARM_CPU_SYS_TypeDef;

-

-

-/* ================================================================================ */

-/* ================            DUT FPGA System (DUT_SYS)           ================ */

-/* ================================================================================ */

-typedef struct

-{

-  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */

-  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */

-  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */

-  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */

-  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */

-  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */

-  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */

-} ARM_DUT_SYS_TypeDef;

-

-

-/* ================================================================================ */

-/* ================                   Timer (TIM)                  ================ */

-/* ================================================================================ */

-typedef struct

-{

-  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */

-  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */

-  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */

-  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */

-  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */

-  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */

-  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */

-       uint32_t RESERVED0[1];

-  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */

-  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */

-  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */

-  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */

-  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */

-  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */

-  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */

-} ARM_TIM_TypeDef;

-

-

-/* ================================================================================ */

-/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */

-/* ================================================================================ */

-typedef struct

-{

-  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 

-  union {

-  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */

-  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */

-  };

-       uint32_t RESERVED0[4];

-  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */

-       uint32_t RESERVED1[1];

-  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */

-  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */

-  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */

-  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */

-  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */

-  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */

-  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */

-  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */

-  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */

-  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */

-  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */

-} ARM_UART_TypeDef;

-

-

-/* --------------------  End of section using anonymous unions  ------------------- */

-#if defined(__CC_ARM)

-  #pragma pop

-#elif defined(__ICCARM__)

-  /* leave anonymous unions enabled */

-#elif defined(__GNUC__)

-  /* anonymous unions are enabled by default */

-#elif defined(__TMS470__)

-  /* anonymous unions are enabled by default */

-#elif defined(__TASKING__)

-  #pragma warning restore

-#else

-  #warning Not supported compiler type

-#endif

-

-

-

-

-/* ================================================================================ */

-/* ================              Peripheral memory map             ================ */

-/* ================================================================================ */

-/* --------------------------  CPU FPGA memory map  ------------------------------- */

-#define ARM_FLASH_BASE            (0x00000000UL)

-#define ARM_RAM_BASE              (0x20000000UL)

-#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)

-#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)

-

-#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)

-#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)

-

-/* --------------------------  DUT FPGA memory map  ------------------------------- */

-#define ARM_APB_BASE              (0x40000000UL)

-#define ARM_AHB_BASE              (0x4FF00000UL)

-#define ARM_DMC_BASE              (0x60000000UL)

-#define ARM_SMC_BASE              (0xA0000000UL)

-

-#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)

-#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)

-#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)

-#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)

-#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)

-#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)

-#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)

-

-

-/* ================================================================================ */

-/* ================             Peripheral declaration             ================ */

-/* ================================================================================ */

-/* --------------------------  CPU FPGA Peripherals  ------------------------------ */

-#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)

-#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)

-

-/* --------------------------  DUT FPGA Peripherals  ------------------------------ */

-#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)

-#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)

-#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)

-#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)

-#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)

-#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)

-#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)

-

-

-#ifdef __cplusplus

-}

-#endif

-

-#endif  /* ARMCM4_H */

diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
deleted file mode 100644
index 9c37ab4e5..000000000
--- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
+++ /dev/null
@@ -1,93 +0,0 @@
-/* ----------------------------------------------------------------------

-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.

-*

-* $Date:        17. January 2013

-* $Revision:    V1.4.1

-*

-* Project:      CMSIS DSP Library

-* Title:        arm_common_tables.h

-*

-* Description:  This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions

-*

-* Target Processor: Cortex-M4/Cortex-M3

-*

-* Redistribution and use in source and binary forms, with or without

-* modification, are permitted provided that the following conditions

-* are met:

-*   - Redistributions of source code must retain the above copyright

-*     notice, this list of conditions and the following disclaimer.

-*   - Redistributions in binary form must reproduce the above copyright

-*     notice, this list of conditions and the following disclaimer in

-*     the documentation and/or other materials provided with the

-*     distribution.

-*   - Neither the name of ARM LIMITED 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.

-* -------------------------------------------------------------------- */

-

-#ifndef _ARM_COMMON_TABLES_H

-#define _ARM_COMMON_TABLES_H

-

-#include "arm_math.h"

-

-extern const uint16_t armBitRevTable[1024];

-extern const q15_t armRecipTableQ15[64];

-extern const q31_t armRecipTableQ31[64];

-extern const q31_t realCoefAQ31[1024];

-extern const q31_t realCoefBQ31[1024];

-extern const float32_t twiddleCoef_16[32];

-extern const float32_t twiddleCoef_32[64];

-extern const float32_t twiddleCoef_64[128];

-extern const float32_t twiddleCoef_128[256];

-extern const float32_t twiddleCoef_256[512];

-extern const float32_t twiddleCoef_512[1024];

-extern const float32_t twiddleCoef_1024[2048];

-extern const float32_t twiddleCoef_2048[4096];

-extern const float32_t twiddleCoef_4096[8192];

-#define twiddleCoef twiddleCoef_4096

-extern const q31_t twiddleCoefQ31[6144];

-extern const q15_t twiddleCoefQ15[6144];

-extern const float32_t twiddleCoef_rfft_32[32];

-extern const float32_t twiddleCoef_rfft_64[64];

-extern const float32_t twiddleCoef_rfft_128[128];

-extern const float32_t twiddleCoef_rfft_256[256];

-extern const float32_t twiddleCoef_rfft_512[512];

-extern const float32_t twiddleCoef_rfft_1024[1024];

-extern const float32_t twiddleCoef_rfft_2048[2048];

-extern const float32_t twiddleCoef_rfft_4096[4096];

-

-

-#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20  )

-#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48  )

-#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56  )

-#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 )

-#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 )

-#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 )

-#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800)

-#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808)

-#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032)

-

-extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH];

-extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH];

-

-#endif /*  ARM_COMMON_TABLES_H */

diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
deleted file mode 100644
index 406f737dc..000000000
--- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/* ----------------------------------------------------------------------

-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.

-*

-* $Date:        17. January 2013

-* $Revision:    V1.4.1

-*

-* Project:      CMSIS DSP Library

-* Title:        arm_const_structs.h

-*

-* Description:  This file has constant structs that are initialized for

-*               user convenience.  For example, some can be given as

-*               arguments to the arm_cfft_f32() function.

-*

-* Target Processor: Cortex-M4/Cortex-M3

-*

-* Redistribution and use in source and binary forms, with or without

-* modification, are permitted provided that the following conditions

-* are met:

-*   - Redistributions of source code must retain the above copyright

-*     notice, this list of conditions and the following disclaimer.

-*   - Redistributions in binary form must reproduce the above copyright

-*     notice, this list of conditions and the following disclaimer in

-*     the documentation and/or other materials provided with the

-*     distribution.

-*   - Neither the name of ARM LIMITED 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.

-* -------------------------------------------------------------------- */

-

-#ifndef _ARM_CONST_STRUCTS_H

-#define _ARM_CONST_STRUCTS_H

-

-#include "arm_math.h"

-#include "arm_common_tables.h"

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = {

-      16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = {

-      32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = {

-      64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = {

-      128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = {

-      256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = {

-      512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = {

-      1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = {

-      2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH

-   };

-

-   const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = {

-      4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH

-   };

-

-#endif

diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/modules/mathlib/CMSIS/Include/arm_math.h
deleted file mode 100644
index 6f66f9ee3..000000000
--- a/src/modules/mathlib/CMSIS/Include/arm_math.h
+++ /dev/null
@@ -1,7318 +0,0 @@
-/* ----------------------------------------------------------------------

-* Copyright (C) 2010-2013 ARM Limited. All rights reserved.

-*

-* $Date:        17. January 2013

-* $Revision:    V1.4.1

-*

-* Project:      CMSIS DSP Library

-* Title:        arm_math.h

-*

-* Description:  Public header file for CMSIS DSP Library

-*

-* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0

-*

-* Redistribution and use in source and binary forms, with or without

-* modification, are permitted provided that the following conditions

-* are met:

-*   - Redistributions of source code must retain the above copyright

-*     notice, this list of conditions and the following disclaimer.

-*   - Redistributions in binary form must reproduce the above copyright

-*     notice, this list of conditions and the following disclaimer in

-*     the documentation and/or other materials provided with the

-*     distribution.

-*   - Neither the name of ARM LIMITED 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.

- * -------------------------------------------------------------------- */

-

-/**

-   \mainpage CMSIS DSP Software Library

-   *

-   * <b>Introduction</b>

-   *

-   * This user manual describes the CMSIS DSP software library,

-   * a suite of common signal processing functions for use on Cortex-M processor based devices.

-   *

-   * The library is divided into a number of functions each covering a specific category:

-   * - Basic math functions

-   * - Fast math functions

-   * - Complex math functions

-   * - Filters

-   * - Matrix functions

-   * - Transforms

-   * - Motor control functions

-   * - Statistical functions

-   * - Support functions

-   * - Interpolation functions

-   *

-   * The library has separate functions for operating on 8-bit integers, 16-bit integers,

-   * 32-bit integer and 32-bit floating-point values.

-   *

-   * <b>Using the Library</b>

-   *

-   * The library installer contains prebuilt versions of the libraries in the <code>Lib</code> folder.

-   * - arm_cortexM4lf_math.lib (Little endian and Floating Point Unit on Cortex-M4)

-   * - arm_cortexM4bf_math.lib (Big endian and Floating Point Unit on Cortex-M4)

-   * - arm_cortexM4l_math.lib (Little endian on Cortex-M4)

-   * - arm_cortexM4b_math.lib (Big endian on Cortex-M4)

-   * - arm_cortexM3l_math.lib (Little endian on Cortex-M3)

-   * - arm_cortexM3b_math.lib (Big endian on Cortex-M3)

-   * - arm_cortexM0l_math.lib (Little endian on Cortex-M0)

-   * - arm_cortexM0b_math.lib (Big endian on Cortex-M3)

-   *

-   * The library functions are declared in the public file <code>arm_math.h</code> which is placed in the <code>Include</code> folder.

-   * Simply include this file and link the appropriate library in the application and begin calling the library functions. The Library supports single

-   * public header file <code> arm_math.h</code> for Cortex-M4/M3/M0 with little endian and big endian. Same header file will be used for floating point unit(FPU) variants.

-   * Define the appropriate pre processor MACRO ARM_MATH_CM4 or  ARM_MATH_CM3 or

-   * ARM_MATH_CM0 or ARM_MATH_CM0PLUS depending on the target processor in the application.

-   *

-   * <b>Examples</b>

-   *

-   * The library ships with a number of examples which demonstrate how to use the library functions.

-   *

-   * <b>Toolchain Support</b>

-   *

-   * The library has been developed and tested with MDK-ARM version 4.60.

-   * The library is being tested in GCC and IAR toolchains and updates on this activity will be made available shortly.

-   *

-   * <b>Building the Library</b>

-   *

-   * The library installer contains project files to re build libraries on MDK Tool chain in the <code>CMSIS\\DSP_Lib\\Source\\ARM</code> folder.

-   * - arm_cortexM0b_math.uvproj

-   * - arm_cortexM0l_math.uvproj

-   * - arm_cortexM3b_math.uvproj

-   * - arm_cortexM3l_math.uvproj

-   * - arm_cortexM4b_math.uvproj

-   * - arm_cortexM4l_math.uvproj

-   * - arm_cortexM4bf_math.uvproj

-   * - arm_cortexM4lf_math.uvproj

-   *

-   *

-   * The project can be built by opening the appropriate project in MDK-ARM 4.60 chain and defining the optional pre processor MACROs detailed above.

-   *

-   * <b>Pre-processor Macros</b>

-   *

-   * Each library project have differant pre-processor macros.

-   *

-   * - UNALIGNED_SUPPORT_DISABLE:

-   *

-   * Define macro UNALIGNED_SUPPORT_DISABLE, If the silicon does not support unaligned memory access

-   *

-   * - ARM_MATH_BIG_ENDIAN:

-   *

-   * Define macro ARM_MATH_BIG_ENDIAN to build the library for big endian targets. By default library builds for little endian targets.

-   *

-   * - ARM_MATH_MATRIX_CHECK:

-   *

-   * Define macro ARM_MATH_MATRIX_CHECK for checking on the input and output sizes of matrices

-   *

-   * - ARM_MATH_ROUNDING:

-   *

-   * Define macro ARM_MATH_ROUNDING for rounding on support functions

-   *

-   * - ARM_MATH_CMx:

-   *

-   * Define macro ARM_MATH_CM4 for building the library on Cortex-M4 target, ARM_MATH_CM3 for building library on Cortex-M3 target

-   * and ARM_MATH_CM0 for building library on cortex-M0 target, ARM_MATH_CM0PLUS for building library on cortex-M0+ target.

-   *

-   * - __FPU_PRESENT:

-   *

-   * Initialize macro __FPU_PRESENT = 1 when building on FPU supported Targets. Enable this macro for M4bf and M4lf libraries

-   *

-   * <b>Copyright Notice</b>

-   *

-   * Copyright (C) 2010-2013 ARM Limited. All rights reserved.

-   */

-

-

-/**

- * @defgroup groupMath Basic Math Functions

- */

-

-/**

- * @defgroup groupFastMath Fast Math Functions

- * This set of functions provides a fast approximation to sine, cosine, and square root.

- * As compared to most of the other functions in the CMSIS math library, the fast math functions

- * operate on individual values and not arrays.

- * There are separate functions for Q15, Q31, and floating-point data.

- *

- */

-

-/**

- * @defgroup groupCmplxMath Complex Math Functions

- * This set of functions operates on complex data vectors.

- * The data in the complex arrays is stored in an interleaved fashion

- * (real, imag, real, imag, ...).

- * In the API functions, the number of samples in a complex array refers

- * to the number of complex values; the array contains twice this number of

- * real values.

- */

-

-/**

- * @defgroup groupFilters Filtering Functions

- */

-

-/**

- * @defgroup groupMatrix Matrix Functions

- *

- * This set of functions provides basic matrix math operations.

- * The functions operate on matrix data structures.  For example,

- * the type

- * definition for the floating-point matrix structure is shown

- * below:

- * <pre>

- *     typedef struct

- *     {

- *       uint16_t numRows;     // number of rows of the matrix.

- *       uint16_t numCols;     // number of columns of the matrix.

- *       float32_t *pData;     // points to the data of the matrix.

- *     } arm_matrix_instance_f32;

- * </pre>

- * There are similar definitions for Q15 and Q31 data types.

- *

- * The structure specifies the size of the matrix and then points to

- * an array of data.  The array is of size <code>numRows X numCols</code>

- * and the values are arranged in row order.  That is, the

- * matrix element (i, j) is stored at:

- * <pre>

- *     pData[i*numCols + j]

- * </pre>

- *

- * \par Init Functions

- * There is an associated initialization function for each type of matrix

- * data structure.

- * The initialization function sets the values of the internal structure fields.

- * Refer to the function <code>arm_mat_init_f32()</code>, <code>arm_mat_init_q31()</code>

- * and <code>arm_mat_init_q15()</code> for floating-point, Q31 and Q15 types,  respectively.

- *

- * \par

- * Use of the initialization function is optional. However, if initialization function is used

- * then the instance structure cannot be placed into a const data section.

- * To place the instance structure in a const data

- * section, manually initialize the data structure.  For example:

- * <pre>

- * <code>arm_matrix_instance_f32 S = {nRows, nColumns, pData};</code>

- * <code>arm_matrix_instance_q31 S = {nRows, nColumns, pData};</code>

- * <code>arm_matrix_instance_q15 S = {nRows, nColumns, pData};</code>

- * </pre>

- * where <code>nRows</code> specifies the number of rows, <code>nColumns</code>

- * specifies the number of columns, and <code>pData</code> points to the

- * data array.

- *

- * \par Size Checking

- * By default all of the matrix functions perform size checking on the input and

- * output matrices.  For example, the matrix addition function verifies that the

- * two input matrices and the output matrix all have the same number of rows and

- * columns.  If the size check fails the functions return:

- * <pre>

- *     ARM_MATH_SIZE_MISMATCH

- * </pre>

- * Otherwise the functions return

- * <pre>

- *     ARM_MATH_SUCCESS

- * </pre>

- * There is some overhead associated with this matrix size checking.

- * The matrix size checking is enabled via the \#define

- * <pre>

- *     ARM_MATH_MATRIX_CHECK

- * </pre>

- * within the library project settings.  By default this macro is defined

- * and size checking is enabled.  By changing the project settings and

- * undefining this macro size checking is eliminated and the functions

- * run a bit faster.  With size checking disabled the functions always

- * return <code>ARM_MATH_SUCCESS</code>.

- */

-

-/**

- * @defgroup groupTransforms Transform Functions

- */

-

-/**

- * @defgroup groupController Controller Functions

- */

-

-/**

- * @defgroup groupStats Statistics Functions

- */

-/**

- * @defgroup groupSupport Support Functions

- */

-

-/**

- * @defgroup groupInterpolation Interpolation Functions

- * These functions perform 1- and 2-dimensional interpolation of data.

- * Linear interpolation is used for 1-dimensional data and

- * bilinear interpolation is used for 2-dimensional data.

- */

-

-/**

- * @defgroup groupExamples Examples

- */

-#ifndef _ARM_MATH_H

-#define _ARM_MATH_H

-

-#define __CMSIS_GENERIC         /* disable NVIC and Systick functions */

-

-/* PX4 */

-#include <nuttx/config.h>

-#ifdef CONFIG_ARCH_CORTEXM4

-# define ARM_MATH_CM4 1

-#endif

-#ifdef CONFIG_ARCH_CORTEXM3

-# define ARM_MATH_CM3 1

-#endif

-#ifdef CONFIG_ARCH_FPU

-# define __FPU_PRESENT 1

-#endif

-

-#if defined (ARM_MATH_CM4)

-#include "core_cm4.h"

-#elif defined (ARM_MATH_CM3)

-#include "core_cm3.h"

-#elif defined (ARM_MATH_CM0)

-#include "core_cm0.h"

-#define ARM_MATH_CM0_FAMILY

-#elif defined (ARM_MATH_CM0PLUS)

-#include "core_cm0plus.h"

-#define ARM_MATH_CM0_FAMILY

-#else

-#include "ARMCM4.h"

-#warning "Define either ARM_MATH_CM4 OR ARM_MATH_CM3...By Default building on ARM_MATH_CM4....."

-#endif

-

-#undef  __CMSIS_GENERIC         /* enable NVIC and Systick functions */

-#include "string.h"

-#include "math.h"

-#ifdef	__cplusplus

-extern "C"

-{

-#endif

-

-

-  /**

-   * @brief Macros required for reciprocal calculation in Normalized LMS

-   */

-

-#define DELTA_Q31 			(0x100)

-#define DELTA_Q15 			0x5

-#define INDEX_MASK 			0x0000003F

-#ifndef PI

-#define PI					3.14159265358979f

-#endif

-

-  /**

-   * @brief Macros required for SINE and COSINE Fast math approximations

-   */

-

-#define TABLE_SIZE			256

-#define TABLE_SPACING_Q31	0x800000

-#define TABLE_SPACING_Q15	0x80

-

-  /**

-   * @brief Macros required for SINE and COSINE Controller functions

-   */

-  /* 1.31(q31) Fixed value of 2/360 */

-  /* -1 to +1 is divided into 360 values so total spacing is (2/360) */

-#define INPUT_SPACING			0xB60B61

-

-  /**

-   * @brief Macro for Unaligned Support

-   */

-#ifndef UNALIGNED_SUPPORT_DISABLE

-    #define ALIGN4

-#else

-  #if defined  (__GNUC__)

-    #define ALIGN4 __attribute__((aligned(4)))

-  #else

-    #define ALIGN4 __align(4)

-  #endif

-#endif	/*	#ifndef UNALIGNED_SUPPORT_DISABLE	*/

-

-  /**

-   * @brief Error status returned by some functions in the library.

-   */

-

-  typedef enum

-  {

-    ARM_MATH_SUCCESS = 0,                /**< No error */

-    ARM_MATH_ARGUMENT_ERROR = -1,        /**< One or more arguments are incorrect */

-    ARM_MATH_LENGTH_ERROR = -2,          /**< Length of data buffer is incorrect */

-    ARM_MATH_SIZE_MISMATCH = -3,         /**< Size of matrices is not compatible with the operation. */

-    ARM_MATH_NANINF = -4,                /**< Not-a-number (NaN) or infinity is generated */

-    ARM_MATH_SINGULAR = -5,              /**< Generated by matrix inversion if the input matrix is singular and cannot be inverted. */

-    ARM_MATH_TEST_FAILURE = -6           /**< Test Failed  */

-  } arm_status;

-

-  /**

-   * @brief 8-bit fractional data type in 1.7 format.

-   */

-  typedef int8_t q7_t;

-

-  /**

-   * @brief 16-bit fractional data type in 1.15 format.

-   */

-  typedef int16_t q15_t;

-

-  /**

-   * @brief 32-bit fractional data type in 1.31 format.

-   */

-  typedef int32_t q31_t;

-

-  /**

-   * @brief 64-bit fractional data type in 1.63 format.

-   */

-  typedef int64_t q63_t;

-

-  /**

-   * @brief 32-bit floating-point type definition.

-   */

-  typedef float float32_t;

-

-  /**

-   * @brief 64-bit floating-point type definition.

-   */

-  typedef double float64_t;

-

-  /**

-   * @brief definition to read/write two 16 bit values.

-   */

-#if defined __CC_ARM

-#define __SIMD32_TYPE int32_t __packed

-#define CMSIS_UNUSED __attribute__((unused))

-#elif defined __ICCARM__

-#define CMSIS_UNUSED

-#define __SIMD32_TYPE int32_t __packed

-#elif defined __GNUC__

-#define __SIMD32_TYPE int32_t

-#define CMSIS_UNUSED __attribute__((unused))

-#else

-#error Unknown compiler

-#endif

-

-#define __SIMD32(addr)  (*(__SIMD32_TYPE **) & (addr))

-#define __SIMD32_CONST(addr)  ((__SIMD32_TYPE *)(addr))

-

-#define _SIMD32_OFFSET(addr)  (*(__SIMD32_TYPE *)  (addr))

-

-#define __SIMD64(addr)  (*(int64_t **) & (addr))

-

-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)

-  /**

-   * @brief definition to pack two 16 bit values.

-   */

-#define __PKHBT(ARG1, ARG2, ARG3)      ( (((int32_t)(ARG1) <<  0) & (int32_t)0x0000FFFF) | \

-                                         (((int32_t)(ARG2) << ARG3) & (int32_t)0xFFFF0000)  )

-#define __PKHTB(ARG1, ARG2, ARG3)      ( (((int32_t)(ARG1) <<  0) & (int32_t)0xFFFF0000) | \

-                                         (((int32_t)(ARG2) >> ARG3) & (int32_t)0x0000FFFF)  )

-

-#endif

-

-

-   /**

-   * @brief definition to pack four 8 bit values.

-   */

-#ifndef ARM_MATH_BIG_ENDIAN

-

-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v0) <<  0) & (int32_t)0x000000FF) |	\

-                                (((int32_t)(v1) <<  8) & (int32_t)0x0000FF00) |	\

-							    (((int32_t)(v2) << 16) & (int32_t)0x00FF0000) |	\

-							    (((int32_t)(v3) << 24) & (int32_t)0xFF000000)  )

-#else

-

-#define __PACKq7(v0,v1,v2,v3) ( (((int32_t)(v3) <<  0) & (int32_t)0x000000FF) |	\

-                                (((int32_t)(v2) <<  8) & (int32_t)0x0000FF00) |	\

-							    (((int32_t)(v1) << 16) & (int32_t)0x00FF0000) |	\

-							    (((int32_t)(v0) << 24) & (int32_t)0xFF000000)  )

-

-#endif

-

-

-  /**

-   * @brief Clips Q63 to Q31 values.

-   */

-  static __INLINE q31_t clip_q63_to_q31(

-  q63_t x)

-  {

-    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?

-      ((0x7FFFFFFF ^ ((q31_t) (x >> 63)))) : (q31_t) x;

-  }

-

-  /**

-   * @brief Clips Q63 to Q15 values.

-   */

-  static __INLINE q15_t clip_q63_to_q15(

-  q63_t x)

-  {

-    return ((q31_t) (x >> 32) != ((q31_t) x >> 31)) ?

-      ((0x7FFF ^ ((q15_t) (x >> 63)))) : (q15_t) (x >> 15);

-  }

-

-  /**

-   * @brief Clips Q31 to Q7 values.

-   */

-  static __INLINE q7_t clip_q31_to_q7(

-  q31_t x)

-  {

-    return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?

-      ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;

-  }

-

-  /**

-   * @brief Clips Q31 to Q15 values.

-   */

-  static __INLINE q15_t clip_q31_to_q15(

-  q31_t x)

-  {

-    return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?

-      ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;

-  }

-

-  /**

-   * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.

-   */

-

-  static __INLINE q63_t mult32x64(

-  q63_t x,

-  q31_t y)

-  {

-    return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +

-            (((q63_t) (x >> 32) * y)));

-  }

-

-

-#if defined (ARM_MATH_CM0_FAMILY) && defined ( __CC_ARM   )

-#define __CLZ __clz

-#endif

-

-#if defined (ARM_MATH_CM0_FAMILY) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )

-

-  static __INLINE uint32_t __CLZ(

-  q31_t data);

-

-

-  static __INLINE uint32_t __CLZ(

-  q31_t data)

-  {

-    uint32_t count = 0;

-    uint32_t mask = 0x80000000;

-

-    while((data & mask) == 0)

-    {

-      count += 1u;

-      mask = mask >> 1u;

-    }

-

-    return (count);

-

-  }

-

-#endif

-

-  /**

-   * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.

-   */

-

-  static __INLINE uint32_t arm_recip_q31(

-  q31_t in,

-  q31_t * dst,

-  q31_t * pRecipTable)

-  {

-

-    uint32_t out, tempVal;

-    uint32_t index, i;

-    uint32_t signBits;

-

-    if(in > 0)

-    {

-      signBits = __CLZ(in) - 1;

-    }

-    else

-    {

-      signBits = __CLZ(-in) - 1;

-    }

-

-    /* Convert input sample to 1.31 format */

-    in = in << signBits;

-

-    /* calculation of index for initial approximated Val */

-    index = (uint32_t) (in >> 24u);

-    index = (index & INDEX_MASK);

-

-    /* 1.31 with exp 1 */

-    out = pRecipTable[index];

-

-    /* calculation of reciprocal value */

-    /* running approximation for two iterations */

-    for (i = 0u; i < 2u; i++)

-    {

-      tempVal = (q31_t) (((q63_t) in * out) >> 31u);

-      tempVal = 0x7FFFFFFF - tempVal;

-      /*      1.31 with exp 1 */

-      //out = (q31_t) (((q63_t) out * tempVal) >> 30u);

-      out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);

-    }

-

-    /* write output */

-    *dst = out;

-

-    /* return num of signbits of out = 1/in value */

-    return (signBits + 1u);

-

-  }

-

-  /**

-   * @brief Function to Calculates 1/in (reciprocal) value of Q15 Data type.

-   */

-  static __INLINE uint32_t arm_recip_q15(

-  q15_t in,

-  q15_t * dst,

-  q15_t * pRecipTable)

-  {

-

-    uint32_t out = 0, tempVal = 0;

-    uint32_t index = 0, i = 0;

-    uint32_t signBits = 0;

-

-    if(in > 0)

-    {

-      signBits = __CLZ(in) - 17;

-    }

-    else

-    {

-      signBits = __CLZ(-in) - 17;

-    }

-

-    /* Convert input sample to 1.15 format */

-    in = in << signBits;

-

-    /* calculation of index for initial approximated Val */

-    index = in >> 8;

-    index = (index & INDEX_MASK);

-

-    /*      1.15 with exp 1  */

-    out = pRecipTable[index];

-

-    /* calculation of reciprocal value */

-    /* running approximation for two iterations */

-    for (i = 0; i < 2; i++)

-    {

-      tempVal = (q15_t) (((q31_t) in * out) >> 15);

-      tempVal = 0x7FFF - tempVal;

-      /*      1.15 with exp 1 */

-      out = (q15_t) (((q31_t) out * tempVal) >> 14);

-    }

-

-    /* write output */

-    *dst = out;

-

-    /* return num of signbits of out = 1/in value */

-    return (signBits + 1);

-

-  }

-

-

-  /*

-   * @brief C custom defined intrinisic function for only M0 processors

-   */

-#if defined(ARM_MATH_CM0_FAMILY)

-

-  static __INLINE q31_t __SSAT(

-  q31_t x,

-  uint32_t y)

-  {

-    int32_t posMax, negMin;

-    uint32_t i;

-

-    posMax = 1;

-    for (i = 0; i < (y - 1); i++)

-    {

-      posMax = posMax * 2;

-    }

-

-    if(x > 0)

-    {

-      posMax = (posMax - 1);

-

-      if(x > posMax)

-      {

-        x = posMax;

-      }

-    }

-    else

-    {

-      negMin = -posMax;

-

-      if(x < negMin)

-      {

-        x = negMin;

-      }

-    }

-    return (x);

-

-

-  }

-

-#endif /* end of ARM_MATH_CM0_FAMILY */

-

-

-

-  /*

-   * @brief C custom defined intrinsic function for M3 and M0 processors

-   */

-#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY)

-

-  /*

-   * @brief C custom defined QADD8 for M3 and M0 processors

-   */

-  static __INLINE q31_t __QADD8(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q7_t r, s, t, u;

-

-    r = (q7_t) x;

-    s = (q7_t) y;

-

-    r = __SSAT((q31_t) (r + s), 8);

-    s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);

-    t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);

-    u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);

-

-    sum =

-      (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |

-      (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);

-

-    return sum;

-

-  }

-

-  /*

-   * @brief C custom defined QSUB8 for M3 and M0 processors

-   */

-  static __INLINE q31_t __QSUB8(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q31_t r, s, t, u;

-

-    r = (q7_t) x;

-    s = (q7_t) y;

-

-    r = __SSAT((r - s), 8);

-    s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;

-    t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;

-    u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;

-

-    sum =

-      (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r &

-                                                                0x000000FF);

-

-    return sum;

-  }

-

-  /*

-   * @brief C custom defined QADD16 for M3 and M0 processors

-   */

-

-  /*

-   * @brief C custom defined QADD16 for M3 and M0 processors

-   */

-  static __INLINE q31_t __QADD16(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q31_t r, s;

-

-    r = (short) x;

-    s = (short) y;

-

-    r = __SSAT(r + s, 16);

-    s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;

-

-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

-

-    return sum;

-

-  }

-

-  /*

-   * @brief C custom defined SHADD16 for M3 and M0 processors

-   */

-  static __INLINE q31_t __SHADD16(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q31_t r, s;

-

-    r = (short) x;

-    s = (short) y;

-

-    r = ((r >> 1) + (s >> 1));

-    s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;

-

-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

-

-    return sum;

-

-  }

-

-  /*

-   * @brief C custom defined QSUB16 for M3 and M0 processors

-   */

-  static __INLINE q31_t __QSUB16(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q31_t r, s;

-

-    r = (short) x;

-    s = (short) y;

-

-    r = __SSAT(r - s, 16);

-    s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;

-

-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

-

-    return sum;

-  }

-

-  /*

-   * @brief C custom defined SHSUB16 for M3 and M0 processors

-   */

-  static __INLINE q31_t __SHSUB16(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t diff;

-    q31_t r, s;

-

-    r = (short) x;

-    s = (short) y;

-

-    r = ((r >> 1) - (s >> 1));

-    s = (((x >> 17) - (y >> 17)) << 16);

-

-    diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);

-

-    return diff;

-  }

-

-  /*

-   * @brief C custom defined QASX for M3 and M0 processors

-   */

-  static __INLINE q31_t __QASX(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum = 0;

-

-    sum =

-      ((sum +

-        clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +

-      clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));

-

-    return sum;

-  }

-

-  /*

-   * @brief C custom defined SHASX for M3 and M0 processors

-   */

-  static __INLINE q31_t __SHASX(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q31_t r, s;

-

-    r = (short) x;

-    s = (short) y;

-

-    r = ((r >> 1) - (y >> 17));

-    s = (((x >> 17) + (s >> 1)) << 16);

-

-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

-

-    return sum;

-  }

-

-

-  /*

-   * @brief C custom defined QSAX for M3 and M0 processors

-   */

-  static __INLINE q31_t __QSAX(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum = 0;

-

-    sum =

-      ((sum +

-        clip_q31_to_q15((q31_t) ((short) (x >> 16) - (short) y))) << 16) +

-      clip_q31_to_q15((q31_t) ((short) x + (short) (y >> 16)));

-

-    return sum;

-  }

-

-  /*

-   * @brief C custom defined SHSAX for M3 and M0 processors

-   */

-  static __INLINE q31_t __SHSAX(

-  q31_t x,

-  q31_t y)

-  {

-

-    q31_t sum;

-    q31_t r, s;

-

-    r = (short) x;

-    s = (short) y;

-

-    r = ((r >> 1) + (y >> 17));

-    s = (((x >> 17) - (s >> 1)) << 16);

-

-    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

-

-    return sum;

-  }

-

-  /*

-   * @brief C custom defined SMUSDX for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMUSDX(

-  q31_t x,

-  q31_t y)

-  {

-

-    return ((q31_t) (((short) x * (short) (y >> 16)) -

-                     ((short) (x >> 16) * (short) y)));

-  }

-

-  /*

-   * @brief C custom defined SMUADX for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMUADX(

-  q31_t x,

-  q31_t y)

-  {

-

-    return ((q31_t) (((short) x * (short) (y >> 16)) +

-                     ((short) (x >> 16) * (short) y)));

-  }

-

-  /*

-   * @brief C custom defined QADD for M3 and M0 processors

-   */

-  static __INLINE q31_t __QADD(

-  q31_t x,

-  q31_t y)

-  {

-    return clip_q63_to_q31((q63_t) x + y);

-  }

-

-  /*

-   * @brief C custom defined QSUB for M3 and M0 processors

-   */

-  static __INLINE q31_t __QSUB(

-  q31_t x,

-  q31_t y)

-  {

-    return clip_q63_to_q31((q63_t) x - y);

-  }

-

-  /*

-   * @brief C custom defined SMLAD for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMLAD(

-  q31_t x,

-  q31_t y,

-  q31_t sum)

-  {

-

-    return (sum + ((short) (x >> 16) * (short) (y >> 16)) +

-            ((short) x * (short) y));

-  }

-

-  /*

-   * @brief C custom defined SMLADX for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMLADX(

-  q31_t x,

-  q31_t y,

-  q31_t sum)

-  {

-

-    return (sum + ((short) (x >> 16) * (short) (y)) +

-            ((short) x * (short) (y >> 16)));

-  }

-

-  /*

-   * @brief C custom defined SMLSDX for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMLSDX(

-  q31_t x,

-  q31_t y,

-  q31_t sum)

-  {

-

-    return (sum - ((short) (x >> 16) * (short) (y)) +

-            ((short) x * (short) (y >> 16)));

-  }

-

-  /*

-   * @brief C custom defined SMLALD for M3 and M0 processors

-   */

-  static __INLINE q63_t __SMLALD(

-  q31_t x,

-  q31_t y,

-  q63_t sum)

-  {

-

-    return (sum + ((short) (x >> 16) * (short) (y >> 16)) +

-            ((short) x * (short) y));

-  }

-

-  /*

-   * @brief C custom defined SMLALDX for M3 and M0 processors

-   */

-  static __INLINE q63_t __SMLALDX(

-  q31_t x,

-  q31_t y,

-  q63_t sum)

-  {

-

-    return (sum + ((short) (x >> 16) * (short) y)) +

-      ((short) x * (short) (y >> 16));

-  }

-

-  /*

-   * @brief C custom defined SMUAD for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMUAD(

-  q31_t x,

-  q31_t y)

-  {

-

-    return (((x >> 16) * (y >> 16)) +

-            (((x << 16) >> 16) * ((y << 16) >> 16)));

-  }

-

-  /*

-   * @brief C custom defined SMUSD for M3 and M0 processors

-   */

-  static __INLINE q31_t __SMUSD(

-  q31_t x,

-  q31_t y)

-  {

-

-    return (-((x >> 16) * (y >> 16)) +

-            (((x << 16) >> 16) * ((y << 16) >> 16)));

-  }

-

-

-  /*

-   * @brief C custom defined SXTB16 for M3 and M0 processors

-   */

-  static __INLINE q31_t __SXTB16(

-  q31_t x)

-  {

-

-    return ((((x << 24) >> 24) & 0x0000FFFF) |

-            (((x << 8) >> 8) & 0xFFFF0000));

-  }

-

-

-#endif /* defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0_FAMILY) */

-

-

-  /**

-   * @brief Instance structure for the Q7 FIR filter.

-   */

-  typedef struct

-  {

-    uint16_t numTaps;        /**< number of filter coefficients in the filter. */

-    q7_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q7_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/

-  } arm_fir_instance_q7;

-

-  /**

-   * @brief Instance structure for the Q15 FIR filter.

-   */

-  typedef struct

-  {

-    uint16_t numTaps;         /**< number of filter coefficients in the filter. */

-    q15_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q15_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/

-  } arm_fir_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 FIR filter.

-   */

-  typedef struct

-  {

-    uint16_t numTaps;         /**< number of filter coefficients in the filter. */

-    q31_t *pState;            /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q31_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps. */

-  } arm_fir_instance_q31;

-

-  /**

-   * @brief Instance structure for the floating-point FIR filter.

-   */

-  typedef struct

-  {

-    uint16_t numTaps;     /**< number of filter coefficients in the filter. */

-    float32_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    float32_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */

-  } arm_fir_instance_f32;

-

-

-  /**

-   * @brief Processing function for the Q7 FIR filter.

-   * @param[in] *S points to an instance of the Q7 FIR filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_q7(

-  const arm_fir_instance_q7 * S,

-  q7_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Initialization function for the Q7 FIR filter.

-   * @param[in,out] *S points to an instance of the Q7 FIR structure.

-   * @param[in] numTaps  Number of filter coefficients in the filter.

-   * @param[in] *pCoeffs points to the filter coefficients.

-   * @param[in] *pState points to the state buffer.

-   * @param[in] blockSize number of samples that are processed.

-   * @return none

-   */

-  void arm_fir_init_q7(

-  arm_fir_instance_q7 * S,

-  uint16_t numTaps,

-  q7_t * pCoeffs,

-  q7_t * pState,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Processing function for the Q15 FIR filter.

-   * @param[in] *S points to an instance of the Q15 FIR structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_q15(

-  const arm_fir_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the fast Q15 FIR filter for Cortex-M3 and Cortex-M4.

-   * @param[in] *S points to an instance of the Q15 FIR filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_fast_q15(

-  const arm_fir_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q15 FIR filter.

-   * @param[in,out] *S points to an instance of the Q15 FIR filter structure.

-   * @param[in] numTaps  Number of filter coefficients in the filter. Must be even and greater than or equal to 4.

-   * @param[in] *pCoeffs points to the filter coefficients.

-   * @param[in] *pState points to the state buffer.

-   * @param[in] blockSize number of samples that are processed at a time.

-   * @return The function returns ARM_MATH_SUCCESS if initialization was successful or ARM_MATH_ARGUMENT_ERROR if

-   * <code>numTaps</code> is not a supported value.

-   */

-

-  arm_status arm_fir_init_q15(

-  arm_fir_instance_q15 * S,

-  uint16_t numTaps,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q31 FIR filter.

-   * @param[in] *S points to an instance of the Q31 FIR filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_q31(

-  const arm_fir_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the fast Q31 FIR filter for Cortex-M3 and Cortex-M4.

-   * @param[in] *S points to an instance of the Q31 FIR structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_fast_q31(

-  const arm_fir_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q31 FIR filter.

-   * @param[in,out] *S points to an instance of the Q31 FIR structure.

-   * @param[in] 	numTaps  Number of filter coefficients in the filter.

-   * @param[in] 	*pCoeffs points to the filter coefficients.

-   * @param[in] 	*pState points to the state buffer.

-   * @param[in] 	blockSize number of samples that are processed at a time.

-   * @return 		none.

-   */

-  void arm_fir_init_q31(

-  arm_fir_instance_q31 * S,

-  uint16_t numTaps,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the floating-point FIR filter.

-   * @param[in] *S points to an instance of the floating-point FIR structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_f32(

-  const arm_fir_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the floating-point FIR filter.

-   * @param[in,out] *S points to an instance of the floating-point FIR filter structure.

-   * @param[in] 	numTaps  Number of filter coefficients in the filter.

-   * @param[in] 	*pCoeffs points to the filter coefficients.

-   * @param[in] 	*pState points to the state buffer.

-   * @param[in] 	blockSize number of samples that are processed at a time.

-   * @return    	none.

-   */

-  void arm_fir_init_f32(

-  arm_fir_instance_f32 * S,

-  uint16_t numTaps,

-  float32_t * pCoeffs,

-  float32_t * pState,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Instance structure for the Q15 Biquad cascade filter.

-   */

-  typedef struct

-  {

-    int8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */

-    q15_t *pState;            /**< Points to the array of state coefficients.  The array is of length 4*numStages. */

-    q15_t *pCoeffs;           /**< Points to the array of coefficients.  The array is of length 5*numStages. */

-    int8_t postShift;         /**< Additional shift, in bits, applied to each output sample. */

-

-  } arm_biquad_casd_df1_inst_q15;

-

-

-  /**

-   * @brief Instance structure for the Q31 Biquad cascade filter.

-   */

-  typedef struct

-  {

-    uint32_t numStages;      /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */

-    q31_t *pState;           /**< Points to the array of state coefficients.  The array is of length 4*numStages. */

-    q31_t *pCoeffs;          /**< Points to the array of coefficients.  The array is of length 5*numStages. */

-    uint8_t postShift;       /**< Additional shift, in bits, applied to each output sample. */

-

-  } arm_biquad_casd_df1_inst_q31;

-

-  /**

-   * @brief Instance structure for the floating-point Biquad cascade filter.

-   */

-  typedef struct

-  {

-    uint32_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */

-    float32_t *pState;          /**< Points to the array of state coefficients.  The array is of length 4*numStages. */

-    float32_t *pCoeffs;         /**< Points to the array of coefficients.  The array is of length 5*numStages. */

-

-

-  } arm_biquad_casd_df1_inst_f32;

-

-

-

-  /**

-   * @brief Processing function for the Q15 Biquad cascade filter.

-   * @param[in]  *S points to an instance of the Q15 Biquad cascade structure.

-   * @param[in]  *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in]  blockSize number of samples to process.

-   * @return     none.

-   */

-

-  void arm_biquad_cascade_df1_q15(

-  const arm_biquad_casd_df1_inst_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q15 Biquad cascade filter.

-   * @param[in,out] *S           points to an instance of the Q15 Biquad cascade structure.

-   * @param[in]     numStages    number of 2nd order stages in the filter.

-   * @param[in]     *pCoeffs     points to the filter coefficients.

-   * @param[in]     *pState      points to the state buffer.

-   * @param[in]     postShift    Shift to be applied to the output. Varies according to the coefficients format

-   * @return        none

-   */

-

-  void arm_biquad_cascade_df1_init_q15(

-  arm_biquad_casd_df1_inst_q15 * S,

-  uint8_t numStages,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  int8_t postShift);

-

-

-  /**

-   * @brief Fast but less precise processing function for the Q15 Biquad cascade filter for Cortex-M3 and Cortex-M4.

-   * @param[in]  *S points to an instance of the Q15 Biquad cascade structure.

-   * @param[in]  *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in]  blockSize number of samples to process.

-   * @return     none.

-   */

-

-  void arm_biquad_cascade_df1_fast_q15(

-  const arm_biquad_casd_df1_inst_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Processing function for the Q31 Biquad cascade filter

-   * @param[in]  *S         points to an instance of the Q31 Biquad cascade structure.

-   * @param[in]  *pSrc      points to the block of input data.

-   * @param[out] *pDst      points to the block of output data.

-   * @param[in]  blockSize  number of samples to process.

-   * @return     none.

-   */

-

-  void arm_biquad_cascade_df1_q31(

-  const arm_biquad_casd_df1_inst_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Fast but less precise processing function for the Q31 Biquad cascade filter for Cortex-M3 and Cortex-M4.

-   * @param[in]  *S         points to an instance of the Q31 Biquad cascade structure.

-   * @param[in]  *pSrc      points to the block of input data.

-   * @param[out] *pDst      points to the block of output data.

-   * @param[in]  blockSize  number of samples to process.

-   * @return     none.

-   */

-

-  void arm_biquad_cascade_df1_fast_q31(

-  const arm_biquad_casd_df1_inst_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q31 Biquad cascade filter.

-   * @param[in,out] *S           points to an instance of the Q31 Biquad cascade structure.

-   * @param[in]     numStages      number of 2nd order stages in the filter.

-   * @param[in]     *pCoeffs     points to the filter coefficients.

-   * @param[in]     *pState      points to the state buffer.

-   * @param[in]     postShift    Shift to be applied to the output. Varies according to the coefficients format

-   * @return        none

-   */

-

-  void arm_biquad_cascade_df1_init_q31(

-  arm_biquad_casd_df1_inst_q31 * S,

-  uint8_t numStages,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  int8_t postShift);

-

-  /**

-   * @brief Processing function for the floating-point Biquad cascade filter.

-   * @param[in]  *S         points to an instance of the floating-point Biquad cascade structure.

-   * @param[in]  *pSrc      points to the block of input data.

-   * @param[out] *pDst      points to the block of output data.

-   * @param[in]  blockSize  number of samples to process.

-   * @return     none.

-   */

-

-  void arm_biquad_cascade_df1_f32(

-  const arm_biquad_casd_df1_inst_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the floating-point Biquad cascade filter.

-   * @param[in,out] *S           points to an instance of the floating-point Biquad cascade structure.

-   * @param[in]     numStages    number of 2nd order stages in the filter.

-   * @param[in]     *pCoeffs     points to the filter coefficients.

-   * @param[in]     *pState      points to the state buffer.

-   * @return        none

-   */

-

-  void arm_biquad_cascade_df1_init_f32(

-  arm_biquad_casd_df1_inst_f32 * S,

-  uint8_t numStages,

-  float32_t * pCoeffs,

-  float32_t * pState);

-

-

-  /**

-   * @brief Instance structure for the floating-point matrix structure.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;     /**< number of rows of the matrix.     */

-    uint16_t numCols;     /**< number of columns of the matrix.  */

-    float32_t *pData;     /**< points to the data of the matrix. */

-  } arm_matrix_instance_f32;

-

-  /**

-   * @brief Instance structure for the Q15 matrix structure.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;     /**< number of rows of the matrix.     */

-    uint16_t numCols;     /**< number of columns of the matrix.  */

-    q15_t *pData;         /**< points to the data of the matrix. */

-

-  } arm_matrix_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 matrix structure.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;     /**< number of rows of the matrix.     */

-    uint16_t numCols;     /**< number of columns of the matrix.  */

-    q31_t *pData;         /**< points to the data of the matrix. */

-

-  } arm_matrix_instance_q31;

-

-

-

-  /**

-   * @brief Floating-point matrix addition.

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_add_f32(

-  const arm_matrix_instance_f32 * pSrcA,

-  const arm_matrix_instance_f32 * pSrcB,

-  arm_matrix_instance_f32 * pDst);

-

-  /**

-   * @brief Q15 matrix addition.

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_add_q15(

-  const arm_matrix_instance_q15 * pSrcA,

-  const arm_matrix_instance_q15 * pSrcB,

-  arm_matrix_instance_q15 * pDst);

-

-  /**

-   * @brief Q31 matrix addition.

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_add_q31(

-  const arm_matrix_instance_q31 * pSrcA,

-  const arm_matrix_instance_q31 * pSrcB,

-  arm_matrix_instance_q31 * pDst);

-

-

-  /**

-   * @brief Floating-point matrix transpose.

-   * @param[in]  *pSrc points to the input matrix

-   * @param[out] *pDst points to the output matrix

-   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>

-   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_trans_f32(

-  const arm_matrix_instance_f32 * pSrc,

-  arm_matrix_instance_f32 * pDst);

-

-

-  /**

-   * @brief Q15 matrix transpose.

-   * @param[in]  *pSrc points to the input matrix

-   * @param[out] *pDst points to the output matrix

-   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>

-   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_trans_q15(

-  const arm_matrix_instance_q15 * pSrc,

-  arm_matrix_instance_q15 * pDst);

-

-  /**

-   * @brief Q31 matrix transpose.

-   * @param[in]  *pSrc points to the input matrix

-   * @param[out] *pDst points to the output matrix

-   * @return 	The function returns either  <code>ARM_MATH_SIZE_MISMATCH</code>

-   * or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_trans_q31(

-  const arm_matrix_instance_q31 * pSrc,

-  arm_matrix_instance_q31 * pDst);

-

-

-  /**

-   * @brief Floating-point matrix multiplication

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_mult_f32(

-  const arm_matrix_instance_f32 * pSrcA,

-  const arm_matrix_instance_f32 * pSrcB,

-  arm_matrix_instance_f32 * pDst);

-

-  /**

-   * @brief Q15 matrix multiplication

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @param[in]		  *pState points to the array for storing intermediate results

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_mult_q15(

-  const arm_matrix_instance_q15 * pSrcA,

-  const arm_matrix_instance_q15 * pSrcB,

-  arm_matrix_instance_q15 * pDst,

-  q15_t * pState);

-

-  /**

-   * @brief Q15 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4

-   * @param[in]       *pSrcA  points to the first input matrix structure

-   * @param[in]       *pSrcB  points to the second input matrix structure

-   * @param[out]      *pDst   points to output matrix structure

-   * @param[in]		  *pState points to the array for storing intermediate results

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_mult_fast_q15(

-  const arm_matrix_instance_q15 * pSrcA,

-  const arm_matrix_instance_q15 * pSrcB,

-  arm_matrix_instance_q15 * pDst,

-  q15_t * pState);

-

-  /**

-   * @brief Q31 matrix multiplication

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_mult_q31(

-  const arm_matrix_instance_q31 * pSrcA,

-  const arm_matrix_instance_q31 * pSrcB,

-  arm_matrix_instance_q31 * pDst);

-

-  /**

-   * @brief Q31 matrix multiplication (fast variant) for Cortex-M3 and Cortex-M4

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_mult_fast_q31(

-  const arm_matrix_instance_q31 * pSrcA,

-  const arm_matrix_instance_q31 * pSrcB,

-  arm_matrix_instance_q31 * pDst);

-

-

-  /**

-   * @brief Floating-point matrix subtraction

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_sub_f32(

-  const arm_matrix_instance_f32 * pSrcA,

-  const arm_matrix_instance_f32 * pSrcB,

-  arm_matrix_instance_f32 * pDst);

-

-  /**

-   * @brief Q15 matrix subtraction

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_sub_q15(

-  const arm_matrix_instance_q15 * pSrcA,

-  const arm_matrix_instance_q15 * pSrcB,

-  arm_matrix_instance_q15 * pDst);

-

-  /**

-   * @brief Q31 matrix subtraction

-   * @param[in]       *pSrcA points to the first input matrix structure

-   * @param[in]       *pSrcB points to the second input matrix structure

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_sub_q31(

-  const arm_matrix_instance_q31 * pSrcA,

-  const arm_matrix_instance_q31 * pSrcB,

-  arm_matrix_instance_q31 * pDst);

-

-  /**

-   * @brief Floating-point matrix scaling.

-   * @param[in]  *pSrc points to the input matrix

-   * @param[in]  scale scale factor

-   * @param[out] *pDst points to the output matrix

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_scale_f32(

-  const arm_matrix_instance_f32 * pSrc,

-  float32_t scale,

-  arm_matrix_instance_f32 * pDst);

-

-  /**

-   * @brief Q15 matrix scaling.

-   * @param[in]       *pSrc points to input matrix

-   * @param[in]       scaleFract fractional portion of the scale factor

-   * @param[in]       shift number of bits to shift the result by

-   * @param[out]      *pDst points to output matrix

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_scale_q15(

-  const arm_matrix_instance_q15 * pSrc,

-  q15_t scaleFract,

-  int32_t shift,

-  arm_matrix_instance_q15 * pDst);

-

-  /**

-   * @brief Q31 matrix scaling.

-   * @param[in]       *pSrc points to input matrix

-   * @param[in]       scaleFract fractional portion of the scale factor

-   * @param[in]       shift number of bits to shift the result by

-   * @param[out]      *pDst points to output matrix structure

-   * @return     The function returns either

-   * <code>ARM_MATH_SIZE_MISMATCH</code> or <code>ARM_MATH_SUCCESS</code> based on the outcome of size checking.

-   */

-

-  arm_status arm_mat_scale_q31(

-  const arm_matrix_instance_q31 * pSrc,

-  q31_t scaleFract,

-  int32_t shift,

-  arm_matrix_instance_q31 * pDst);

-

-

-  /**

-   * @brief  Q31 matrix initialization.

-   * @param[in,out] *S             points to an instance of the floating-point matrix structure.

-   * @param[in]     nRows          number of rows in the matrix.

-   * @param[in]     nColumns       number of columns in the matrix.

-   * @param[in]     *pData	       points to the matrix data array.

-   * @return        none

-   */

-

-  void arm_mat_init_q31(

-  arm_matrix_instance_q31 * S,

-  uint16_t nRows,

-  uint16_t nColumns,

-  q31_t * pData);

-

-  /**

-   * @brief  Q15 matrix initialization.

-   * @param[in,out] *S             points to an instance of the floating-point matrix structure.

-   * @param[in]     nRows          number of rows in the matrix.

-   * @param[in]     nColumns       number of columns in the matrix.

-   * @param[in]     *pData	       points to the matrix data array.

-   * @return        none

-   */

-

-  void arm_mat_init_q15(

-  arm_matrix_instance_q15 * S,

-  uint16_t nRows,

-  uint16_t nColumns,

-  q15_t * pData);

-

-  /**

-   * @brief  Floating-point matrix initialization.

-   * @param[in,out] *S             points to an instance of the floating-point matrix structure.

-   * @param[in]     nRows          number of rows in the matrix.

-   * @param[in]     nColumns       number of columns in the matrix.

-   * @param[in]     *pData	       points to the matrix data array.

-   * @return        none

-   */

-

-  void arm_mat_init_f32(

-  arm_matrix_instance_f32 * S,

-  uint16_t nRows,

-  uint16_t nColumns,

-  float32_t * pData);

-

-

-

-  /**

-   * @brief Instance structure for the Q15 PID Control.

-   */

-  typedef struct

-  {

-    q15_t A0;    /**< The derived gain, A0 = Kp + Ki + Kd . */

-#ifdef ARM_MATH_CM0_FAMILY

-    q15_t A1;

-    q15_t A2;

-#else

-    q31_t A1;           /**< The derived gain A1 = -Kp - 2Kd | Kd.*/

-#endif

-    q15_t state[3];       /**< The state array of length 3. */

-    q15_t Kp;           /**< The proportional gain. */

-    q15_t Ki;           /**< The integral gain. */

-    q15_t Kd;           /**< The derivative gain. */

-  } arm_pid_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 PID Control.

-   */

-  typedef struct

-  {

-    q31_t A0;            /**< The derived gain, A0 = Kp + Ki + Kd . */

-    q31_t A1;            /**< The derived gain, A1 = -Kp - 2Kd. */

-    q31_t A2;            /**< The derived gain, A2 = Kd . */

-    q31_t state[3];      /**< The state array of length 3. */

-    q31_t Kp;            /**< The proportional gain. */

-    q31_t Ki;            /**< The integral gain. */

-    q31_t Kd;            /**< The derivative gain. */

-

-  } arm_pid_instance_q31;

-

-  /**

-   * @brief Instance structure for the floating-point PID Control.

-   */

-  typedef struct

-  {

-    float32_t A0;          /**< The derived gain, A0 = Kp + Ki + Kd . */

-    float32_t A1;          /**< The derived gain, A1 = -Kp - 2Kd. */

-    float32_t A2;          /**< The derived gain, A2 = Kd . */

-    float32_t state[3];    /**< The state array of length 3. */

-    float32_t Kp;               /**< The proportional gain. */

-    float32_t Ki;               /**< The integral gain. */

-    float32_t Kd;               /**< The derivative gain. */

-  } arm_pid_instance_f32;

-

-

-

-  /**

-   * @brief  Initialization function for the floating-point PID Control.

-   * @param[in,out] *S      points to an instance of the PID structure.

-   * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.

-   * @return none.

-   */

-  void arm_pid_init_f32(

-  arm_pid_instance_f32 * S,

-  int32_t resetStateFlag);

-

-  /**

-   * @brief  Reset function for the floating-point PID Control.

-   * @param[in,out] *S is an instance of the floating-point PID Control structure

-   * @return none

-   */

-  void arm_pid_reset_f32(

-  arm_pid_instance_f32 * S);

-

-

-  /**

-   * @brief  Initialization function for the Q31 PID Control.

-   * @param[in,out] *S points to an instance of the Q15 PID structure.

-   * @param[in]     resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.

-   * @return none.

-   */

-  void arm_pid_init_q31(

-  arm_pid_instance_q31 * S,

-  int32_t resetStateFlag);

-

-

-  /**

-   * @brief  Reset function for the Q31 PID Control.

-   * @param[in,out] *S points to an instance of the Q31 PID Control structure

-   * @return none

-   */

-

-  void arm_pid_reset_q31(

-  arm_pid_instance_q31 * S);

-

-  /**

-   * @brief  Initialization function for the Q15 PID Control.

-   * @param[in,out] *S points to an instance of the Q15 PID structure.

-   * @param[in] resetStateFlag  flag to reset the state. 0 = no change in state 1 = reset the state.

-   * @return none.

-   */

-  void arm_pid_init_q15(

-  arm_pid_instance_q15 * S,

-  int32_t resetStateFlag);

-

-  /**

-   * @brief  Reset function for the Q15 PID Control.

-   * @param[in,out] *S points to an instance of the q15 PID Control structure

-   * @return none

-   */

-  void arm_pid_reset_q15(

-  arm_pid_instance_q15 * S);

-

-

-  /**

-   * @brief Instance structure for the floating-point Linear Interpolate function.

-   */

-  typedef struct

-  {

-    uint32_t nValues;           /**< nValues */

-    float32_t x1;               /**< x1 */

-    float32_t xSpacing;         /**< xSpacing */

-    float32_t *pYData;          /**< pointer to the table of Y values */

-  } arm_linear_interp_instance_f32;

-

-  /**

-   * @brief Instance structure for the floating-point bilinear interpolation function.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;   /**< number of rows in the data table. */

-    uint16_t numCols;   /**< number of columns in the data table. */

-    float32_t *pData;   /**< points to the data table. */

-  } arm_bilinear_interp_instance_f32;

-

-   /**

-   * @brief Instance structure for the Q31 bilinear interpolation function.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;   /**< number of rows in the data table. */

-    uint16_t numCols;   /**< number of columns in the data table. */

-    q31_t *pData;       /**< points to the data table. */

-  } arm_bilinear_interp_instance_q31;

-

-   /**

-   * @brief Instance structure for the Q15 bilinear interpolation function.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;   /**< number of rows in the data table. */

-    uint16_t numCols;   /**< number of columns in the data table. */

-    q15_t *pData;       /**< points to the data table. */

-  } arm_bilinear_interp_instance_q15;

-

-   /**

-   * @brief Instance structure for the Q15 bilinear interpolation function.

-   */

-

-  typedef struct

-  {

-    uint16_t numRows;   /**< number of rows in the data table. */

-    uint16_t numCols;   /**< number of columns in the data table. */

-    q7_t *pData;                /**< points to the data table. */

-  } arm_bilinear_interp_instance_q7;

-

-

-  /**

-   * @brief Q7 vector multiplication.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst  points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_mult_q7(

-  q7_t * pSrcA,

-  q7_t * pSrcB,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q15 vector multiplication.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst  points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_mult_q15(

-  q15_t * pSrcA,

-  q15_t * pSrcB,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q31 vector multiplication.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_mult_q31(

-  q31_t * pSrcA,

-  q31_t * pSrcB,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Floating-point vector multiplication.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_mult_f32(

-  float32_t * pSrcA,

-  float32_t * pSrcB,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-

-

-

-

-

-  /**

-   * @brief Instance structure for the Q15 CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                 /**< length of the FFT. */

-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */

-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */

-    q15_t *pTwiddle;                     /**< points to the Sin twiddle factor table. */

-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */

-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */

-  } arm_cfft_radix2_instance_q15;

-

-  arm_status arm_cfft_radix2_init_q15(

-  arm_cfft_radix2_instance_q15 * S,

-  uint16_t fftLen,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-  void arm_cfft_radix2_q15(

-  const arm_cfft_radix2_instance_q15 * S,

-  q15_t * pSrc);

-

-

-

-  /**

-   * @brief Instance structure for the Q15 CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                 /**< length of the FFT. */

-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */

-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */

-    q15_t *pTwiddle;                 /**< points to the twiddle factor table. */

-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */

-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */

-  } arm_cfft_radix4_instance_q15;

-

-  arm_status arm_cfft_radix4_init_q15(

-  arm_cfft_radix4_instance_q15 * S,

-  uint16_t fftLen,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-  void arm_cfft_radix4_q15(

-  const arm_cfft_radix4_instance_q15 * S,

-  q15_t * pSrc);

-

-  /**

-   * @brief Instance structure for the Radix-2 Q31 CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                 /**< length of the FFT. */

-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */

-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */

-    q31_t *pTwiddle;                     /**< points to the Twiddle factor table. */

-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */

-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */

-  } arm_cfft_radix2_instance_q31;

-

-  arm_status arm_cfft_radix2_init_q31(

-  arm_cfft_radix2_instance_q31 * S,

-  uint16_t fftLen,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-  void arm_cfft_radix2_q31(

-  const arm_cfft_radix2_instance_q31 * S,

-  q31_t * pSrc);

-

-  /**

-   * @brief Instance structure for the Q31 CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                 /**< length of the FFT. */

-    uint8_t ifftFlag;                /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */

-    uint8_t bitReverseFlag;          /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */

-    q31_t *pTwiddle;                 /**< points to the twiddle factor table. */

-    uint16_t *pBitRevTable;          /**< points to the bit reversal table. */

-    uint16_t twidCoefModifier;       /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    uint16_t bitRevFactor;           /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */

-  } arm_cfft_radix4_instance_q31;

-

-

-  void arm_cfft_radix4_q31(

-  const arm_cfft_radix4_instance_q31 * S,

-  q31_t * pSrc);

-

-  arm_status arm_cfft_radix4_init_q31(

-  arm_cfft_radix4_instance_q31 * S,

-  uint16_t fftLen,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-  /**

-   * @brief Instance structure for the floating-point CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                   /**< length of the FFT. */

-    uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */

-    uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */

-    float32_t *pTwiddle;               /**< points to the Twiddle factor table. */

-    uint16_t *pBitRevTable;            /**< points to the bit reversal table. */

-    uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */

-    float32_t onebyfftLen;                 /**< value of 1/fftLen. */

-  } arm_cfft_radix2_instance_f32;

-

-/* Deprecated */

-  arm_status arm_cfft_radix2_init_f32(

-  arm_cfft_radix2_instance_f32 * S,

-  uint16_t fftLen,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-/* Deprecated */

-  void arm_cfft_radix2_f32(

-  const arm_cfft_radix2_instance_f32 * S,

-  float32_t * pSrc);

-

-  /**

-   * @brief Instance structure for the floating-point CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                   /**< length of the FFT. */

-    uint8_t ifftFlag;                  /**< flag that selects forward (ifftFlag=0) or inverse (ifftFlag=1) transform. */

-    uint8_t bitReverseFlag;            /**< flag that enables (bitReverseFlag=1) or disables (bitReverseFlag=0) bit reversal of output. */

-    float32_t *pTwiddle;               /**< points to the Twiddle factor table. */

-    uint16_t *pBitRevTable;            /**< points to the bit reversal table. */

-    uint16_t twidCoefModifier;         /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    uint16_t bitRevFactor;             /**< bit reversal modifier that supports different size FFTs with the same bit reversal table. */

-    float32_t onebyfftLen;                 /**< value of 1/fftLen. */

-  } arm_cfft_radix4_instance_f32;

-

-/* Deprecated */

-  arm_status arm_cfft_radix4_init_f32(

-  arm_cfft_radix4_instance_f32 * S,

-  uint16_t fftLen,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-/* Deprecated */

-  void arm_cfft_radix4_f32(

-  const arm_cfft_radix4_instance_f32 * S,

-  float32_t * pSrc);

-

-  /**

-   * @brief Instance structure for the floating-point CFFT/CIFFT function.

-   */

-

-  typedef struct

-  {

-    uint16_t fftLen;                   /**< length of the FFT. */

-    const float32_t *pTwiddle;         /**< points to the Twiddle factor table. */

-    const uint16_t *pBitRevTable;      /**< points to the bit reversal table. */

-    uint16_t bitRevLength;             /**< bit reversal table length. */

-  } arm_cfft_instance_f32;

-

-  void arm_cfft_f32(

-  const arm_cfft_instance_f32 * S,

-  float32_t * p1,

-  uint8_t ifftFlag,

-  uint8_t bitReverseFlag);

-

-  /**

-   * @brief Instance structure for the Q15 RFFT/RIFFT function.

-   */

-

-  typedef struct

-  {

-    uint32_t fftLenReal;                      /**< length of the real FFT. */

-    uint32_t fftLenBy2;                       /**< length of the complex FFT. */

-    uint8_t ifftFlagR;                        /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */

-    uint8_t bitReverseFlagR;                      /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */

-    uint32_t twidCoefRModifier;               /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    q15_t *pTwiddleAReal;                     /**< points to the real twiddle factor table. */

-    q15_t *pTwiddleBReal;                     /**< points to the imag twiddle factor table. */

-    arm_cfft_radix4_instance_q15 *pCfft;          /**< points to the complex FFT instance. */

-  } arm_rfft_instance_q15;

-

-  arm_status arm_rfft_init_q15(

-  arm_rfft_instance_q15 * S,

-  arm_cfft_radix4_instance_q15 * S_CFFT,

-  uint32_t fftLenReal,

-  uint32_t ifftFlagR,

-  uint32_t bitReverseFlag);

-

-  void arm_rfft_q15(

-  const arm_rfft_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst);

-

-  /**

-   * @brief Instance structure for the Q31 RFFT/RIFFT function.

-   */

-

-  typedef struct

-  {

-    uint32_t fftLenReal;                        /**< length of the real FFT. */

-    uint32_t fftLenBy2;                         /**< length of the complex FFT. */

-    uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */

-    uint8_t bitReverseFlagR;                        /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */

-    uint32_t twidCoefRModifier;                 /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    q31_t *pTwiddleAReal;                       /**< points to the real twiddle factor table. */

-    q31_t *pTwiddleBReal;                       /**< points to the imag twiddle factor table. */

-    arm_cfft_radix4_instance_q31 *pCfft;        /**< points to the complex FFT instance. */

-  } arm_rfft_instance_q31;

-

-  arm_status arm_rfft_init_q31(

-  arm_rfft_instance_q31 * S,

-  arm_cfft_radix4_instance_q31 * S_CFFT,

-  uint32_t fftLenReal,

-  uint32_t ifftFlagR,

-  uint32_t bitReverseFlag);

-

-  void arm_rfft_q31(

-  const arm_rfft_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst);

-

-  /**

-   * @brief Instance structure for the floating-point RFFT/RIFFT function.

-   */

-

-  typedef struct

-  {

-    uint32_t fftLenReal;                        /**< length of the real FFT. */

-    uint16_t fftLenBy2;                         /**< length of the complex FFT. */

-    uint8_t ifftFlagR;                          /**< flag that selects forward (ifftFlagR=0) or inverse (ifftFlagR=1) transform. */

-    uint8_t bitReverseFlagR;                    /**< flag that enables (bitReverseFlagR=1) or disables (bitReverseFlagR=0) bit reversal of output. */

-    uint32_t twidCoefRModifier;                     /**< twiddle coefficient modifier that supports different size FFTs with the same twiddle factor table. */

-    float32_t *pTwiddleAReal;                   /**< points to the real twiddle factor table. */

-    float32_t *pTwiddleBReal;                   /**< points to the imag twiddle factor table. */

-    arm_cfft_radix4_instance_f32 *pCfft;        /**< points to the complex FFT instance. */

-  } arm_rfft_instance_f32;

-

-  arm_status arm_rfft_init_f32(

-  arm_rfft_instance_f32 * S,

-  arm_cfft_radix4_instance_f32 * S_CFFT,

-  uint32_t fftLenReal,

-  uint32_t ifftFlagR,

-  uint32_t bitReverseFlag);

-

-  void arm_rfft_f32(

-  const arm_rfft_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst);

-

-  /**

-   * @brief Instance structure for the floating-point RFFT/RIFFT function.

-   */

-

-typedef struct

-  {

-    arm_cfft_instance_f32 Sint;      /**< Internal CFFT structure. */

-    uint16_t fftLenRFFT;                        /**< length of the real sequence */

-	float32_t * pTwiddleRFFT;					/**< Twiddle factors real stage  */

-  } arm_rfft_fast_instance_f32 ;

-

-arm_status arm_rfft_fast_init_f32 (

-	arm_rfft_fast_instance_f32 * S,

-	uint16_t fftLen);

-

-void arm_rfft_fast_f32(

-  arm_rfft_fast_instance_f32 * S,

-  float32_t * p, float32_t * pOut,

-  uint8_t ifftFlag);

-

-  /**

-   * @brief Instance structure for the floating-point DCT4/IDCT4 function.

-   */

-

-  typedef struct

-  {

-    uint16_t N;                         /**< length of the DCT4. */

-    uint16_t Nby2;                      /**< half of the length of the DCT4. */

-    float32_t normalize;                /**< normalizing factor. */

-    float32_t *pTwiddle;                /**< points to the twiddle factor table. */

-    float32_t *pCosFactor;              /**< points to the cosFactor table. */

-    arm_rfft_instance_f32 *pRfft;        /**< points to the real FFT instance. */

-    arm_cfft_radix4_instance_f32 *pCfft; /**< points to the complex FFT instance. */

-  } arm_dct4_instance_f32;

-

-  /**

-   * @brief  Initialization function for the floating-point DCT4/IDCT4.

-   * @param[in,out] *S         points to an instance of floating-point DCT4/IDCT4 structure.

-   * @param[in]     *S_RFFT    points to an instance of floating-point RFFT/RIFFT structure.

-   * @param[in]     *S_CFFT    points to an instance of floating-point CFFT/CIFFT structure.

-   * @param[in]     N          length of the DCT4.

-   * @param[in]     Nby2       half of the length of the DCT4.

-   * @param[in]     normalize  normalizing factor.

-   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>fftLenReal</code> is not a supported transform length.

-   */

-

-  arm_status arm_dct4_init_f32(

-  arm_dct4_instance_f32 * S,

-  arm_rfft_instance_f32 * S_RFFT,

-  arm_cfft_radix4_instance_f32 * S_CFFT,

-  uint16_t N,

-  uint16_t Nby2,

-  float32_t normalize);

-

-  /**

-   * @brief Processing function for the floating-point DCT4/IDCT4.

-   * @param[in]       *S             points to an instance of the floating-point DCT4/IDCT4 structure.

-   * @param[in]       *pState        points to state buffer.

-   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.

-   * @return none.

-   */

-

-  void arm_dct4_f32(

-  const arm_dct4_instance_f32 * S,

-  float32_t * pState,

-  float32_t * pInlineBuffer);

-

-  /**

-   * @brief Instance structure for the Q31 DCT4/IDCT4 function.

-   */

-

-  typedef struct

-  {

-    uint16_t N;                         /**< length of the DCT4. */

-    uint16_t Nby2;                      /**< half of the length of the DCT4. */

-    q31_t normalize;                    /**< normalizing factor. */

-    q31_t *pTwiddle;                    /**< points to the twiddle factor table. */

-    q31_t *pCosFactor;                  /**< points to the cosFactor table. */

-    arm_rfft_instance_q31 *pRfft;        /**< points to the real FFT instance. */

-    arm_cfft_radix4_instance_q31 *pCfft; /**< points to the complex FFT instance. */

-  } arm_dct4_instance_q31;

-

-  /**

-   * @brief  Initialization function for the Q31 DCT4/IDCT4.

-   * @param[in,out] *S         points to an instance of Q31 DCT4/IDCT4 structure.

-   * @param[in]     *S_RFFT    points to an instance of Q31 RFFT/RIFFT structure

-   * @param[in]     *S_CFFT    points to an instance of Q31 CFFT/CIFFT structure

-   * @param[in]     N          length of the DCT4.

-   * @param[in]     Nby2       half of the length of the DCT4.

-   * @param[in]     normalize  normalizing factor.

-   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.

-   */

-

-  arm_status arm_dct4_init_q31(

-  arm_dct4_instance_q31 * S,

-  arm_rfft_instance_q31 * S_RFFT,

-  arm_cfft_radix4_instance_q31 * S_CFFT,

-  uint16_t N,

-  uint16_t Nby2,

-  q31_t normalize);

-

-  /**

-   * @brief Processing function for the Q31 DCT4/IDCT4.

-   * @param[in]       *S             points to an instance of the Q31 DCT4 structure.

-   * @param[in]       *pState        points to state buffer.

-   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.

-   * @return none.

-   */

-

-  void arm_dct4_q31(

-  const arm_dct4_instance_q31 * S,

-  q31_t * pState,

-  q31_t * pInlineBuffer);

-

-  /**

-   * @brief Instance structure for the Q15 DCT4/IDCT4 function.

-   */

-

-  typedef struct

-  {

-    uint16_t N;                         /**< length of the DCT4. */

-    uint16_t Nby2;                      /**< half of the length of the DCT4. */

-    q15_t normalize;                    /**< normalizing factor. */

-    q15_t *pTwiddle;                    /**< points to the twiddle factor table. */

-    q15_t *pCosFactor;                  /**< points to the cosFactor table. */

-    arm_rfft_instance_q15 *pRfft;        /**< points to the real FFT instance. */

-    arm_cfft_radix4_instance_q15 *pCfft; /**< points to the complex FFT instance. */

-  } arm_dct4_instance_q15;

-

-  /**

-   * @brief  Initialization function for the Q15 DCT4/IDCT4.

-   * @param[in,out] *S         points to an instance of Q15 DCT4/IDCT4 structure.

-   * @param[in]     *S_RFFT    points to an instance of Q15 RFFT/RIFFT structure.

-   * @param[in]     *S_CFFT    points to an instance of Q15 CFFT/CIFFT structure.

-   * @param[in]     N          length of the DCT4.

-   * @param[in]     Nby2       half of the length of the DCT4.

-   * @param[in]     normalize  normalizing factor.

-   * @return		arm_status function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_ARGUMENT_ERROR if <code>N</code> is not a supported transform length.

-   */

-

-  arm_status arm_dct4_init_q15(

-  arm_dct4_instance_q15 * S,

-  arm_rfft_instance_q15 * S_RFFT,

-  arm_cfft_radix4_instance_q15 * S_CFFT,

-  uint16_t N,

-  uint16_t Nby2,

-  q15_t normalize);

-

-  /**

-   * @brief Processing function for the Q15 DCT4/IDCT4.

-   * @param[in]       *S             points to an instance of the Q15 DCT4 structure.

-   * @param[in]       *pState        points to state buffer.

-   * @param[in,out]   *pInlineBuffer points to the in-place input and output buffer.

-   * @return none.

-   */

-

-  void arm_dct4_q15(

-  const arm_dct4_instance_q15 * S,

-  q15_t * pState,

-  q15_t * pInlineBuffer);

-

-  /**

-   * @brief Floating-point vector addition.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_add_f32(

-  float32_t * pSrcA,

-  float32_t * pSrcB,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q7 vector addition.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_add_q7(

-  q7_t * pSrcA,

-  q7_t * pSrcB,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q15 vector addition.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_add_q15(

-  q15_t * pSrcA,

-  q15_t * pSrcB,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q31 vector addition.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_add_q31(

-  q31_t * pSrcA,

-  q31_t * pSrcB,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Floating-point vector subtraction.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_sub_f32(

-  float32_t * pSrcA,

-  float32_t * pSrcB,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q7 vector subtraction.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_sub_q7(

-  q7_t * pSrcA,

-  q7_t * pSrcB,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q15 vector subtraction.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_sub_q15(

-  q15_t * pSrcA,

-  q15_t * pSrcB,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q31 vector subtraction.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_sub_q31(

-  q31_t * pSrcA,

-  q31_t * pSrcB,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Multiplies a floating-point vector by a scalar.

-   * @param[in]       *pSrc points to the input vector

-   * @param[in]       scale scale factor to be applied

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_scale_f32(

-  float32_t * pSrc,

-  float32_t scale,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Multiplies a Q7 vector by a scalar.

-   * @param[in]       *pSrc points to the input vector

-   * @param[in]       scaleFract fractional portion of the scale value

-   * @param[in]       shift number of bits to shift the result by

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_scale_q7(

-  q7_t * pSrc,

-  q7_t scaleFract,

-  int8_t shift,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Multiplies a Q15 vector by a scalar.

-   * @param[in]       *pSrc points to the input vector

-   * @param[in]       scaleFract fractional portion of the scale value

-   * @param[in]       shift number of bits to shift the result by

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_scale_q15(

-  q15_t * pSrc,

-  q15_t scaleFract,

-  int8_t shift,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Multiplies a Q31 vector by a scalar.

-   * @param[in]       *pSrc points to the input vector

-   * @param[in]       scaleFract fractional portion of the scale value

-   * @param[in]       shift number of bits to shift the result by

-   * @param[out]      *pDst points to the output vector

-   * @param[in]       blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_scale_q31(

-  q31_t * pSrc,

-  q31_t scaleFract,

-  int8_t shift,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q7 vector absolute value.

-   * @param[in]       *pSrc points to the input buffer

-   * @param[out]      *pDst points to the output buffer

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_abs_q7(

-  q7_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Floating-point vector absolute value.

-   * @param[in]       *pSrc points to the input buffer

-   * @param[out]      *pDst points to the output buffer

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_abs_f32(

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q15 vector absolute value.

-   * @param[in]       *pSrc points to the input buffer

-   * @param[out]      *pDst points to the output buffer

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_abs_q15(

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Q31 vector absolute value.

-   * @param[in]       *pSrc points to the input buffer

-   * @param[out]      *pDst points to the output buffer

-   * @param[in]       blockSize number of samples in each vector

-   * @return none.

-   */

-

-  void arm_abs_q31(

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Dot product of floating-point vectors.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[in]       blockSize number of samples in each vector

-   * @param[out]      *result output result returned here

-   * @return none.

-   */

-

-  void arm_dot_prod_f32(

-  float32_t * pSrcA,

-  float32_t * pSrcB,

-  uint32_t blockSize,

-  float32_t * result);

-

-  /**

-   * @brief Dot product of Q7 vectors.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[in]       blockSize number of samples in each vector

-   * @param[out]      *result output result returned here

-   * @return none.

-   */

-

-  void arm_dot_prod_q7(

-  q7_t * pSrcA,

-  q7_t * pSrcB,

-  uint32_t blockSize,

-  q31_t * result);

-

-  /**

-   * @brief Dot product of Q15 vectors.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[in]       blockSize number of samples in each vector

-   * @param[out]      *result output result returned here

-   * @return none.

-   */

-

-  void arm_dot_prod_q15(

-  q15_t * pSrcA,

-  q15_t * pSrcB,

-  uint32_t blockSize,

-  q63_t * result);

-

-  /**

-   * @brief Dot product of Q31 vectors.

-   * @param[in]       *pSrcA points to the first input vector

-   * @param[in]       *pSrcB points to the second input vector

-   * @param[in]       blockSize number of samples in each vector

-   * @param[out]      *result output result returned here

-   * @return none.

-   */

-

-  void arm_dot_prod_q31(

-  q31_t * pSrcA,

-  q31_t * pSrcB,

-  uint32_t blockSize,

-  q63_t * result);

-

-  /**

-   * @brief  Shifts the elements of a Q7 vector a specified number of bits.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_shift_q7(

-  q7_t * pSrc,

-  int8_t shiftBits,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Shifts the elements of a Q15 vector a specified number of bits.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_shift_q15(

-  q15_t * pSrc,

-  int8_t shiftBits,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Shifts the elements of a Q31 vector a specified number of bits.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  shiftBits number of bits to shift.  A positive value shifts left; a negative value shifts right.

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_shift_q31(

-  q31_t * pSrc,

-  int8_t shiftBits,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Adds a constant offset to a floating-point vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  offset is the offset to be added

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_offset_f32(

-  float32_t * pSrc,

-  float32_t offset,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Adds a constant offset to a Q7 vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  offset is the offset to be added

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_offset_q7(

-  q7_t * pSrc,

-  q7_t offset,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Adds a constant offset to a Q15 vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  offset is the offset to be added

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_offset_q15(

-  q15_t * pSrc,

-  q15_t offset,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Adds a constant offset to a Q31 vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[in]  offset is the offset to be added

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_offset_q31(

-  q31_t * pSrc,

-  q31_t offset,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Negates the elements of a floating-point vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_negate_f32(

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Negates the elements of a Q7 vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_negate_q7(

-  q7_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Negates the elements of a Q15 vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_negate_q15(

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Negates the elements of a Q31 vector.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  blockSize number of samples in the vector

-   * @return none.

-   */

-

-  void arm_negate_q31(

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-  /**

-   * @brief  Copies the elements of a floating-point vector.

-   * @param[in]  *pSrc input pointer

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_copy_f32(

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Copies the elements of a Q7 vector.

-   * @param[in]  *pSrc input pointer

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_copy_q7(

-  q7_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Copies the elements of a Q15 vector.

-   * @param[in]  *pSrc input pointer

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_copy_q15(

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Copies the elements of a Q31 vector.

-   * @param[in]  *pSrc input pointer

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_copy_q31(

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-  /**

-   * @brief  Fills a constant value into a floating-point vector.

-   * @param[in]  value input value to be filled

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_fill_f32(

-  float32_t value,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Fills a constant value into a Q7 vector.

-   * @param[in]  value input value to be filled

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_fill_q7(

-  q7_t value,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Fills a constant value into a Q15 vector.

-   * @param[in]  value input value to be filled

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_fill_q15(

-  q15_t value,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Fills a constant value into a Q31 vector.

-   * @param[in]  value input value to be filled

-   * @param[out]  *pDst output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_fill_q31(

-  q31_t value,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-/**

- * @brief Convolution of floating-point sequences.

- * @param[in] *pSrcA points to the first input sequence.

- * @param[in] srcALen length of the first input sequence.

- * @param[in] *pSrcB points to the second input sequence.

- * @param[in] srcBLen length of the second input sequence.

- * @param[out] *pDst points to the location where the output result is written.  Length srcALen+srcBLen-1.

- * @return none.

- */

-

-  void arm_conv_f32(

-  float32_t * pSrcA,

-  uint32_t srcALen,

-  float32_t * pSrcB,

-  uint32_t srcBLen,

-  float32_t * pDst);

-

-

-  /**

-   * @brief Convolution of Q15 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @param[in]  *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]  *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).

-   * @return none.

-   */

-

-

-  void arm_conv_opt_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-/**

- * @brief Convolution of Q15 sequences.

- * @param[in] *pSrcA points to the first input sequence.

- * @param[in] srcALen length of the first input sequence.

- * @param[in] *pSrcB points to the second input sequence.

- * @param[in] srcBLen length of the second input sequence.

- * @param[out] *pDst points to the location where the output result is written.  Length srcALen+srcBLen-1.

- * @return none.

- */

-

-  void arm_conv_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst);

-

-  /**

-   * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @return none.

-   */

-

-  void arm_conv_fast_q15(

-			  q15_t * pSrcA,

-			 uint32_t srcALen,

-			  q15_t * pSrcB,

-			 uint32_t srcBLen,

-			 q15_t * pDst);

-

-  /**

-   * @brief Convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @param[in]  *pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]  *pScratch2 points to scratch buffer of size min(srcALen, srcBLen).

-   * @return none.

-   */

-

-  void arm_conv_fast_opt_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-

-  /**

-   * @brief Convolution of Q31 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @return none.

-   */

-

-  void arm_conv_q31(

-  q31_t * pSrcA,

-  uint32_t srcALen,

-  q31_t * pSrcB,

-  uint32_t srcBLen,

-  q31_t * pDst);

-

-  /**

-   * @brief Convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @return none.

-   */

-

-  void arm_conv_fast_q31(

-  q31_t * pSrcA,

-  uint32_t srcALen,

-  q31_t * pSrcB,

-  uint32_t srcBLen,

-  q31_t * pDst);

-

-

-    /**

-   * @brief Convolution of Q7 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).

-   * @return none.

-   */

-

-  void arm_conv_opt_q7(

-  q7_t * pSrcA,

-  uint32_t srcALen,

-  q7_t * pSrcB,

-  uint32_t srcBLen,

-  q7_t * pDst,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-

-  /**

-   * @brief Convolution of Q7 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length srcALen+srcBLen-1.

-   * @return none.

-   */

-

-  void arm_conv_q7(

-  q7_t * pSrcA,

-  uint32_t srcALen,

-  q7_t * pSrcB,

-  uint32_t srcBLen,

-  q7_t * pDst);

-

-

-  /**

-   * @brief Partial convolution of floating-point sequences.

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_f32(

-  float32_t * pSrcA,

-  uint32_t srcALen,

-  float32_t * pSrcB,

-  uint32_t srcBLen,

-  float32_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints);

-

-    /**

-   * @brief Partial convolution of Q15 sequences.

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @param[in]       * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]       * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_opt_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-/**

-   * @brief Partial convolution of Q15 sequences.

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints);

-

-  /**

-   * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_fast_q15(

-				        q15_t * pSrcA,

-				       uint32_t srcALen,

-				        q15_t * pSrcB,

-				       uint32_t srcBLen,

-				       q15_t * pDst,

-				       uint32_t firstIndex,

-				       uint32_t numPoints);

-

-

-  /**

-   * @brief Partial convolution of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @param[in]       * pScratch1 points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]       * pScratch2 points to scratch buffer of size min(srcALen, srcBLen).

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_fast_opt_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-  /**

-   * @brief Partial convolution of Q31 sequences.

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_q31(

-  q31_t * pSrcA,

-  uint32_t srcALen,

-  q31_t * pSrcB,

-  uint32_t srcBLen,

-  q31_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints);

-

-

-  /**

-   * @brief Partial convolution of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_fast_q31(

-  q31_t * pSrcA,

-  uint32_t srcALen,

-  q31_t * pSrcB,

-  uint32_t srcBLen,

-  q31_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints);

-

-

-  /**

-   * @brief Partial convolution of Q7 sequences

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_opt_q7(

-  q7_t * pSrcA,

-  uint32_t srcALen,

-  q7_t * pSrcB,

-  uint32_t srcBLen,

-  q7_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-/**

-   * @brief Partial convolution of Q7 sequences.

-   * @param[in]       *pSrcA points to the first input sequence.

-   * @param[in]       srcALen length of the first input sequence.

-   * @param[in]       *pSrcB points to the second input sequence.

-   * @param[in]       srcBLen length of the second input sequence.

-   * @param[out]      *pDst points to the block of output data

-   * @param[in]       firstIndex is the first output sample to start with.

-   * @param[in]       numPoints is the number of output points to be computed.

-   * @return  Returns either ARM_MATH_SUCCESS if the function completed correctly or ARM_MATH_ARGUMENT_ERROR if the requested subset is not in the range [0 srcALen+srcBLen-2].

-   */

-

-  arm_status arm_conv_partial_q7(

-  q7_t * pSrcA,

-  uint32_t srcALen,

-  q7_t * pSrcB,

-  uint32_t srcBLen,

-  q7_t * pDst,

-  uint32_t firstIndex,

-  uint32_t numPoints);

-

-

-

-  /**

-   * @brief Instance structure for the Q15 FIR decimator.

-   */

-

-  typedef struct

-  {

-    uint8_t M;                      /**< decimation factor. */

-    uint16_t numTaps;               /**< number of coefficients in the filter. */

-    q15_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numTaps.*/

-    q15_t *pState;                   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-  } arm_fir_decimate_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 FIR decimator.

-   */

-

-  typedef struct

-  {

-    uint8_t M;                  /**< decimation factor. */

-    uint16_t numTaps;           /**< number of coefficients in the filter. */

-    q31_t *pCoeffs;              /**< points to the coefficient array. The array is of length numTaps.*/

-    q31_t *pState;               /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-

-  } arm_fir_decimate_instance_q31;

-

-  /**

-   * @brief Instance structure for the floating-point FIR decimator.

-   */

-

-  typedef struct

-  {

-    uint8_t M;                          /**< decimation factor. */

-    uint16_t numTaps;                   /**< number of coefficients in the filter. */

-    float32_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numTaps.*/

-    float32_t *pState;                   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-

-  } arm_fir_decimate_instance_f32;

-

-

-

-  /**

-   * @brief Processing function for the floating-point FIR decimator.

-   * @param[in] *S points to an instance of the floating-point FIR decimator structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none

-   */

-

-  void arm_fir_decimate_f32(

-  const arm_fir_decimate_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Initialization function for the floating-point FIR decimator.

-   * @param[in,out] *S points to an instance of the floating-point FIR decimator structure.

-   * @param[in] numTaps  number of coefficients in the filter.

-   * @param[in] M  decimation factor.

-   * @param[in] *pCoeffs points to the filter coefficients.

-   * @param[in] *pState points to the state buffer.

-   * @param[in] blockSize number of input samples to process per call.

-   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if

-   * <code>blockSize</code> is not a multiple of <code>M</code>.

-   */

-

-  arm_status arm_fir_decimate_init_f32(

-  arm_fir_decimate_instance_f32 * S,

-  uint16_t numTaps,

-  uint8_t M,

-  float32_t * pCoeffs,

-  float32_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q15 FIR decimator.

-   * @param[in] *S points to an instance of the Q15 FIR decimator structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none

-   */

-

-  void arm_fir_decimate_q15(

-  const arm_fir_decimate_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q15 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.

-   * @param[in] *S points to an instance of the Q15 FIR decimator structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none

-   */

-

-  void arm_fir_decimate_fast_q15(

-  const arm_fir_decimate_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-

-

-  /**

-   * @brief  Initialization function for the Q15 FIR decimator.

-   * @param[in,out] *S points to an instance of the Q15 FIR decimator structure.

-   * @param[in] numTaps  number of coefficients in the filter.

-   * @param[in] M  decimation factor.

-   * @param[in] *pCoeffs points to the filter coefficients.

-   * @param[in] *pState points to the state buffer.

-   * @param[in] blockSize number of input samples to process per call.

-   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if

-   * <code>blockSize</code> is not a multiple of <code>M</code>.

-   */

-

-  arm_status arm_fir_decimate_init_q15(

-  arm_fir_decimate_instance_q15 * S,

-  uint16_t numTaps,

-  uint8_t M,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q31 FIR decimator.

-   * @param[in] *S points to an instance of the Q31 FIR decimator structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none

-   */

-

-  void arm_fir_decimate_q31(

-  const arm_fir_decimate_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q31 FIR decimator (fast variant) for Cortex-M3 and Cortex-M4.

-   * @param[in] *S points to an instance of the Q31 FIR decimator structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none

-   */

-

-  void arm_fir_decimate_fast_q31(

-  arm_fir_decimate_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Initialization function for the Q31 FIR decimator.

-   * @param[in,out] *S points to an instance of the Q31 FIR decimator structure.

-   * @param[in] numTaps  number of coefficients in the filter.

-   * @param[in] M  decimation factor.

-   * @param[in] *pCoeffs points to the filter coefficients.

-   * @param[in] *pState points to the state buffer.

-   * @param[in] blockSize number of input samples to process per call.

-   * @return    The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if

-   * <code>blockSize</code> is not a multiple of <code>M</code>.

-   */

-

-  arm_status arm_fir_decimate_init_q31(

-  arm_fir_decimate_instance_q31 * S,

-  uint16_t numTaps,

-  uint8_t M,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  uint32_t blockSize);

-

-

-

-  /**

-   * @brief Instance structure for the Q15 FIR interpolator.

-   */

-

-  typedef struct

-  {

-    uint8_t L;                      /**< upsample factor. */

-    uint16_t phaseLength;           /**< length of each polyphase filter component. */

-    q15_t *pCoeffs;                 /**< points to the coefficient array. The array is of length L*phaseLength. */

-    q15_t *pState;                  /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */

-  } arm_fir_interpolate_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 FIR interpolator.

-   */

-

-  typedef struct

-  {

-    uint8_t L;                      /**< upsample factor. */

-    uint16_t phaseLength;           /**< length of each polyphase filter component. */

-    q31_t *pCoeffs;                  /**< points to the coefficient array. The array is of length L*phaseLength. */

-    q31_t *pState;                   /**< points to the state variable array. The array is of length blockSize+phaseLength-1. */

-  } arm_fir_interpolate_instance_q31;

-

-  /**

-   * @brief Instance structure for the floating-point FIR interpolator.

-   */

-

-  typedef struct

-  {

-    uint8_t L;                     /**< upsample factor. */

-    uint16_t phaseLength;          /**< length of each polyphase filter component. */

-    float32_t *pCoeffs;             /**< points to the coefficient array. The array is of length L*phaseLength. */

-    float32_t *pState;              /**< points to the state variable array. The array is of length phaseLength+numTaps-1. */

-  } arm_fir_interpolate_instance_f32;

-

-

-  /**

-   * @brief Processing function for the Q15 FIR interpolator.

-   * @param[in] *S        points to an instance of the Q15 FIR interpolator structure.

-   * @param[in] *pSrc     points to the block of input data.

-   * @param[out] *pDst    points to the block of output data.

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_interpolate_q15(

-  const arm_fir_interpolate_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Initialization function for the Q15 FIR interpolator.

-   * @param[in,out] *S        points to an instance of the Q15 FIR interpolator structure.

-   * @param[in]     L         upsample factor.

-   * @param[in]     numTaps   number of filter coefficients in the filter.

-   * @param[in]     *pCoeffs  points to the filter coefficient buffer.

-   * @param[in]     *pState   points to the state buffer.

-   * @param[in]     blockSize number of input samples to process per call.

-   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if

-   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.

-   */

-

-  arm_status arm_fir_interpolate_init_q15(

-  arm_fir_interpolate_instance_q15 * S,

-  uint8_t L,

-  uint16_t numTaps,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q31 FIR interpolator.

-   * @param[in] *S        points to an instance of the Q15 FIR interpolator structure.

-   * @param[in] *pSrc     points to the block of input data.

-   * @param[out] *pDst    points to the block of output data.

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_interpolate_q31(

-  const arm_fir_interpolate_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q31 FIR interpolator.

-   * @param[in,out] *S        points to an instance of the Q31 FIR interpolator structure.

-   * @param[in]     L         upsample factor.

-   * @param[in]     numTaps   number of filter coefficients in the filter.

-   * @param[in]     *pCoeffs  points to the filter coefficient buffer.

-   * @param[in]     *pState   points to the state buffer.

-   * @param[in]     blockSize number of input samples to process per call.

-   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if

-   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.

-   */

-

-  arm_status arm_fir_interpolate_init_q31(

-  arm_fir_interpolate_instance_q31 * S,

-  uint8_t L,

-  uint16_t numTaps,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Processing function for the floating-point FIR interpolator.

-   * @param[in] *S        points to an instance of the floating-point FIR interpolator structure.

-   * @param[in] *pSrc     points to the block of input data.

-   * @param[out] *pDst    points to the block of output data.

-   * @param[in] blockSize number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_interpolate_f32(

-  const arm_fir_interpolate_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the floating-point FIR interpolator.

-   * @param[in,out] *S        points to an instance of the floating-point FIR interpolator structure.

-   * @param[in]     L         upsample factor.

-   * @param[in]     numTaps   number of filter coefficients in the filter.

-   * @param[in]     *pCoeffs  points to the filter coefficient buffer.

-   * @param[in]     *pState   points to the state buffer.

-   * @param[in]     blockSize number of input samples to process per call.

-   * @return        The function returns ARM_MATH_SUCCESS if initialization is successful or ARM_MATH_LENGTH_ERROR if

-   * the filter length <code>numTaps</code> is not a multiple of the interpolation factor <code>L</code>.

-   */

-

-  arm_status arm_fir_interpolate_init_f32(

-  arm_fir_interpolate_instance_f32 * S,

-  uint8_t L,

-  uint16_t numTaps,

-  float32_t * pCoeffs,

-  float32_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Instance structure for the high precision Q31 Biquad cascade filter.

-   */

-

-  typedef struct

-  {

-    uint8_t numStages;       /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */

-    q63_t *pState;           /**< points to the array of state coefficients.  The array is of length 4*numStages. */

-    q31_t *pCoeffs;          /**< points to the array of coefficients.  The array is of length 5*numStages. */

-    uint8_t postShift;       /**< additional shift, in bits, applied to each output sample. */

-

-  } arm_biquad_cas_df1_32x64_ins_q31;

-

-

-  /**

-   * @param[in]  *S        points to an instance of the high precision Q31 Biquad cascade filter structure.

-   * @param[in]  *pSrc     points to the block of input data.

-   * @param[out] *pDst     points to the block of output data

-   * @param[in]  blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_biquad_cas_df1_32x64_q31(

-  const arm_biquad_cas_df1_32x64_ins_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @param[in,out] *S           points to an instance of the high precision Q31 Biquad cascade filter structure.

-   * @param[in]     numStages    number of 2nd order stages in the filter.

-   * @param[in]     *pCoeffs     points to the filter coefficients.

-   * @param[in]     *pState      points to the state buffer.

-   * @param[in]     postShift    shift to be applied to the output. Varies according to the coefficients format

-   * @return        none

-   */

-

-  void arm_biquad_cas_df1_32x64_init_q31(

-  arm_biquad_cas_df1_32x64_ins_q31 * S,

-  uint8_t numStages,

-  q31_t * pCoeffs,

-  q63_t * pState,

-  uint8_t postShift);

-

-

-

-  /**

-   * @brief Instance structure for the floating-point transposed direct form II Biquad cascade filter.

-   */

-

-  typedef struct

-  {

-    uint8_t numStages;         /**< number of 2nd order stages in the filter.  Overall order is 2*numStages. */

-    float32_t *pState;         /**< points to the array of state coefficients.  The array is of length 2*numStages. */

-    float32_t *pCoeffs;        /**< points to the array of coefficients.  The array is of length 5*numStages. */

-  } arm_biquad_cascade_df2T_instance_f32;

-

-

-  /**

-   * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.

-   * @param[in]  *S        points to an instance of the filter data structure.

-   * @param[in]  *pSrc     points to the block of input data.

-   * @param[out] *pDst     points to the block of output data

-   * @param[in]  blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_biquad_cascade_df2T_f32(

-  const arm_biquad_cascade_df2T_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Initialization function for the floating-point transposed direct form II Biquad cascade filter.

-   * @param[in,out] *S           points to an instance of the filter data structure.

-   * @param[in]     numStages    number of 2nd order stages in the filter.

-   * @param[in]     *pCoeffs     points to the filter coefficients.

-   * @param[in]     *pState      points to the state buffer.

-   * @return        none

-   */

-

-  void arm_biquad_cascade_df2T_init_f32(

-  arm_biquad_cascade_df2T_instance_f32 * S,

-  uint8_t numStages,

-  float32_t * pCoeffs,

-  float32_t * pState);

-

-

-

-  /**

-   * @brief Instance structure for the Q15 FIR lattice filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numStages;                          /**< number of filter stages. */

-    q15_t *pState;                               /**< points to the state variable array. The array is of length numStages. */

-    q15_t *pCoeffs;                              /**< points to the coefficient array. The array is of length numStages. */

-  } arm_fir_lattice_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 FIR lattice filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numStages;                          /**< number of filter stages. */

-    q31_t *pState;                               /**< points to the state variable array. The array is of length numStages. */

-    q31_t *pCoeffs;                              /**< points to the coefficient array. The array is of length numStages. */

-  } arm_fir_lattice_instance_q31;

-

-  /**

-   * @brief Instance structure for the floating-point FIR lattice filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numStages;                  /**< number of filter stages. */

-    float32_t *pState;                   /**< points to the state variable array. The array is of length numStages. */

-    float32_t *pCoeffs;                  /**< points to the coefficient array. The array is of length numStages. */

-  } arm_fir_lattice_instance_f32;

-

-  /**

-   * @brief Initialization function for the Q15 FIR lattice filter.

-   * @param[in] *S points to an instance of the Q15 FIR lattice structure.

-   * @param[in] numStages  number of filter stages.

-   * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.

-   * @param[in] *pState points to the state buffer.  The array is of length numStages.

-   * @return none.

-   */

-

-  void arm_fir_lattice_init_q15(

-  arm_fir_lattice_instance_q15 * S,

-  uint16_t numStages,

-  q15_t * pCoeffs,

-  q15_t * pState);

-

-

-  /**

-   * @brief Processing function for the Q15 FIR lattice filter.

-   * @param[in] *S points to an instance of the Q15 FIR lattice structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-  void arm_fir_lattice_q15(

-  const arm_fir_lattice_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Initialization function for the Q31 FIR lattice filter.

-   * @param[in] *S points to an instance of the Q31 FIR lattice structure.

-   * @param[in] numStages  number of filter stages.

-   * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.

-   * @param[in] *pState points to the state buffer.   The array is of length numStages.

-   * @return none.

-   */

-

-  void arm_fir_lattice_init_q31(

-  arm_fir_lattice_instance_q31 * S,

-  uint16_t numStages,

-  q31_t * pCoeffs,

-  q31_t * pState);

-

-

-  /**

-   * @brief Processing function for the Q31 FIR lattice filter.

-   * @param[in]  *S        points to an instance of the Q31 FIR lattice structure.

-   * @param[in]  *pSrc     points to the block of input data.

-   * @param[out] *pDst     points to the block of output data

-   * @param[in]  blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_fir_lattice_q31(

-  const arm_fir_lattice_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-/**

- * @brief Initialization function for the floating-point FIR lattice filter.

- * @param[in] *S points to an instance of the floating-point FIR lattice structure.

- * @param[in] numStages  number of filter stages.

- * @param[in] *pCoeffs points to the coefficient buffer.  The array is of length numStages.

- * @param[in] *pState points to the state buffer.  The array is of length numStages.

- * @return none.

- */

-

-  void arm_fir_lattice_init_f32(

-  arm_fir_lattice_instance_f32 * S,

-  uint16_t numStages,

-  float32_t * pCoeffs,

-  float32_t * pState);

-

-  /**

-   * @brief Processing function for the floating-point FIR lattice filter.

-   * @param[in]  *S        points to an instance of the floating-point FIR lattice structure.

-   * @param[in]  *pSrc     points to the block of input data.

-   * @param[out] *pDst     points to the block of output data

-   * @param[in]  blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_fir_lattice_f32(

-  const arm_fir_lattice_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Instance structure for the Q15 IIR lattice filter.

-   */

-  typedef struct

-  {

-    uint16_t numStages;                         /**< number of stages in the filter. */

-    q15_t *pState;                              /**< points to the state variable array. The array is of length numStages+blockSize. */

-    q15_t *pkCoeffs;                            /**< points to the reflection coefficient array. The array is of length numStages. */

-    q15_t *pvCoeffs;                            /**< points to the ladder coefficient array. The array is of length numStages+1. */

-  } arm_iir_lattice_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q31 IIR lattice filter.

-   */

-  typedef struct

-  {

-    uint16_t numStages;                         /**< number of stages in the filter. */

-    q31_t *pState;                              /**< points to the state variable array. The array is of length numStages+blockSize. */

-    q31_t *pkCoeffs;                            /**< points to the reflection coefficient array. The array is of length numStages. */

-    q31_t *pvCoeffs;                            /**< points to the ladder coefficient array. The array is of length numStages+1. */

-  } arm_iir_lattice_instance_q31;

-

-  /**

-   * @brief Instance structure for the floating-point IIR lattice filter.

-   */

-  typedef struct

-  {

-    uint16_t numStages;                         /**< number of stages in the filter. */

-    float32_t *pState;                          /**< points to the state variable array. The array is of length numStages+blockSize. */

-    float32_t *pkCoeffs;                        /**< points to the reflection coefficient array. The array is of length numStages. */

-    float32_t *pvCoeffs;                        /**< points to the ladder coefficient array. The array is of length numStages+1. */

-  } arm_iir_lattice_instance_f32;

-

-  /**

-   * @brief Processing function for the floating-point IIR lattice filter.

-   * @param[in] *S points to an instance of the floating-point IIR lattice structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_iir_lattice_f32(

-  const arm_iir_lattice_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Initialization function for the floating-point IIR lattice filter.

-   * @param[in] *S points to an instance of the floating-point IIR lattice structure.

-   * @param[in] numStages number of stages in the filter.

-   * @param[in] *pkCoeffs points to the reflection coefficient buffer.  The array is of length numStages.

-   * @param[in] *pvCoeffs points to the ladder coefficient buffer.  The array is of length numStages+1.

-   * @param[in] *pState points to the state buffer.  The array is of length numStages+blockSize-1.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_iir_lattice_init_f32(

-  arm_iir_lattice_instance_f32 * S,

-  uint16_t numStages,

-  float32_t * pkCoeffs,

-  float32_t * pvCoeffs,

-  float32_t * pState,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Processing function for the Q31 IIR lattice filter.

-   * @param[in] *S points to an instance of the Q31 IIR lattice structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_iir_lattice_q31(

-  const arm_iir_lattice_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Initialization function for the Q31 IIR lattice filter.

-   * @param[in] *S points to an instance of the Q31 IIR lattice structure.

-   * @param[in] numStages number of stages in the filter.

-   * @param[in] *pkCoeffs points to the reflection coefficient buffer.  The array is of length numStages.

-   * @param[in] *pvCoeffs points to the ladder coefficient buffer.  The array is of length numStages+1.

-   * @param[in] *pState points to the state buffer.  The array is of length numStages+blockSize.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_iir_lattice_init_q31(

-  arm_iir_lattice_instance_q31 * S,

-  uint16_t numStages,

-  q31_t * pkCoeffs,

-  q31_t * pvCoeffs,

-  q31_t * pState,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Processing function for the Q15 IIR lattice filter.

-   * @param[in] *S points to an instance of the Q15 IIR lattice structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[out] *pDst points to the block of output data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_iir_lattice_q15(

-  const arm_iir_lattice_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-

-/**

- * @brief Initialization function for the Q15 IIR lattice filter.

- * @param[in] *S points to an instance of the fixed-point Q15 IIR lattice structure.

- * @param[in] numStages  number of stages in the filter.

- * @param[in] *pkCoeffs points to reflection coefficient buffer.  The array is of length numStages.

- * @param[in] *pvCoeffs points to ladder coefficient buffer.  The array is of length numStages+1.

- * @param[in] *pState points to state buffer.  The array is of length numStages+blockSize.

- * @param[in] blockSize number of samples to process per call.

- * @return none.

- */

-

-  void arm_iir_lattice_init_q15(

-  arm_iir_lattice_instance_q15 * S,

-  uint16_t numStages,

-  q15_t * pkCoeffs,

-  q15_t * pvCoeffs,

-  q15_t * pState,

-  uint32_t blockSize);

-

-  /**

-   * @brief Instance structure for the floating-point LMS filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;    /**< number of coefficients in the filter. */

-    float32_t *pState;   /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    float32_t *pCoeffs;  /**< points to the coefficient array. The array is of length numTaps. */

-    float32_t mu;        /**< step size that controls filter coefficient updates. */

-  } arm_lms_instance_f32;

-

-  /**

-   * @brief Processing function for floating-point LMS filter.

-   * @param[in]  *S points to an instance of the floating-point LMS filter structure.

-   * @param[in]  *pSrc points to the block of input data.

-   * @param[in]  *pRef points to the block of reference data.

-   * @param[out] *pOut points to the block of output data.

-   * @param[out] *pErr points to the block of error data.

-   * @param[in]  blockSize number of samples to process.

-   * @return     none.

-   */

-

-  void arm_lms_f32(

-  const arm_lms_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pRef,

-  float32_t * pOut,

-  float32_t * pErr,

-  uint32_t blockSize);

-

-  /**

-   * @brief Initialization function for floating-point LMS filter.

-   * @param[in] *S points to an instance of the floating-point LMS filter structure.

-   * @param[in] numTaps  number of filter coefficients.

-   * @param[in] *pCoeffs points to the coefficient buffer.

-   * @param[in] *pState points to state buffer.

-   * @param[in] mu step size that controls filter coefficient updates.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_lms_init_f32(

-  arm_lms_instance_f32 * S,

-  uint16_t numTaps,

-  float32_t * pCoeffs,

-  float32_t * pState,

-  float32_t mu,

-  uint32_t blockSize);

-

-  /**

-   * @brief Instance structure for the Q15 LMS filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;    /**< number of coefficients in the filter. */

-    q15_t *pState;       /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q15_t *pCoeffs;      /**< points to the coefficient array. The array is of length numTaps. */

-    q15_t mu;            /**< step size that controls filter coefficient updates. */

-    uint32_t postShift;  /**< bit shift applied to coefficients. */

-  } arm_lms_instance_q15;

-

-

-  /**

-   * @brief Initialization function for the Q15 LMS filter.

-   * @param[in] *S points to an instance of the Q15 LMS filter structure.

-   * @param[in] numTaps  number of filter coefficients.

-   * @param[in] *pCoeffs points to the coefficient buffer.

-   * @param[in] *pState points to the state buffer.

-   * @param[in] mu step size that controls filter coefficient updates.

-   * @param[in] blockSize number of samples to process.

-   * @param[in] postShift bit shift applied to coefficients.

-   * @return    none.

-   */

-

-  void arm_lms_init_q15(

-  arm_lms_instance_q15 * S,

-  uint16_t numTaps,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  q15_t mu,

-  uint32_t blockSize,

-  uint32_t postShift);

-

-  /**

-   * @brief Processing function for Q15 LMS filter.

-   * @param[in] *S points to an instance of the Q15 LMS filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[in] *pRef points to the block of reference data.

-   * @param[out] *pOut points to the block of output data.

-   * @param[out] *pErr points to the block of error data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_lms_q15(

-  const arm_lms_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pRef,

-  q15_t * pOut,

-  q15_t * pErr,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Instance structure for the Q31 LMS filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;    /**< number of coefficients in the filter. */

-    q31_t *pState;       /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q31_t *pCoeffs;      /**< points to the coefficient array. The array is of length numTaps. */

-    q31_t mu;            /**< step size that controls filter coefficient updates. */

-    uint32_t postShift;  /**< bit shift applied to coefficients. */

-

-  } arm_lms_instance_q31;

-

-  /**

-   * @brief Processing function for Q31 LMS filter.

-   * @param[in]  *S points to an instance of the Q15 LMS filter structure.

-   * @param[in]  *pSrc points to the block of input data.

-   * @param[in]  *pRef points to the block of reference data.

-   * @param[out] *pOut points to the block of output data.

-   * @param[out] *pErr points to the block of error data.

-   * @param[in]  blockSize number of samples to process.

-   * @return     none.

-   */

-

-  void arm_lms_q31(

-  const arm_lms_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pRef,

-  q31_t * pOut,

-  q31_t * pErr,

-  uint32_t blockSize);

-

-  /**

-   * @brief Initialization function for Q31 LMS filter.

-   * @param[in] *S points to an instance of the Q31 LMS filter structure.

-   * @param[in] numTaps  number of filter coefficients.

-   * @param[in] *pCoeffs points to coefficient buffer.

-   * @param[in] *pState points to state buffer.

-   * @param[in] mu step size that controls filter coefficient updates.

-   * @param[in] blockSize number of samples to process.

-   * @param[in] postShift bit shift applied to coefficients.

-   * @return none.

-   */

-

-  void arm_lms_init_q31(

-  arm_lms_instance_q31 * S,

-  uint16_t numTaps,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  q31_t mu,

-  uint32_t blockSize,

-  uint32_t postShift);

-

-  /**

-   * @brief Instance structure for the floating-point normalized LMS filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;     /**< number of coefficients in the filter. */

-    float32_t *pState;    /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    float32_t *pCoeffs;   /**< points to the coefficient array. The array is of length numTaps. */

-    float32_t mu;        /**< step size that control filter coefficient updates. */

-    float32_t energy;    /**< saves previous frame energy. */

-    float32_t x0;        /**< saves previous input sample. */

-  } arm_lms_norm_instance_f32;

-

-  /**

-   * @brief Processing function for floating-point normalized LMS filter.

-   * @param[in] *S points to an instance of the floating-point normalized LMS filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[in] *pRef points to the block of reference data.

-   * @param[out] *pOut points to the block of output data.

-   * @param[out] *pErr points to the block of error data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_lms_norm_f32(

-  arm_lms_norm_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pRef,

-  float32_t * pOut,

-  float32_t * pErr,

-  uint32_t blockSize);

-

-  /**

-   * @brief Initialization function for floating-point normalized LMS filter.

-   * @param[in] *S points to an instance of the floating-point LMS filter structure.

-   * @param[in] numTaps  number of filter coefficients.

-   * @param[in] *pCoeffs points to coefficient buffer.

-   * @param[in] *pState points to state buffer.

-   * @param[in] mu step size that controls filter coefficient updates.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_lms_norm_init_f32(

-  arm_lms_norm_instance_f32 * S,

-  uint16_t numTaps,

-  float32_t * pCoeffs,

-  float32_t * pState,

-  float32_t mu,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Instance structure for the Q31 normalized LMS filter.

-   */

-  typedef struct

-  {

-    uint16_t numTaps;     /**< number of coefficients in the filter. */

-    q31_t *pState;        /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q31_t *pCoeffs;       /**< points to the coefficient array. The array is of length numTaps. */

-    q31_t mu;             /**< step size that controls filter coefficient updates. */

-    uint8_t postShift;    /**< bit shift applied to coefficients. */

-    q31_t *recipTable;    /**< points to the reciprocal initial value table. */

-    q31_t energy;         /**< saves previous frame energy. */

-    q31_t x0;             /**< saves previous input sample. */

-  } arm_lms_norm_instance_q31;

-

-  /**

-   * @brief Processing function for Q31 normalized LMS filter.

-   * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[in] *pRef points to the block of reference data.

-   * @param[out] *pOut points to the block of output data.

-   * @param[out] *pErr points to the block of error data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_lms_norm_q31(

-  arm_lms_norm_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pRef,

-  q31_t * pOut,

-  q31_t * pErr,

-  uint32_t blockSize);

-

-  /**

-   * @brief Initialization function for Q31 normalized LMS filter.

-   * @param[in] *S points to an instance of the Q31 normalized LMS filter structure.

-   * @param[in] numTaps  number of filter coefficients.

-   * @param[in] *pCoeffs points to coefficient buffer.

-   * @param[in] *pState points to state buffer.

-   * @param[in] mu step size that controls filter coefficient updates.

-   * @param[in] blockSize number of samples to process.

-   * @param[in] postShift bit shift applied to coefficients.

-   * @return none.

-   */

-

-  void arm_lms_norm_init_q31(

-  arm_lms_norm_instance_q31 * S,

-  uint16_t numTaps,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  q31_t mu,

-  uint32_t blockSize,

-  uint8_t postShift);

-

-  /**

-   * @brief Instance structure for the Q15 normalized LMS filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;    /**< Number of coefficients in the filter. */

-    q15_t *pState;        /**< points to the state variable array. The array is of length numTaps+blockSize-1. */

-    q15_t *pCoeffs;       /**< points to the coefficient array. The array is of length numTaps. */

-    q15_t mu;            /**< step size that controls filter coefficient updates. */

-    uint8_t postShift;   /**< bit shift applied to coefficients. */

-    q15_t *recipTable;   /**< Points to the reciprocal initial value table. */

-    q15_t energy;        /**< saves previous frame energy. */

-    q15_t x0;            /**< saves previous input sample. */

-  } arm_lms_norm_instance_q15;

-

-  /**

-   * @brief Processing function for Q15 normalized LMS filter.

-   * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.

-   * @param[in] *pSrc points to the block of input data.

-   * @param[in] *pRef points to the block of reference data.

-   * @param[out] *pOut points to the block of output data.

-   * @param[out] *pErr points to the block of error data.

-   * @param[in] blockSize number of samples to process.

-   * @return none.

-   */

-

-  void arm_lms_norm_q15(

-  arm_lms_norm_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pRef,

-  q15_t * pOut,

-  q15_t * pErr,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief Initialization function for Q15 normalized LMS filter.

-   * @param[in] *S points to an instance of the Q15 normalized LMS filter structure.

-   * @param[in] numTaps  number of filter coefficients.

-   * @param[in] *pCoeffs points to coefficient buffer.

-   * @param[in] *pState points to state buffer.

-   * @param[in] mu step size that controls filter coefficient updates.

-   * @param[in] blockSize number of samples to process.

-   * @param[in] postShift bit shift applied to coefficients.

-   * @return none.

-   */

-

-  void arm_lms_norm_init_q15(

-  arm_lms_norm_instance_q15 * S,

-  uint16_t numTaps,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  q15_t mu,

-  uint32_t blockSize,

-  uint8_t postShift);

-

-  /**

-   * @brief Correlation of floating-point sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @return none.

-   */

-

-  void arm_correlate_f32(

-  float32_t * pSrcA,

-  uint32_t srcALen,

-  float32_t * pSrcB,

-  uint32_t srcBLen,

-  float32_t * pDst);

-

-

-   /**

-   * @brief Correlation of Q15 sequences

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @param[in]  *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @return none.

-   */

-  void arm_correlate_opt_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  q15_t * pScratch);

-

-

-  /**

-   * @brief Correlation of Q15 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @return none.

-   */

-

-  void arm_correlate_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst);

-

-  /**

-   * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @return none.

-   */

-

-  void arm_correlate_fast_q15(

-			       q15_t * pSrcA,

-			      uint32_t srcALen,

-			       q15_t * pSrcB,

-			      uint32_t srcBLen,

-			      q15_t * pDst);

-

-

-

-  /**

-   * @brief Correlation of Q15 sequences (fast version) for Cortex-M3 and Cortex-M4.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @param[in]  *pScratch points to scratch buffer of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @return none.

-   */

-

-  void arm_correlate_fast_opt_q15(

-  q15_t * pSrcA,

-  uint32_t srcALen,

-  q15_t * pSrcB,

-  uint32_t srcBLen,

-  q15_t * pDst,

-  q15_t * pScratch);

-

-  /**

-   * @brief Correlation of Q31 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @return none.

-   */

-

-  void arm_correlate_q31(

-  q31_t * pSrcA,

-  uint32_t srcALen,

-  q31_t * pSrcB,

-  uint32_t srcBLen,

-  q31_t * pDst);

-

-  /**

-   * @brief Correlation of Q31 sequences (fast version) for Cortex-M3 and Cortex-M4

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @return none.

-   */

-

-  void arm_correlate_fast_q31(

-  q31_t * pSrcA,

-  uint32_t srcALen,

-  q31_t * pSrcB,

-  uint32_t srcBLen,

-  q31_t * pDst);

-

-

-

- /**

-   * @brief Correlation of Q7 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @param[in]  *pScratch1 points to scratch buffer(of type q15_t) of size max(srcALen, srcBLen) + 2*min(srcALen, srcBLen) - 2.

-   * @param[in]  *pScratch2 points to scratch buffer (of type q15_t) of size min(srcALen, srcBLen).

-   * @return none.

-   */

-

-  void arm_correlate_opt_q7(

-  q7_t * pSrcA,

-  uint32_t srcALen,

-  q7_t * pSrcB,

-  uint32_t srcBLen,

-  q7_t * pDst,

-  q15_t * pScratch1,

-  q15_t * pScratch2);

-

-

-  /**

-   * @brief Correlation of Q7 sequences.

-   * @param[in] *pSrcA points to the first input sequence.

-   * @param[in] srcALen length of the first input sequence.

-   * @param[in] *pSrcB points to the second input sequence.

-   * @param[in] srcBLen length of the second input sequence.

-   * @param[out] *pDst points to the block of output data  Length 2 * max(srcALen, srcBLen) - 1.

-   * @return none.

-   */

-

-  void arm_correlate_q7(

-  q7_t * pSrcA,

-  uint32_t srcALen,

-  q7_t * pSrcB,

-  uint32_t srcBLen,

-  q7_t * pDst);

-

-

-  /**

-   * @brief Instance structure for the floating-point sparse FIR filter.

-   */

-  typedef struct

-  {

-    uint16_t numTaps;             /**< number of coefficients in the filter. */

-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */

-    float32_t *pState;            /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */

-    float32_t *pCoeffs;           /**< points to the coefficient array. The array is of length numTaps.*/

-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */

-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */

-  } arm_fir_sparse_instance_f32;

-

-  /**

-   * @brief Instance structure for the Q31 sparse FIR filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;             /**< number of coefficients in the filter. */

-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */

-    q31_t *pState;                /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */

-    q31_t *pCoeffs;               /**< points to the coefficient array. The array is of length numTaps.*/

-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */

-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */

-  } arm_fir_sparse_instance_q31;

-

-  /**

-   * @brief Instance structure for the Q15 sparse FIR filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;             /**< number of coefficients in the filter. */

-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */

-    q15_t *pState;                /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */

-    q15_t *pCoeffs;               /**< points to the coefficient array. The array is of length numTaps.*/

-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */

-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */

-  } arm_fir_sparse_instance_q15;

-

-  /**

-   * @brief Instance structure for the Q7 sparse FIR filter.

-   */

-

-  typedef struct

-  {

-    uint16_t numTaps;             /**< number of coefficients in the filter. */

-    uint16_t stateIndex;          /**< state buffer index.  Points to the oldest sample in the state buffer. */

-    q7_t *pState;                 /**< points to the state buffer array. The array is of length maxDelay+blockSize-1. */

-    q7_t *pCoeffs;                /**< points to the coefficient array. The array is of length numTaps.*/

-    uint16_t maxDelay;            /**< maximum offset specified by the pTapDelay array. */

-    int32_t *pTapDelay;           /**< points to the array of delay values.  The array is of length numTaps. */

-  } arm_fir_sparse_instance_q7;

-

-  /**

-   * @brief Processing function for the floating-point sparse FIR filter.

-   * @param[in]  *S          points to an instance of the floating-point sparse FIR structure.

-   * @param[in]  *pSrc       points to the block of input data.

-   * @param[out] *pDst       points to the block of output data

-   * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.

-   * @param[in]  blockSize   number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_sparse_f32(

-  arm_fir_sparse_instance_f32 * S,

-  float32_t * pSrc,

-  float32_t * pDst,

-  float32_t * pScratchIn,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the floating-point sparse FIR filter.

-   * @param[in,out] *S         points to an instance of the floating-point sparse FIR structure.

-   * @param[in]     numTaps    number of nonzero coefficients in the filter.

-   * @param[in]     *pCoeffs   points to the array of filter coefficients.

-   * @param[in]     *pState    points to the state buffer.

-   * @param[in]     *pTapDelay points to the array of offset times.

-   * @param[in]     maxDelay   maximum offset time supported.

-   * @param[in]     blockSize  number of samples that will be processed per block.

-   * @return none

-   */

-

-  void arm_fir_sparse_init_f32(

-  arm_fir_sparse_instance_f32 * S,

-  uint16_t numTaps,

-  float32_t * pCoeffs,

-  float32_t * pState,

-  int32_t * pTapDelay,

-  uint16_t maxDelay,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q31 sparse FIR filter.

-   * @param[in]  *S          points to an instance of the Q31 sparse FIR structure.

-   * @param[in]  *pSrc       points to the block of input data.

-   * @param[out] *pDst       points to the block of output data

-   * @param[in]  *pScratchIn points to a temporary buffer of size blockSize.

-   * @param[in]  blockSize   number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_sparse_q31(

-  arm_fir_sparse_instance_q31 * S,

-  q31_t * pSrc,

-  q31_t * pDst,

-  q31_t * pScratchIn,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q31 sparse FIR filter.

-   * @param[in,out] *S         points to an instance of the Q31 sparse FIR structure.

-   * @param[in]     numTaps    number of nonzero coefficients in the filter.

-   * @param[in]     *pCoeffs   points to the array of filter coefficients.

-   * @param[in]     *pState    points to the state buffer.

-   * @param[in]     *pTapDelay points to the array of offset times.

-   * @param[in]     maxDelay   maximum offset time supported.

-   * @param[in]     blockSize  number of samples that will be processed per block.

-   * @return none

-   */

-

-  void arm_fir_sparse_init_q31(

-  arm_fir_sparse_instance_q31 * S,

-  uint16_t numTaps,

-  q31_t * pCoeffs,

-  q31_t * pState,

-  int32_t * pTapDelay,

-  uint16_t maxDelay,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q15 sparse FIR filter.

-   * @param[in]  *S           points to an instance of the Q15 sparse FIR structure.

-   * @param[in]  *pSrc        points to the block of input data.

-   * @param[out] *pDst        points to the block of output data

-   * @param[in]  *pScratchIn  points to a temporary buffer of size blockSize.

-   * @param[in]  *pScratchOut points to a temporary buffer of size blockSize.

-   * @param[in]  blockSize    number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_sparse_q15(

-  arm_fir_sparse_instance_q15 * S,

-  q15_t * pSrc,

-  q15_t * pDst,

-  q15_t * pScratchIn,

-  q31_t * pScratchOut,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Initialization function for the Q15 sparse FIR filter.

-   * @param[in,out] *S         points to an instance of the Q15 sparse FIR structure.

-   * @param[in]     numTaps    number of nonzero coefficients in the filter.

-   * @param[in]     *pCoeffs   points to the array of filter coefficients.

-   * @param[in]     *pState    points to the state buffer.

-   * @param[in]     *pTapDelay points to the array of offset times.

-   * @param[in]     maxDelay   maximum offset time supported.

-   * @param[in]     blockSize  number of samples that will be processed per block.

-   * @return none

-   */

-

-  void arm_fir_sparse_init_q15(

-  arm_fir_sparse_instance_q15 * S,

-  uint16_t numTaps,

-  q15_t * pCoeffs,

-  q15_t * pState,

-  int32_t * pTapDelay,

-  uint16_t maxDelay,

-  uint32_t blockSize);

-

-  /**

-   * @brief Processing function for the Q7 sparse FIR filter.

-   * @param[in]  *S           points to an instance of the Q7 sparse FIR structure.

-   * @param[in]  *pSrc        points to the block of input data.

-   * @param[out] *pDst        points to the block of output data

-   * @param[in]  *pScratchIn  points to a temporary buffer of size blockSize.

-   * @param[in]  *pScratchOut points to a temporary buffer of size blockSize.

-   * @param[in]  blockSize    number of input samples to process per call.

-   * @return none.

-   */

-

-  void arm_fir_sparse_q7(

-  arm_fir_sparse_instance_q7 * S,

-  q7_t * pSrc,

-  q7_t * pDst,

-  q7_t * pScratchIn,

-  q31_t * pScratchOut,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Initialization function for the Q7 sparse FIR filter.

-   * @param[in,out] *S         points to an instance of the Q7 sparse FIR structure.

-   * @param[in]     numTaps    number of nonzero coefficients in the filter.

-   * @param[in]     *pCoeffs   points to the array of filter coefficients.

-   * @param[in]     *pState    points to the state buffer.

-   * @param[in]     *pTapDelay points to the array of offset times.

-   * @param[in]     maxDelay   maximum offset time supported.

-   * @param[in]     blockSize  number of samples that will be processed per block.

-   * @return none

-   */

-

-  void arm_fir_sparse_init_q7(

-  arm_fir_sparse_instance_q7 * S,

-  uint16_t numTaps,

-  q7_t * pCoeffs,

-  q7_t * pState,

-  int32_t * pTapDelay,

-  uint16_t maxDelay,

-  uint32_t blockSize);

-

-

-  /*

-   * @brief  Floating-point sin_cos function.

-   * @param[in]  theta    input value in degrees

-   * @param[out] *pSinVal points to the processed sine output.

-   * @param[out] *pCosVal points to the processed cos output.

-   * @return none.

-   */

-

-  void arm_sin_cos_f32(

-  float32_t theta,

-  float32_t * pSinVal,

-  float32_t * pCcosVal);

-

-  /*

-   * @brief  Q31 sin_cos function.

-   * @param[in]  theta    scaled input value in degrees

-   * @param[out] *pSinVal points to the processed sine output.

-   * @param[out] *pCosVal points to the processed cosine output.

-   * @return none.

-   */

-

-  void arm_sin_cos_q31(

-  q31_t theta,

-  q31_t * pSinVal,

-  q31_t * pCosVal);

-

-

-  /**

-   * @brief  Floating-point complex conjugate.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_conj_f32(

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q31 complex conjugate.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_conj_q31(

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q15 complex conjugate.

-   * @param[in]  *pSrc points to the input vector

-   * @param[out]  *pDst points to the output vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_conj_q15(

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t numSamples);

-

-

-

-  /**

-   * @brief  Floating-point complex magnitude squared

-   * @param[in]  *pSrc points to the complex input vector

-   * @param[out]  *pDst points to the real output vector

-   * @param[in]  numSamples number of complex samples in the input vector

-   * @return none.

-   */

-

-  void arm_cmplx_mag_squared_f32(

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q31 complex magnitude squared

-   * @param[in]  *pSrc points to the complex input vector

-   * @param[out]  *pDst points to the real output vector

-   * @param[in]  numSamples number of complex samples in the input vector

-   * @return none.

-   */

-

-  void arm_cmplx_mag_squared_q31(

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q15 complex magnitude squared

-   * @param[in]  *pSrc points to the complex input vector

-   * @param[out]  *pDst points to the real output vector

-   * @param[in]  numSamples number of complex samples in the input vector

-   * @return none.

-   */

-

-  void arm_cmplx_mag_squared_q15(

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t numSamples);

-

-

- /**

-   * @ingroup groupController

-   */

-

-  /**

-   * @defgroup PID PID Motor Control

-   *

-   * A Proportional Integral Derivative (PID) controller is a generic feedback control

-   * loop mechanism widely used in industrial control systems.

-   * A PID controller is the most commonly used type of feedback controller.

-   *

-   * This set of functions implements (PID) controllers

-   * for Q15, Q31, and floating-point data types.  The functions operate on a single sample

-   * of data and each call to the function returns a single processed value.

-   * <code>S</code> points to an instance of the PID control data structure.  <code>in</code>

-   * is the input sample value. The functions return the output value.

-   *

-   * \par Algorithm:

-   * <pre>

-   *    y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]

-   *    A0 = Kp + Ki + Kd

-   *    A1 = (-Kp ) - (2 * Kd )

-   *    A2 = Kd  </pre>

-   *

-   * \par

-   * where \c Kp is proportional constant, \c Ki is Integral constant and \c Kd is Derivative constant

-   *

-   * \par

-   * \image html PID.gif "Proportional Integral Derivative Controller"

-   *

-   * \par

-   * The PID controller calculates an "error" value as the difference between

-   * the measured output and the reference input.

-   * The controller attempts to minimize the error by adjusting the process control inputs.

-   * The proportional value determines the reaction to the current error,

-   * the integral value determines the reaction based on the sum of recent errors,

-   * and the derivative value determines the reaction based on the rate at which the error has been changing.

-   *

-   * \par Instance Structure

-   * The Gains A0, A1, A2 and state variables for a PID controller are stored together in an instance data structure.

-   * A separate instance structure must be defined for each PID Controller.

-   * There are separate instance structure declarations for each of the 3 supported data types.

-   *

-   * \par Reset Functions

-   * There is also an associated reset function for each data type which clears the state array.

-   *

-   * \par Initialization Functions

-   * There is also an associated initialization function for each data type.

-   * The initialization function performs the following operations:

-   * - Initializes the Gains A0, A1, A2 from Kp,Ki, Kd gains.

-   * - Zeros out the values in the state buffer.

-   *

-   * \par

-   * Instance structure cannot be placed into a const data section and it is recommended to use the initialization function.

-   *

-   * \par Fixed-Point Behavior

-   * Care must be taken when using the fixed-point versions of the PID Controller functions.

-   * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.

-   * Refer to the function specific documentation below for usage guidelines.

-   */

-

-  /**

-   * @addtogroup PID

-   * @{

-   */

-

-  /**

-   * @brief  Process function for the floating-point PID Control.

-   * @param[in,out] *S is an instance of the floating-point PID Control structure

-   * @param[in] in input sample to process

-   * @return out processed output sample.

-   */

-

-

-  static __INLINE float32_t arm_pid_f32(

-  arm_pid_instance_f32 * S,

-  float32_t in)

-  {

-    float32_t out;

-

-    /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2]  */

-    out = (S->A0 * in) +

-      (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]);

-

-    /* Update state */

-    S->state[1] = S->state[0];

-    S->state[0] = in;

-    S->state[2] = out;

-

-    /* return to application */

-    return (out);

-

-  }

-

-  /**

-   * @brief  Process function for the Q31 PID Control.

-   * @param[in,out] *S points to an instance of the Q31 PID Control structure

-   * @param[in] in input sample to process

-   * @return out processed output sample.

-   *

-   * <b>Scaling and Overflow Behavior:</b>

-   * \par

-   * The function is implemented using an internal 64-bit accumulator.

-   * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.

-   * Thus, if the accumulator result overflows it wraps around rather than clip.

-   * In order to avoid overflows completely the input signal must be scaled down by 2 bits as there are four additions.

-   * After all multiply-accumulates are performed, the 2.62 accumulator is truncated to 1.32 format and then saturated to 1.31 format.

-   */

-

-  static __INLINE q31_t arm_pid_q31(

-  arm_pid_instance_q31 * S,

-  q31_t in)

-  {

-    q63_t acc;

-    q31_t out;

-

-    /* acc = A0 * x[n]  */

-    acc = (q63_t) S->A0 * in;

-

-    /* acc += A1 * x[n-1] */

-    acc += (q63_t) S->A1 * S->state[0];

-

-    /* acc += A2 * x[n-2]  */

-    acc += (q63_t) S->A2 * S->state[1];

-

-    /* convert output to 1.31 format to add y[n-1] */

-    out = (q31_t) (acc >> 31u);

-

-    /* out += y[n-1] */

-    out += S->state[2];

-

-    /* Update state */

-    S->state[1] = S->state[0];

-    S->state[0] = in;

-    S->state[2] = out;

-

-    /* return to application */

-    return (out);

-

-  }

-

-  /**

-   * @brief  Process function for the Q15 PID Control.

-   * @param[in,out] *S points to an instance of the Q15 PID Control structure

-   * @param[in] in input sample to process

-   * @return out processed output sample.

-   *

-   * <b>Scaling and Overflow Behavior:</b>

-   * \par

-   * The function is implemented using a 64-bit internal accumulator.

-   * Both Gains and state variables are represented in 1.15 format and multiplications yield a 2.30 result.

-   * The 2.30 intermediate results are accumulated in a 64-bit accumulator in 34.30 format.

-   * There is no risk of internal overflow with this approach and the full precision of intermediate multiplications is preserved.

-   * After all additions have been performed, the accumulator is truncated to 34.15 format by discarding low 15 bits.

-   * Lastly, the accumulator is saturated to yield a result in 1.15 format.

-   */

-

-  static __INLINE q15_t arm_pid_q15(

-  arm_pid_instance_q15 * S,

-  q15_t in)

-  {

-    q63_t acc;

-    q15_t out;

-

-#ifndef ARM_MATH_CM0_FAMILY

-    __SIMD32_TYPE *vstate;

-

-    /* Implementation of PID controller */

-

-    /* acc = A0 * x[n]  */

-    acc = (q31_t) __SMUAD(S->A0, in);

-

-    /* acc += A1 * x[n-1] + A2 * x[n-2]  */

-    vstate = __SIMD32_CONST(S->state);

-    acc = __SMLALD(S->A1, (q31_t) *vstate, acc);

-

-#else

-    /* acc = A0 * x[n]  */

-    acc = ((q31_t) S->A0) * in;

-

-    /* acc += A1 * x[n-1] + A2 * x[n-2]  */

-    acc += (q31_t) S->A1 * S->state[0];

-    acc += (q31_t) S->A2 * S->state[1];

-

-#endif

-

-    /* acc += y[n-1] */

-    acc += (q31_t) S->state[2] << 15;

-

-    /* saturate the output */

-    out = (q15_t) (__SSAT((acc >> 15), 16));

-

-    /* Update state */

-    S->state[1] = S->state[0];

-    S->state[0] = in;

-    S->state[2] = out;

-

-    /* return to application */

-    return (out);

-

-  }

-

-  /**

-   * @} end of PID group

-   */

-

-

-  /**

-   * @brief Floating-point matrix inverse.

-   * @param[in]  *src points to the instance of the input floating-point matrix structure.

-   * @param[out] *dst points to the instance of the output floating-point matrix structure.

-   * @return The function returns ARM_MATH_SIZE_MISMATCH, if the dimensions do not match.

-   * If the input matrix is singular (does not have an inverse), then the algorithm terminates and returns error status ARM_MATH_SINGULAR.

-   */

-

-  arm_status arm_mat_inverse_f32(

-  const arm_matrix_instance_f32 * src,

-  arm_matrix_instance_f32 * dst);

-

-

-

-  /**

-   * @ingroup groupController

-   */

-

-

-  /**

-   * @defgroup clarke Vector Clarke Transform

-   * Forward Clarke transform converts the instantaneous stator phases into a two-coordinate time invariant vector.

-   * Generally the Clarke transform uses three-phase currents <code>Ia, Ib and Ic</code> to calculate currents

-   * in the two-phase orthogonal stator axis <code>Ialpha</code> and <code>Ibeta</code>.

-   * When <code>Ialpha</code> is superposed with <code>Ia</code> as shown in the figure below

-   * \image html clarke.gif Stator current space vector and its components in (a,b).

-   * and <code>Ia + Ib + Ic = 0</code>, in this condition <code>Ialpha</code> and <code>Ibeta</code>

-   * can be calculated using only <code>Ia</code> and <code>Ib</code>.

-   *

-   * The function operates on a single sample of data and each call to the function returns the processed output.

-   * The library provides separate functions for Q31 and floating-point data types.

-   * \par Algorithm

-   * \image html clarkeFormula.gif

-   * where <code>Ia</code> and <code>Ib</code> are the instantaneous stator phases and

-   * <code>pIalpha</code> and <code>pIbeta</code> are the two coordinates of time invariant vector.

-   * \par Fixed-Point Behavior

-   * Care must be taken when using the Q31 version of the Clarke transform.

-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.

-   * Refer to the function specific documentation below for usage guidelines.

-   */

-

-  /**

-   * @addtogroup clarke

-   * @{

-   */

-

-  /**

-   *

-   * @brief  Floating-point Clarke transform

-   * @param[in]       Ia       input three-phase coordinate <code>a</code>

-   * @param[in]       Ib       input three-phase coordinate <code>b</code>

-   * @param[out]      *pIalpha points to output two-phase orthogonal vector axis alpha

-   * @param[out]      *pIbeta  points to output two-phase orthogonal vector axis beta

-   * @return none.

-   */

-

-  static __INLINE void arm_clarke_f32(

-  float32_t Ia,

-  float32_t Ib,

-  float32_t * pIalpha,

-  float32_t * pIbeta)

-  {

-    /* Calculate pIalpha using the equation, pIalpha = Ia */

-    *pIalpha = Ia;

-

-    /* Calculate pIbeta using the equation, pIbeta = (1/sqrt(3)) * Ia + (2/sqrt(3)) * Ib */

-    *pIbeta =

-      ((float32_t) 0.57735026919 * Ia + (float32_t) 1.15470053838 * Ib);

-

-  }

-

-  /**

-   * @brief  Clarke transform for Q31 version

-   * @param[in]       Ia       input three-phase coordinate <code>a</code>

-   * @param[in]       Ib       input three-phase coordinate <code>b</code>

-   * @param[out]      *pIalpha points to output two-phase orthogonal vector axis alpha

-   * @param[out]      *pIbeta  points to output two-phase orthogonal vector axis beta

-   * @return none.

-   *

-   * <b>Scaling and Overflow Behavior:</b>

-   * \par

-   * The function is implemented using an internal 32-bit accumulator.

-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.

-   * There is saturation on the addition, hence there is no risk of overflow.

-   */

-

-  static __INLINE void arm_clarke_q31(

-  q31_t Ia,

-  q31_t Ib,

-  q31_t * pIalpha,

-  q31_t * pIbeta)

-  {

-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */

-

-    /* Calculating pIalpha from Ia by equation pIalpha = Ia */

-    *pIalpha = Ia;

-

-    /* Intermediate product is calculated by (1/(sqrt(3)) * Ia) */

-    product1 = (q31_t) (((q63_t) Ia * 0x24F34E8B) >> 30);

-

-    /* Intermediate product is calculated by (2/sqrt(3) * Ib) */

-    product2 = (q31_t) (((q63_t) Ib * 0x49E69D16) >> 30);

-

-    /* pIbeta is calculated by adding the intermediate products */

-    *pIbeta = __QADD(product1, product2);

-  }

-

-  /**

-   * @} end of clarke group

-   */

-

-  /**

-   * @brief  Converts the elements of the Q7 vector to Q31 vector.

-   * @param[in]  *pSrc     input pointer

-   * @param[out]  *pDst    output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_q7_to_q31(

-  q7_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-

-

-

-  /**

-   * @ingroup groupController

-   */

-

-  /**

-   * @defgroup inv_clarke Vector Inverse Clarke Transform

-   * Inverse Clarke transform converts the two-coordinate time invariant vector into instantaneous stator phases.

-   *

-   * The function operates on a single sample of data and each call to the function returns the processed output.

-   * The library provides separate functions for Q31 and floating-point data types.

-   * \par Algorithm

-   * \image html clarkeInvFormula.gif

-   * where <code>pIa</code> and <code>pIb</code> are the instantaneous stator phases and

-   * <code>Ialpha</code> and <code>Ibeta</code> are the two coordinates of time invariant vector.

-   * \par Fixed-Point Behavior

-   * Care must be taken when using the Q31 version of the Clarke transform.

-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.

-   * Refer to the function specific documentation below for usage guidelines.

-   */

-

-  /**

-   * @addtogroup inv_clarke

-   * @{

-   */

-

-   /**

-   * @brief  Floating-point Inverse Clarke transform

-   * @param[in]       Ialpha  input two-phase orthogonal vector axis alpha

-   * @param[in]       Ibeta   input two-phase orthogonal vector axis beta

-   * @param[out]      *pIa    points to output three-phase coordinate <code>a</code>

-   * @param[out]      *pIb    points to output three-phase coordinate <code>b</code>

-   * @return none.

-   */

-

-

-  static __INLINE void arm_inv_clarke_f32(

-  float32_t Ialpha,

-  float32_t Ibeta,

-  float32_t * pIa,

-  float32_t * pIb)

-  {

-    /* Calculating pIa from Ialpha by equation pIa = Ialpha */

-    *pIa = Ialpha;

-

-    /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */

-    *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

-

-  }

-

-  /**

-   * @brief  Inverse Clarke transform for Q31 version

-   * @param[in]       Ialpha  input two-phase orthogonal vector axis alpha

-   * @param[in]       Ibeta   input two-phase orthogonal vector axis beta

-   * @param[out]      *pIa    points to output three-phase coordinate <code>a</code>

-   * @param[out]      *pIb    points to output three-phase coordinate <code>b</code>

-   * @return none.

-   *

-   * <b>Scaling and Overflow Behavior:</b>

-   * \par

-   * The function is implemented using an internal 32-bit accumulator.

-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.

-   * There is saturation on the subtraction, hence there is no risk of overflow.

-   */

-

-  static __INLINE void arm_inv_clarke_q31(

-  q31_t Ialpha,

-  q31_t Ibeta,

-  q31_t * pIa,

-  q31_t * pIb)

-  {

-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */

-

-    /* Calculating pIa from Ialpha by equation pIa = Ialpha */

-    *pIa = Ialpha;

-

-    /* Intermediate product is calculated by (1/(2*sqrt(3)) * Ia) */

-    product1 = (q31_t) (((q63_t) (Ialpha) * (0x40000000)) >> 31);

-

-    /* Intermediate product is calculated by (1/sqrt(3) * pIb) */

-    product2 = (q31_t) (((q63_t) (Ibeta) * (0x6ED9EBA1)) >> 31);

-

-    /* pIb is calculated by subtracting the products */

-    *pIb = __QSUB(product2, product1);

-

-  }

-

-  /**

-   * @} end of inv_clarke group

-   */

-

-  /**

-   * @brief  Converts the elements of the Q7 vector to Q15 vector.

-   * @param[in]  *pSrc     input pointer

-   * @param[out] *pDst     output pointer

-   * @param[in]  blockSize number of samples to process

-   * @return none.

-   */

-  void arm_q7_to_q15(

-  q7_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-

-

-  /**

-   * @ingroup groupController

-   */

-

-  /**

-   * @defgroup park Vector Park Transform

-   *

-   * Forward Park transform converts the input two-coordinate vector to flux and torque components.

-   * The Park transform can be used to realize the transformation of the <code>Ialpha</code> and the <code>Ibeta</code> currents

-   * from the stationary to the moving reference frame and control the spatial relationship between

-   * the stator vector current and rotor flux vector.

-   * If we consider the d axis aligned with the rotor flux, the diagram below shows the

-   * current vector and the relationship from the two reference frames:

-   * \image html park.gif "Stator current space vector and its component in (a,b) and in the d,q rotating reference frame"

-   *

-   * The function operates on a single sample of data and each call to the function returns the processed output.

-   * The library provides separate functions for Q31 and floating-point data types.

-   * \par Algorithm

-   * \image html parkFormula.gif

-   * where <code>Ialpha</code> and <code>Ibeta</code> are the stator vector components,

-   * <code>pId</code> and <code>pIq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the

-   * cosine and sine values of theta (rotor flux position).

-   * \par Fixed-Point Behavior

-   * Care must be taken when using the Q31 version of the Park transform.

-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.

-   * Refer to the function specific documentation below for usage guidelines.

-   */

-

-  /**

-   * @addtogroup park

-   * @{

-   */

-

-  /**

-   * @brief Floating-point Park transform

-   * @param[in]       Ialpha input two-phase vector coordinate alpha

-   * @param[in]       Ibeta  input two-phase vector coordinate beta

-   * @param[out]      *pId   points to output	rotor reference frame d

-   * @param[out]      *pIq   points to output	rotor reference frame q

-   * @param[in]       sinVal sine value of rotation angle theta

-   * @param[in]       cosVal cosine value of rotation angle theta

-   * @return none.

-   *

-   * The function implements the forward Park transform.

-   *

-   */

-

-  static __INLINE void arm_park_f32(

-  float32_t Ialpha,

-  float32_t Ibeta,

-  float32_t * pId,

-  float32_t * pIq,

-  float32_t sinVal,

-  float32_t cosVal)

-  {

-    /* Calculate pId using the equation, pId = Ialpha * cosVal + Ibeta * sinVal */

-    *pId = Ialpha * cosVal + Ibeta * sinVal;

-

-    /* Calculate pIq using the equation, pIq = - Ialpha * sinVal + Ibeta * cosVal */

-    *pIq = -Ialpha * sinVal + Ibeta * cosVal;

-

-  }

-

-  /**

-   * @brief  Park transform for Q31 version

-   * @param[in]       Ialpha input two-phase vector coordinate alpha

-   * @param[in]       Ibeta  input two-phase vector coordinate beta

-   * @param[out]      *pId   points to output rotor reference frame d

-   * @param[out]      *pIq   points to output rotor reference frame q

-   * @param[in]       sinVal sine value of rotation angle theta

-   * @param[in]       cosVal cosine value of rotation angle theta

-   * @return none.

-   *

-   * <b>Scaling and Overflow Behavior:</b>

-   * \par

-   * The function is implemented using an internal 32-bit accumulator.

-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.

-   * There is saturation on the addition and subtraction, hence there is no risk of overflow.

-   */

-

-

-  static __INLINE void arm_park_q31(

-  q31_t Ialpha,

-  q31_t Ibeta,

-  q31_t * pId,

-  q31_t * pIq,

-  q31_t sinVal,

-  q31_t cosVal)

-  {

-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */

-    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */

-

-    /* Intermediate product is calculated by (Ialpha * cosVal) */

-    product1 = (q31_t) (((q63_t) (Ialpha) * (cosVal)) >> 31);

-

-    /* Intermediate product is calculated by (Ibeta * sinVal) */

-    product2 = (q31_t) (((q63_t) (Ibeta) * (sinVal)) >> 31);

-

-

-    /* Intermediate product is calculated by (Ialpha * sinVal) */

-    product3 = (q31_t) (((q63_t) (Ialpha) * (sinVal)) >> 31);

-

-    /* Intermediate product is calculated by (Ibeta * cosVal) */

-    product4 = (q31_t) (((q63_t) (Ibeta) * (cosVal)) >> 31);

-

-    /* Calculate pId by adding the two intermediate products 1 and 2 */

-    *pId = __QADD(product1, product2);

-

-    /* Calculate pIq by subtracting the two intermediate products 3 from 4 */

-    *pIq = __QSUB(product4, product3);

-  }

-

-  /**

-   * @} end of park group

-   */

-

-  /**

-   * @brief  Converts the elements of the Q7 vector to floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q7_to_float(

-  q7_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @ingroup groupController

-   */

-

-  /**

-   * @defgroup inv_park Vector Inverse Park transform

-   * Inverse Park transform converts the input flux and torque components to two-coordinate vector.

-   *

-   * The function operates on a single sample of data and each call to the function returns the processed output.

-   * The library provides separate functions for Q31 and floating-point data types.

-   * \par Algorithm

-   * \image html parkInvFormula.gif

-   * where <code>pIalpha</code> and <code>pIbeta</code> are the stator vector components,

-   * <code>Id</code> and <code>Iq</code> are rotor vector components and <code>cosVal</code> and <code>sinVal</code> are the

-   * cosine and sine values of theta (rotor flux position).

-   * \par Fixed-Point Behavior

-   * Care must be taken when using the Q31 version of the Park transform.

-   * In particular, the overflow and saturation behavior of the accumulator used must be considered.

-   * Refer to the function specific documentation below for usage guidelines.

-   */

-

-  /**

-   * @addtogroup inv_park

-   * @{

-   */

-

-   /**

-   * @brief  Floating-point Inverse Park transform

-   * @param[in]       Id        input coordinate of rotor reference frame d

-   * @param[in]       Iq        input coordinate of rotor reference frame q

-   * @param[out]      *pIalpha  points to output two-phase orthogonal vector axis alpha

-   * @param[out]      *pIbeta   points to output two-phase orthogonal vector axis beta

-   * @param[in]       sinVal    sine value of rotation angle theta

-   * @param[in]       cosVal    cosine value of rotation angle theta

-   * @return none.

-   */

-

-  static __INLINE void arm_inv_park_f32(

-  float32_t Id,

-  float32_t Iq,

-  float32_t * pIalpha,

-  float32_t * pIbeta,

-  float32_t sinVal,

-  float32_t cosVal)

-  {

-    /* Calculate pIalpha using the equation, pIalpha = Id * cosVal - Iq * sinVal */

-    *pIalpha = Id * cosVal - Iq * sinVal;

-

-    /* Calculate pIbeta using the equation, pIbeta = Id * sinVal + Iq * cosVal */

-    *pIbeta = Id * sinVal + Iq * cosVal;

-

-  }

-

-

-  /**

-   * @brief  Inverse Park transform for	Q31 version

-   * @param[in]       Id        input coordinate of rotor reference frame d

-   * @param[in]       Iq        input coordinate of rotor reference frame q

-   * @param[out]      *pIalpha  points to output two-phase orthogonal vector axis alpha

-   * @param[out]      *pIbeta   points to output two-phase orthogonal vector axis beta

-   * @param[in]       sinVal    sine value of rotation angle theta

-   * @param[in]       cosVal    cosine value of rotation angle theta

-   * @return none.

-   *

-   * <b>Scaling and Overflow Behavior:</b>

-   * \par

-   * The function is implemented using an internal 32-bit accumulator.

-   * The accumulator maintains 1.31 format by truncating lower 31 bits of the intermediate multiplication in 2.62 format.

-   * There is saturation on the addition, hence there is no risk of overflow.

-   */

-

-

-  static __INLINE void arm_inv_park_q31(

-  q31_t Id,

-  q31_t Iq,

-  q31_t * pIalpha,

-  q31_t * pIbeta,

-  q31_t sinVal,

-  q31_t cosVal)

-  {

-    q31_t product1, product2;                    /* Temporary variables used to store intermediate results */

-    q31_t product3, product4;                    /* Temporary variables used to store intermediate results */

-

-    /* Intermediate product is calculated by (Id * cosVal) */

-    product1 = (q31_t) (((q63_t) (Id) * (cosVal)) >> 31);

-

-    /* Intermediate product is calculated by (Iq * sinVal) */

-    product2 = (q31_t) (((q63_t) (Iq) * (sinVal)) >> 31);

-

-

-    /* Intermediate product is calculated by (Id * sinVal) */

-    product3 = (q31_t) (((q63_t) (Id) * (sinVal)) >> 31);

-

-    /* Intermediate product is calculated by (Iq * cosVal) */

-    product4 = (q31_t) (((q63_t) (Iq) * (cosVal)) >> 31);

-

-    /* Calculate pIalpha by using the two intermediate products 1 and 2 */

-    *pIalpha = __QSUB(product1, product2);

-

-    /* Calculate pIbeta by using the two intermediate products 3 and 4 */

-    *pIbeta = __QADD(product4, product3);

-

-  }

-

-  /**

-   * @} end of Inverse park group

-   */

-

-

-  /**

-   * @brief  Converts the elements of the Q31 vector to floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q31_to_float(

-  q31_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @ingroup groupInterpolation

-   */

-

-  /**

-   * @defgroup LinearInterpolate Linear Interpolation

-   *

-   * Linear interpolation is a method of curve fitting using linear polynomials.

-   * Linear interpolation works by effectively drawing a straight line between two neighboring samples and returning the appropriate point along that line

-   *

-   * \par

-   * \image html LinearInterp.gif "Linear interpolation"

-   *

-   * \par

-   * A  Linear Interpolate function calculates an output value(y), for the input(x)

-   * using linear interpolation of the input values x0, x1( nearest input values) and the output values y0 and y1(nearest output values)

-   *

-   * \par Algorithm:

-   * <pre>

-   *       y = y0 + (x - x0) * ((y1 - y0)/(x1-x0))

-   *       where x0, x1 are nearest values of input x

-   *             y0, y1 are nearest values to output y

-   * </pre>

-   *

-   * \par

-   * This set of functions implements Linear interpolation process

-   * for Q7, Q15, Q31, and floating-point data types.  The functions operate on a single

-   * sample of data and each call to the function returns a single processed value.

-   * <code>S</code> points to an instance of the Linear Interpolate function data structure.

-   * <code>x</code> is the input sample value. The functions returns the output value.

-   *

-   * \par

-   * if x is outside of the table boundary, Linear interpolation returns first value of the table

-   * if x is below input range and returns last value of table if x is above range.

-   */

-

-  /**

-   * @addtogroup LinearInterpolate

-   * @{

-   */

-

-  /**

-   * @brief  Process function for the floating-point Linear Interpolation Function.

-   * @param[in,out] *S is an instance of the floating-point Linear Interpolation structure

-   * @param[in] x input sample to process

-   * @return y processed output sample.

-   *

-   */

-

-  static __INLINE float32_t arm_linear_interp_f32(

-  arm_linear_interp_instance_f32 * S,

-  float32_t x)

-  {

-

-    float32_t y;

-    float32_t x0, x1;                            /* Nearest input values */

-    float32_t y0, y1;                            /* Nearest output values */

-    float32_t xSpacing = S->xSpacing;            /* spacing between input values */

-    int32_t i;                                   /* Index variable */

-    float32_t *pYData = S->pYData;               /* pointer to output table */

-

-    /* Calculation of index */

-    i = (int32_t) ((x - S->x1) / xSpacing);

-

-    if(i < 0)

-    {

-      /* Iniatilize output for below specified range as least output value of table */

-      y = pYData[0];

-    }

-    else if((uint32_t)i >= S->nValues)

-    {

-      /* Iniatilize output for above specified range as last output value of table */

-      y = pYData[S->nValues - 1];

-    }

-    else

-    {

-      /* Calculation of nearest input values */

-      x0 = S->x1 + i * xSpacing;

-      x1 = S->x1 + (i + 1) * xSpacing;

-

-      /* Read of nearest output values */

-      y0 = pYData[i];

-      y1 = pYData[i + 1];

-

-      /* Calculation of output */

-      y = y0 + (x - x0) * ((y1 - y0) / (x1 - x0));

-

-    }

-

-    /* returns output value */

-    return (y);

-  }

-

-   /**

-   *

-   * @brief  Process function for the Q31 Linear Interpolation Function.

-   * @param[in] *pYData  pointer to Q31 Linear Interpolation table

-   * @param[in] x input sample to process

-   * @param[in] nValues number of table values

-   * @return y processed output sample.

-   *

-   * \par

-   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.

-   * This function can support maximum of table size 2^12.

-   *

-   */

-

-

-  static __INLINE q31_t arm_linear_interp_q31(

-  q31_t * pYData,

-  q31_t x,

-  uint32_t nValues)

-  {

-    q31_t y;                                     /* output */

-    q31_t y0, y1;                                /* Nearest output values */

-    q31_t fract;                                 /* fractional part */

-    int32_t index;                               /* Index to read nearest output values */

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    index = ((x & 0xFFF00000) >> 20);

-

-    if(index >= (int32_t)(nValues - 1))

-    {

-      return (pYData[nValues - 1]);

-    }

-    else if(index < 0)

-    {

-      return (pYData[0]);

-    }

-    else

-    {

-

-      /* 20 bits for the fractional part */

-      /* shift left by 11 to keep fract in 1.31 format */

-      fract = (x & 0x000FFFFF) << 11;

-

-      /* Read two nearest output values from the index in 1.31(q31) format */

-      y0 = pYData[index];

-      y1 = pYData[index + 1u];

-

-      /* Calculation of y0 * (1-fract) and y is in 2.30 format */

-      y = ((q31_t) ((q63_t) y0 * (0x7FFFFFFF - fract) >> 32));

-

-      /* Calculation of y0 * (1-fract) + y1 *fract and y is in 2.30 format */

-      y += ((q31_t) (((q63_t) y1 * fract) >> 32));

-

-      /* Convert y to 1.31 format */

-      return (y << 1u);

-

-    }

-

-  }

-

-  /**

-   *

-   * @brief  Process function for the Q15 Linear Interpolation Function.

-   * @param[in] *pYData  pointer to Q15 Linear Interpolation table

-   * @param[in] x input sample to process

-   * @param[in] nValues number of table values

-   * @return y processed output sample.

-   *

-   * \par

-   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.

-   * This function can support maximum of table size 2^12.

-   *

-   */

-

-

-  static __INLINE q15_t arm_linear_interp_q15(

-  q15_t * pYData,

-  q31_t x,

-  uint32_t nValues)

-  {

-    q63_t y;                                     /* output */

-    q15_t y0, y1;                                /* Nearest output values */

-    q31_t fract;                                 /* fractional part */

-    int32_t index;                               /* Index to read nearest output values */

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    index = ((x & 0xFFF00000) >> 20u);

-

-    if(index >= (int32_t)(nValues - 1))

-    {

-      return (pYData[nValues - 1]);

-    }

-    else if(index < 0)

-    {

-      return (pYData[0]);

-    }

-    else

-    {

-      /* 20 bits for the fractional part */

-      /* fract is in 12.20 format */

-      fract = (x & 0x000FFFFF);

-

-      /* Read two nearest output values from the index */

-      y0 = pYData[index];

-      y1 = pYData[index + 1u];

-

-      /* Calculation of y0 * (1-fract) and y is in 13.35 format */

-      y = ((q63_t) y0 * (0xFFFFF - fract));

-

-      /* Calculation of (y0 * (1-fract) + y1 * fract) and y is in 13.35 format */

-      y += ((q63_t) y1 * (fract));

-

-      /* convert y to 1.15 format */

-      return (y >> 20);

-    }

-

-

-  }

-

-  /**

-   *

-   * @brief  Process function for the Q7 Linear Interpolation Function.

-   * @param[in] *pYData  pointer to Q7 Linear Interpolation table

-   * @param[in] x input sample to process

-   * @param[in] nValues number of table values

-   * @return y processed output sample.

-   *

-   * \par

-   * Input sample <code>x</code> is in 12.20 format which contains 12 bits for table index and 20 bits for fractional part.

-   * This function can support maximum of table size 2^12.

-   */

-

-

-  static __INLINE q7_t arm_linear_interp_q7(

-  q7_t * pYData,

-  q31_t x,

-  uint32_t nValues)

-  {

-    q31_t y;                                     /* output */

-    q7_t y0, y1;                                 /* Nearest output values */

-    q31_t fract;                                 /* fractional part */

-    uint32_t index;                              /* Index to read nearest output values */

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    if (x < 0)

-    {

-      return (pYData[0]);

-    }

-    index = (x >> 20) & 0xfff;

-

-

-    if(index >= (nValues - 1))

-    {

-      return (pYData[nValues - 1]);

-    }

-    else

-    {

-

-      /* 20 bits for the fractional part */

-      /* fract is in 12.20 format */

-      fract = (x & 0x000FFFFF);

-

-      /* Read two nearest output values from the index and are in 1.7(q7) format */

-      y0 = pYData[index];

-      y1 = pYData[index + 1u];

-

-      /* Calculation of y0 * (1-fract ) and y is in 13.27(q27) format */

-      y = ((y0 * (0xFFFFF - fract)));

-

-      /* Calculation of y1 * fract + y0 * (1-fract) and y is in 13.27(q27) format */

-      y += (y1 * fract);

-

-      /* convert y to 1.7(q7) format */

-      return (y >> 20u);

-

-    }

-

-  }

-  /**

-   * @} end of LinearInterpolate group

-   */

-

-  /**

-   * @brief  Fast approximation to the trigonometric sine function for floating-point data.

-   * @param[in] x input value in radians.

-   * @return  sin(x).

-   */

-

-  float32_t arm_sin_f32(

-  float32_t x);

-

-  /**

-   * @brief  Fast approximation to the trigonometric sine function for Q31 data.

-   * @param[in] x Scaled input value in radians.

-   * @return  sin(x).

-   */

-

-  q31_t arm_sin_q31(

-  q31_t x);

-

-  /**

-   * @brief  Fast approximation to the trigonometric sine function for Q15 data.

-   * @param[in] x Scaled input value in radians.

-   * @return  sin(x).

-   */

-

-  q15_t arm_sin_q15(

-  q15_t x);

-

-  /**

-   * @brief  Fast approximation to the trigonometric cosine function for floating-point data.

-   * @param[in] x input value in radians.

-   * @return  cos(x).

-   */

-

-  float32_t arm_cos_f32(

-  float32_t x);

-

-  /**

-   * @brief Fast approximation to the trigonometric cosine function for Q31 data.

-   * @param[in] x Scaled input value in radians.

-   * @return  cos(x).

-   */

-

-  q31_t arm_cos_q31(

-  q31_t x);

-

-  /**

-   * @brief  Fast approximation to the trigonometric cosine function for Q15 data.

-   * @param[in] x Scaled input value in radians.

-   * @return  cos(x).

-   */

-

-  q15_t arm_cos_q15(

-  q15_t x);

-

-

-  /**

-   * @ingroup groupFastMath

-   */

-

-

-  /**

-   * @defgroup SQRT Square Root

-   *

-   * Computes the square root of a number.

-   * There are separate functions for Q15, Q31, and floating-point data types.

-   * The square root function is computed using the Newton-Raphson algorithm.

-   * This is an iterative algorithm of the form:

-   * <pre>

-   *      x1 = x0 - f(x0)/f'(x0)

-   * </pre>

-   * where <code>x1</code> is the current estimate,

-   * <code>x0</code> is the previous estimate, and

-   * <code>f'(x0)</code> is the derivative of <code>f()</code> evaluated at <code>x0</code>.

-   * For the square root function, the algorithm reduces to:

-   * <pre>

-   *     x0 = in/2                         [initial guess]

-   *     x1 = 1/2 * ( x0 + in / x0)        [each iteration]

-   * </pre>

-   */

-

-

-  /**

-   * @addtogroup SQRT

-   * @{

-   */

-

-  /**

-   * @brief  Floating-point square root function.

-   * @param[in]  in     input value.

-   * @param[out] *pOut  square root of input value.

-   * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if

-   * <code>in</code> is negative value and returns zero output for negative values.

-   */

-

-  static __INLINE arm_status arm_sqrt_f32(

-  float32_t in,

-  float32_t * pOut)

-  {

-    if(in > 0)

-    {

-

-//      #if __FPU_USED

-#if (__FPU_USED == 1) && defined ( __CC_ARM   )

-      *pOut = __sqrtf(in);

-#else

-      *pOut = sqrtf(in);

-#endif

-

-      return (ARM_MATH_SUCCESS);

-    }

-    else

-    {

-      *pOut = 0.0f;

-      return (ARM_MATH_ARGUMENT_ERROR);

-    }

-

-  }

-

-

-  /**

-   * @brief Q31 square root function.

-   * @param[in]   in    input value.  The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.

-   * @param[out]  *pOut square root of input value.

-   * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if

-   * <code>in</code> is negative value and returns zero output for negative values.

-   */

-  arm_status arm_sqrt_q31(

-  q31_t in,

-  q31_t * pOut);

-

-  /**

-   * @brief  Q15 square root function.

-   * @param[in]   in     input value.  The range of the input value is [0 +1) or 0x0000 to 0x7FFF.

-   * @param[out]  *pOut  square root of input value.

-   * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if

-   * <code>in</code> is negative value and returns zero output for negative values.

-   */

-  arm_status arm_sqrt_q15(

-  q15_t in,

-  q15_t * pOut);

-

-  /**

-   * @} end of SQRT group

-   */

-

-

-

-

-

-

-  /**

-   * @brief floating-point Circular write function.

-   */

-

-  static __INLINE void arm_circularWrite_f32(

-  int32_t * circBuffer,

-  int32_t L,

-  uint16_t * writeOffset,

-  int32_t bufferInc,

-  const int32_t * src,

-  int32_t srcInc,

-  uint32_t blockSize)

-  {

-    uint32_t i = 0u;

-    int32_t wOffset;

-

-    /* Copy the value of Index pointer that points

-     * to the current location where the input samples to be copied */

-    wOffset = *writeOffset;

-

-    /* Loop over the blockSize */

-    i = blockSize;

-

-    while(i > 0u)

-    {

-      /* copy the input sample to the circular buffer */

-      circBuffer[wOffset] = *src;

-

-      /* Update the input pointer */

-      src += srcInc;

-

-      /* Circularly update wOffset.  Watch out for positive and negative value */

-      wOffset += bufferInc;

-      if(wOffset >= L)

-        wOffset -= L;

-

-      /* Decrement the loop counter */

-      i--;

-    }

-

-    /* Update the index pointer */

-    *writeOffset = wOffset;

-  }

-

-

-

-  /**

-   * @brief floating-point Circular Read function.

-   */

-  static __INLINE void arm_circularRead_f32(

-  int32_t * circBuffer,

-  int32_t L,

-  int32_t * readOffset,

-  int32_t bufferInc,

-  int32_t * dst,

-  int32_t * dst_base,

-  int32_t dst_length,

-  int32_t dstInc,

-  uint32_t blockSize)

-  {

-    uint32_t i = 0u;

-    int32_t rOffset, dst_end;

-

-    /* Copy the value of Index pointer that points

-     * to the current location from where the input samples to be read */

-    rOffset = *readOffset;

-    dst_end = (int32_t) (dst_base + dst_length);

-

-    /* Loop over the blockSize */

-    i = blockSize;

-

-    while(i > 0u)

-    {

-      /* copy the sample from the circular buffer to the destination buffer */

-      *dst = circBuffer[rOffset];

-

-      /* Update the input pointer */

-      dst += dstInc;

-

-      if(dst == (int32_t *) dst_end)

-      {

-        dst = dst_base;

-      }

-

-      /* Circularly update rOffset.  Watch out for positive and negative value  */

-      rOffset += bufferInc;

-

-      if(rOffset >= L)

-      {

-        rOffset -= L;

-      }

-

-      /* Decrement the loop counter */

-      i--;

-    }

-

-    /* Update the index pointer */

-    *readOffset = rOffset;

-  }

-

-  /**

-   * @brief Q15 Circular write function.

-   */

-

-  static __INLINE void arm_circularWrite_q15(

-  q15_t * circBuffer,

-  int32_t L,

-  uint16_t * writeOffset,

-  int32_t bufferInc,

-  const q15_t * src,

-  int32_t srcInc,

-  uint32_t blockSize)

-  {

-    uint32_t i = 0u;

-    int32_t wOffset;

-

-    /* Copy the value of Index pointer that points

-     * to the current location where the input samples to be copied */

-    wOffset = *writeOffset;

-

-    /* Loop over the blockSize */

-    i = blockSize;

-

-    while(i > 0u)

-    {

-      /* copy the input sample to the circular buffer */

-      circBuffer[wOffset] = *src;

-

-      /* Update the input pointer */

-      src += srcInc;

-

-      /* Circularly update wOffset.  Watch out for positive and negative value */

-      wOffset += bufferInc;

-      if(wOffset >= L)

-        wOffset -= L;

-

-      /* Decrement the loop counter */

-      i--;

-    }

-

-    /* Update the index pointer */

-    *writeOffset = wOffset;

-  }

-

-

-

-  /**

-   * @brief Q15 Circular Read function.

-   */

-  static __INLINE void arm_circularRead_q15(

-  q15_t * circBuffer,

-  int32_t L,

-  int32_t * readOffset,

-  int32_t bufferInc,

-  q15_t * dst,

-  q15_t * dst_base,

-  int32_t dst_length,

-  int32_t dstInc,

-  uint32_t blockSize)

-  {

-    uint32_t i = 0;

-    int32_t rOffset, dst_end;

-

-    /* Copy the value of Index pointer that points

-     * to the current location from where the input samples to be read */

-    rOffset = *readOffset;

-

-    dst_end = (int32_t) (dst_base + dst_length);

-

-    /* Loop over the blockSize */

-    i = blockSize;

-

-    while(i > 0u)

-    {

-      /* copy the sample from the circular buffer to the destination buffer */

-      *dst = circBuffer[rOffset];

-

-      /* Update the input pointer */

-      dst += dstInc;

-

-      if(dst == (q15_t *) dst_end)

-      {

-        dst = dst_base;

-      }

-

-      /* Circularly update wOffset.  Watch out for positive and negative value */

-      rOffset += bufferInc;

-

-      if(rOffset >= L)

-      {

-        rOffset -= L;

-      }

-

-      /* Decrement the loop counter */

-      i--;

-    }

-

-    /* Update the index pointer */

-    *readOffset = rOffset;

-  }

-

-

-  /**

-   * @brief Q7 Circular write function.

-   */

-

-  static __INLINE void arm_circularWrite_q7(

-  q7_t * circBuffer,

-  int32_t L,

-  uint16_t * writeOffset,

-  int32_t bufferInc,

-  const q7_t * src,

-  int32_t srcInc,

-  uint32_t blockSize)

-  {

-    uint32_t i = 0u;

-    int32_t wOffset;

-

-    /* Copy the value of Index pointer that points

-     * to the current location where the input samples to be copied */

-    wOffset = *writeOffset;

-

-    /* Loop over the blockSize */

-    i = blockSize;

-

-    while(i > 0u)

-    {

-      /* copy the input sample to the circular buffer */

-      circBuffer[wOffset] = *src;

-

-      /* Update the input pointer */

-      src += srcInc;

-

-      /* Circularly update wOffset.  Watch out for positive and negative value */

-      wOffset += bufferInc;

-      if(wOffset >= L)

-        wOffset -= L;

-

-      /* Decrement the loop counter */

-      i--;

-    }

-

-    /* Update the index pointer */

-    *writeOffset = wOffset;

-  }

-

-

-

-  /**

-   * @brief Q7 Circular Read function.

-   */

-  static __INLINE void arm_circularRead_q7(

-  q7_t * circBuffer,

-  int32_t L,

-  int32_t * readOffset,

-  int32_t bufferInc,

-  q7_t * dst,

-  q7_t * dst_base,

-  int32_t dst_length,

-  int32_t dstInc,

-  uint32_t blockSize)

-  {

-    uint32_t i = 0;

-    int32_t rOffset, dst_end;

-

-    /* Copy the value of Index pointer that points

-     * to the current location from where the input samples to be read */

-    rOffset = *readOffset;

-

-    dst_end = (int32_t) (dst_base + dst_length);

-

-    /* Loop over the blockSize */

-    i = blockSize;

-

-    while(i > 0u)

-    {

-      /* copy the sample from the circular buffer to the destination buffer */

-      *dst = circBuffer[rOffset];

-

-      /* Update the input pointer */

-      dst += dstInc;

-

-      if(dst == (q7_t *) dst_end)

-      {

-        dst = dst_base;

-      }

-

-      /* Circularly update rOffset.  Watch out for positive and negative value */

-      rOffset += bufferInc;

-

-      if(rOffset >= L)

-      {

-        rOffset -= L;

-      }

-

-      /* Decrement the loop counter */

-      i--;

-    }

-

-    /* Update the index pointer */

-    *readOffset = rOffset;

-  }

-

-

-  /**

-   * @brief  Sum of the squares of the elements of a Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_power_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q63_t * pResult);

-

-  /**

-   * @brief  Sum of the squares of the elements of a floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_power_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult);

-

-  /**

-   * @brief  Sum of the squares of the elements of a Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_power_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q63_t * pResult);

-

-  /**

-   * @brief  Sum of the squares of the elements of a Q7 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_power_q7(

-  q7_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult);

-

-  /**

-   * @brief  Mean value of a Q7 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_mean_q7(

-  q7_t * pSrc,

-  uint32_t blockSize,

-  q7_t * pResult);

-

-  /**

-   * @brief  Mean value of a Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-  void arm_mean_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q15_t * pResult);

-

-  /**

-   * @brief  Mean value of a Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-  void arm_mean_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult);

-

-  /**

-   * @brief  Mean value of a floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-  void arm_mean_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult);

-

-  /**

-   * @brief  Variance of the elements of a floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_var_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult);

-

-  /**

-   * @brief  Variance of the elements of a Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_var_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q63_t * pResult);

-

-  /**

-   * @brief  Variance of the elements of a Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_var_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult);

-

-  /**

-   * @brief  Root Mean Square of the elements of a floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_rms_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult);

-

-  /**

-   * @brief  Root Mean Square of the elements of a Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_rms_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult);

-

-  /**

-   * @brief  Root Mean Square of the elements of a Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_rms_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q15_t * pResult);

-

-  /**

-   * @brief  Standard deviation of the elements of a floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_std_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult);

-

-  /**

-   * @brief  Standard deviation of the elements of a Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_std_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult);

-

-  /**

-   * @brief  Standard deviation of the elements of a Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output value.

-   * @return none.

-   */

-

-  void arm_std_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q15_t * pResult);

-

-  /**

-   * @brief  Floating-point complex magnitude

-   * @param[in]  *pSrc points to the complex input vector

-   * @param[out]  *pDst points to the real output vector

-   * @param[in]  numSamples number of complex samples in the input vector

-   * @return none.

-   */

-

-  void arm_cmplx_mag_f32(

-  float32_t * pSrc,

-  float32_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q31 complex magnitude

-   * @param[in]  *pSrc points to the complex input vector

-   * @param[out]  *pDst points to the real output vector

-   * @param[in]  numSamples number of complex samples in the input vector

-   * @return none.

-   */

-

-  void arm_cmplx_mag_q31(

-  q31_t * pSrc,

-  q31_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q15 complex magnitude

-   * @param[in]  *pSrc points to the complex input vector

-   * @param[out]  *pDst points to the real output vector

-   * @param[in]  numSamples number of complex samples in the input vector

-   * @return none.

-   */

-

-  void arm_cmplx_mag_q15(

-  q15_t * pSrc,

-  q15_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q15 complex dot product

-   * @param[in]  *pSrcA points to the first input vector

-   * @param[in]  *pSrcB points to the second input vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @param[out]  *realResult real part of the result returned here

-   * @param[out]  *imagResult imaginary part of the result returned here

-   * @return none.

-   */

-

-  void arm_cmplx_dot_prod_q15(

-  q15_t * pSrcA,

-  q15_t * pSrcB,

-  uint32_t numSamples,

-  q31_t * realResult,

-  q31_t * imagResult);

-

-  /**

-   * @brief  Q31 complex dot product

-   * @param[in]  *pSrcA points to the first input vector

-   * @param[in]  *pSrcB points to the second input vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @param[out]  *realResult real part of the result returned here

-   * @param[out]  *imagResult imaginary part of the result returned here

-   * @return none.

-   */

-

-  void arm_cmplx_dot_prod_q31(

-  q31_t * pSrcA,

-  q31_t * pSrcB,

-  uint32_t numSamples,

-  q63_t * realResult,

-  q63_t * imagResult);

-

-  /**

-   * @brief  Floating-point complex dot product

-   * @param[in]  *pSrcA points to the first input vector

-   * @param[in]  *pSrcB points to the second input vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @param[out]  *realResult real part of the result returned here

-   * @param[out]  *imagResult imaginary part of the result returned here

-   * @return none.

-   */

-

-  void arm_cmplx_dot_prod_f32(

-  float32_t * pSrcA,

-  float32_t * pSrcB,

-  uint32_t numSamples,

-  float32_t * realResult,

-  float32_t * imagResult);

-

-  /**

-   * @brief  Q15 complex-by-real multiplication

-   * @param[in]  *pSrcCmplx points to the complex input vector

-   * @param[in]  *pSrcReal points to the real input vector

-   * @param[out]  *pCmplxDst points to the complex output vector

-   * @param[in]  numSamples number of samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_mult_real_q15(

-  q15_t * pSrcCmplx,

-  q15_t * pSrcReal,

-  q15_t * pCmplxDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q31 complex-by-real multiplication

-   * @param[in]  *pSrcCmplx points to the complex input vector

-   * @param[in]  *pSrcReal points to the real input vector

-   * @param[out]  *pCmplxDst points to the complex output vector

-   * @param[in]  numSamples number of samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_mult_real_q31(

-  q31_t * pSrcCmplx,

-  q31_t * pSrcReal,

-  q31_t * pCmplxDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Floating-point complex-by-real multiplication

-   * @param[in]  *pSrcCmplx points to the complex input vector

-   * @param[in]  *pSrcReal points to the real input vector

-   * @param[out]  *pCmplxDst points to the complex output vector

-   * @param[in]  numSamples number of samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_mult_real_f32(

-  float32_t * pSrcCmplx,

-  float32_t * pSrcReal,

-  float32_t * pCmplxDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Minimum value of a Q7 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *result is output pointer

-   * @param[in]  index is the array index of the minimum value in the input buffer.

-   * @return none.

-   */

-

-  void arm_min_q7(

-  q7_t * pSrc,

-  uint32_t blockSize,

-  q7_t * result,

-  uint32_t * index);

-

-  /**

-   * @brief  Minimum value of a Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output pointer

-   * @param[in]  *pIndex is the array index of the minimum value in the input buffer.

-   * @return none.

-   */

-

-  void arm_min_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q15_t * pResult,

-  uint32_t * pIndex);

-

-  /**

-   * @brief  Minimum value of a Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output pointer

-   * @param[out]  *pIndex is the array index of the minimum value in the input buffer.

-   * @return none.

-   */

-  void arm_min_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult,

-  uint32_t * pIndex);

-

-  /**

-   * @brief  Minimum value of a floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @param[out]  *pResult is output pointer

-   * @param[out]  *pIndex is the array index of the minimum value in the input buffer.

-   * @return none.

-   */

-

-  void arm_min_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult,

-  uint32_t * pIndex);

-

-/**

- * @brief Maximum value of a Q7 vector.

- * @param[in]       *pSrc points to the input buffer

- * @param[in]       blockSize length of the input vector

- * @param[out]      *pResult maximum value returned here

- * @param[out]      *pIndex index of maximum value returned here

- * @return none.

- */

-

-  void arm_max_q7(

-  q7_t * pSrc,

-  uint32_t blockSize,

-  q7_t * pResult,

-  uint32_t * pIndex);

-

-/**

- * @brief Maximum value of a Q15 vector.

- * @param[in]       *pSrc points to the input buffer

- * @param[in]       blockSize length of the input vector

- * @param[out]      *pResult maximum value returned here

- * @param[out]      *pIndex index of maximum value returned here

- * @return none.

- */

-

-  void arm_max_q15(

-  q15_t * pSrc,

-  uint32_t blockSize,

-  q15_t * pResult,

-  uint32_t * pIndex);

-

-/**

- * @brief Maximum value of a Q31 vector.

- * @param[in]       *pSrc points to the input buffer

- * @param[in]       blockSize length of the input vector

- * @param[out]      *pResult maximum value returned here

- * @param[out]      *pIndex index of maximum value returned here

- * @return none.

- */

-

-  void arm_max_q31(

-  q31_t * pSrc,

-  uint32_t blockSize,

-  q31_t * pResult,

-  uint32_t * pIndex);

-

-/**

- * @brief Maximum value of a floating-point vector.

- * @param[in]       *pSrc points to the input buffer

- * @param[in]       blockSize length of the input vector

- * @param[out]      *pResult maximum value returned here

- * @param[out]      *pIndex index of maximum value returned here

- * @return none.

- */

-

-  void arm_max_f32(

-  float32_t * pSrc,

-  uint32_t blockSize,

-  float32_t * pResult,

-  uint32_t * pIndex);

-

-  /**

-   * @brief  Q15 complex-by-complex multiplication

-   * @param[in]  *pSrcA points to the first input vector

-   * @param[in]  *pSrcB points to the second input vector

-   * @param[out]  *pDst  points to the output vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_mult_cmplx_q15(

-  q15_t * pSrcA,

-  q15_t * pSrcB,

-  q15_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Q31 complex-by-complex multiplication

-   * @param[in]  *pSrcA points to the first input vector

-   * @param[in]  *pSrcB points to the second input vector

-   * @param[out]  *pDst  points to the output vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_mult_cmplx_q31(

-  q31_t * pSrcA,

-  q31_t * pSrcB,

-  q31_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief  Floating-point complex-by-complex multiplication

-   * @param[in]  *pSrcA points to the first input vector

-   * @param[in]  *pSrcB points to the second input vector

-   * @param[out]  *pDst  points to the output vector

-   * @param[in]  numSamples number of complex samples in each vector

-   * @return none.

-   */

-

-  void arm_cmplx_mult_cmplx_f32(

-  float32_t * pSrcA,

-  float32_t * pSrcB,

-  float32_t * pDst,

-  uint32_t numSamples);

-

-  /**

-   * @brief Converts the elements of the floating-point vector to Q31 vector.

-   * @param[in]       *pSrc points to the floating-point input vector

-   * @param[out]      *pDst points to the Q31 output vector

-   * @param[in]       blockSize length of the input vector

-   * @return none.

-   */

-  void arm_float_to_q31(

-  float32_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Converts the elements of the floating-point vector to Q15 vector.

-   * @param[in]       *pSrc points to the floating-point input vector

-   * @param[out]      *pDst points to the Q15 output vector

-   * @param[in]       blockSize length of the input vector

-   * @return          none

-   */

-  void arm_float_to_q15(

-  float32_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief Converts the elements of the floating-point vector to Q7 vector.

-   * @param[in]       *pSrc points to the floating-point input vector

-   * @param[out]      *pDst points to the Q7 output vector

-   * @param[in]       blockSize length of the input vector

-   * @return          none

-   */

-  void arm_float_to_q7(

-  float32_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Converts the elements of the Q31 vector to Q15 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q31_to_q15(

-  q31_t * pSrc,

-  q15_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Converts the elements of the Q31 vector to Q7 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q31_to_q7(

-  q31_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-  /**

-   * @brief  Converts the elements of the Q15 vector to floating-point vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q15_to_float(

-  q15_t * pSrc,

-  float32_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Converts the elements of the Q15 vector to Q31 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q15_to_q31(

-  q15_t * pSrc,

-  q31_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @brief  Converts the elements of the Q15 vector to Q7 vector.

-   * @param[in]  *pSrc is input pointer

-   * @param[out]  *pDst is output pointer

-   * @param[in]  blockSize is the number of samples to process

-   * @return none.

-   */

-  void arm_q15_to_q7(

-  q15_t * pSrc,

-  q7_t * pDst,

-  uint32_t blockSize);

-

-

-  /**

-   * @ingroup groupInterpolation

-   */

-

-  /**

-   * @defgroup BilinearInterpolate Bilinear Interpolation

-   *

-   * Bilinear interpolation is an extension of linear interpolation applied to a two dimensional grid.

-   * The underlying function <code>f(x, y)</code> is sampled on a regular grid and the interpolation process

-   * determines values between the grid points.

-   * Bilinear interpolation is equivalent to two step linear interpolation, first in the x-dimension and then in the y-dimension.

-   * Bilinear interpolation is often used in image processing to rescale images.

-   * The CMSIS DSP library provides bilinear interpolation functions for Q7, Q15, Q31, and floating-point data types.

-   *

-   * <b>Algorithm</b>

-   * \par

-   * The instance structure used by the bilinear interpolation functions describes a two dimensional data table.

-   * For floating-point, the instance structure is defined as:

-   * <pre>

-   *   typedef struct

-   *   {

-   *     uint16_t numRows;

-   *     uint16_t numCols;

-   *     float32_t *pData;

-   * } arm_bilinear_interp_instance_f32;

-   * </pre>

-   *

-   * \par

-   * where <code>numRows</code> specifies the number of rows in the table;

-   * <code>numCols</code> specifies the number of columns in the table;

-   * and <code>pData</code> points to an array of size <code>numRows*numCols</code> values.

-   * The data table <code>pTable</code> is organized in row order and the supplied data values fall on integer indexes.

-   * That is, table element (x,y) is located at <code>pTable[x + y*numCols]</code> where x and y are integers.

-   *

-   * \par

-   * Let <code>(x, y)</code> specify the desired interpolation point.  Then define:

-   * <pre>

-   *     XF = floor(x)

-   *     YF = floor(y)

-   * </pre>

-   * \par

-   * The interpolated output point is computed as:

-   * <pre>

-   *  f(x, y) = f(XF, YF) * (1-(x-XF)) * (1-(y-YF))

-   *           + f(XF+1, YF) * (x-XF)*(1-(y-YF))

-   *           + f(XF, YF+1) * (1-(x-XF))*(y-YF)

-   *           + f(XF+1, YF+1) * (x-XF)*(y-YF)

-   * </pre>

-   * Note that the coordinates (x, y) contain integer and fractional components.

-   * The integer components specify which portion of the table to use while the

-   * fractional components control the interpolation processor.

-   *

-   * \par

-   * if (x,y) are outside of the table boundary, Bilinear interpolation returns zero output.

-   */

-

-  /**

-   * @addtogroup BilinearInterpolate

-   * @{

-   */

-

-  /**

-  *

-  * @brief  Floating-point bilinear interpolation.

-  * @param[in,out] *S points to an instance of the interpolation structure.

-  * @param[in] X interpolation coordinate.

-  * @param[in] Y interpolation coordinate.

-  * @return out interpolated value.

-  */

-

-

-  static __INLINE float32_t arm_bilinear_interp_f32(

-  const arm_bilinear_interp_instance_f32 * S,

-  float32_t X,

-  float32_t Y)

-  {

-    float32_t out;

-    float32_t f00, f01, f10, f11;

-    float32_t *pData = S->pData;

-    int32_t xIndex, yIndex, index;

-    float32_t xdiff, ydiff;

-    float32_t b1, b2, b3, b4;

-

-    xIndex = (int32_t) X;

-    yIndex = (int32_t) Y;

-

-    /* Care taken for table outside boundary */

-    /* Returns zero output when values are outside table boundary */

-    if(xIndex < 0 || xIndex > (S->numRows - 1) || yIndex < 0

-       || yIndex > (S->numCols - 1))

-    {

-      return (0);

-    }

-

-    /* Calculation of index for two nearest points in X-direction */

-    index = (xIndex - 1) + (yIndex - 1) * S->numCols;

-

-

-    /* Read two nearest points in X-direction */

-    f00 = pData[index];

-    f01 = pData[index + 1];

-

-    /* Calculation of index for two nearest points in Y-direction */

-    index = (xIndex - 1) + (yIndex) * S->numCols;

-

-

-    /* Read two nearest points in Y-direction */

-    f10 = pData[index];

-    f11 = pData[index + 1];

-

-    /* Calculation of intermediate values */

-    b1 = f00;

-    b2 = f01 - f00;

-    b3 = f10 - f00;

-    b4 = f00 - f01 - f10 + f11;

-

-    /* Calculation of fractional part in X */

-    xdiff = X - xIndex;

-

-    /* Calculation of fractional part in Y */

-    ydiff = Y - yIndex;

-

-    /* Calculation of bi-linear interpolated output */

-    out = b1 + b2 * xdiff + b3 * ydiff + b4 * xdiff * ydiff;

-

-    /* return to application */

-    return (out);

-

-  }

-

-  /**

-  *

-  * @brief  Q31 bilinear interpolation.

-  * @param[in,out] *S points to an instance of the interpolation structure.

-  * @param[in] X interpolation coordinate in 12.20 format.

-  * @param[in] Y interpolation coordinate in 12.20 format.

-  * @return out interpolated value.

-  */

-

-  static __INLINE q31_t arm_bilinear_interp_q31(

-  arm_bilinear_interp_instance_q31 * S,

-  q31_t X,

-  q31_t Y)

-  {

-    q31_t out;                                   /* Temporary output */

-    q31_t acc = 0;                               /* output */

-    q31_t xfract, yfract;                        /* X, Y fractional parts */

-    q31_t x1, x2, y1, y2;                        /* Nearest output values */

-    int32_t rI, cI;                              /* Row and column indices */

-    q31_t *pYData = S->pData;                    /* pointer to output table values */

-    uint32_t nCols = S->numCols;                 /* num of rows */

-

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    rI = ((X & 0xFFF00000) >> 20u);

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    cI = ((Y & 0xFFF00000) >> 20u);

-

-    /* Care taken for table outside boundary */

-    /* Returns zero output when values are outside table boundary */

-    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))

-    {

-      return (0);

-    }

-

-    /* 20 bits for the fractional part */

-    /* shift left xfract by 11 to keep 1.31 format */

-    xfract = (X & 0x000FFFFF) << 11u;

-

-    /* Read two nearest output values from the index */

-    x1 = pYData[(rI) + nCols * (cI)];

-    x2 = pYData[(rI) + nCols * (cI) + 1u];

-

-    /* 20 bits for the fractional part */

-    /* shift left yfract by 11 to keep 1.31 format */

-    yfract = (Y & 0x000FFFFF) << 11u;

-

-    /* Read two nearest output values from the index */

-    y1 = pYData[(rI) + nCols * (cI + 1)];

-    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];

-

-    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 3.29(q29) format */

-    out = ((q31_t) (((q63_t) x1 * (0x7FFFFFFF - xfract)) >> 32));

-    acc = ((q31_t) (((q63_t) out * (0x7FFFFFFF - yfract)) >> 32));

-

-    /* x2 * (xfract) * (1-yfract)  in 3.29(q29) and adding to acc */

-    out = ((q31_t) ((q63_t) x2 * (0x7FFFFFFF - yfract) >> 32));

-    acc += ((q31_t) ((q63_t) out * (xfract) >> 32));

-

-    /* y1 * (1 - xfract) * (yfract)  in 3.29(q29) and adding to acc */

-    out = ((q31_t) ((q63_t) y1 * (0x7FFFFFFF - xfract) >> 32));

-    acc += ((q31_t) ((q63_t) out * (yfract) >> 32));

-

-    /* y2 * (xfract) * (yfract)  in 3.29(q29) and adding to acc */

-    out = ((q31_t) ((q63_t) y2 * (xfract) >> 32));

-    acc += ((q31_t) ((q63_t) out * (yfract) >> 32));

-

-    /* Convert acc to 1.31(q31) format */

-    return (acc << 2u);

-

-  }

-

-  /**

-  * @brief  Q15 bilinear interpolation.

-  * @param[in,out] *S points to an instance of the interpolation structure.

-  * @param[in] X interpolation coordinate in 12.20 format.

-  * @param[in] Y interpolation coordinate in 12.20 format.

-  * @return out interpolated value.

-  */

-

-  static __INLINE q15_t arm_bilinear_interp_q15(

-  arm_bilinear_interp_instance_q15 * S,

-  q31_t X,

-  q31_t Y)

-  {

-    q63_t acc = 0;                               /* output */

-    q31_t out;                                   /* Temporary output */

-    q15_t x1, x2, y1, y2;                        /* Nearest output values */

-    q31_t xfract, yfract;                        /* X, Y fractional parts */

-    int32_t rI, cI;                              /* Row and column indices */

-    q15_t *pYData = S->pData;                    /* pointer to output table values */

-    uint32_t nCols = S->numCols;                 /* num of rows */

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    rI = ((X & 0xFFF00000) >> 20);

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    cI = ((Y & 0xFFF00000) >> 20);

-

-    /* Care taken for table outside boundary */

-    /* Returns zero output when values are outside table boundary */

-    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))

-    {

-      return (0);

-    }

-

-    /* 20 bits for the fractional part */

-    /* xfract should be in 12.20 format */

-    xfract = (X & 0x000FFFFF);

-

-    /* Read two nearest output values from the index */

-    x1 = pYData[(rI) + nCols * (cI)];

-    x2 = pYData[(rI) + nCols * (cI) + 1u];

-

-

-    /* 20 bits for the fractional part */

-    /* yfract should be in 12.20 format */

-    yfract = (Y & 0x000FFFFF);

-

-    /* Read two nearest output values from the index */

-    y1 = pYData[(rI) + nCols * (cI + 1)];

-    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];

-

-    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 13.51 format */

-

-    /* x1 is in 1.15(q15), xfract in 12.20 format and out is in 13.35 format */

-    /* convert 13.35 to 13.31 by right shifting  and out is in 1.31 */

-    out = (q31_t) (((q63_t) x1 * (0xFFFFF - xfract)) >> 4u);

-    acc = ((q63_t) out * (0xFFFFF - yfract));

-

-    /* x2 * (xfract) * (1-yfract)  in 1.51 and adding to acc */

-    out = (q31_t) (((q63_t) x2 * (0xFFFFF - yfract)) >> 4u);

-    acc += ((q63_t) out * (xfract));

-

-    /* y1 * (1 - xfract) * (yfract)  in 1.51 and adding to acc */

-    out = (q31_t) (((q63_t) y1 * (0xFFFFF - xfract)) >> 4u);

-    acc += ((q63_t) out * (yfract));

-

-    /* y2 * (xfract) * (yfract)  in 1.51 and adding to acc */

-    out = (q31_t) (((q63_t) y2 * (xfract)) >> 4u);

-    acc += ((q63_t) out * (yfract));

-

-    /* acc is in 13.51 format and down shift acc by 36 times */

-    /* Convert out to 1.15 format */

-    return (acc >> 36);

-

-  }

-

-  /**

-  * @brief  Q7 bilinear interpolation.

-  * @param[in,out] *S points to an instance of the interpolation structure.

-  * @param[in] X interpolation coordinate in 12.20 format.

-  * @param[in] Y interpolation coordinate in 12.20 format.

-  * @return out interpolated value.

-  */

-

-  static __INLINE q7_t arm_bilinear_interp_q7(

-  arm_bilinear_interp_instance_q7 * S,

-  q31_t X,

-  q31_t Y)

-  {

-    q63_t acc = 0;                               /* output */

-    q31_t out;                                   /* Temporary output */

-    q31_t xfract, yfract;                        /* X, Y fractional parts */

-    q7_t x1, x2, y1, y2;                         /* Nearest output values */

-    int32_t rI, cI;                              /* Row and column indices */

-    q7_t *pYData = S->pData;                     /* pointer to output table values */

-    uint32_t nCols = S->numCols;                 /* num of rows */

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    rI = ((X & 0xFFF00000) >> 20);

-

-    /* Input is in 12.20 format */

-    /* 12 bits for the table index */

-    /* Index value calculation */

-    cI = ((Y & 0xFFF00000) >> 20);

-

-    /* Care taken for table outside boundary */

-    /* Returns zero output when values are outside table boundary */

-    if(rI < 0 || rI > (S->numRows - 1) || cI < 0 || cI > (S->numCols - 1))

-    {

-      return (0);

-    }

-

-    /* 20 bits for the fractional part */

-    /* xfract should be in 12.20 format */

-    xfract = (X & 0x000FFFFF);

-

-    /* Read two nearest output values from the index */

-    x1 = pYData[(rI) + nCols * (cI)];

-    x2 = pYData[(rI) + nCols * (cI) + 1u];

-

-

-    /* 20 bits for the fractional part */

-    /* yfract should be in 12.20 format */

-    yfract = (Y & 0x000FFFFF);

-

-    /* Read two nearest output values from the index */

-    y1 = pYData[(rI) + nCols * (cI + 1)];

-    y2 = pYData[(rI) + nCols * (cI + 1) + 1u];

-

-    /* Calculation of x1 * (1-xfract ) * (1-yfract) and acc is in 16.47 format */

-    out = ((x1 * (0xFFFFF - xfract)));

-    acc = (((q63_t) out * (0xFFFFF - yfract)));

-

-    /* x2 * (xfract) * (1-yfract)  in 2.22 and adding to acc */

-    out = ((x2 * (0xFFFFF - yfract)));

-    acc += (((q63_t) out * (xfract)));

-

-    /* y1 * (1 - xfract) * (yfract)  in 2.22 and adding to acc */

-    out = ((y1 * (0xFFFFF - xfract)));

-    acc += (((q63_t) out * (yfract)));

-

-    /* y2 * (xfract) * (yfract)  in 2.22 and adding to acc */

-    out = ((y2 * (yfract)));

-    acc += (((q63_t) out * (xfract)));

-

-    /* acc in 16.47 format and down shift by 40 to convert to 1.7 format */

-    return (acc >> 40);

-

-  }

-

-  /**

-   * @} end of BilinearInterpolate group

-   */

-

-

-#if   defined ( __CC_ARM ) //Keil

-//SMMLAR

-  #define multAcc_32x32_keep32_R(a, x, y) \

-  a = (q31_t) (((((q63_t) a) << 32) + ((q63_t) x * y) + 0x80000000LL ) >> 32)

-

-//SMMLSR

-  #define multSub_32x32_keep32_R(a, x, y) \

-  a = (q31_t) (((((q63_t) a) << 32) - ((q63_t) x * y) + 0x80000000LL ) >> 32)

-

-//SMMULR

-  #define mult_32x32_keep32_R(a, x, y) \

-  a = (q31_t) (((q63_t) x * y + 0x80000000LL ) >> 32)

-

-//Enter low optimization region - place directly above function definition

-  #define LOW_OPTIMIZATION_ENTER \

-     _Pragma ("push")         \

-     _Pragma ("O1")

-

-//Exit low optimization region - place directly after end of function definition

-  #define LOW_OPTIMIZATION_EXIT \

-     _Pragma ("pop")

-

-//Enter low optimization region - place directly above function definition

-  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER

-

-//Exit low optimization region - place directly after end of function definition

-  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT

-

-#elif defined(__ICCARM__) //IAR

- //SMMLA

-  #define multAcc_32x32_keep32_R(a, x, y) \

-  a += (q31_t) (((q63_t) x * y) >> 32)

-

- //SMMLS

-  #define multSub_32x32_keep32_R(a, x, y) \

-  a -= (q31_t) (((q63_t) x * y) >> 32)

-

-//SMMUL

-  #define mult_32x32_keep32_R(a, x, y) \

-  a = (q31_t) (((q63_t) x * y ) >> 32)

-

-//Enter low optimization region - place directly above function definition

-  #define LOW_OPTIMIZATION_ENTER \

-     _Pragma ("optimize=low")

-

-//Exit low optimization region - place directly after end of function definition

-  #define LOW_OPTIMIZATION_EXIT

-

-//Enter low optimization region - place directly above function definition

-  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER \

-     _Pragma ("optimize=low")

-

-//Exit low optimization region - place directly after end of function definition

-  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT

-

-#elif defined(__GNUC__)

- //SMMLA

-  #define multAcc_32x32_keep32_R(a, x, y) \

-  a += (q31_t) (((q63_t) x * y) >> 32)

-

- //SMMLS

-  #define multSub_32x32_keep32_R(a, x, y) \

-  a -= (q31_t) (((q63_t) x * y) >> 32)

-

-//SMMUL

-  #define mult_32x32_keep32_R(a, x, y) \

-  a = (q31_t) (((q63_t) x * y ) >> 32)

-

-  #define LOW_OPTIMIZATION_ENTER __attribute__(( optimize("-O1") ))

-

-  #define LOW_OPTIMIZATION_EXIT

-

-  #define IAR_ONLY_LOW_OPTIMIZATION_ENTER

-

-  #define IAR_ONLY_LOW_OPTIMIZATION_EXIT

-

-#endif

-

-

-

-

-

-#ifdef	__cplusplus

-}

-#endif

-

-

-#endif /* _ARM_MATH_H */

-

-

-/**

- *

- * End of file.

- */

diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/modules/mathlib/CMSIS/Include/core_cm3.h
deleted file mode 100644
index 8ac6dc078..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cm3.h
+++ /dev/null
@@ -1,1627 +0,0 @@
-/**************************************************************************//**

- * @file     core_cm3.h

- * @brief    CMSIS Cortex-M3 Core Peripheral Access Layer Header File

- * @version  V3.20

- * @date     25. February 2013

- *

- * @note

- *

- ******************************************************************************/

-/* Copyright (c) 2009 - 2013 ARM LIMITED

-

-   All rights reserved.

-   Redistribution and use in source and binary forms, with or without

-   modification, are permitted provided that the following conditions are met:

-   - Redistributions of source code must retain the above copyright

-     notice, this list of conditions and the following disclaimer.

-   - Redistributions in binary form must reproduce the above copyright

-     notice, this list of conditions and the following disclaimer in the

-     documentation and/or other materials provided with the distribution.

-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

-   ---------------------------------------------------------------------------*/

-

-

-#if defined ( __ICCARM__ )

- #pragma system_include  /* treat file as system include file for MISRA check */

-#endif

-

-#ifdef __cplusplus

- extern "C" {

-#endif

-

-#ifndef __CORE_CM3_H_GENERIC

-#define __CORE_CM3_H_GENERIC

-

-/** \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions

-  CMSIS violates the following MISRA-C:2004 rules:

-

-   \li Required Rule 8.5, object/function definition in header file.<br>

-     Function definitions in header files are used to allow 'inlining'.

-

-   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>

-     Unions are used for effective representation of core registers.

-

-   \li Advisory Rule 19.7, Function-like macro defined.<br>

-     Function-like macros are used to allow more efficient code.

- */

-

-

-/*******************************************************************************

- *                 CMSIS definitions

- ******************************************************************************/

-/** \ingroup Cortex_M3

-  @{

- */

-

-/*  CMSIS CM3 definitions */

-#define __CM3_CMSIS_VERSION_MAIN  (0x03)                                   /*!< [31:16] CMSIS HAL main version   */

-#define __CM3_CMSIS_VERSION_SUB   (0x20)                                   /*!< [15:0]  CMSIS HAL sub version    */

-#define __CM3_CMSIS_VERSION       ((__CM3_CMSIS_VERSION_MAIN << 16) | \

-                                    __CM3_CMSIS_VERSION_SUB          )     /*!< CMSIS HAL version number         */

-

-#define __CORTEX_M                (0x03)                                   /*!< Cortex-M Core                    */

-

-

-#if   defined ( __CC_ARM )

-  #define __ASM            __asm                                      /*!< asm keyword for ARM Compiler          */

-  #define __INLINE         __inline                                   /*!< inline keyword for ARM Compiler       */

-  #define __STATIC_INLINE  static __inline

-

-#elif defined ( __ICCARM__ )

-  #define __ASM            __asm                                      /*!< asm keyword for IAR Compiler          */

-  #define __INLINE         inline                                     /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */

-  #define __STATIC_INLINE  static inline

-

-#elif defined ( __TMS470__ )

-  #define __ASM            __asm                                      /*!< asm keyword for TI CCS Compiler       */

-  #define __STATIC_INLINE  static inline

-

-#elif defined ( __GNUC__ )

-  #define __ASM            __asm                                      /*!< asm keyword for GNU Compiler          */

-  #define __INLINE         inline                                     /*!< inline keyword for GNU Compiler       */

-  #define __STATIC_INLINE  static inline

-

-#elif defined ( __TASKING__ )

-  #define __ASM            __asm                                      /*!< asm keyword for TASKING Compiler      */

-  #define __INLINE         inline                                     /*!< inline keyword for TASKING Compiler   */

-  #define __STATIC_INLINE  static inline

-

-#endif

-

-/** __FPU_USED indicates whether an FPU is used or not. This core does not support an FPU at all

-*/

-#define __FPU_USED       0

-

-#if defined ( __CC_ARM )

-  #if defined __TARGET_FPU_VFP

-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-  #endif

-

-#elif defined ( __ICCARM__ )

-  #if defined __ARMVFP__

-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-  #endif

-

-#elif defined ( __TMS470__ )

-  #if defined __TI__VFP_SUPPORT____

-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-  #endif

-

-#elif defined ( __GNUC__ )

-  #if defined (__VFP_FP__) && !defined(__SOFTFP__)

-    #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-  #endif

-

-#elif defined ( __TASKING__ )

-  #if defined __FPU_VFP__

-    #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-  #endif

-#endif

-

-#include <stdint.h>                      /* standard types definitions                      */

-#include "core_cmInstr.h"                /* Core Instruction Access                         */

-#include "core_cmFunc.h"                 /* Core Function Access                            */

-

-#endif /* __CORE_CM3_H_GENERIC */

-

-#ifndef __CMSIS_GENERIC

-

-#ifndef __CORE_CM3_H_DEPENDANT

-#define __CORE_CM3_H_DEPENDANT

-

-/* check device defines and use defaults */

-#if defined __CHECK_DEVICE_DEFINES

-  #ifndef __CM3_REV

-    #define __CM3_REV               0x0200

-    #warning "__CM3_REV not defined in device header file; using default!"

-  #endif

-

-  #ifndef __MPU_PRESENT

-    #define __MPU_PRESENT             0

-    #warning "__MPU_PRESENT not defined in device header file; using default!"

-  #endif

-

-  #ifndef __NVIC_PRIO_BITS

-    #define __NVIC_PRIO_BITS          4

-    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"

-  #endif

-

-  #ifndef __Vendor_SysTickConfig

-    #define __Vendor_SysTickConfig    0

-    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"

-  #endif

-#endif

-

-/* IO definitions (access restrictions to peripheral registers) */

-/**

-    \defgroup CMSIS_glob_defs CMSIS Global Defines

-

-    <strong>IO Type Qualifiers</strong> are used

-    \li to specify the access to peripheral variables.

-    \li for automatic generation of peripheral register debug information.

-*/

-#ifdef __cplusplus

-  #define   __I     volatile             /*!< Defines 'read only' permissions                 */

-#else

-  #define   __I     volatile const       /*!< Defines 'read only' permissions                 */

-#endif

-#define     __O     volatile             /*!< Defines 'write only' permissions                */

-#define     __IO    volatile             /*!< Defines 'read / write' permissions              */

-

-/*@} end of group Cortex_M3 */

-

-

-

-/*******************************************************************************

- *                 Register Abstraction

-  Core Register contain:

-  - Core Register

-  - Core NVIC Register

-  - Core SCB Register

-  - Core SysTick Register

-  - Core Debug Register

-  - Core MPU Register

- ******************************************************************************/

-/** \defgroup CMSIS_core_register Defines and Type Definitions

-    \brief Type definitions and defines for Cortex-M processor based devices.

-*/

-

-/** \ingroup    CMSIS_core_register

-    \defgroup   CMSIS_CORE  Status and Control Registers

-    \brief  Core Register type definitions.

-  @{

- */

-

-/** \brief  Union type to access the Application Program Status Register (APSR).

- */

-typedef union

-{

-  struct

-  {

-#if (__CORTEX_M != 0x04)

-    uint32_t _reserved0:27;              /*!< bit:  0..26  Reserved                           */

-#else

-    uint32_t _reserved0:16;              /*!< bit:  0..15  Reserved                           */

-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */

-    uint32_t _reserved1:7;               /*!< bit: 20..26  Reserved                           */

-#endif

-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */

-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */

-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */

-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */

-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} APSR_Type;

-

-

-/** \brief  Union type to access the Interrupt Program Status Register (IPSR).

- */

-typedef union

-{

-  struct

-  {

-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */

-    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved                           */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} IPSR_Type;

-

-

-/** \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).

- */

-typedef union

-{

-  struct

-  {

-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */

-#if (__CORTEX_M != 0x04)

-    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved                           */

-#else

-    uint32_t _reserved0:7;               /*!< bit:  9..15  Reserved                           */

-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */

-    uint32_t _reserved1:4;               /*!< bit: 20..23  Reserved                           */

-#endif

-    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0)          */

-    uint32_t IT:2;                       /*!< bit: 25..26  saved IT state   (read 0)          */

-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */

-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */

-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */

-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */

-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} xPSR_Type;

-

-

-/** \brief  Union type to access the Control Registers (CONTROL).

- */

-typedef union

-{

-  struct

-  {

-    uint32_t nPRIV:1;                    /*!< bit:      0  Execution privilege in Thread mode */

-    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used                   */

-    uint32_t FPCA:1;                     /*!< bit:      2  FP extension active flag           */

-    uint32_t _reserved0:29;              /*!< bit:  3..31  Reserved                           */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} CONTROL_Type;

-

-/*@} end of group CMSIS_CORE */

-

-

-/** \ingroup    CMSIS_core_register

-    \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)

-    \brief      Type definitions for the NVIC Registers

-  @{

- */

-

-/** \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).

- */

-typedef struct

-{

-  __IO uint32_t ISER[8];                 /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register           */

-       uint32_t RESERVED0[24];

-  __IO uint32_t ICER[8];                 /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register         */

-       uint32_t RSERVED1[24];

-  __IO uint32_t ISPR[8];                 /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register          */

-       uint32_t RESERVED2[24];

-  __IO uint32_t ICPR[8];                 /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register        */

-       uint32_t RESERVED3[24];

-  __IO uint32_t IABR[8];                 /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register           */

-       uint32_t RESERVED4[56];

-  __IO uint8_t  IP[240];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */

-       uint32_t RESERVED5[644];

-  __O  uint32_t STIR;                    /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register     */

-}  NVIC_Type;

-

-/* Software Triggered Interrupt Register Definitions */

-#define NVIC_STIR_INTID_Pos                 0                                          /*!< STIR: INTLINESNUM Position */

-#define NVIC_STIR_INTID_Msk                (0x1FFUL << NVIC_STIR_INTID_Pos)            /*!< STIR: INTLINESNUM Mask */

-

-/*@} end of group CMSIS_NVIC */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_SCB     System Control Block (SCB)

-    \brief      Type definitions for the System Control Block Registers

-  @{

- */

-

-/** \brief  Structure type to access the System Control Block (SCB).

- */

-typedef struct

-{

-  __I  uint32_t CPUID;                   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */

-  __IO uint32_t ICSR;                    /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */

-  __IO uint32_t VTOR;                    /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */

-  __IO uint32_t AIRCR;                   /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */

-  __IO uint32_t SCR;                     /*!< Offset: 0x010 (R/W)  System Control Register                               */

-  __IO uint32_t CCR;                     /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */

-  __IO uint8_t  SHP[12];                 /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */

-  __IO uint32_t SHCSR;                   /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */

-  __IO uint32_t CFSR;                    /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */

-  __IO uint32_t HFSR;                    /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */

-  __IO uint32_t DFSR;                    /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */

-  __IO uint32_t MMFAR;                   /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */

-  __IO uint32_t BFAR;                    /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */

-  __IO uint32_t AFSR;                    /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */

-  __I  uint32_t PFR[2];                  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */

-  __I  uint32_t DFR;                     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */

-  __I  uint32_t ADR;                     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */

-  __I  uint32_t MMFR[4];                 /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */

-  __I  uint32_t ISAR[5];                 /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */

-       uint32_t RESERVED0[5];

-  __IO uint32_t CPACR;                   /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */

-} SCB_Type;

-

-/* SCB CPUID Register Definitions */

-#define SCB_CPUID_IMPLEMENTER_Pos          24                                             /*!< SCB CPUID: IMPLEMENTER Position */

-#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */

-

-#define SCB_CPUID_VARIANT_Pos              20                                             /*!< SCB CPUID: VARIANT Position */

-#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */

-

-#define SCB_CPUID_ARCHITECTURE_Pos         16                                             /*!< SCB CPUID: ARCHITECTURE Position */

-#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */

-

-#define SCB_CPUID_PARTNO_Pos                4                                             /*!< SCB CPUID: PARTNO Position */

-#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */

-

-#define SCB_CPUID_REVISION_Pos              0                                             /*!< SCB CPUID: REVISION Position */

-#define SCB_CPUID_REVISION_Msk             (0xFUL << SCB_CPUID_REVISION_Pos)              /*!< SCB CPUID: REVISION Mask */

-

-/* SCB Interrupt Control State Register Definitions */

-#define SCB_ICSR_NMIPENDSET_Pos            31                                             /*!< SCB ICSR: NMIPENDSET Position */

-#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */

-

-#define SCB_ICSR_PENDSVSET_Pos             28                                             /*!< SCB ICSR: PENDSVSET Position */

-#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */

-

-#define SCB_ICSR_PENDSVCLR_Pos             27                                             /*!< SCB ICSR: PENDSVCLR Position */

-#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */

-

-#define SCB_ICSR_PENDSTSET_Pos             26                                             /*!< SCB ICSR: PENDSTSET Position */

-#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */

-

-#define SCB_ICSR_PENDSTCLR_Pos             25                                             /*!< SCB ICSR: PENDSTCLR Position */

-#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */

-

-#define SCB_ICSR_ISRPREEMPT_Pos            23                                             /*!< SCB ICSR: ISRPREEMPT Position */

-#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */

-

-#define SCB_ICSR_ISRPENDING_Pos            22                                             /*!< SCB ICSR: ISRPENDING Position */

-#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */

-

-#define SCB_ICSR_VECTPENDING_Pos           12                                             /*!< SCB ICSR: VECTPENDING Position */

-#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */

-

-#define SCB_ICSR_RETTOBASE_Pos             11                                             /*!< SCB ICSR: RETTOBASE Position */

-#define SCB_ICSR_RETTOBASE_Msk             (1UL << SCB_ICSR_RETTOBASE_Pos)                /*!< SCB ICSR: RETTOBASE Mask */

-

-#define SCB_ICSR_VECTACTIVE_Pos             0                                             /*!< SCB ICSR: VECTACTIVE Position */

-#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos)           /*!< SCB ICSR: VECTACTIVE Mask */

-

-/* SCB Vector Table Offset Register Definitions */

-#if (__CM3_REV < 0x0201)                   /* core r2p1 */

-#define SCB_VTOR_TBLBASE_Pos               29                                             /*!< SCB VTOR: TBLBASE Position */

-#define SCB_VTOR_TBLBASE_Msk               (1UL << SCB_VTOR_TBLBASE_Pos)                  /*!< SCB VTOR: TBLBASE Mask */

-

-#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */

-#define SCB_VTOR_TBLOFF_Msk                (0x3FFFFFUL << SCB_VTOR_TBLOFF_Pos)            /*!< SCB VTOR: TBLOFF Mask */

-#else

-#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */

-#define SCB_VTOR_TBLOFF_Msk                (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos)           /*!< SCB VTOR: TBLOFF Mask */

-#endif

-

-/* SCB Application Interrupt and Reset Control Register Definitions */

-#define SCB_AIRCR_VECTKEY_Pos              16                                             /*!< SCB AIRCR: VECTKEY Position */

-#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */

-

-#define SCB_AIRCR_VECTKEYSTAT_Pos          16                                             /*!< SCB AIRCR: VECTKEYSTAT Position */

-#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */

-

-#define SCB_AIRCR_ENDIANESS_Pos            15                                             /*!< SCB AIRCR: ENDIANESS Position */

-#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */

-

-#define SCB_AIRCR_PRIGROUP_Pos              8                                             /*!< SCB AIRCR: PRIGROUP Position */

-#define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */

-

-#define SCB_AIRCR_SYSRESETREQ_Pos           2                                             /*!< SCB AIRCR: SYSRESETREQ Position */

-#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */

-

-#define SCB_AIRCR_VECTCLRACTIVE_Pos         1                                             /*!< SCB AIRCR: VECTCLRACTIVE Position */

-#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */

-

-#define SCB_AIRCR_VECTRESET_Pos             0                                             /*!< SCB AIRCR: VECTRESET Position */

-#define SCB_AIRCR_VECTRESET_Msk            (1UL << SCB_AIRCR_VECTRESET_Pos)               /*!< SCB AIRCR: VECTRESET Mask */

-

-/* SCB System Control Register Definitions */

-#define SCB_SCR_SEVONPEND_Pos               4                                             /*!< SCB SCR: SEVONPEND Position */

-#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */

-

-#define SCB_SCR_SLEEPDEEP_Pos               2                                             /*!< SCB SCR: SLEEPDEEP Position */

-#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */

-

-#define SCB_SCR_SLEEPONEXIT_Pos             1                                             /*!< SCB SCR: SLEEPONEXIT Position */

-#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */

-

-/* SCB Configuration Control Register Definitions */

-#define SCB_CCR_STKALIGN_Pos                9                                             /*!< SCB CCR: STKALIGN Position */

-#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */

-

-#define SCB_CCR_BFHFNMIGN_Pos               8                                             /*!< SCB CCR: BFHFNMIGN Position */

-#define SCB_CCR_BFHFNMIGN_Msk              (1UL << SCB_CCR_BFHFNMIGN_Pos)                 /*!< SCB CCR: BFHFNMIGN Mask */

-

-#define SCB_CCR_DIV_0_TRP_Pos               4                                             /*!< SCB CCR: DIV_0_TRP Position */

-#define SCB_CCR_DIV_0_TRP_Msk              (1UL << SCB_CCR_DIV_0_TRP_Pos)                 /*!< SCB CCR: DIV_0_TRP Mask */

-

-#define SCB_CCR_UNALIGN_TRP_Pos             3                                             /*!< SCB CCR: UNALIGN_TRP Position */

-#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */

-

-#define SCB_CCR_USERSETMPEND_Pos            1                                             /*!< SCB CCR: USERSETMPEND Position */

-#define SCB_CCR_USERSETMPEND_Msk           (1UL << SCB_CCR_USERSETMPEND_Pos)              /*!< SCB CCR: USERSETMPEND Mask */

-

-#define SCB_CCR_NONBASETHRDENA_Pos          0                                             /*!< SCB CCR: NONBASETHRDENA Position */

-#define SCB_CCR_NONBASETHRDENA_Msk         (1UL << SCB_CCR_NONBASETHRDENA_Pos)            /*!< SCB CCR: NONBASETHRDENA Mask */

-

-/* SCB System Handler Control and State Register Definitions */

-#define SCB_SHCSR_USGFAULTENA_Pos          18                                             /*!< SCB SHCSR: USGFAULTENA Position */

-#define SCB_SHCSR_USGFAULTENA_Msk          (1UL << SCB_SHCSR_USGFAULTENA_Pos)             /*!< SCB SHCSR: USGFAULTENA Mask */

-

-#define SCB_SHCSR_BUSFAULTENA_Pos          17                                             /*!< SCB SHCSR: BUSFAULTENA Position */

-#define SCB_SHCSR_BUSFAULTENA_Msk          (1UL << SCB_SHCSR_BUSFAULTENA_Pos)             /*!< SCB SHCSR: BUSFAULTENA Mask */

-

-#define SCB_SHCSR_MEMFAULTENA_Pos          16                                             /*!< SCB SHCSR: MEMFAULTENA Position */

-#define SCB_SHCSR_MEMFAULTENA_Msk          (1UL << SCB_SHCSR_MEMFAULTENA_Pos)             /*!< SCB SHCSR: MEMFAULTENA Mask */

-

-#define SCB_SHCSR_SVCALLPENDED_Pos         15                                             /*!< SCB SHCSR: SVCALLPENDED Position */

-#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */

-

-#define SCB_SHCSR_BUSFAULTPENDED_Pos       14                                             /*!< SCB SHCSR: BUSFAULTPENDED Position */

-#define SCB_SHCSR_BUSFAULTPENDED_Msk       (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos)          /*!< SCB SHCSR: BUSFAULTPENDED Mask */

-

-#define SCB_SHCSR_MEMFAULTPENDED_Pos       13                                             /*!< SCB SHCSR: MEMFAULTPENDED Position */

-#define SCB_SHCSR_MEMFAULTPENDED_Msk       (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos)          /*!< SCB SHCSR: MEMFAULTPENDED Mask */

-

-#define SCB_SHCSR_USGFAULTPENDED_Pos       12                                             /*!< SCB SHCSR: USGFAULTPENDED Position */

-#define SCB_SHCSR_USGFAULTPENDED_Msk       (1UL << SCB_SHCSR_USGFAULTPENDED_Pos)          /*!< SCB SHCSR: USGFAULTPENDED Mask */

-

-#define SCB_SHCSR_SYSTICKACT_Pos           11                                             /*!< SCB SHCSR: SYSTICKACT Position */

-#define SCB_SHCSR_SYSTICKACT_Msk           (1UL << SCB_SHCSR_SYSTICKACT_Pos)              /*!< SCB SHCSR: SYSTICKACT Mask */

-

-#define SCB_SHCSR_PENDSVACT_Pos            10                                             /*!< SCB SHCSR: PENDSVACT Position */

-#define SCB_SHCSR_PENDSVACT_Msk            (1UL << SCB_SHCSR_PENDSVACT_Pos)               /*!< SCB SHCSR: PENDSVACT Mask */

-

-#define SCB_SHCSR_MONITORACT_Pos            8                                             /*!< SCB SHCSR: MONITORACT Position */

-#define SCB_SHCSR_MONITORACT_Msk           (1UL << SCB_SHCSR_MONITORACT_Pos)              /*!< SCB SHCSR: MONITORACT Mask */

-

-#define SCB_SHCSR_SVCALLACT_Pos             7                                             /*!< SCB SHCSR: SVCALLACT Position */

-#define SCB_SHCSR_SVCALLACT_Msk            (1UL << SCB_SHCSR_SVCALLACT_Pos)               /*!< SCB SHCSR: SVCALLACT Mask */

-

-#define SCB_SHCSR_USGFAULTACT_Pos           3                                             /*!< SCB SHCSR: USGFAULTACT Position */

-#define SCB_SHCSR_USGFAULTACT_Msk          (1UL << SCB_SHCSR_USGFAULTACT_Pos)             /*!< SCB SHCSR: USGFAULTACT Mask */

-

-#define SCB_SHCSR_BUSFAULTACT_Pos           1                                             /*!< SCB SHCSR: BUSFAULTACT Position */

-#define SCB_SHCSR_BUSFAULTACT_Msk          (1UL << SCB_SHCSR_BUSFAULTACT_Pos)             /*!< SCB SHCSR: BUSFAULTACT Mask */

-

-#define SCB_SHCSR_MEMFAULTACT_Pos           0                                             /*!< SCB SHCSR: MEMFAULTACT Position */

-#define SCB_SHCSR_MEMFAULTACT_Msk          (1UL << SCB_SHCSR_MEMFAULTACT_Pos)             /*!< SCB SHCSR: MEMFAULTACT Mask */

-

-/* SCB Configurable Fault Status Registers Definitions */

-#define SCB_CFSR_USGFAULTSR_Pos            16                                             /*!< SCB CFSR: Usage Fault Status Register Position */

-#define SCB_CFSR_USGFAULTSR_Msk            (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos)          /*!< SCB CFSR: Usage Fault Status Register Mask */

-

-#define SCB_CFSR_BUSFAULTSR_Pos             8                                             /*!< SCB CFSR: Bus Fault Status Register Position */

-#define SCB_CFSR_BUSFAULTSR_Msk            (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos)            /*!< SCB CFSR: Bus Fault Status Register Mask */

-

-#define SCB_CFSR_MEMFAULTSR_Pos             0                                             /*!< SCB CFSR: Memory Manage Fault Status Register Position */

-#define SCB_CFSR_MEMFAULTSR_Msk            (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos)            /*!< SCB CFSR: Memory Manage Fault Status Register Mask */

-

-/* SCB Hard Fault Status Registers Definitions */

-#define SCB_HFSR_DEBUGEVT_Pos              31                                             /*!< SCB HFSR: DEBUGEVT Position */

-#define SCB_HFSR_DEBUGEVT_Msk              (1UL << SCB_HFSR_DEBUGEVT_Pos)                 /*!< SCB HFSR: DEBUGEVT Mask */

-

-#define SCB_HFSR_FORCED_Pos                30                                             /*!< SCB HFSR: FORCED Position */

-#define SCB_HFSR_FORCED_Msk                (1UL << SCB_HFSR_FORCED_Pos)                   /*!< SCB HFSR: FORCED Mask */

-

-#define SCB_HFSR_VECTTBL_Pos                1                                             /*!< SCB HFSR: VECTTBL Position */

-#define SCB_HFSR_VECTTBL_Msk               (1UL << SCB_HFSR_VECTTBL_Pos)                  /*!< SCB HFSR: VECTTBL Mask */

-

-/* SCB Debug Fault Status Register Definitions */

-#define SCB_DFSR_EXTERNAL_Pos               4                                             /*!< SCB DFSR: EXTERNAL Position */

-#define SCB_DFSR_EXTERNAL_Msk              (1UL << SCB_DFSR_EXTERNAL_Pos)                 /*!< SCB DFSR: EXTERNAL Mask */

-

-#define SCB_DFSR_VCATCH_Pos                 3                                             /*!< SCB DFSR: VCATCH Position */

-#define SCB_DFSR_VCATCH_Msk                (1UL << SCB_DFSR_VCATCH_Pos)                   /*!< SCB DFSR: VCATCH Mask */

-

-#define SCB_DFSR_DWTTRAP_Pos                2                                             /*!< SCB DFSR: DWTTRAP Position */

-#define SCB_DFSR_DWTTRAP_Msk               (1UL << SCB_DFSR_DWTTRAP_Pos)                  /*!< SCB DFSR: DWTTRAP Mask */

-

-#define SCB_DFSR_BKPT_Pos                   1                                             /*!< SCB DFSR: BKPT Position */

-#define SCB_DFSR_BKPT_Msk                  (1UL << SCB_DFSR_BKPT_Pos)                     /*!< SCB DFSR: BKPT Mask */

-

-#define SCB_DFSR_HALTED_Pos                 0                                             /*!< SCB DFSR: HALTED Position */

-#define SCB_DFSR_HALTED_Msk                (1UL << SCB_DFSR_HALTED_Pos)                   /*!< SCB DFSR: HALTED Mask */

-

-/*@} end of group CMSIS_SCB */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)

-    \brief      Type definitions for the System Control and ID Register not in the SCB

-  @{

- */

-

-/** \brief  Structure type to access the System Control and ID Register not in the SCB.

- */

-typedef struct

-{

-       uint32_t RESERVED0[1];

-  __I  uint32_t ICTR;                    /*!< Offset: 0x004 (R/ )  Interrupt Controller Type Register      */

-#if ((defined __CM3_REV) && (__CM3_REV >= 0x200))

-  __IO uint32_t ACTLR;                   /*!< Offset: 0x008 (R/W)  Auxiliary Control Register      */

-#else

-       uint32_t RESERVED1[1];

-#endif

-} SCnSCB_Type;

-

-/* Interrupt Controller Type Register Definitions */

-#define SCnSCB_ICTR_INTLINESNUM_Pos         0                                          /*!< ICTR: INTLINESNUM Position */

-#define SCnSCB_ICTR_INTLINESNUM_Msk        (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos)      /*!< ICTR: INTLINESNUM Mask */

-

-/* Auxiliary Control Register Definitions */

-

-#define SCnSCB_ACTLR_DISFOLD_Pos            2                                          /*!< ACTLR: DISFOLD Position */

-#define SCnSCB_ACTLR_DISFOLD_Msk           (1UL << SCnSCB_ACTLR_DISFOLD_Pos)           /*!< ACTLR: DISFOLD Mask */

-

-#define SCnSCB_ACTLR_DISDEFWBUF_Pos         1                                          /*!< ACTLR: DISDEFWBUF Position */

-#define SCnSCB_ACTLR_DISDEFWBUF_Msk        (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos)        /*!< ACTLR: DISDEFWBUF Mask */

-

-#define SCnSCB_ACTLR_DISMCYCINT_Pos         0                                          /*!< ACTLR: DISMCYCINT Position */

-#define SCnSCB_ACTLR_DISMCYCINT_Msk        (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos)        /*!< ACTLR: DISMCYCINT Mask */

-

-/*@} end of group CMSIS_SCnotSCB */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_SysTick     System Tick Timer (SysTick)

-    \brief      Type definitions for the System Timer Registers.

-  @{

- */

-

-/** \brief  Structure type to access the System Timer (SysTick).

- */

-typedef struct

-{

-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */

-  __IO uint32_t LOAD;                    /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register       */

-  __IO uint32_t VAL;                     /*!< Offset: 0x008 (R/W)  SysTick Current Value Register      */

-  __I  uint32_t CALIB;                   /*!< Offset: 0x00C (R/ )  SysTick Calibration Register        */

-} SysTick_Type;

-

-/* SysTick Control / Status Register Definitions */

-#define SysTick_CTRL_COUNTFLAG_Pos         16                                             /*!< SysTick CTRL: COUNTFLAG Position */

-#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */

-

-#define SysTick_CTRL_CLKSOURCE_Pos          2                                             /*!< SysTick CTRL: CLKSOURCE Position */

-#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */

-

-#define SysTick_CTRL_TICKINT_Pos            1                                             /*!< SysTick CTRL: TICKINT Position */

-#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */

-

-#define SysTick_CTRL_ENABLE_Pos             0                                             /*!< SysTick CTRL: ENABLE Position */

-#define SysTick_CTRL_ENABLE_Msk            (1UL << SysTick_CTRL_ENABLE_Pos)               /*!< SysTick CTRL: ENABLE Mask */

-

-/* SysTick Reload Register Definitions */

-#define SysTick_LOAD_RELOAD_Pos             0                                             /*!< SysTick LOAD: RELOAD Position */

-#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos)        /*!< SysTick LOAD: RELOAD Mask */

-

-/* SysTick Current Register Definitions */

-#define SysTick_VAL_CURRENT_Pos             0                                             /*!< SysTick VAL: CURRENT Position */

-#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick VAL: CURRENT Mask */

-

-/* SysTick Calibration Register Definitions */

-#define SysTick_CALIB_NOREF_Pos            31                                             /*!< SysTick CALIB: NOREF Position */

-#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */

-

-#define SysTick_CALIB_SKEW_Pos             30                                             /*!< SysTick CALIB: SKEW Position */

-#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */

-

-#define SysTick_CALIB_TENMS_Pos             0                                             /*!< SysTick CALIB: TENMS Position */

-#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick CALIB: TENMS Mask */

-

-/*@} end of group CMSIS_SysTick */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_ITM     Instrumentation Trace Macrocell (ITM)

-    \brief      Type definitions for the Instrumentation Trace Macrocell (ITM)

-  @{

- */

-

-/** \brief  Structure type to access the Instrumentation Trace Macrocell Register (ITM).

- */

-typedef struct

-{

-  __O  union

-  {

-    __O  uint8_t    u8;                  /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 8-bit                   */

-    __O  uint16_t   u16;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 16-bit                  */

-    __O  uint32_t   u32;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 32-bit                  */

-  }  PORT [32];                          /*!< Offset: 0x000 ( /W)  ITM Stimulus Port Registers               */

-       uint32_t RESERVED0[864];

-  __IO uint32_t TER;                     /*!< Offset: 0xE00 (R/W)  ITM Trace Enable Register                 */

-       uint32_t RESERVED1[15];

-  __IO uint32_t TPR;                     /*!< Offset: 0xE40 (R/W)  ITM Trace Privilege Register              */

-       uint32_t RESERVED2[15];

-  __IO uint32_t TCR;                     /*!< Offset: 0xE80 (R/W)  ITM Trace Control Register                */

-       uint32_t RESERVED3[29];

-  __O  uint32_t IWR;                     /*!< Offset: 0xEF8 ( /W)  ITM Integration Write Register            */

-  __I  uint32_t IRR;                     /*!< Offset: 0xEFC (R/ )  ITM Integration Read Register             */

-  __IO uint32_t IMCR;                    /*!< Offset: 0xF00 (R/W)  ITM Integration Mode Control Register     */

-       uint32_t RESERVED4[43];

-  __O  uint32_t LAR;                     /*!< Offset: 0xFB0 ( /W)  ITM Lock Access Register                  */

-  __I  uint32_t LSR;                     /*!< Offset: 0xFB4 (R/ )  ITM Lock Status Register                  */

-       uint32_t RESERVED5[6];

-  __I  uint32_t PID4;                    /*!< Offset: 0xFD0 (R/ )  ITM Peripheral Identification Register #4 */

-  __I  uint32_t PID5;                    /*!< Offset: 0xFD4 (R/ )  ITM Peripheral Identification Register #5 */

-  __I  uint32_t PID6;                    /*!< Offset: 0xFD8 (R/ )  ITM Peripheral Identification Register #6 */

-  __I  uint32_t PID7;                    /*!< Offset: 0xFDC (R/ )  ITM Peripheral Identification Register #7 */

-  __I  uint32_t PID0;                    /*!< Offset: 0xFE0 (R/ )  ITM Peripheral Identification Register #0 */

-  __I  uint32_t PID1;                    /*!< Offset: 0xFE4 (R/ )  ITM Peripheral Identification Register #1 */

-  __I  uint32_t PID2;                    /*!< Offset: 0xFE8 (R/ )  ITM Peripheral Identification Register #2 */

-  __I  uint32_t PID3;                    /*!< Offset: 0xFEC (R/ )  ITM Peripheral Identification Register #3 */

-  __I  uint32_t CID0;                    /*!< Offset: 0xFF0 (R/ )  ITM Component  Identification Register #0 */

-  __I  uint32_t CID1;                    /*!< Offset: 0xFF4 (R/ )  ITM Component  Identification Register #1 */

-  __I  uint32_t CID2;                    /*!< Offset: 0xFF8 (R/ )  ITM Component  Identification Register #2 */

-  __I  uint32_t CID3;                    /*!< Offset: 0xFFC (R/ )  ITM Component  Identification Register #3 */

-} ITM_Type;

-

-/* ITM Trace Privilege Register Definitions */

-#define ITM_TPR_PRIVMASK_Pos                0                                             /*!< ITM TPR: PRIVMASK Position */

-#define ITM_TPR_PRIVMASK_Msk               (0xFUL << ITM_TPR_PRIVMASK_Pos)                /*!< ITM TPR: PRIVMASK Mask */

-

-/* ITM Trace Control Register Definitions */

-#define ITM_TCR_BUSY_Pos                   23                                             /*!< ITM TCR: BUSY Position */

-#define ITM_TCR_BUSY_Msk                   (1UL << ITM_TCR_BUSY_Pos)                      /*!< ITM TCR: BUSY Mask */

-

-#define ITM_TCR_TraceBusID_Pos             16                                             /*!< ITM TCR: ATBID Position */

-#define ITM_TCR_TraceBusID_Msk             (0x7FUL << ITM_TCR_TraceBusID_Pos)             /*!< ITM TCR: ATBID Mask */

-

-#define ITM_TCR_GTSFREQ_Pos                10                                             /*!< ITM TCR: Global timestamp frequency Position */

-#define ITM_TCR_GTSFREQ_Msk                (3UL << ITM_TCR_GTSFREQ_Pos)                   /*!< ITM TCR: Global timestamp frequency Mask */

-

-#define ITM_TCR_TSPrescale_Pos              8                                             /*!< ITM TCR: TSPrescale Position */

-#define ITM_TCR_TSPrescale_Msk             (3UL << ITM_TCR_TSPrescale_Pos)                /*!< ITM TCR: TSPrescale Mask */

-

-#define ITM_TCR_SWOENA_Pos                  4                                             /*!< ITM TCR: SWOENA Position */

-#define ITM_TCR_SWOENA_Msk                 (1UL << ITM_TCR_SWOENA_Pos)                    /*!< ITM TCR: SWOENA Mask */

-

-#define ITM_TCR_DWTENA_Pos                  3                                             /*!< ITM TCR: DWTENA Position */

-#define ITM_TCR_DWTENA_Msk                 (1UL << ITM_TCR_DWTENA_Pos)                    /*!< ITM TCR: DWTENA Mask */

-

-#define ITM_TCR_SYNCENA_Pos                 2                                             /*!< ITM TCR: SYNCENA Position */

-#define ITM_TCR_SYNCENA_Msk                (1UL << ITM_TCR_SYNCENA_Pos)                   /*!< ITM TCR: SYNCENA Mask */

-

-#define ITM_TCR_TSENA_Pos                   1                                             /*!< ITM TCR: TSENA Position */

-#define ITM_TCR_TSENA_Msk                  (1UL << ITM_TCR_TSENA_Pos)                     /*!< ITM TCR: TSENA Mask */

-

-#define ITM_TCR_ITMENA_Pos                  0                                             /*!< ITM TCR: ITM Enable bit Position */

-#define ITM_TCR_ITMENA_Msk                 (1UL << ITM_TCR_ITMENA_Pos)                    /*!< ITM TCR: ITM Enable bit Mask */

-

-/* ITM Integration Write Register Definitions */

-#define ITM_IWR_ATVALIDM_Pos                0                                             /*!< ITM IWR: ATVALIDM Position */

-#define ITM_IWR_ATVALIDM_Msk               (1UL << ITM_IWR_ATVALIDM_Pos)                  /*!< ITM IWR: ATVALIDM Mask */

-

-/* ITM Integration Read Register Definitions */

-#define ITM_IRR_ATREADYM_Pos                0                                             /*!< ITM IRR: ATREADYM Position */

-#define ITM_IRR_ATREADYM_Msk               (1UL << ITM_IRR_ATREADYM_Pos)                  /*!< ITM IRR: ATREADYM Mask */

-

-/* ITM Integration Mode Control Register Definitions */

-#define ITM_IMCR_INTEGRATION_Pos            0                                             /*!< ITM IMCR: INTEGRATION Position */

-#define ITM_IMCR_INTEGRATION_Msk           (1UL << ITM_IMCR_INTEGRATION_Pos)              /*!< ITM IMCR: INTEGRATION Mask */

-

-/* ITM Lock Status Register Definitions */

-#define ITM_LSR_ByteAcc_Pos                 2                                             /*!< ITM LSR: ByteAcc Position */

-#define ITM_LSR_ByteAcc_Msk                (1UL << ITM_LSR_ByteAcc_Pos)                   /*!< ITM LSR: ByteAcc Mask */

-

-#define ITM_LSR_Access_Pos                  1                                             /*!< ITM LSR: Access Position */

-#define ITM_LSR_Access_Msk                 (1UL << ITM_LSR_Access_Pos)                    /*!< ITM LSR: Access Mask */

-

-#define ITM_LSR_Present_Pos                 0                                             /*!< ITM LSR: Present Position */

-#define ITM_LSR_Present_Msk                (1UL << ITM_LSR_Present_Pos)                   /*!< ITM LSR: Present Mask */

-

-/*@}*/ /* end of group CMSIS_ITM */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_DWT     Data Watchpoint and Trace (DWT)

-    \brief      Type definitions for the Data Watchpoint and Trace (DWT)

-  @{

- */

-

-/** \brief  Structure type to access the Data Watchpoint and Trace Register (DWT).

- */

-typedef struct

-{

-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  Control Register                          */

-  __IO uint32_t CYCCNT;                  /*!< Offset: 0x004 (R/W)  Cycle Count Register                      */

-  __IO uint32_t CPICNT;                  /*!< Offset: 0x008 (R/W)  CPI Count Register                        */

-  __IO uint32_t EXCCNT;                  /*!< Offset: 0x00C (R/W)  Exception Overhead Count Register         */

-  __IO uint32_t SLEEPCNT;                /*!< Offset: 0x010 (R/W)  Sleep Count Register                      */

-  __IO uint32_t LSUCNT;                  /*!< Offset: 0x014 (R/W)  LSU Count Register                        */

-  __IO uint32_t FOLDCNT;                 /*!< Offset: 0x018 (R/W)  Folded-instruction Count Register         */

-  __I  uint32_t PCSR;                    /*!< Offset: 0x01C (R/ )  Program Counter Sample Register           */

-  __IO uint32_t COMP0;                   /*!< Offset: 0x020 (R/W)  Comparator Register 0                     */

-  __IO uint32_t MASK0;                   /*!< Offset: 0x024 (R/W)  Mask Register 0                           */

-  __IO uint32_t FUNCTION0;               /*!< Offset: 0x028 (R/W)  Function Register 0                       */

-       uint32_t RESERVED0[1];

-  __IO uint32_t COMP1;                   /*!< Offset: 0x030 (R/W)  Comparator Register 1                     */

-  __IO uint32_t MASK1;                   /*!< Offset: 0x034 (R/W)  Mask Register 1                           */

-  __IO uint32_t FUNCTION1;               /*!< Offset: 0x038 (R/W)  Function Register 1                       */

-       uint32_t RESERVED1[1];

-  __IO uint32_t COMP2;                   /*!< Offset: 0x040 (R/W)  Comparator Register 2                     */

-  __IO uint32_t MASK2;                   /*!< Offset: 0x044 (R/W)  Mask Register 2                           */

-  __IO uint32_t FUNCTION2;               /*!< Offset: 0x048 (R/W)  Function Register 2                       */

-       uint32_t RESERVED2[1];

-  __IO uint32_t COMP3;                   /*!< Offset: 0x050 (R/W)  Comparator Register 3                     */

-  __IO uint32_t MASK3;                   /*!< Offset: 0x054 (R/W)  Mask Register 3                           */

-  __IO uint32_t FUNCTION3;               /*!< Offset: 0x058 (R/W)  Function Register 3                       */

-} DWT_Type;

-

-/* DWT Control Register Definitions */

-#define DWT_CTRL_NUMCOMP_Pos               28                                          /*!< DWT CTRL: NUMCOMP Position */

-#define DWT_CTRL_NUMCOMP_Msk               (0xFUL << DWT_CTRL_NUMCOMP_Pos)             /*!< DWT CTRL: NUMCOMP Mask */

-

-#define DWT_CTRL_NOTRCPKT_Pos              27                                          /*!< DWT CTRL: NOTRCPKT Position */

-#define DWT_CTRL_NOTRCPKT_Msk              (0x1UL << DWT_CTRL_NOTRCPKT_Pos)            /*!< DWT CTRL: NOTRCPKT Mask */

-

-#define DWT_CTRL_NOEXTTRIG_Pos             26                                          /*!< DWT CTRL: NOEXTTRIG Position */

-#define DWT_CTRL_NOEXTTRIG_Msk             (0x1UL << DWT_CTRL_NOEXTTRIG_Pos)           /*!< DWT CTRL: NOEXTTRIG Mask */

-

-#define DWT_CTRL_NOCYCCNT_Pos              25                                          /*!< DWT CTRL: NOCYCCNT Position */

-#define DWT_CTRL_NOCYCCNT_Msk              (0x1UL << DWT_CTRL_NOCYCCNT_Pos)            /*!< DWT CTRL: NOCYCCNT Mask */

-

-#define DWT_CTRL_NOPRFCNT_Pos              24                                          /*!< DWT CTRL: NOPRFCNT Position */

-#define DWT_CTRL_NOPRFCNT_Msk              (0x1UL << DWT_CTRL_NOPRFCNT_Pos)            /*!< DWT CTRL: NOPRFCNT Mask */

-

-#define DWT_CTRL_CYCEVTENA_Pos             22                                          /*!< DWT CTRL: CYCEVTENA Position */

-#define DWT_CTRL_CYCEVTENA_Msk             (0x1UL << DWT_CTRL_CYCEVTENA_Pos)           /*!< DWT CTRL: CYCEVTENA Mask */

-

-#define DWT_CTRL_FOLDEVTENA_Pos            21                                          /*!< DWT CTRL: FOLDEVTENA Position */

-#define DWT_CTRL_FOLDEVTENA_Msk            (0x1UL << DWT_CTRL_FOLDEVTENA_Pos)          /*!< DWT CTRL: FOLDEVTENA Mask */

-

-#define DWT_CTRL_LSUEVTENA_Pos             20                                          /*!< DWT CTRL: LSUEVTENA Position */

-#define DWT_CTRL_LSUEVTENA_Msk             (0x1UL << DWT_CTRL_LSUEVTENA_Pos)           /*!< DWT CTRL: LSUEVTENA Mask */

-

-#define DWT_CTRL_SLEEPEVTENA_Pos           19                                          /*!< DWT CTRL: SLEEPEVTENA Position */

-#define DWT_CTRL_SLEEPEVTENA_Msk           (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos)         /*!< DWT CTRL: SLEEPEVTENA Mask */

-

-#define DWT_CTRL_EXCEVTENA_Pos             18                                          /*!< DWT CTRL: EXCEVTENA Position */

-#define DWT_CTRL_EXCEVTENA_Msk             (0x1UL << DWT_CTRL_EXCEVTENA_Pos)           /*!< DWT CTRL: EXCEVTENA Mask */

-

-#define DWT_CTRL_CPIEVTENA_Pos             17                                          /*!< DWT CTRL: CPIEVTENA Position */

-#define DWT_CTRL_CPIEVTENA_Msk             (0x1UL << DWT_CTRL_CPIEVTENA_Pos)           /*!< DWT CTRL: CPIEVTENA Mask */

-

-#define DWT_CTRL_EXCTRCENA_Pos             16                                          /*!< DWT CTRL: EXCTRCENA Position */

-#define DWT_CTRL_EXCTRCENA_Msk             (0x1UL << DWT_CTRL_EXCTRCENA_Pos)           /*!< DWT CTRL: EXCTRCENA Mask */

-

-#define DWT_CTRL_PCSAMPLENA_Pos            12                                          /*!< DWT CTRL: PCSAMPLENA Position */

-#define DWT_CTRL_PCSAMPLENA_Msk            (0x1UL << DWT_CTRL_PCSAMPLENA_Pos)          /*!< DWT CTRL: PCSAMPLENA Mask */

-

-#define DWT_CTRL_SYNCTAP_Pos               10                                          /*!< DWT CTRL: SYNCTAP Position */

-#define DWT_CTRL_SYNCTAP_Msk               (0x3UL << DWT_CTRL_SYNCTAP_Pos)             /*!< DWT CTRL: SYNCTAP Mask */

-

-#define DWT_CTRL_CYCTAP_Pos                 9                                          /*!< DWT CTRL: CYCTAP Position */

-#define DWT_CTRL_CYCTAP_Msk                (0x1UL << DWT_CTRL_CYCTAP_Pos)              /*!< DWT CTRL: CYCTAP Mask */

-

-#define DWT_CTRL_POSTINIT_Pos               5                                          /*!< DWT CTRL: POSTINIT Position */

-#define DWT_CTRL_POSTINIT_Msk              (0xFUL << DWT_CTRL_POSTINIT_Pos)            /*!< DWT CTRL: POSTINIT Mask */

-

-#define DWT_CTRL_POSTPRESET_Pos             1                                          /*!< DWT CTRL: POSTPRESET Position */

-#define DWT_CTRL_POSTPRESET_Msk            (0xFUL << DWT_CTRL_POSTPRESET_Pos)          /*!< DWT CTRL: POSTPRESET Mask */

-

-#define DWT_CTRL_CYCCNTENA_Pos              0                                          /*!< DWT CTRL: CYCCNTENA Position */

-#define DWT_CTRL_CYCCNTENA_Msk             (0x1UL << DWT_CTRL_CYCCNTENA_Pos)           /*!< DWT CTRL: CYCCNTENA Mask */

-

-/* DWT CPI Count Register Definitions */

-#define DWT_CPICNT_CPICNT_Pos               0                                          /*!< DWT CPICNT: CPICNT Position */

-#define DWT_CPICNT_CPICNT_Msk              (0xFFUL << DWT_CPICNT_CPICNT_Pos)           /*!< DWT CPICNT: CPICNT Mask */

-

-/* DWT Exception Overhead Count Register Definitions */

-#define DWT_EXCCNT_EXCCNT_Pos               0                                          /*!< DWT EXCCNT: EXCCNT Position */

-#define DWT_EXCCNT_EXCCNT_Msk              (0xFFUL << DWT_EXCCNT_EXCCNT_Pos)           /*!< DWT EXCCNT: EXCCNT Mask */

-

-/* DWT Sleep Count Register Definitions */

-#define DWT_SLEEPCNT_SLEEPCNT_Pos           0                                          /*!< DWT SLEEPCNT: SLEEPCNT Position */

-#define DWT_SLEEPCNT_SLEEPCNT_Msk          (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos)       /*!< DWT SLEEPCNT: SLEEPCNT Mask */

-

-/* DWT LSU Count Register Definitions */

-#define DWT_LSUCNT_LSUCNT_Pos               0                                          /*!< DWT LSUCNT: LSUCNT Position */

-#define DWT_LSUCNT_LSUCNT_Msk              (0xFFUL << DWT_LSUCNT_LSUCNT_Pos)           /*!< DWT LSUCNT: LSUCNT Mask */

-

-/* DWT Folded-instruction Count Register Definitions */

-#define DWT_FOLDCNT_FOLDCNT_Pos             0                                          /*!< DWT FOLDCNT: FOLDCNT Position */

-#define DWT_FOLDCNT_FOLDCNT_Msk            (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos)         /*!< DWT FOLDCNT: FOLDCNT Mask */

-

-/* DWT Comparator Mask Register Definitions */

-#define DWT_MASK_MASK_Pos                   0                                          /*!< DWT MASK: MASK Position */

-#define DWT_MASK_MASK_Msk                  (0x1FUL << DWT_MASK_MASK_Pos)               /*!< DWT MASK: MASK Mask */

-

-/* DWT Comparator Function Register Definitions */

-#define DWT_FUNCTION_MATCHED_Pos           24                                          /*!< DWT FUNCTION: MATCHED Position */

-#define DWT_FUNCTION_MATCHED_Msk           (0x1UL << DWT_FUNCTION_MATCHED_Pos)         /*!< DWT FUNCTION: MATCHED Mask */

-

-#define DWT_FUNCTION_DATAVADDR1_Pos        16                                          /*!< DWT FUNCTION: DATAVADDR1 Position */

-#define DWT_FUNCTION_DATAVADDR1_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos)      /*!< DWT FUNCTION: DATAVADDR1 Mask */

-

-#define DWT_FUNCTION_DATAVADDR0_Pos        12                                          /*!< DWT FUNCTION: DATAVADDR0 Position */

-#define DWT_FUNCTION_DATAVADDR0_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos)      /*!< DWT FUNCTION: DATAVADDR0 Mask */

-

-#define DWT_FUNCTION_DATAVSIZE_Pos         10                                          /*!< DWT FUNCTION: DATAVSIZE Position */

-#define DWT_FUNCTION_DATAVSIZE_Msk         (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos)       /*!< DWT FUNCTION: DATAVSIZE Mask */

-

-#define DWT_FUNCTION_LNK1ENA_Pos            9                                          /*!< DWT FUNCTION: LNK1ENA Position */

-#define DWT_FUNCTION_LNK1ENA_Msk           (0x1UL << DWT_FUNCTION_LNK1ENA_Pos)         /*!< DWT FUNCTION: LNK1ENA Mask */

-

-#define DWT_FUNCTION_DATAVMATCH_Pos         8                                          /*!< DWT FUNCTION: DATAVMATCH Position */

-#define DWT_FUNCTION_DATAVMATCH_Msk        (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos)      /*!< DWT FUNCTION: DATAVMATCH Mask */

-

-#define DWT_FUNCTION_CYCMATCH_Pos           7                                          /*!< DWT FUNCTION: CYCMATCH Position */

-#define DWT_FUNCTION_CYCMATCH_Msk          (0x1UL << DWT_FUNCTION_CYCMATCH_Pos)        /*!< DWT FUNCTION: CYCMATCH Mask */

-

-#define DWT_FUNCTION_EMITRANGE_Pos          5                                          /*!< DWT FUNCTION: EMITRANGE Position */

-#define DWT_FUNCTION_EMITRANGE_Msk         (0x1UL << DWT_FUNCTION_EMITRANGE_Pos)       /*!< DWT FUNCTION: EMITRANGE Mask */

-

-#define DWT_FUNCTION_FUNCTION_Pos           0                                          /*!< DWT FUNCTION: FUNCTION Position */

-#define DWT_FUNCTION_FUNCTION_Msk          (0xFUL << DWT_FUNCTION_FUNCTION_Pos)        /*!< DWT FUNCTION: FUNCTION Mask */

-

-/*@}*/ /* end of group CMSIS_DWT */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_TPI     Trace Port Interface (TPI)

-    \brief      Type definitions for the Trace Port Interface (TPI)

-  @{

- */

-

-/** \brief  Structure type to access the Trace Port Interface Register (TPI).

- */

-typedef struct

-{

-  __IO uint32_t SSPSR;                   /*!< Offset: 0x000 (R/ )  Supported Parallel Port Size Register     */

-  __IO uint32_t CSPSR;                   /*!< Offset: 0x004 (R/W)  Current Parallel Port Size Register */

-       uint32_t RESERVED0[2];

-  __IO uint32_t ACPR;                    /*!< Offset: 0x010 (R/W)  Asynchronous Clock Prescaler Register */

-       uint32_t RESERVED1[55];

-  __IO uint32_t SPPR;                    /*!< Offset: 0x0F0 (R/W)  Selected Pin Protocol Register */

-       uint32_t RESERVED2[131];

-  __I  uint32_t FFSR;                    /*!< Offset: 0x300 (R/ )  Formatter and Flush Status Register */

-  __IO uint32_t FFCR;                    /*!< Offset: 0x304 (R/W)  Formatter and Flush Control Register */

-  __I  uint32_t FSCR;                    /*!< Offset: 0x308 (R/ )  Formatter Synchronization Counter Register */

-       uint32_t RESERVED3[759];

-  __I  uint32_t TRIGGER;                 /*!< Offset: 0xEE8 (R/ )  TRIGGER */

-  __I  uint32_t FIFO0;                   /*!< Offset: 0xEEC (R/ )  Integration ETM Data */

-  __I  uint32_t ITATBCTR2;               /*!< Offset: 0xEF0 (R/ )  ITATBCTR2 */

-       uint32_t RESERVED4[1];

-  __I  uint32_t ITATBCTR0;               /*!< Offset: 0xEF8 (R/ )  ITATBCTR0 */

-  __I  uint32_t FIFO1;                   /*!< Offset: 0xEFC (R/ )  Integration ITM Data */

-  __IO uint32_t ITCTRL;                  /*!< Offset: 0xF00 (R/W)  Integration Mode Control */

-       uint32_t RESERVED5[39];

-  __IO uint32_t CLAIMSET;                /*!< Offset: 0xFA0 (R/W)  Claim tag set */

-  __IO uint32_t CLAIMCLR;                /*!< Offset: 0xFA4 (R/W)  Claim tag clear */

-       uint32_t RESERVED7[8];

-  __I  uint32_t DEVID;                   /*!< Offset: 0xFC8 (R/ )  TPIU_DEVID */

-  __I  uint32_t DEVTYPE;                 /*!< Offset: 0xFCC (R/ )  TPIU_DEVTYPE */

-} TPI_Type;

-

-/* TPI Asynchronous Clock Prescaler Register Definitions */

-#define TPI_ACPR_PRESCALER_Pos              0                                          /*!< TPI ACPR: PRESCALER Position */

-#define TPI_ACPR_PRESCALER_Msk             (0x1FFFUL << TPI_ACPR_PRESCALER_Pos)        /*!< TPI ACPR: PRESCALER Mask */

-

-/* TPI Selected Pin Protocol Register Definitions */

-#define TPI_SPPR_TXMODE_Pos                 0                                          /*!< TPI SPPR: TXMODE Position */

-#define TPI_SPPR_TXMODE_Msk                (0x3UL << TPI_SPPR_TXMODE_Pos)              /*!< TPI SPPR: TXMODE Mask */

-

-/* TPI Formatter and Flush Status Register Definitions */

-#define TPI_FFSR_FtNonStop_Pos              3                                          /*!< TPI FFSR: FtNonStop Position */

-#define TPI_FFSR_FtNonStop_Msk             (0x1UL << TPI_FFSR_FtNonStop_Pos)           /*!< TPI FFSR: FtNonStop Mask */

-

-#define TPI_FFSR_TCPresent_Pos              2                                          /*!< TPI FFSR: TCPresent Position */

-#define TPI_FFSR_TCPresent_Msk             (0x1UL << TPI_FFSR_TCPresent_Pos)           /*!< TPI FFSR: TCPresent Mask */

-

-#define TPI_FFSR_FtStopped_Pos              1                                          /*!< TPI FFSR: FtStopped Position */

-#define TPI_FFSR_FtStopped_Msk             (0x1UL << TPI_FFSR_FtStopped_Pos)           /*!< TPI FFSR: FtStopped Mask */

-

-#define TPI_FFSR_FlInProg_Pos               0                                          /*!< TPI FFSR: FlInProg Position */

-#define TPI_FFSR_FlInProg_Msk              (0x1UL << TPI_FFSR_FlInProg_Pos)            /*!< TPI FFSR: FlInProg Mask */

-

-/* TPI Formatter and Flush Control Register Definitions */

-#define TPI_FFCR_TrigIn_Pos                 8                                          /*!< TPI FFCR: TrigIn Position */

-#define TPI_FFCR_TrigIn_Msk                (0x1UL << TPI_FFCR_TrigIn_Pos)              /*!< TPI FFCR: TrigIn Mask */

-

-#define TPI_FFCR_EnFCont_Pos                1                                          /*!< TPI FFCR: EnFCont Position */

-#define TPI_FFCR_EnFCont_Msk               (0x1UL << TPI_FFCR_EnFCont_Pos)             /*!< TPI FFCR: EnFCont Mask */

-

-/* TPI TRIGGER Register Definitions */

-#define TPI_TRIGGER_TRIGGER_Pos             0                                          /*!< TPI TRIGGER: TRIGGER Position */

-#define TPI_TRIGGER_TRIGGER_Msk            (0x1UL << TPI_TRIGGER_TRIGGER_Pos)          /*!< TPI TRIGGER: TRIGGER Mask */

-

-/* TPI Integration ETM Data Register Definitions (FIFO0) */

-#define TPI_FIFO0_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO0: ITM_ATVALID Position */

-#define TPI_FIFO0_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos)        /*!< TPI FIFO0: ITM_ATVALID Mask */

-

-#define TPI_FIFO0_ITM_bytecount_Pos        27                                          /*!< TPI FIFO0: ITM_bytecount Position */

-#define TPI_FIFO0_ITM_bytecount_Msk        (0x3UL << TPI_FIFO0_ITM_bytecount_Pos)      /*!< TPI FIFO0: ITM_bytecount Mask */

-

-#define TPI_FIFO0_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO0: ETM_ATVALID Position */

-#define TPI_FIFO0_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos)        /*!< TPI FIFO0: ETM_ATVALID Mask */

-

-#define TPI_FIFO0_ETM_bytecount_Pos        24                                          /*!< TPI FIFO0: ETM_bytecount Position */

-#define TPI_FIFO0_ETM_bytecount_Msk        (0x3UL << TPI_FIFO0_ETM_bytecount_Pos)      /*!< TPI FIFO0: ETM_bytecount Mask */

-

-#define TPI_FIFO0_ETM2_Pos                 16                                          /*!< TPI FIFO0: ETM2 Position */

-#define TPI_FIFO0_ETM2_Msk                 (0xFFUL << TPI_FIFO0_ETM2_Pos)              /*!< TPI FIFO0: ETM2 Mask */

-

-#define TPI_FIFO0_ETM1_Pos                  8                                          /*!< TPI FIFO0: ETM1 Position */

-#define TPI_FIFO0_ETM1_Msk                 (0xFFUL << TPI_FIFO0_ETM1_Pos)              /*!< TPI FIFO0: ETM1 Mask */

-

-#define TPI_FIFO0_ETM0_Pos                  0                                          /*!< TPI FIFO0: ETM0 Position */

-#define TPI_FIFO0_ETM0_Msk                 (0xFFUL << TPI_FIFO0_ETM0_Pos)              /*!< TPI FIFO0: ETM0 Mask */

-

-/* TPI ITATBCTR2 Register Definitions */

-#define TPI_ITATBCTR2_ATREADY_Pos           0                                          /*!< TPI ITATBCTR2: ATREADY Position */

-#define TPI_ITATBCTR2_ATREADY_Msk          (0x1UL << TPI_ITATBCTR2_ATREADY_Pos)        /*!< TPI ITATBCTR2: ATREADY Mask */

-

-/* TPI Integration ITM Data Register Definitions (FIFO1) */

-#define TPI_FIFO1_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO1: ITM_ATVALID Position */

-#define TPI_FIFO1_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos)        /*!< TPI FIFO1: ITM_ATVALID Mask */

-

-#define TPI_FIFO1_ITM_bytecount_Pos        27                                          /*!< TPI FIFO1: ITM_bytecount Position */

-#define TPI_FIFO1_ITM_bytecount_Msk        (0x3UL << TPI_FIFO1_ITM_bytecount_Pos)      /*!< TPI FIFO1: ITM_bytecount Mask */

-

-#define TPI_FIFO1_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO1: ETM_ATVALID Position */

-#define TPI_FIFO1_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos)        /*!< TPI FIFO1: ETM_ATVALID Mask */

-

-#define TPI_FIFO1_ETM_bytecount_Pos        24                                          /*!< TPI FIFO1: ETM_bytecount Position */

-#define TPI_FIFO1_ETM_bytecount_Msk        (0x3UL << TPI_FIFO1_ETM_bytecount_Pos)      /*!< TPI FIFO1: ETM_bytecount Mask */

-

-#define TPI_FIFO1_ITM2_Pos                 16                                          /*!< TPI FIFO1: ITM2 Position */

-#define TPI_FIFO1_ITM2_Msk                 (0xFFUL << TPI_FIFO1_ITM2_Pos)              /*!< TPI FIFO1: ITM2 Mask */

-

-#define TPI_FIFO1_ITM1_Pos                  8                                          /*!< TPI FIFO1: ITM1 Position */

-#define TPI_FIFO1_ITM1_Msk                 (0xFFUL << TPI_FIFO1_ITM1_Pos)              /*!< TPI FIFO1: ITM1 Mask */

-

-#define TPI_FIFO1_ITM0_Pos                  0                                          /*!< TPI FIFO1: ITM0 Position */

-#define TPI_FIFO1_ITM0_Msk                 (0xFFUL << TPI_FIFO1_ITM0_Pos)              /*!< TPI FIFO1: ITM0 Mask */

-

-/* TPI ITATBCTR0 Register Definitions */

-#define TPI_ITATBCTR0_ATREADY_Pos           0                                          /*!< TPI ITATBCTR0: ATREADY Position */

-#define TPI_ITATBCTR0_ATREADY_Msk          (0x1UL << TPI_ITATBCTR0_ATREADY_Pos)        /*!< TPI ITATBCTR0: ATREADY Mask */

-

-/* TPI Integration Mode Control Register Definitions */

-#define TPI_ITCTRL_Mode_Pos                 0                                          /*!< TPI ITCTRL: Mode Position */

-#define TPI_ITCTRL_Mode_Msk                (0x1UL << TPI_ITCTRL_Mode_Pos)              /*!< TPI ITCTRL: Mode Mask */

-

-/* TPI DEVID Register Definitions */

-#define TPI_DEVID_NRZVALID_Pos             11                                          /*!< TPI DEVID: NRZVALID Position */

-#define TPI_DEVID_NRZVALID_Msk             (0x1UL << TPI_DEVID_NRZVALID_Pos)           /*!< TPI DEVID: NRZVALID Mask */

-

-#define TPI_DEVID_MANCVALID_Pos            10                                          /*!< TPI DEVID: MANCVALID Position */

-#define TPI_DEVID_MANCVALID_Msk            (0x1UL << TPI_DEVID_MANCVALID_Pos)          /*!< TPI DEVID: MANCVALID Mask */

-

-#define TPI_DEVID_PTINVALID_Pos             9                                          /*!< TPI DEVID: PTINVALID Position */

-#define TPI_DEVID_PTINVALID_Msk            (0x1UL << TPI_DEVID_PTINVALID_Pos)          /*!< TPI DEVID: PTINVALID Mask */

-

-#define TPI_DEVID_MinBufSz_Pos              6                                          /*!< TPI DEVID: MinBufSz Position */

-#define TPI_DEVID_MinBufSz_Msk             (0x7UL << TPI_DEVID_MinBufSz_Pos)           /*!< TPI DEVID: MinBufSz Mask */

-

-#define TPI_DEVID_AsynClkIn_Pos             5                                          /*!< TPI DEVID: AsynClkIn Position */

-#define TPI_DEVID_AsynClkIn_Msk            (0x1UL << TPI_DEVID_AsynClkIn_Pos)          /*!< TPI DEVID: AsynClkIn Mask */

-

-#define TPI_DEVID_NrTraceInput_Pos          0                                          /*!< TPI DEVID: NrTraceInput Position */

-#define TPI_DEVID_NrTraceInput_Msk         (0x1FUL << TPI_DEVID_NrTraceInput_Pos)      /*!< TPI DEVID: NrTraceInput Mask */

-

-/* TPI DEVTYPE Register Definitions */

-#define TPI_DEVTYPE_SubType_Pos             0                                          /*!< TPI DEVTYPE: SubType Position */

-#define TPI_DEVTYPE_SubType_Msk            (0xFUL << TPI_DEVTYPE_SubType_Pos)          /*!< TPI DEVTYPE: SubType Mask */

-

-#define TPI_DEVTYPE_MajorType_Pos           4                                          /*!< TPI DEVTYPE: MajorType Position */

-#define TPI_DEVTYPE_MajorType_Msk          (0xFUL << TPI_DEVTYPE_MajorType_Pos)        /*!< TPI DEVTYPE: MajorType Mask */

-

-/*@}*/ /* end of group CMSIS_TPI */

-

-

-#if (__MPU_PRESENT == 1)

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_MPU     Memory Protection Unit (MPU)

-    \brief      Type definitions for the Memory Protection Unit (MPU)

-  @{

- */

-

-/** \brief  Structure type to access the Memory Protection Unit (MPU).

- */

-typedef struct

-{

-  __I  uint32_t TYPE;                    /*!< Offset: 0x000 (R/ )  MPU Type Register                              */

-  __IO uint32_t CTRL;                    /*!< Offset: 0x004 (R/W)  MPU Control Register                           */

-  __IO uint32_t RNR;                     /*!< Offset: 0x008 (R/W)  MPU Region RNRber Register                     */

-  __IO uint32_t RBAR;                    /*!< Offset: 0x00C (R/W)  MPU Region Base Address Register               */

-  __IO uint32_t RASR;                    /*!< Offset: 0x010 (R/W)  MPU Region Attribute and Size Register         */

-  __IO uint32_t RBAR_A1;                 /*!< Offset: 0x014 (R/W)  MPU Alias 1 Region Base Address Register       */

-  __IO uint32_t RASR_A1;                 /*!< Offset: 0x018 (R/W)  MPU Alias 1 Region Attribute and Size Register */

-  __IO uint32_t RBAR_A2;                 /*!< Offset: 0x01C (R/W)  MPU Alias 2 Region Base Address Register       */

-  __IO uint32_t RASR_A2;                 /*!< Offset: 0x020 (R/W)  MPU Alias 2 Region Attribute and Size Register */

-  __IO uint32_t RBAR_A3;                 /*!< Offset: 0x024 (R/W)  MPU Alias 3 Region Base Address Register       */

-  __IO uint32_t RASR_A3;                 /*!< Offset: 0x028 (R/W)  MPU Alias 3 Region Attribute and Size Register */

-} MPU_Type;

-

-/* MPU Type Register */

-#define MPU_TYPE_IREGION_Pos               16                                             /*!< MPU TYPE: IREGION Position */

-#define MPU_TYPE_IREGION_Msk               (0xFFUL << MPU_TYPE_IREGION_Pos)               /*!< MPU TYPE: IREGION Mask */

-

-#define MPU_TYPE_DREGION_Pos                8                                             /*!< MPU TYPE: DREGION Position */

-#define MPU_TYPE_DREGION_Msk               (0xFFUL << MPU_TYPE_DREGION_Pos)               /*!< MPU TYPE: DREGION Mask */

-

-#define MPU_TYPE_SEPARATE_Pos               0                                             /*!< MPU TYPE: SEPARATE Position */

-#define MPU_TYPE_SEPARATE_Msk              (1UL << MPU_TYPE_SEPARATE_Pos)                 /*!< MPU TYPE: SEPARATE Mask */

-

-/* MPU Control Register */

-#define MPU_CTRL_PRIVDEFENA_Pos             2                                             /*!< MPU CTRL: PRIVDEFENA Position */

-#define MPU_CTRL_PRIVDEFENA_Msk            (1UL << MPU_CTRL_PRIVDEFENA_Pos)               /*!< MPU CTRL: PRIVDEFENA Mask */

-

-#define MPU_CTRL_HFNMIENA_Pos               1                                             /*!< MPU CTRL: HFNMIENA Position */

-#define MPU_CTRL_HFNMIENA_Msk              (1UL << MPU_CTRL_HFNMIENA_Pos)                 /*!< MPU CTRL: HFNMIENA Mask */

-

-#define MPU_CTRL_ENABLE_Pos                 0                                             /*!< MPU CTRL: ENABLE Position */

-#define MPU_CTRL_ENABLE_Msk                (1UL << MPU_CTRL_ENABLE_Pos)                   /*!< MPU CTRL: ENABLE Mask */

-

-/* MPU Region Number Register */

-#define MPU_RNR_REGION_Pos                  0                                             /*!< MPU RNR: REGION Position */

-#define MPU_RNR_REGION_Msk                 (0xFFUL << MPU_RNR_REGION_Pos)                 /*!< MPU RNR: REGION Mask */

-

-/* MPU Region Base Address Register */

-#define MPU_RBAR_ADDR_Pos                   5                                             /*!< MPU RBAR: ADDR Position */

-#define MPU_RBAR_ADDR_Msk                  (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos)             /*!< MPU RBAR: ADDR Mask */

-

-#define MPU_RBAR_VALID_Pos                  4                                             /*!< MPU RBAR: VALID Position */

-#define MPU_RBAR_VALID_Msk                 (1UL << MPU_RBAR_VALID_Pos)                    /*!< MPU RBAR: VALID Mask */

-

-#define MPU_RBAR_REGION_Pos                 0                                             /*!< MPU RBAR: REGION Position */

-#define MPU_RBAR_REGION_Msk                (0xFUL << MPU_RBAR_REGION_Pos)                 /*!< MPU RBAR: REGION Mask */

-

-/* MPU Region Attribute and Size Register */

-#define MPU_RASR_ATTRS_Pos                 16                                             /*!< MPU RASR: MPU Region Attribute field Position */

-#define MPU_RASR_ATTRS_Msk                 (0xFFFFUL << MPU_RASR_ATTRS_Pos)               /*!< MPU RASR: MPU Region Attribute field Mask */

-

-#define MPU_RASR_XN_Pos                    28                                             /*!< MPU RASR: ATTRS.XN Position */

-#define MPU_RASR_XN_Msk                    (1UL << MPU_RASR_XN_Pos)                       /*!< MPU RASR: ATTRS.XN Mask */

-

-#define MPU_RASR_AP_Pos                    24                                             /*!< MPU RASR: ATTRS.AP Position */

-#define MPU_RASR_AP_Msk                    (0x7UL << MPU_RASR_AP_Pos)                     /*!< MPU RASR: ATTRS.AP Mask */

-

-#define MPU_RASR_TEX_Pos                   19                                             /*!< MPU RASR: ATTRS.TEX Position */

-#define MPU_RASR_TEX_Msk                   (0x7UL << MPU_RASR_TEX_Pos)                    /*!< MPU RASR: ATTRS.TEX Mask */

-

-#define MPU_RASR_S_Pos                     18                                             /*!< MPU RASR: ATTRS.S Position */

-#define MPU_RASR_S_Msk                     (1UL << MPU_RASR_S_Pos)                        /*!< MPU RASR: ATTRS.S Mask */

-

-#define MPU_RASR_C_Pos                     17                                             /*!< MPU RASR: ATTRS.C Position */

-#define MPU_RASR_C_Msk                     (1UL << MPU_RASR_C_Pos)                        /*!< MPU RASR: ATTRS.C Mask */

-

-#define MPU_RASR_B_Pos                     16                                             /*!< MPU RASR: ATTRS.B Position */

-#define MPU_RASR_B_Msk                     (1UL << MPU_RASR_B_Pos)                        /*!< MPU RASR: ATTRS.B Mask */

-

-#define MPU_RASR_SRD_Pos                    8                                             /*!< MPU RASR: Sub-Region Disable Position */

-#define MPU_RASR_SRD_Msk                   (0xFFUL << MPU_RASR_SRD_Pos)                   /*!< MPU RASR: Sub-Region Disable Mask */

-

-#define MPU_RASR_SIZE_Pos                   1                                             /*!< MPU RASR: Region Size Field Position */

-#define MPU_RASR_SIZE_Msk                  (0x1FUL << MPU_RASR_SIZE_Pos)                  /*!< MPU RASR: Region Size Field Mask */

-

-#define MPU_RASR_ENABLE_Pos                 0                                             /*!< MPU RASR: Region enable bit Position */

-#define MPU_RASR_ENABLE_Msk                (1UL << MPU_RASR_ENABLE_Pos)                   /*!< MPU RASR: Region enable bit Disable Mask */

-

-/*@} end of group CMSIS_MPU */

-#endif

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)

-    \brief      Type definitions for the Core Debug Registers

-  @{

- */

-

-/** \brief  Structure type to access the Core Debug Register (CoreDebug).

- */

-typedef struct

-{

-  __IO uint32_t DHCSR;                   /*!< Offset: 0x000 (R/W)  Debug Halting Control and Status Register    */

-  __O  uint32_t DCRSR;                   /*!< Offset: 0x004 ( /W)  Debug Core Register Selector Register        */

-  __IO uint32_t DCRDR;                   /*!< Offset: 0x008 (R/W)  Debug Core Register Data Register            */

-  __IO uint32_t DEMCR;                   /*!< Offset: 0x00C (R/W)  Debug Exception and Monitor Control Register */

-} CoreDebug_Type;

-

-/* Debug Halting Control and Status Register */

-#define CoreDebug_DHCSR_DBGKEY_Pos         16                                             /*!< CoreDebug DHCSR: DBGKEY Position */

-#define CoreDebug_DHCSR_DBGKEY_Msk         (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos)       /*!< CoreDebug DHCSR: DBGKEY Mask */

-

-#define CoreDebug_DHCSR_S_RESET_ST_Pos     25                                             /*!< CoreDebug DHCSR: S_RESET_ST Position */

-#define CoreDebug_DHCSR_S_RESET_ST_Msk     (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos)        /*!< CoreDebug DHCSR: S_RESET_ST Mask */

-

-#define CoreDebug_DHCSR_S_RETIRE_ST_Pos    24                                             /*!< CoreDebug DHCSR: S_RETIRE_ST Position */

-#define CoreDebug_DHCSR_S_RETIRE_ST_Msk    (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos)       /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */

-

-#define CoreDebug_DHCSR_S_LOCKUP_Pos       19                                             /*!< CoreDebug DHCSR: S_LOCKUP Position */

-#define CoreDebug_DHCSR_S_LOCKUP_Msk       (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos)          /*!< CoreDebug DHCSR: S_LOCKUP Mask */

-

-#define CoreDebug_DHCSR_S_SLEEP_Pos        18                                             /*!< CoreDebug DHCSR: S_SLEEP Position */

-#define CoreDebug_DHCSR_S_SLEEP_Msk        (1UL << CoreDebug_DHCSR_S_SLEEP_Pos)           /*!< CoreDebug DHCSR: S_SLEEP Mask */

-

-#define CoreDebug_DHCSR_S_HALT_Pos         17                                             /*!< CoreDebug DHCSR: S_HALT Position */

-#define CoreDebug_DHCSR_S_HALT_Msk         (1UL << CoreDebug_DHCSR_S_HALT_Pos)            /*!< CoreDebug DHCSR: S_HALT Mask */

-

-#define CoreDebug_DHCSR_S_REGRDY_Pos       16                                             /*!< CoreDebug DHCSR: S_REGRDY Position */

-#define CoreDebug_DHCSR_S_REGRDY_Msk       (1UL << CoreDebug_DHCSR_S_REGRDY_Pos)          /*!< CoreDebug DHCSR: S_REGRDY Mask */

-

-#define CoreDebug_DHCSR_C_SNAPSTALL_Pos     5                                             /*!< CoreDebug DHCSR: C_SNAPSTALL Position */

-#define CoreDebug_DHCSR_C_SNAPSTALL_Msk    (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos)       /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */

-

-#define CoreDebug_DHCSR_C_MASKINTS_Pos      3                                             /*!< CoreDebug DHCSR: C_MASKINTS Position */

-#define CoreDebug_DHCSR_C_MASKINTS_Msk     (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos)        /*!< CoreDebug DHCSR: C_MASKINTS Mask */

-

-#define CoreDebug_DHCSR_C_STEP_Pos          2                                             /*!< CoreDebug DHCSR: C_STEP Position */

-#define CoreDebug_DHCSR_C_STEP_Msk         (1UL << CoreDebug_DHCSR_C_STEP_Pos)            /*!< CoreDebug DHCSR: C_STEP Mask */

-

-#define CoreDebug_DHCSR_C_HALT_Pos          1                                             /*!< CoreDebug DHCSR: C_HALT Position */

-#define CoreDebug_DHCSR_C_HALT_Msk         (1UL << CoreDebug_DHCSR_C_HALT_Pos)            /*!< CoreDebug DHCSR: C_HALT Mask */

-

-#define CoreDebug_DHCSR_C_DEBUGEN_Pos       0                                             /*!< CoreDebug DHCSR: C_DEBUGEN Position */

-#define CoreDebug_DHCSR_C_DEBUGEN_Msk      (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos)         /*!< CoreDebug DHCSR: C_DEBUGEN Mask */

-

-/* Debug Core Register Selector Register */

-#define CoreDebug_DCRSR_REGWnR_Pos         16                                             /*!< CoreDebug DCRSR: REGWnR Position */

-#define CoreDebug_DCRSR_REGWnR_Msk         (1UL << CoreDebug_DCRSR_REGWnR_Pos)            /*!< CoreDebug DCRSR: REGWnR Mask */

-

-#define CoreDebug_DCRSR_REGSEL_Pos          0                                             /*!< CoreDebug DCRSR: REGSEL Position */

-#define CoreDebug_DCRSR_REGSEL_Msk         (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos)         /*!< CoreDebug DCRSR: REGSEL Mask */

-

-/* Debug Exception and Monitor Control Register */

-#define CoreDebug_DEMCR_TRCENA_Pos         24                                             /*!< CoreDebug DEMCR: TRCENA Position */

-#define CoreDebug_DEMCR_TRCENA_Msk         (1UL << CoreDebug_DEMCR_TRCENA_Pos)            /*!< CoreDebug DEMCR: TRCENA Mask */

-

-#define CoreDebug_DEMCR_MON_REQ_Pos        19                                             /*!< CoreDebug DEMCR: MON_REQ Position */

-#define CoreDebug_DEMCR_MON_REQ_Msk        (1UL << CoreDebug_DEMCR_MON_REQ_Pos)           /*!< CoreDebug DEMCR: MON_REQ Mask */

-

-#define CoreDebug_DEMCR_MON_STEP_Pos       18                                             /*!< CoreDebug DEMCR: MON_STEP Position */

-#define CoreDebug_DEMCR_MON_STEP_Msk       (1UL << CoreDebug_DEMCR_MON_STEP_Pos)          /*!< CoreDebug DEMCR: MON_STEP Mask */

-

-#define CoreDebug_DEMCR_MON_PEND_Pos       17                                             /*!< CoreDebug DEMCR: MON_PEND Position */

-#define CoreDebug_DEMCR_MON_PEND_Msk       (1UL << CoreDebug_DEMCR_MON_PEND_Pos)          /*!< CoreDebug DEMCR: MON_PEND Mask */

-

-#define CoreDebug_DEMCR_MON_EN_Pos         16                                             /*!< CoreDebug DEMCR: MON_EN Position */

-#define CoreDebug_DEMCR_MON_EN_Msk         (1UL << CoreDebug_DEMCR_MON_EN_Pos)            /*!< CoreDebug DEMCR: MON_EN Mask */

-

-#define CoreDebug_DEMCR_VC_HARDERR_Pos     10                                             /*!< CoreDebug DEMCR: VC_HARDERR Position */

-#define CoreDebug_DEMCR_VC_HARDERR_Msk     (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos)        /*!< CoreDebug DEMCR: VC_HARDERR Mask */

-

-#define CoreDebug_DEMCR_VC_INTERR_Pos       9                                             /*!< CoreDebug DEMCR: VC_INTERR Position */

-#define CoreDebug_DEMCR_VC_INTERR_Msk      (1UL << CoreDebug_DEMCR_VC_INTERR_Pos)         /*!< CoreDebug DEMCR: VC_INTERR Mask */

-

-#define CoreDebug_DEMCR_VC_BUSERR_Pos       8                                             /*!< CoreDebug DEMCR: VC_BUSERR Position */

-#define CoreDebug_DEMCR_VC_BUSERR_Msk      (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos)         /*!< CoreDebug DEMCR: VC_BUSERR Mask */

-

-#define CoreDebug_DEMCR_VC_STATERR_Pos      7                                             /*!< CoreDebug DEMCR: VC_STATERR Position */

-#define CoreDebug_DEMCR_VC_STATERR_Msk     (1UL << CoreDebug_DEMCR_VC_STATERR_Pos)        /*!< CoreDebug DEMCR: VC_STATERR Mask */

-

-#define CoreDebug_DEMCR_VC_CHKERR_Pos       6                                             /*!< CoreDebug DEMCR: VC_CHKERR Position */

-#define CoreDebug_DEMCR_VC_CHKERR_Msk      (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos)         /*!< CoreDebug DEMCR: VC_CHKERR Mask */

-

-#define CoreDebug_DEMCR_VC_NOCPERR_Pos      5                                             /*!< CoreDebug DEMCR: VC_NOCPERR Position */

-#define CoreDebug_DEMCR_VC_NOCPERR_Msk     (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos)        /*!< CoreDebug DEMCR: VC_NOCPERR Mask */

-

-#define CoreDebug_DEMCR_VC_MMERR_Pos        4                                             /*!< CoreDebug DEMCR: VC_MMERR Position */

-#define CoreDebug_DEMCR_VC_MMERR_Msk       (1UL << CoreDebug_DEMCR_VC_MMERR_Pos)          /*!< CoreDebug DEMCR: VC_MMERR Mask */

-

-#define CoreDebug_DEMCR_VC_CORERESET_Pos    0                                             /*!< CoreDebug DEMCR: VC_CORERESET Position */

-#define CoreDebug_DEMCR_VC_CORERESET_Msk   (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos)      /*!< CoreDebug DEMCR: VC_CORERESET Mask */

-

-/*@} end of group CMSIS_CoreDebug */

-

-

-/** \ingroup    CMSIS_core_register

-    \defgroup   CMSIS_core_base     Core Definitions

-    \brief      Definitions for base addresses, unions, and structures.

-  @{

- */

-

-/* Memory mapping of Cortex-M3 Hardware */

-#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address  */

-#define ITM_BASE            (0xE0000000UL)                            /*!< ITM Base Address                   */

-#define DWT_BASE            (0xE0001000UL)                            /*!< DWT Base Address                   */

-#define TPI_BASE            (0xE0040000UL)                            /*!< TPI Base Address                   */

-#define CoreDebug_BASE      (0xE000EDF0UL)                            /*!< Core Debug Base Address            */

-#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address               */

-#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address                  */

-#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address  */

-

-#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */

-#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct           */

-#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct       */

-#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct          */

-#define ITM                 ((ITM_Type       *)     ITM_BASE      )   /*!< ITM configuration struct           */

-#define DWT                 ((DWT_Type       *)     DWT_BASE      )   /*!< DWT configuration struct           */

-#define TPI                 ((TPI_Type       *)     TPI_BASE      )   /*!< TPI configuration struct           */

-#define CoreDebug           ((CoreDebug_Type *)     CoreDebug_BASE)   /*!< Core Debug configuration struct    */

-

-#if (__MPU_PRESENT == 1)

-  #define MPU_BASE          (SCS_BASE +  0x0D90UL)                    /*!< Memory Protection Unit             */

-  #define MPU               ((MPU_Type       *)     MPU_BASE      )   /*!< Memory Protection Unit             */

-#endif

-

-/*@} */

-

-

-

-/*******************************************************************************

- *                Hardware Abstraction Layer

-  Core Function Interface contains:

-  - Core NVIC Functions

-  - Core SysTick Functions

-  - Core Debug Functions

-  - Core Register Access Functions

- ******************************************************************************/

-/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference

-*/

-

-

-

-/* ##########################   NVIC functions  #################################### */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_Core_NVICFunctions NVIC Functions

-    \brief      Functions that manage interrupts and exceptions via the NVIC.

-    @{

- */

-

-/** \brief  Set Priority Grouping

-

-  The function sets the priority grouping field using the required unlock sequence.

-  The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.

-  Only values from 0..7 are used.

-  In case of a conflict between priority grouping and available

-  priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.

-

-    \param [in]      PriorityGroup  Priority grouping field.

- */

-__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)

-{

-  uint32_t reg_value;

-  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07);               /* only values 0..7 are used          */

-

-  reg_value  =  SCB->AIRCR;                                                   /* read old register configuration    */

-  reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk);             /* clear bits to change               */

-  reg_value  =  (reg_value                                 |

-                ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |

-                (PriorityGroupTmp << 8));                                     /* Insert write key and priorty group */

-  SCB->AIRCR =  reg_value;

-}

-

-

-/** \brief  Get Priority Grouping

-

-  The function reads the priority grouping field from the NVIC Interrupt Controller.

-

-    \return                Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).

- */

-__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)

-{

-  return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos);   /* read priority grouping field */

-}

-

-

-/** \brief  Enable External Interrupt

-

-    The function enables a device-specific interrupt in the NVIC interrupt controller.

-

-    \param [in]      IRQn  External interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)

-{

-  NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* enable interrupt */

-}

-

-

-/** \brief  Disable External Interrupt

-

-    The function disables a device-specific interrupt in the NVIC interrupt controller.

-

-    \param [in]      IRQn  External interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)

-{

-  NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */

-}

-

-

-/** \brief  Get Pending Interrupt

-

-    The function reads the pending register in the NVIC and returns the pending bit

-    for the specified interrupt.

-

-    \param [in]      IRQn  Interrupt number.

-

-    \return             0  Interrupt status is not pending.

-    \return             1  Interrupt status is pending.

- */

-__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)

-{

-  return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */

-}

-

-

-/** \brief  Set Pending Interrupt

-

-    The function sets the pending bit of an external interrupt.

-

-    \param [in]      IRQn  Interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)

-{

-  NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */

-}

-

-

-/** \brief  Clear Pending Interrupt

-

-    The function clears the pending bit of an external interrupt.

-

-    \param [in]      IRQn  External interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)

-{

-  NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */

-}

-

-

-/** \brief  Get Active Interrupt

-

-    The function reads the active register in NVIC and returns the active bit.

-

-    \param [in]      IRQn  Interrupt number.

-

-    \return             0  Interrupt status is not active.

-    \return             1  Interrupt status is active.

- */

-__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)

-{

-  return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */

-}

-

-

-/** \brief  Set Interrupt Priority

-

-    The function sets the priority of an interrupt.

-

-    \note The priority cannot be set for every core interrupt.

-

-    \param [in]      IRQn  Interrupt number.

-    \param [in]  priority  Priority to set.

- */

-__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)

-{

-  if(IRQn < 0) {

-    SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M  System Interrupts */

-  else {

-    NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff);    }        /* set Priority for device specific Interrupts  */

-}

-

-

-/** \brief  Get Interrupt Priority

-

-    The function reads the priority of an interrupt. The interrupt

-    number can be positive to specify an external (device specific)

-    interrupt, or negative to specify an internal (core) interrupt.

-

-

-    \param [in]   IRQn  Interrupt number.

-    \return             Interrupt Priority. Value is aligned automatically to the implemented

-                        priority bits of the microcontroller.

- */

-__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)

-{

-

-  if(IRQn < 0) {

-    return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for Cortex-M  system interrupts */

-  else {

-    return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)]           >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for device specific interrupts  */

-}

-

-

-/** \brief  Encode Priority

-

-    The function encodes the priority for an interrupt with the given priority group,

-    preemptive priority value, and subpriority value.

-    In case of a conflict between priority grouping and available

-    priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.

-

-    \param [in]     PriorityGroup  Used priority group.

-    \param [in]   PreemptPriority  Preemptive priority value (starting from 0).

-    \param [in]       SubPriority  Subpriority value (starting from 0).

-    \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().

- */

-__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)

-{

-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */

-  uint32_t PreemptPriorityBits;

-  uint32_t SubPriorityBits;

-

-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;

-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;

-

-  return (

-           ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |

-           ((SubPriority     & ((1 << (SubPriorityBits    )) - 1)))

-         );

-}

-

-

-/** \brief  Decode Priority

-

-    The function decodes an interrupt priority value with a given priority group to

-    preemptive priority value and subpriority value.

-    In case of a conflict between priority grouping and available

-    priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.

-

-    \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().

-    \param [in]     PriorityGroup  Used priority group.

-    \param [out] pPreemptPriority  Preemptive priority value (starting from 0).

-    \param [out]     pSubPriority  Subpriority value (starting from 0).

- */

-__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)

-{

-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */

-  uint32_t PreemptPriorityBits;

-  uint32_t SubPriorityBits;

-

-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;

-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;

-

-  *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);

-  *pSubPriority     = (Priority                   ) & ((1 << (SubPriorityBits    )) - 1);

-}

-

-

-/** \brief  System Reset

-

-    The function initiates a system reset request to reset the MCU.

- */

-__STATIC_INLINE void NVIC_SystemReset(void)

-{

-  __DSB();                                                     /* Ensure all outstanding memory accesses included

-                                                                  buffered write are completed before reset */

-  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |

-                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |

-                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */

-  __DSB();                                                     /* Ensure completion of memory access */

-  while(1);                                                    /* wait until reset */

-}

-

-/*@} end of CMSIS_Core_NVICFunctions */

-

-

-

-/* ##################################    SysTick function  ############################################ */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions

-    \brief      Functions that configure the System.

-  @{

- */

-

-#if (__Vendor_SysTickConfig == 0)

-

-/** \brief  System Tick Configuration

-

-    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.

-    Counter is in free running mode to generate periodic interrupts.

-

-    \param [in]  ticks  Number of ticks between two interrupts.

-

-    \return          0  Function succeeded.

-    \return          1  Function failed.

-

-    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the

-    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>

-    must contain a vendor-specific implementation of this function.

-

- */

-__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)

-{

-  if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk)  return (1);      /* Reload value impossible */

-

-  SysTick->LOAD  = ticks - 1;                                  /* set reload register */

-  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Systick Interrupt */

-  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */

-  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |

-                   SysTick_CTRL_TICKINT_Msk   |

-                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */

-  return (0);                                                  /* Function successful */

-}

-

-#endif

-

-/*@} end of CMSIS_Core_SysTickFunctions */

-

-

-

-/* ##################################### Debug In/Output function ########################################### */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_core_DebugFunctions ITM Functions

-    \brief   Functions that access the ITM debug interface.

-  @{

- */

-

-extern volatile int32_t ITM_RxBuffer;                    /*!< External variable to receive characters.                         */

-#define                 ITM_RXBUFFER_EMPTY    0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */

-

-

-/** \brief  ITM Send Character

-

-    The function transmits a character via the ITM channel 0, and

-    \li Just returns when no debugger is connected that has booked the output.

-    \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.

-

-    \param [in]     ch  Character to transmit.

-

-    \returns            Character to transmit.

- */

-__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)

-{

-  if ((ITM->TCR & ITM_TCR_ITMENA_Msk)                  &&      /* ITM enabled */

-      (ITM->TER & (1UL << 0)        )                    )     /* ITM Port #0 enabled */

-  {

-    while (ITM->PORT[0].u32 == 0);

-    ITM->PORT[0].u8 = (uint8_t) ch;

-  }

-  return (ch);

-}

-

-

-/** \brief  ITM Receive Character

-

-    The function inputs a character via the external variable \ref ITM_RxBuffer.

-

-    \return             Received character.

-    \return         -1  No character pending.

- */

-__STATIC_INLINE int32_t ITM_ReceiveChar (void) {

-  int32_t ch = -1;                           /* no character available */

-

-  if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {

-    ch = ITM_RxBuffer;

-    ITM_RxBuffer = ITM_RXBUFFER_EMPTY;       /* ready for next character */

-  }

-

-  return (ch);

-}

-

-

-/** \brief  ITM Check Character

-

-    The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.

-

-    \return          0  No character available.

-    \return          1  Character available.

- */

-__STATIC_INLINE int32_t ITM_CheckChar (void) {

-

-  if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {

-    return (0);                                 /* no character available */

-  } else {

-    return (1);                                 /*    character available */

-  }

-}

-

-/*@} end of CMSIS_core_DebugFunctions */

-

-#endif /* __CORE_CM3_H_DEPENDANT */

-

-#endif /* __CMSIS_GENERIC */

-

-#ifdef __cplusplus

-}

-#endif

diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/modules/mathlib/CMSIS/Include/core_cm4.h
deleted file mode 100644
index 93efd3a7a..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cm4.h
+++ /dev/null
@@ -1,1772 +0,0 @@
-/**************************************************************************//**

- * @file     core_cm4.h

- * @brief    CMSIS Cortex-M4 Core Peripheral Access Layer Header File

- * @version  V3.20

- * @date     25. February 2013

- *

- * @note

- *

- ******************************************************************************/

-/* Copyright (c) 2009 - 2013 ARM LIMITED

-

-   All rights reserved.

-   Redistribution and use in source and binary forms, with or without

-   modification, are permitted provided that the following conditions are met:

-   - Redistributions of source code must retain the above copyright

-     notice, this list of conditions and the following disclaimer.

-   - Redistributions in binary form must reproduce the above copyright

-     notice, this list of conditions and the following disclaimer in the

-     documentation and/or other materials provided with the distribution.

-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

-   ---------------------------------------------------------------------------*/

-

-

-#if defined ( __ICCARM__ )

- #pragma system_include  /* treat file as system include file for MISRA check */

-#endif

-

-#ifdef __cplusplus

- extern "C" {

-#endif

-

-#ifndef __CORE_CM4_H_GENERIC

-#define __CORE_CM4_H_GENERIC

-

-/** \page CMSIS_MISRA_Exceptions  MISRA-C:2004 Compliance Exceptions

-  CMSIS violates the following MISRA-C:2004 rules:

-

-   \li Required Rule 8.5, object/function definition in header file.<br>

-     Function definitions in header files are used to allow 'inlining'.

-

-   \li Required Rule 18.4, declaration of union type or object of union type: '{...}'.<br>

-     Unions are used for effective representation of core registers.

-

-   \li Advisory Rule 19.7, Function-like macro defined.<br>

-     Function-like macros are used to allow more efficient code.

- */

-

-

-/*******************************************************************************

- *                 CMSIS definitions

- ******************************************************************************/

-/** \ingroup Cortex_M4

-  @{

- */

-

-/*  CMSIS CM4 definitions */

-#define __CM4_CMSIS_VERSION_MAIN  (0x03)                                   /*!< [31:16] CMSIS HAL main version   */

-#define __CM4_CMSIS_VERSION_SUB   (0x20)                                   /*!< [15:0]  CMSIS HAL sub version    */

-#define __CM4_CMSIS_VERSION       ((__CM4_CMSIS_VERSION_MAIN << 16) | \

-                                    __CM4_CMSIS_VERSION_SUB          )     /*!< CMSIS HAL version number         */

-

-#define __CORTEX_M                (0x04)                                   /*!< Cortex-M Core                    */

-

-

-#if   defined ( __CC_ARM )

-  #define __ASM            __asm                                      /*!< asm keyword for ARM Compiler          */

-  #define __INLINE         __inline                                   /*!< inline keyword for ARM Compiler       */

-  #define __STATIC_INLINE  static __inline

-

-#elif defined ( __ICCARM__ )

-  #define __ASM            __asm                                      /*!< asm keyword for IAR Compiler          */

-  #define __INLINE         inline                                     /*!< inline keyword for IAR Compiler. Only available in High optimization mode! */

-  #define __STATIC_INLINE  static inline

-

-#elif defined ( __TMS470__ )

-  #define __ASM            __asm                                      /*!< asm keyword for TI CCS Compiler       */

-  #define __STATIC_INLINE  static inline

-

-#elif defined ( __GNUC__ )

-  #define __ASM            __asm                                      /*!< asm keyword for GNU Compiler          */

-  #define __INLINE         inline                                     /*!< inline keyword for GNU Compiler       */

-  #define __STATIC_INLINE  static inline

-

-#elif defined ( __TASKING__ )

-  #define __ASM            __asm                                      /*!< asm keyword for TASKING Compiler      */

-  #define __INLINE         inline                                     /*!< inline keyword for TASKING Compiler   */

-  #define __STATIC_INLINE  static inline

-

-#endif

-

-/** __FPU_USED indicates whether an FPU is used or not. For this, __FPU_PRESENT has to be checked prior to making use of FPU specific registers and functions.

-*/

-#if defined ( __CC_ARM )

-  #if defined __TARGET_FPU_VFP

-    #if (__FPU_PRESENT == 1)

-      #define __FPU_USED       1

-    #else

-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-      #define __FPU_USED       0

-    #endif

-  #else

-    #define __FPU_USED         0

-  #endif

-

-#elif defined ( __ICCARM__ )

-  #if defined __ARMVFP__

-    #if (__FPU_PRESENT == 1)

-      #define __FPU_USED       1

-    #else

-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-      #define __FPU_USED       0

-    #endif

-  #else

-    #define __FPU_USED         0

-  #endif

-

-#elif defined ( __TMS470__ )

-  #if defined __TI_VFP_SUPPORT__

-    #if (__FPU_PRESENT == 1)

-      #define __FPU_USED       1

-    #else

-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-      #define __FPU_USED       0

-    #endif

-  #else

-    #define __FPU_USED         0

-  #endif

-

-#elif defined ( __GNUC__ )

-  #if defined (__VFP_FP__) && !defined(__SOFTFP__)

-    #if (__FPU_PRESENT == 1)

-      #define __FPU_USED       1

-    #else

-      #warning "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-      #define __FPU_USED       0

-    #endif

-  #else

-    #define __FPU_USED         0

-  #endif

-

-#elif defined ( __TASKING__ )

-  #if defined __FPU_VFP__

-    #if (__FPU_PRESENT == 1)

-      #define __FPU_USED       1

-    #else

-      #error "Compiler generates FPU instructions for a device without an FPU (check __FPU_PRESENT)"

-      #define __FPU_USED       0

-    #endif

-  #else

-    #define __FPU_USED         0

-  #endif

-#endif

-

-#include <stdint.h>                      /* standard types definitions                      */

-#include "core_cmInstr.h"                /* Core Instruction Access                         */

-#include "core_cmFunc.h"                 /* Core Function Access                            */

-#include "core_cm4_simd.h"               /* Compiler specific SIMD Intrinsics               */

-

-#endif /* __CORE_CM4_H_GENERIC */

-

-#ifndef __CMSIS_GENERIC

-

-#ifndef __CORE_CM4_H_DEPENDANT

-#define __CORE_CM4_H_DEPENDANT

-

-/* check device defines and use defaults */

-#if defined __CHECK_DEVICE_DEFINES

-  #ifndef __CM4_REV

-    #define __CM4_REV               0x0000

-    #warning "__CM4_REV not defined in device header file; using default!"

-  #endif

-

-  #ifndef __FPU_PRESENT

-    #define __FPU_PRESENT             0

-    #warning "__FPU_PRESENT not defined in device header file; using default!"

-  #endif

-

-  #ifndef __MPU_PRESENT

-    #define __MPU_PRESENT             0

-    #warning "__MPU_PRESENT not defined in device header file; using default!"

-  #endif

-

-  #ifndef __NVIC_PRIO_BITS

-    #define __NVIC_PRIO_BITS          4

-    #warning "__NVIC_PRIO_BITS not defined in device header file; using default!"

-  #endif

-

-  #ifndef __Vendor_SysTickConfig

-    #define __Vendor_SysTickConfig    0

-    #warning "__Vendor_SysTickConfig not defined in device header file; using default!"

-  #endif

-#endif

-

-/* IO definitions (access restrictions to peripheral registers) */

-/**

-    \defgroup CMSIS_glob_defs CMSIS Global Defines

-

-    <strong>IO Type Qualifiers</strong> are used

-    \li to specify the access to peripheral variables.

-    \li for automatic generation of peripheral register debug information.

-*/

-#ifdef __cplusplus

-  #define   __I     volatile             /*!< Defines 'read only' permissions                 */

-#else

-  #define   __I     volatile const       /*!< Defines 'read only' permissions                 */

-#endif

-#define     __O     volatile             /*!< Defines 'write only' permissions                */

-#define     __IO    volatile             /*!< Defines 'read / write' permissions              */

-

-/*@} end of group Cortex_M4 */

-

-

-

-/*******************************************************************************

- *                 Register Abstraction

-  Core Register contain:

-  - Core Register

-  - Core NVIC Register

-  - Core SCB Register

-  - Core SysTick Register

-  - Core Debug Register

-  - Core MPU Register

-  - Core FPU Register

- ******************************************************************************/

-/** \defgroup CMSIS_core_register Defines and Type Definitions

-    \brief Type definitions and defines for Cortex-M processor based devices.

-*/

-

-/** \ingroup    CMSIS_core_register

-    \defgroup   CMSIS_CORE  Status and Control Registers

-    \brief  Core Register type definitions.

-  @{

- */

-

-/** \brief  Union type to access the Application Program Status Register (APSR).

- */

-typedef union

-{

-  struct

-  {

-#if (__CORTEX_M != 0x04)

-    uint32_t _reserved0:27;              /*!< bit:  0..26  Reserved                           */

-#else

-    uint32_t _reserved0:16;              /*!< bit:  0..15  Reserved                           */

-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */

-    uint32_t _reserved1:7;               /*!< bit: 20..26  Reserved                           */

-#endif

-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */

-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */

-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */

-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */

-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} APSR_Type;

-

-

-/** \brief  Union type to access the Interrupt Program Status Register (IPSR).

- */

-typedef union

-{

-  struct

-  {

-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */

-    uint32_t _reserved0:23;              /*!< bit:  9..31  Reserved                           */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} IPSR_Type;

-

-

-/** \brief  Union type to access the Special-Purpose Program Status Registers (xPSR).

- */

-typedef union

-{

-  struct

-  {

-    uint32_t ISR:9;                      /*!< bit:  0.. 8  Exception number                   */

-#if (__CORTEX_M != 0x04)

-    uint32_t _reserved0:15;              /*!< bit:  9..23  Reserved                           */

-#else

-    uint32_t _reserved0:7;               /*!< bit:  9..15  Reserved                           */

-    uint32_t GE:4;                       /*!< bit: 16..19  Greater than or Equal flags        */

-    uint32_t _reserved1:4;               /*!< bit: 20..23  Reserved                           */

-#endif

-    uint32_t T:1;                        /*!< bit:     24  Thumb bit        (read 0)          */

-    uint32_t IT:2;                       /*!< bit: 25..26  saved IT state   (read 0)          */

-    uint32_t Q:1;                        /*!< bit:     27  Saturation condition flag          */

-    uint32_t V:1;                        /*!< bit:     28  Overflow condition code flag       */

-    uint32_t C:1;                        /*!< bit:     29  Carry condition code flag          */

-    uint32_t Z:1;                        /*!< bit:     30  Zero condition code flag           */

-    uint32_t N:1;                        /*!< bit:     31  Negative condition code flag       */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} xPSR_Type;

-

-

-/** \brief  Union type to access the Control Registers (CONTROL).

- */

-typedef union

-{

-  struct

-  {

-    uint32_t nPRIV:1;                    /*!< bit:      0  Execution privilege in Thread mode */

-    uint32_t SPSEL:1;                    /*!< bit:      1  Stack to be used                   */

-    uint32_t FPCA:1;                     /*!< bit:      2  FP extension active flag           */

-    uint32_t _reserved0:29;              /*!< bit:  3..31  Reserved                           */

-  } b;                                   /*!< Structure used for bit  access                  */

-  uint32_t w;                            /*!< Type      used for word access                  */

-} CONTROL_Type;

-

-/*@} end of group CMSIS_CORE */

-

-

-/** \ingroup    CMSIS_core_register

-    \defgroup   CMSIS_NVIC  Nested Vectored Interrupt Controller (NVIC)

-    \brief      Type definitions for the NVIC Registers

-  @{

- */

-

-/** \brief  Structure type to access the Nested Vectored Interrupt Controller (NVIC).

- */

-typedef struct

-{

-  __IO uint32_t ISER[8];                 /*!< Offset: 0x000 (R/W)  Interrupt Set Enable Register           */

-       uint32_t RESERVED0[24];

-  __IO uint32_t ICER[8];                 /*!< Offset: 0x080 (R/W)  Interrupt Clear Enable Register         */

-       uint32_t RSERVED1[24];

-  __IO uint32_t ISPR[8];                 /*!< Offset: 0x100 (R/W)  Interrupt Set Pending Register          */

-       uint32_t RESERVED2[24];

-  __IO uint32_t ICPR[8];                 /*!< Offset: 0x180 (R/W)  Interrupt Clear Pending Register        */

-       uint32_t RESERVED3[24];

-  __IO uint32_t IABR[8];                 /*!< Offset: 0x200 (R/W)  Interrupt Active bit Register           */

-       uint32_t RESERVED4[56];

-  __IO uint8_t  IP[240];                 /*!< Offset: 0x300 (R/W)  Interrupt Priority Register (8Bit wide) */

-       uint32_t RESERVED5[644];

-  __O  uint32_t STIR;                    /*!< Offset: 0xE00 ( /W)  Software Trigger Interrupt Register     */

-}  NVIC_Type;

-

-/* Software Triggered Interrupt Register Definitions */

-#define NVIC_STIR_INTID_Pos                 0                                          /*!< STIR: INTLINESNUM Position */

-#define NVIC_STIR_INTID_Msk                (0x1FFUL << NVIC_STIR_INTID_Pos)            /*!< STIR: INTLINESNUM Mask */

-

-/*@} end of group CMSIS_NVIC */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_SCB     System Control Block (SCB)

-    \brief      Type definitions for the System Control Block Registers

-  @{

- */

-

-/** \brief  Structure type to access the System Control Block (SCB).

- */

-typedef struct

-{

-  __I  uint32_t CPUID;                   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */

-  __IO uint32_t ICSR;                    /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */

-  __IO uint32_t VTOR;                    /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */

-  __IO uint32_t AIRCR;                   /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */

-  __IO uint32_t SCR;                     /*!< Offset: 0x010 (R/W)  System Control Register                               */

-  __IO uint32_t CCR;                     /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */

-  __IO uint8_t  SHP[12];                 /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */

-  __IO uint32_t SHCSR;                   /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */

-  __IO uint32_t CFSR;                    /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */

-  __IO uint32_t HFSR;                    /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */

-  __IO uint32_t DFSR;                    /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */

-  __IO uint32_t MMFAR;                   /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */

-  __IO uint32_t BFAR;                    /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */

-  __IO uint32_t AFSR;                    /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */

-  __I  uint32_t PFR[2];                  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */

-  __I  uint32_t DFR;                     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */

-  __I  uint32_t ADR;                     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */

-  __I  uint32_t MMFR[4];                 /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */

-  __I  uint32_t ISAR[5];                 /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */

-       uint32_t RESERVED0[5];

-  __IO uint32_t CPACR;                   /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */

-} SCB_Type;

-

-/* SCB CPUID Register Definitions */

-#define SCB_CPUID_IMPLEMENTER_Pos          24                                             /*!< SCB CPUID: IMPLEMENTER Position */

-#define SCB_CPUID_IMPLEMENTER_Msk          (0xFFUL << SCB_CPUID_IMPLEMENTER_Pos)          /*!< SCB CPUID: IMPLEMENTER Mask */

-

-#define SCB_CPUID_VARIANT_Pos              20                                             /*!< SCB CPUID: VARIANT Position */

-#define SCB_CPUID_VARIANT_Msk              (0xFUL << SCB_CPUID_VARIANT_Pos)               /*!< SCB CPUID: VARIANT Mask */

-

-#define SCB_CPUID_ARCHITECTURE_Pos         16                                             /*!< SCB CPUID: ARCHITECTURE Position */

-#define SCB_CPUID_ARCHITECTURE_Msk         (0xFUL << SCB_CPUID_ARCHITECTURE_Pos)          /*!< SCB CPUID: ARCHITECTURE Mask */

-

-#define SCB_CPUID_PARTNO_Pos                4                                             /*!< SCB CPUID: PARTNO Position */

-#define SCB_CPUID_PARTNO_Msk               (0xFFFUL << SCB_CPUID_PARTNO_Pos)              /*!< SCB CPUID: PARTNO Mask */

-

-#define SCB_CPUID_REVISION_Pos              0                                             /*!< SCB CPUID: REVISION Position */

-#define SCB_CPUID_REVISION_Msk             (0xFUL << SCB_CPUID_REVISION_Pos)              /*!< SCB CPUID: REVISION Mask */

-

-/* SCB Interrupt Control State Register Definitions */

-#define SCB_ICSR_NMIPENDSET_Pos            31                                             /*!< SCB ICSR: NMIPENDSET Position */

-#define SCB_ICSR_NMIPENDSET_Msk            (1UL << SCB_ICSR_NMIPENDSET_Pos)               /*!< SCB ICSR: NMIPENDSET Mask */

-

-#define SCB_ICSR_PENDSVSET_Pos             28                                             /*!< SCB ICSR: PENDSVSET Position */

-#define SCB_ICSR_PENDSVSET_Msk             (1UL << SCB_ICSR_PENDSVSET_Pos)                /*!< SCB ICSR: PENDSVSET Mask */

-

-#define SCB_ICSR_PENDSVCLR_Pos             27                                             /*!< SCB ICSR: PENDSVCLR Position */

-#define SCB_ICSR_PENDSVCLR_Msk             (1UL << SCB_ICSR_PENDSVCLR_Pos)                /*!< SCB ICSR: PENDSVCLR Mask */

-

-#define SCB_ICSR_PENDSTSET_Pos             26                                             /*!< SCB ICSR: PENDSTSET Position */

-#define SCB_ICSR_PENDSTSET_Msk             (1UL << SCB_ICSR_PENDSTSET_Pos)                /*!< SCB ICSR: PENDSTSET Mask */

-

-#define SCB_ICSR_PENDSTCLR_Pos             25                                             /*!< SCB ICSR: PENDSTCLR Position */

-#define SCB_ICSR_PENDSTCLR_Msk             (1UL << SCB_ICSR_PENDSTCLR_Pos)                /*!< SCB ICSR: PENDSTCLR Mask */

-

-#define SCB_ICSR_ISRPREEMPT_Pos            23                                             /*!< SCB ICSR: ISRPREEMPT Position */

-#define SCB_ICSR_ISRPREEMPT_Msk            (1UL << SCB_ICSR_ISRPREEMPT_Pos)               /*!< SCB ICSR: ISRPREEMPT Mask */

-

-#define SCB_ICSR_ISRPENDING_Pos            22                                             /*!< SCB ICSR: ISRPENDING Position */

-#define SCB_ICSR_ISRPENDING_Msk            (1UL << SCB_ICSR_ISRPENDING_Pos)               /*!< SCB ICSR: ISRPENDING Mask */

-

-#define SCB_ICSR_VECTPENDING_Pos           12                                             /*!< SCB ICSR: VECTPENDING Position */

-#define SCB_ICSR_VECTPENDING_Msk           (0x1FFUL << SCB_ICSR_VECTPENDING_Pos)          /*!< SCB ICSR: VECTPENDING Mask */

-

-#define SCB_ICSR_RETTOBASE_Pos             11                                             /*!< SCB ICSR: RETTOBASE Position */

-#define SCB_ICSR_RETTOBASE_Msk             (1UL << SCB_ICSR_RETTOBASE_Pos)                /*!< SCB ICSR: RETTOBASE Mask */

-

-#define SCB_ICSR_VECTACTIVE_Pos             0                                             /*!< SCB ICSR: VECTACTIVE Position */

-#define SCB_ICSR_VECTACTIVE_Msk            (0x1FFUL << SCB_ICSR_VECTACTIVE_Pos)           /*!< SCB ICSR: VECTACTIVE Mask */

-

-/* SCB Vector Table Offset Register Definitions */

-#define SCB_VTOR_TBLOFF_Pos                 7                                             /*!< SCB VTOR: TBLOFF Position */

-#define SCB_VTOR_TBLOFF_Msk                (0x1FFFFFFUL << SCB_VTOR_TBLOFF_Pos)           /*!< SCB VTOR: TBLOFF Mask */

-

-/* SCB Application Interrupt and Reset Control Register Definitions */

-#define SCB_AIRCR_VECTKEY_Pos              16                                             /*!< SCB AIRCR: VECTKEY Position */

-#define SCB_AIRCR_VECTKEY_Msk              (0xFFFFUL << SCB_AIRCR_VECTKEY_Pos)            /*!< SCB AIRCR: VECTKEY Mask */

-

-#define SCB_AIRCR_VECTKEYSTAT_Pos          16                                             /*!< SCB AIRCR: VECTKEYSTAT Position */

-#define SCB_AIRCR_VECTKEYSTAT_Msk          (0xFFFFUL << SCB_AIRCR_VECTKEYSTAT_Pos)        /*!< SCB AIRCR: VECTKEYSTAT Mask */

-

-#define SCB_AIRCR_ENDIANESS_Pos            15                                             /*!< SCB AIRCR: ENDIANESS Position */

-#define SCB_AIRCR_ENDIANESS_Msk            (1UL << SCB_AIRCR_ENDIANESS_Pos)               /*!< SCB AIRCR: ENDIANESS Mask */

-

-#define SCB_AIRCR_PRIGROUP_Pos              8                                             /*!< SCB AIRCR: PRIGROUP Position */

-#define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */

-

-#define SCB_AIRCR_SYSRESETREQ_Pos           2                                             /*!< SCB AIRCR: SYSRESETREQ Position */

-#define SCB_AIRCR_SYSRESETREQ_Msk          (1UL << SCB_AIRCR_SYSRESETREQ_Pos)             /*!< SCB AIRCR: SYSRESETREQ Mask */

-

-#define SCB_AIRCR_VECTCLRACTIVE_Pos         1                                             /*!< SCB AIRCR: VECTCLRACTIVE Position */

-#define SCB_AIRCR_VECTCLRACTIVE_Msk        (1UL << SCB_AIRCR_VECTCLRACTIVE_Pos)           /*!< SCB AIRCR: VECTCLRACTIVE Mask */

-

-#define SCB_AIRCR_VECTRESET_Pos             0                                             /*!< SCB AIRCR: VECTRESET Position */

-#define SCB_AIRCR_VECTRESET_Msk            (1UL << SCB_AIRCR_VECTRESET_Pos)               /*!< SCB AIRCR: VECTRESET Mask */

-

-/* SCB System Control Register Definitions */

-#define SCB_SCR_SEVONPEND_Pos               4                                             /*!< SCB SCR: SEVONPEND Position */

-#define SCB_SCR_SEVONPEND_Msk              (1UL << SCB_SCR_SEVONPEND_Pos)                 /*!< SCB SCR: SEVONPEND Mask */

-

-#define SCB_SCR_SLEEPDEEP_Pos               2                                             /*!< SCB SCR: SLEEPDEEP Position */

-#define SCB_SCR_SLEEPDEEP_Msk              (1UL << SCB_SCR_SLEEPDEEP_Pos)                 /*!< SCB SCR: SLEEPDEEP Mask */

-

-#define SCB_SCR_SLEEPONEXIT_Pos             1                                             /*!< SCB SCR: SLEEPONEXIT Position */

-#define SCB_SCR_SLEEPONEXIT_Msk            (1UL << SCB_SCR_SLEEPONEXIT_Pos)               /*!< SCB SCR: SLEEPONEXIT Mask */

-

-/* SCB Configuration Control Register Definitions */

-#define SCB_CCR_STKALIGN_Pos                9                                             /*!< SCB CCR: STKALIGN Position */

-#define SCB_CCR_STKALIGN_Msk               (1UL << SCB_CCR_STKALIGN_Pos)                  /*!< SCB CCR: STKALIGN Mask */

-

-#define SCB_CCR_BFHFNMIGN_Pos               8                                             /*!< SCB CCR: BFHFNMIGN Position */

-#define SCB_CCR_BFHFNMIGN_Msk              (1UL << SCB_CCR_BFHFNMIGN_Pos)                 /*!< SCB CCR: BFHFNMIGN Mask */

-

-#define SCB_CCR_DIV_0_TRP_Pos               4                                             /*!< SCB CCR: DIV_0_TRP Position */

-#define SCB_CCR_DIV_0_TRP_Msk              (1UL << SCB_CCR_DIV_0_TRP_Pos)                 /*!< SCB CCR: DIV_0_TRP Mask */

-

-#define SCB_CCR_UNALIGN_TRP_Pos             3                                             /*!< SCB CCR: UNALIGN_TRP Position */

-#define SCB_CCR_UNALIGN_TRP_Msk            (1UL << SCB_CCR_UNALIGN_TRP_Pos)               /*!< SCB CCR: UNALIGN_TRP Mask */

-

-#define SCB_CCR_USERSETMPEND_Pos            1                                             /*!< SCB CCR: USERSETMPEND Position */

-#define SCB_CCR_USERSETMPEND_Msk           (1UL << SCB_CCR_USERSETMPEND_Pos)              /*!< SCB CCR: USERSETMPEND Mask */

-

-#define SCB_CCR_NONBASETHRDENA_Pos          0                                             /*!< SCB CCR: NONBASETHRDENA Position */

-#define SCB_CCR_NONBASETHRDENA_Msk         (1UL << SCB_CCR_NONBASETHRDENA_Pos)            /*!< SCB CCR: NONBASETHRDENA Mask */

-

-/* SCB System Handler Control and State Register Definitions */

-#define SCB_SHCSR_USGFAULTENA_Pos          18                                             /*!< SCB SHCSR: USGFAULTENA Position */

-#define SCB_SHCSR_USGFAULTENA_Msk          (1UL << SCB_SHCSR_USGFAULTENA_Pos)             /*!< SCB SHCSR: USGFAULTENA Mask */

-

-#define SCB_SHCSR_BUSFAULTENA_Pos          17                                             /*!< SCB SHCSR: BUSFAULTENA Position */

-#define SCB_SHCSR_BUSFAULTENA_Msk          (1UL << SCB_SHCSR_BUSFAULTENA_Pos)             /*!< SCB SHCSR: BUSFAULTENA Mask */

-

-#define SCB_SHCSR_MEMFAULTENA_Pos          16                                             /*!< SCB SHCSR: MEMFAULTENA Position */

-#define SCB_SHCSR_MEMFAULTENA_Msk          (1UL << SCB_SHCSR_MEMFAULTENA_Pos)             /*!< SCB SHCSR: MEMFAULTENA Mask */

-

-#define SCB_SHCSR_SVCALLPENDED_Pos         15                                             /*!< SCB SHCSR: SVCALLPENDED Position */

-#define SCB_SHCSR_SVCALLPENDED_Msk         (1UL << SCB_SHCSR_SVCALLPENDED_Pos)            /*!< SCB SHCSR: SVCALLPENDED Mask */

-

-#define SCB_SHCSR_BUSFAULTPENDED_Pos       14                                             /*!< SCB SHCSR: BUSFAULTPENDED Position */

-#define SCB_SHCSR_BUSFAULTPENDED_Msk       (1UL << SCB_SHCSR_BUSFAULTPENDED_Pos)          /*!< SCB SHCSR: BUSFAULTPENDED Mask */

-

-#define SCB_SHCSR_MEMFAULTPENDED_Pos       13                                             /*!< SCB SHCSR: MEMFAULTPENDED Position */

-#define SCB_SHCSR_MEMFAULTPENDED_Msk       (1UL << SCB_SHCSR_MEMFAULTPENDED_Pos)          /*!< SCB SHCSR: MEMFAULTPENDED Mask */

-

-#define SCB_SHCSR_USGFAULTPENDED_Pos       12                                             /*!< SCB SHCSR: USGFAULTPENDED Position */

-#define SCB_SHCSR_USGFAULTPENDED_Msk       (1UL << SCB_SHCSR_USGFAULTPENDED_Pos)          /*!< SCB SHCSR: USGFAULTPENDED Mask */

-

-#define SCB_SHCSR_SYSTICKACT_Pos           11                                             /*!< SCB SHCSR: SYSTICKACT Position */

-#define SCB_SHCSR_SYSTICKACT_Msk           (1UL << SCB_SHCSR_SYSTICKACT_Pos)              /*!< SCB SHCSR: SYSTICKACT Mask */

-

-#define SCB_SHCSR_PENDSVACT_Pos            10                                             /*!< SCB SHCSR: PENDSVACT Position */

-#define SCB_SHCSR_PENDSVACT_Msk            (1UL << SCB_SHCSR_PENDSVACT_Pos)               /*!< SCB SHCSR: PENDSVACT Mask */

-

-#define SCB_SHCSR_MONITORACT_Pos            8                                             /*!< SCB SHCSR: MONITORACT Position */

-#define SCB_SHCSR_MONITORACT_Msk           (1UL << SCB_SHCSR_MONITORACT_Pos)              /*!< SCB SHCSR: MONITORACT Mask */

-

-#define SCB_SHCSR_SVCALLACT_Pos             7                                             /*!< SCB SHCSR: SVCALLACT Position */

-#define SCB_SHCSR_SVCALLACT_Msk            (1UL << SCB_SHCSR_SVCALLACT_Pos)               /*!< SCB SHCSR: SVCALLACT Mask */

-

-#define SCB_SHCSR_USGFAULTACT_Pos           3                                             /*!< SCB SHCSR: USGFAULTACT Position */

-#define SCB_SHCSR_USGFAULTACT_Msk          (1UL << SCB_SHCSR_USGFAULTACT_Pos)             /*!< SCB SHCSR: USGFAULTACT Mask */

-

-#define SCB_SHCSR_BUSFAULTACT_Pos           1                                             /*!< SCB SHCSR: BUSFAULTACT Position */

-#define SCB_SHCSR_BUSFAULTACT_Msk          (1UL << SCB_SHCSR_BUSFAULTACT_Pos)             /*!< SCB SHCSR: BUSFAULTACT Mask */

-

-#define SCB_SHCSR_MEMFAULTACT_Pos           0                                             /*!< SCB SHCSR: MEMFAULTACT Position */

-#define SCB_SHCSR_MEMFAULTACT_Msk          (1UL << SCB_SHCSR_MEMFAULTACT_Pos)             /*!< SCB SHCSR: MEMFAULTACT Mask */

-

-/* SCB Configurable Fault Status Registers Definitions */

-#define SCB_CFSR_USGFAULTSR_Pos            16                                             /*!< SCB CFSR: Usage Fault Status Register Position */

-#define SCB_CFSR_USGFAULTSR_Msk            (0xFFFFUL << SCB_CFSR_USGFAULTSR_Pos)          /*!< SCB CFSR: Usage Fault Status Register Mask */

-

-#define SCB_CFSR_BUSFAULTSR_Pos             8                                             /*!< SCB CFSR: Bus Fault Status Register Position */

-#define SCB_CFSR_BUSFAULTSR_Msk            (0xFFUL << SCB_CFSR_BUSFAULTSR_Pos)            /*!< SCB CFSR: Bus Fault Status Register Mask */

-

-#define SCB_CFSR_MEMFAULTSR_Pos             0                                             /*!< SCB CFSR: Memory Manage Fault Status Register Position */

-#define SCB_CFSR_MEMFAULTSR_Msk            (0xFFUL << SCB_CFSR_MEMFAULTSR_Pos)            /*!< SCB CFSR: Memory Manage Fault Status Register Mask */

-

-/* SCB Hard Fault Status Registers Definitions */

-#define SCB_HFSR_DEBUGEVT_Pos              31                                             /*!< SCB HFSR: DEBUGEVT Position */

-#define SCB_HFSR_DEBUGEVT_Msk              (1UL << SCB_HFSR_DEBUGEVT_Pos)                 /*!< SCB HFSR: DEBUGEVT Mask */

-

-#define SCB_HFSR_FORCED_Pos                30                                             /*!< SCB HFSR: FORCED Position */

-#define SCB_HFSR_FORCED_Msk                (1UL << SCB_HFSR_FORCED_Pos)                   /*!< SCB HFSR: FORCED Mask */

-

-#define SCB_HFSR_VECTTBL_Pos                1                                             /*!< SCB HFSR: VECTTBL Position */

-#define SCB_HFSR_VECTTBL_Msk               (1UL << SCB_HFSR_VECTTBL_Pos)                  /*!< SCB HFSR: VECTTBL Mask */

-

-/* SCB Debug Fault Status Register Definitions */

-#define SCB_DFSR_EXTERNAL_Pos               4                                             /*!< SCB DFSR: EXTERNAL Position */

-#define SCB_DFSR_EXTERNAL_Msk              (1UL << SCB_DFSR_EXTERNAL_Pos)                 /*!< SCB DFSR: EXTERNAL Mask */

-

-#define SCB_DFSR_VCATCH_Pos                 3                                             /*!< SCB DFSR: VCATCH Position */

-#define SCB_DFSR_VCATCH_Msk                (1UL << SCB_DFSR_VCATCH_Pos)                   /*!< SCB DFSR: VCATCH Mask */

-

-#define SCB_DFSR_DWTTRAP_Pos                2                                             /*!< SCB DFSR: DWTTRAP Position */

-#define SCB_DFSR_DWTTRAP_Msk               (1UL << SCB_DFSR_DWTTRAP_Pos)                  /*!< SCB DFSR: DWTTRAP Mask */

-

-#define SCB_DFSR_BKPT_Pos                   1                                             /*!< SCB DFSR: BKPT Position */

-#define SCB_DFSR_BKPT_Msk                  (1UL << SCB_DFSR_BKPT_Pos)                     /*!< SCB DFSR: BKPT Mask */

-

-#define SCB_DFSR_HALTED_Pos                 0                                             /*!< SCB DFSR: HALTED Position */

-#define SCB_DFSR_HALTED_Msk                (1UL << SCB_DFSR_HALTED_Pos)                   /*!< SCB DFSR: HALTED Mask */

-

-/*@} end of group CMSIS_SCB */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_SCnSCB System Controls not in SCB (SCnSCB)

-    \brief      Type definitions for the System Control and ID Register not in the SCB

-  @{

- */

-

-/** \brief  Structure type to access the System Control and ID Register not in the SCB.

- */

-typedef struct

-{

-       uint32_t RESERVED0[1];

-  __I  uint32_t ICTR;                    /*!< Offset: 0x004 (R/ )  Interrupt Controller Type Register      */

-  __IO uint32_t ACTLR;                   /*!< Offset: 0x008 (R/W)  Auxiliary Control Register              */

-} SCnSCB_Type;

-

-/* Interrupt Controller Type Register Definitions */

-#define SCnSCB_ICTR_INTLINESNUM_Pos         0                                          /*!< ICTR: INTLINESNUM Position */

-#define SCnSCB_ICTR_INTLINESNUM_Msk        (0xFUL << SCnSCB_ICTR_INTLINESNUM_Pos)      /*!< ICTR: INTLINESNUM Mask */

-

-/* Auxiliary Control Register Definitions */

-#define SCnSCB_ACTLR_DISOOFP_Pos            9                                          /*!< ACTLR: DISOOFP Position */

-#define SCnSCB_ACTLR_DISOOFP_Msk           (1UL << SCnSCB_ACTLR_DISOOFP_Pos)           /*!< ACTLR: DISOOFP Mask */

-

-#define SCnSCB_ACTLR_DISFPCA_Pos            8                                          /*!< ACTLR: DISFPCA Position */

-#define SCnSCB_ACTLR_DISFPCA_Msk           (1UL << SCnSCB_ACTLR_DISFPCA_Pos)           /*!< ACTLR: DISFPCA Mask */

-

-#define SCnSCB_ACTLR_DISFOLD_Pos            2                                          /*!< ACTLR: DISFOLD Position */

-#define SCnSCB_ACTLR_DISFOLD_Msk           (1UL << SCnSCB_ACTLR_DISFOLD_Pos)           /*!< ACTLR: DISFOLD Mask */

-

-#define SCnSCB_ACTLR_DISDEFWBUF_Pos         1                                          /*!< ACTLR: DISDEFWBUF Position */

-#define SCnSCB_ACTLR_DISDEFWBUF_Msk        (1UL << SCnSCB_ACTLR_DISDEFWBUF_Pos)        /*!< ACTLR: DISDEFWBUF Mask */

-

-#define SCnSCB_ACTLR_DISMCYCINT_Pos         0                                          /*!< ACTLR: DISMCYCINT Position */

-#define SCnSCB_ACTLR_DISMCYCINT_Msk        (1UL << SCnSCB_ACTLR_DISMCYCINT_Pos)        /*!< ACTLR: DISMCYCINT Mask */

-

-/*@} end of group CMSIS_SCnotSCB */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_SysTick     System Tick Timer (SysTick)

-    \brief      Type definitions for the System Timer Registers.

-  @{

- */

-

-/** \brief  Structure type to access the System Timer (SysTick).

- */

-typedef struct

-{

-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  SysTick Control and Status Register */

-  __IO uint32_t LOAD;                    /*!< Offset: 0x004 (R/W)  SysTick Reload Value Register       */

-  __IO uint32_t VAL;                     /*!< Offset: 0x008 (R/W)  SysTick Current Value Register      */

-  __I  uint32_t CALIB;                   /*!< Offset: 0x00C (R/ )  SysTick Calibration Register        */

-} SysTick_Type;

-

-/* SysTick Control / Status Register Definitions */

-#define SysTick_CTRL_COUNTFLAG_Pos         16                                             /*!< SysTick CTRL: COUNTFLAG Position */

-#define SysTick_CTRL_COUNTFLAG_Msk         (1UL << SysTick_CTRL_COUNTFLAG_Pos)            /*!< SysTick CTRL: COUNTFLAG Mask */

-

-#define SysTick_CTRL_CLKSOURCE_Pos          2                                             /*!< SysTick CTRL: CLKSOURCE Position */

-#define SysTick_CTRL_CLKSOURCE_Msk         (1UL << SysTick_CTRL_CLKSOURCE_Pos)            /*!< SysTick CTRL: CLKSOURCE Mask */

-

-#define SysTick_CTRL_TICKINT_Pos            1                                             /*!< SysTick CTRL: TICKINT Position */

-#define SysTick_CTRL_TICKINT_Msk           (1UL << SysTick_CTRL_TICKINT_Pos)              /*!< SysTick CTRL: TICKINT Mask */

-

-#define SysTick_CTRL_ENABLE_Pos             0                                             /*!< SysTick CTRL: ENABLE Position */

-#define SysTick_CTRL_ENABLE_Msk            (1UL << SysTick_CTRL_ENABLE_Pos)               /*!< SysTick CTRL: ENABLE Mask */

-

-/* SysTick Reload Register Definitions */

-#define SysTick_LOAD_RELOAD_Pos             0                                             /*!< SysTick LOAD: RELOAD Position */

-#define SysTick_LOAD_RELOAD_Msk            (0xFFFFFFUL << SysTick_LOAD_RELOAD_Pos)        /*!< SysTick LOAD: RELOAD Mask */

-

-/* SysTick Current Register Definitions */

-#define SysTick_VAL_CURRENT_Pos             0                                             /*!< SysTick VAL: CURRENT Position */

-#define SysTick_VAL_CURRENT_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick VAL: CURRENT Mask */

-

-/* SysTick Calibration Register Definitions */

-#define SysTick_CALIB_NOREF_Pos            31                                             /*!< SysTick CALIB: NOREF Position */

-#define SysTick_CALIB_NOREF_Msk            (1UL << SysTick_CALIB_NOREF_Pos)               /*!< SysTick CALIB: NOREF Mask */

-

-#define SysTick_CALIB_SKEW_Pos             30                                             /*!< SysTick CALIB: SKEW Position */

-#define SysTick_CALIB_SKEW_Msk             (1UL << SysTick_CALIB_SKEW_Pos)                /*!< SysTick CALIB: SKEW Mask */

-

-#define SysTick_CALIB_TENMS_Pos             0                                             /*!< SysTick CALIB: TENMS Position */

-#define SysTick_CALIB_TENMS_Msk            (0xFFFFFFUL << SysTick_VAL_CURRENT_Pos)        /*!< SysTick CALIB: TENMS Mask */

-

-/*@} end of group CMSIS_SysTick */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_ITM     Instrumentation Trace Macrocell (ITM)

-    \brief      Type definitions for the Instrumentation Trace Macrocell (ITM)

-  @{

- */

-

-/** \brief  Structure type to access the Instrumentation Trace Macrocell Register (ITM).

- */

-typedef struct

-{

-  __O  union

-  {

-    __O  uint8_t    u8;                  /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 8-bit                   */

-    __O  uint16_t   u16;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 16-bit                  */

-    __O  uint32_t   u32;                 /*!< Offset: 0x000 ( /W)  ITM Stimulus Port 32-bit                  */

-  }  PORT [32];                          /*!< Offset: 0x000 ( /W)  ITM Stimulus Port Registers               */

-       uint32_t RESERVED0[864];

-  __IO uint32_t TER;                     /*!< Offset: 0xE00 (R/W)  ITM Trace Enable Register                 */

-       uint32_t RESERVED1[15];

-  __IO uint32_t TPR;                     /*!< Offset: 0xE40 (R/W)  ITM Trace Privilege Register              */

-       uint32_t RESERVED2[15];

-  __IO uint32_t TCR;                     /*!< Offset: 0xE80 (R/W)  ITM Trace Control Register                */

-       uint32_t RESERVED3[29];

-  __O  uint32_t IWR;                     /*!< Offset: 0xEF8 ( /W)  ITM Integration Write Register            */

-  __I  uint32_t IRR;                     /*!< Offset: 0xEFC (R/ )  ITM Integration Read Register             */

-  __IO uint32_t IMCR;                    /*!< Offset: 0xF00 (R/W)  ITM Integration Mode Control Register     */

-       uint32_t RESERVED4[43];

-  __O  uint32_t LAR;                     /*!< Offset: 0xFB0 ( /W)  ITM Lock Access Register                  */

-  __I  uint32_t LSR;                     /*!< Offset: 0xFB4 (R/ )  ITM Lock Status Register                  */

-       uint32_t RESERVED5[6];

-  __I  uint32_t PID4;                    /*!< Offset: 0xFD0 (R/ )  ITM Peripheral Identification Register #4 */

-  __I  uint32_t PID5;                    /*!< Offset: 0xFD4 (R/ )  ITM Peripheral Identification Register #5 */

-  __I  uint32_t PID6;                    /*!< Offset: 0xFD8 (R/ )  ITM Peripheral Identification Register #6 */

-  __I  uint32_t PID7;                    /*!< Offset: 0xFDC (R/ )  ITM Peripheral Identification Register #7 */

-  __I  uint32_t PID0;                    /*!< Offset: 0xFE0 (R/ )  ITM Peripheral Identification Register #0 */

-  __I  uint32_t PID1;                    /*!< Offset: 0xFE4 (R/ )  ITM Peripheral Identification Register #1 */

-  __I  uint32_t PID2;                    /*!< Offset: 0xFE8 (R/ )  ITM Peripheral Identification Register #2 */

-  __I  uint32_t PID3;                    /*!< Offset: 0xFEC (R/ )  ITM Peripheral Identification Register #3 */

-  __I  uint32_t CID0;                    /*!< Offset: 0xFF0 (R/ )  ITM Component  Identification Register #0 */

-  __I  uint32_t CID1;                    /*!< Offset: 0xFF4 (R/ )  ITM Component  Identification Register #1 */

-  __I  uint32_t CID2;                    /*!< Offset: 0xFF8 (R/ )  ITM Component  Identification Register #2 */

-  __I  uint32_t CID3;                    /*!< Offset: 0xFFC (R/ )  ITM Component  Identification Register #3 */

-} ITM_Type;

-

-/* ITM Trace Privilege Register Definitions */

-#define ITM_TPR_PRIVMASK_Pos                0                                             /*!< ITM TPR: PRIVMASK Position */

-#define ITM_TPR_PRIVMASK_Msk               (0xFUL << ITM_TPR_PRIVMASK_Pos)                /*!< ITM TPR: PRIVMASK Mask */

-

-/* ITM Trace Control Register Definitions */

-#define ITM_TCR_BUSY_Pos                   23                                             /*!< ITM TCR: BUSY Position */

-#define ITM_TCR_BUSY_Msk                   (1UL << ITM_TCR_BUSY_Pos)                      /*!< ITM TCR: BUSY Mask */

-

-#define ITM_TCR_TraceBusID_Pos             16                                             /*!< ITM TCR: ATBID Position */

-#define ITM_TCR_TraceBusID_Msk             (0x7FUL << ITM_TCR_TraceBusID_Pos)             /*!< ITM TCR: ATBID Mask */

-

-#define ITM_TCR_GTSFREQ_Pos                10                                             /*!< ITM TCR: Global timestamp frequency Position */

-#define ITM_TCR_GTSFREQ_Msk                (3UL << ITM_TCR_GTSFREQ_Pos)                   /*!< ITM TCR: Global timestamp frequency Mask */

-

-#define ITM_TCR_TSPrescale_Pos              8                                             /*!< ITM TCR: TSPrescale Position */

-#define ITM_TCR_TSPrescale_Msk             (3UL << ITM_TCR_TSPrescale_Pos)                /*!< ITM TCR: TSPrescale Mask */

-

-#define ITM_TCR_SWOENA_Pos                  4                                             /*!< ITM TCR: SWOENA Position */

-#define ITM_TCR_SWOENA_Msk                 (1UL << ITM_TCR_SWOENA_Pos)                    /*!< ITM TCR: SWOENA Mask */

-

-#define ITM_TCR_DWTENA_Pos                  3                                             /*!< ITM TCR: DWTENA Position */

-#define ITM_TCR_DWTENA_Msk                 (1UL << ITM_TCR_DWTENA_Pos)                    /*!< ITM TCR: DWTENA Mask */

-

-#define ITM_TCR_SYNCENA_Pos                 2                                             /*!< ITM TCR: SYNCENA Position */

-#define ITM_TCR_SYNCENA_Msk                (1UL << ITM_TCR_SYNCENA_Pos)                   /*!< ITM TCR: SYNCENA Mask */

-

-#define ITM_TCR_TSENA_Pos                   1                                             /*!< ITM TCR: TSENA Position */

-#define ITM_TCR_TSENA_Msk                  (1UL << ITM_TCR_TSENA_Pos)                     /*!< ITM TCR: TSENA Mask */

-

-#define ITM_TCR_ITMENA_Pos                  0                                             /*!< ITM TCR: ITM Enable bit Position */

-#define ITM_TCR_ITMENA_Msk                 (1UL << ITM_TCR_ITMENA_Pos)                    /*!< ITM TCR: ITM Enable bit Mask */

-

-/* ITM Integration Write Register Definitions */

-#define ITM_IWR_ATVALIDM_Pos                0                                             /*!< ITM IWR: ATVALIDM Position */

-#define ITM_IWR_ATVALIDM_Msk               (1UL << ITM_IWR_ATVALIDM_Pos)                  /*!< ITM IWR: ATVALIDM Mask */

-

-/* ITM Integration Read Register Definitions */

-#define ITM_IRR_ATREADYM_Pos                0                                             /*!< ITM IRR: ATREADYM Position */

-#define ITM_IRR_ATREADYM_Msk               (1UL << ITM_IRR_ATREADYM_Pos)                  /*!< ITM IRR: ATREADYM Mask */

-

-/* ITM Integration Mode Control Register Definitions */

-#define ITM_IMCR_INTEGRATION_Pos            0                                             /*!< ITM IMCR: INTEGRATION Position */

-#define ITM_IMCR_INTEGRATION_Msk           (1UL << ITM_IMCR_INTEGRATION_Pos)              /*!< ITM IMCR: INTEGRATION Mask */

-

-/* ITM Lock Status Register Definitions */

-#define ITM_LSR_ByteAcc_Pos                 2                                             /*!< ITM LSR: ByteAcc Position */

-#define ITM_LSR_ByteAcc_Msk                (1UL << ITM_LSR_ByteAcc_Pos)                   /*!< ITM LSR: ByteAcc Mask */

-

-#define ITM_LSR_Access_Pos                  1                                             /*!< ITM LSR: Access Position */

-#define ITM_LSR_Access_Msk                 (1UL << ITM_LSR_Access_Pos)                    /*!< ITM LSR: Access Mask */

-

-#define ITM_LSR_Present_Pos                 0                                             /*!< ITM LSR: Present Position */

-#define ITM_LSR_Present_Msk                (1UL << ITM_LSR_Present_Pos)                   /*!< ITM LSR: Present Mask */

-

-/*@}*/ /* end of group CMSIS_ITM */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_DWT     Data Watchpoint and Trace (DWT)

-    \brief      Type definitions for the Data Watchpoint and Trace (DWT)

-  @{

- */

-

-/** \brief  Structure type to access the Data Watchpoint and Trace Register (DWT).

- */

-typedef struct

-{

-  __IO uint32_t CTRL;                    /*!< Offset: 0x000 (R/W)  Control Register                          */

-  __IO uint32_t CYCCNT;                  /*!< Offset: 0x004 (R/W)  Cycle Count Register                      */

-  __IO uint32_t CPICNT;                  /*!< Offset: 0x008 (R/W)  CPI Count Register                        */

-  __IO uint32_t EXCCNT;                  /*!< Offset: 0x00C (R/W)  Exception Overhead Count Register         */

-  __IO uint32_t SLEEPCNT;                /*!< Offset: 0x010 (R/W)  Sleep Count Register                      */

-  __IO uint32_t LSUCNT;                  /*!< Offset: 0x014 (R/W)  LSU Count Register                        */

-  __IO uint32_t FOLDCNT;                 /*!< Offset: 0x018 (R/W)  Folded-instruction Count Register         */

-  __I  uint32_t PCSR;                    /*!< Offset: 0x01C (R/ )  Program Counter Sample Register           */

-  __IO uint32_t COMP0;                   /*!< Offset: 0x020 (R/W)  Comparator Register 0                     */

-  __IO uint32_t MASK0;                   /*!< Offset: 0x024 (R/W)  Mask Register 0                           */

-  __IO uint32_t FUNCTION0;               /*!< Offset: 0x028 (R/W)  Function Register 0                       */

-       uint32_t RESERVED0[1];

-  __IO uint32_t COMP1;                   /*!< Offset: 0x030 (R/W)  Comparator Register 1                     */

-  __IO uint32_t MASK1;                   /*!< Offset: 0x034 (R/W)  Mask Register 1                           */

-  __IO uint32_t FUNCTION1;               /*!< Offset: 0x038 (R/W)  Function Register 1                       */

-       uint32_t RESERVED1[1];

-  __IO uint32_t COMP2;                   /*!< Offset: 0x040 (R/W)  Comparator Register 2                     */

-  __IO uint32_t MASK2;                   /*!< Offset: 0x044 (R/W)  Mask Register 2                           */

-  __IO uint32_t FUNCTION2;               /*!< Offset: 0x048 (R/W)  Function Register 2                       */

-       uint32_t RESERVED2[1];

-  __IO uint32_t COMP3;                   /*!< Offset: 0x050 (R/W)  Comparator Register 3                     */

-  __IO uint32_t MASK3;                   /*!< Offset: 0x054 (R/W)  Mask Register 3                           */

-  __IO uint32_t FUNCTION3;               /*!< Offset: 0x058 (R/W)  Function Register 3                       */

-} DWT_Type;

-

-/* DWT Control Register Definitions */

-#define DWT_CTRL_NUMCOMP_Pos               28                                          /*!< DWT CTRL: NUMCOMP Position */

-#define DWT_CTRL_NUMCOMP_Msk               (0xFUL << DWT_CTRL_NUMCOMP_Pos)             /*!< DWT CTRL: NUMCOMP Mask */

-

-#define DWT_CTRL_NOTRCPKT_Pos              27                                          /*!< DWT CTRL: NOTRCPKT Position */

-#define DWT_CTRL_NOTRCPKT_Msk              (0x1UL << DWT_CTRL_NOTRCPKT_Pos)            /*!< DWT CTRL: NOTRCPKT Mask */

-

-#define DWT_CTRL_NOEXTTRIG_Pos             26                                          /*!< DWT CTRL: NOEXTTRIG Position */

-#define DWT_CTRL_NOEXTTRIG_Msk             (0x1UL << DWT_CTRL_NOEXTTRIG_Pos)           /*!< DWT CTRL: NOEXTTRIG Mask */

-

-#define DWT_CTRL_NOCYCCNT_Pos              25                                          /*!< DWT CTRL: NOCYCCNT Position */

-#define DWT_CTRL_NOCYCCNT_Msk              (0x1UL << DWT_CTRL_NOCYCCNT_Pos)            /*!< DWT CTRL: NOCYCCNT Mask */

-

-#define DWT_CTRL_NOPRFCNT_Pos              24                                          /*!< DWT CTRL: NOPRFCNT Position */

-#define DWT_CTRL_NOPRFCNT_Msk              (0x1UL << DWT_CTRL_NOPRFCNT_Pos)            /*!< DWT CTRL: NOPRFCNT Mask */

-

-#define DWT_CTRL_CYCEVTENA_Pos             22                                          /*!< DWT CTRL: CYCEVTENA Position */

-#define DWT_CTRL_CYCEVTENA_Msk             (0x1UL << DWT_CTRL_CYCEVTENA_Pos)           /*!< DWT CTRL: CYCEVTENA Mask */

-

-#define DWT_CTRL_FOLDEVTENA_Pos            21                                          /*!< DWT CTRL: FOLDEVTENA Position */

-#define DWT_CTRL_FOLDEVTENA_Msk            (0x1UL << DWT_CTRL_FOLDEVTENA_Pos)          /*!< DWT CTRL: FOLDEVTENA Mask */

-

-#define DWT_CTRL_LSUEVTENA_Pos             20                                          /*!< DWT CTRL: LSUEVTENA Position */

-#define DWT_CTRL_LSUEVTENA_Msk             (0x1UL << DWT_CTRL_LSUEVTENA_Pos)           /*!< DWT CTRL: LSUEVTENA Mask */

-

-#define DWT_CTRL_SLEEPEVTENA_Pos           19                                          /*!< DWT CTRL: SLEEPEVTENA Position */

-#define DWT_CTRL_SLEEPEVTENA_Msk           (0x1UL << DWT_CTRL_SLEEPEVTENA_Pos)         /*!< DWT CTRL: SLEEPEVTENA Mask */

-

-#define DWT_CTRL_EXCEVTENA_Pos             18                                          /*!< DWT CTRL: EXCEVTENA Position */

-#define DWT_CTRL_EXCEVTENA_Msk             (0x1UL << DWT_CTRL_EXCEVTENA_Pos)           /*!< DWT CTRL: EXCEVTENA Mask */

-

-#define DWT_CTRL_CPIEVTENA_Pos             17                                          /*!< DWT CTRL: CPIEVTENA Position */

-#define DWT_CTRL_CPIEVTENA_Msk             (0x1UL << DWT_CTRL_CPIEVTENA_Pos)           /*!< DWT CTRL: CPIEVTENA Mask */

-

-#define DWT_CTRL_EXCTRCENA_Pos             16                                          /*!< DWT CTRL: EXCTRCENA Position */

-#define DWT_CTRL_EXCTRCENA_Msk             (0x1UL << DWT_CTRL_EXCTRCENA_Pos)           /*!< DWT CTRL: EXCTRCENA Mask */

-

-#define DWT_CTRL_PCSAMPLENA_Pos            12                                          /*!< DWT CTRL: PCSAMPLENA Position */

-#define DWT_CTRL_PCSAMPLENA_Msk            (0x1UL << DWT_CTRL_PCSAMPLENA_Pos)          /*!< DWT CTRL: PCSAMPLENA Mask */

-

-#define DWT_CTRL_SYNCTAP_Pos               10                                          /*!< DWT CTRL: SYNCTAP Position */

-#define DWT_CTRL_SYNCTAP_Msk               (0x3UL << DWT_CTRL_SYNCTAP_Pos)             /*!< DWT CTRL: SYNCTAP Mask */

-

-#define DWT_CTRL_CYCTAP_Pos                 9                                          /*!< DWT CTRL: CYCTAP Position */

-#define DWT_CTRL_CYCTAP_Msk                (0x1UL << DWT_CTRL_CYCTAP_Pos)              /*!< DWT CTRL: CYCTAP Mask */

-

-#define DWT_CTRL_POSTINIT_Pos               5                                          /*!< DWT CTRL: POSTINIT Position */

-#define DWT_CTRL_POSTINIT_Msk              (0xFUL << DWT_CTRL_POSTINIT_Pos)            /*!< DWT CTRL: POSTINIT Mask */

-

-#define DWT_CTRL_POSTPRESET_Pos             1                                          /*!< DWT CTRL: POSTPRESET Position */

-#define DWT_CTRL_POSTPRESET_Msk            (0xFUL << DWT_CTRL_POSTPRESET_Pos)          /*!< DWT CTRL: POSTPRESET Mask */

-

-#define DWT_CTRL_CYCCNTENA_Pos              0                                          /*!< DWT CTRL: CYCCNTENA Position */

-#define DWT_CTRL_CYCCNTENA_Msk             (0x1UL << DWT_CTRL_CYCCNTENA_Pos)           /*!< DWT CTRL: CYCCNTENA Mask */

-

-/* DWT CPI Count Register Definitions */

-#define DWT_CPICNT_CPICNT_Pos               0                                          /*!< DWT CPICNT: CPICNT Position */

-#define DWT_CPICNT_CPICNT_Msk              (0xFFUL << DWT_CPICNT_CPICNT_Pos)           /*!< DWT CPICNT: CPICNT Mask */

-

-/* DWT Exception Overhead Count Register Definitions */

-#define DWT_EXCCNT_EXCCNT_Pos               0                                          /*!< DWT EXCCNT: EXCCNT Position */

-#define DWT_EXCCNT_EXCCNT_Msk              (0xFFUL << DWT_EXCCNT_EXCCNT_Pos)           /*!< DWT EXCCNT: EXCCNT Mask */

-

-/* DWT Sleep Count Register Definitions */

-#define DWT_SLEEPCNT_SLEEPCNT_Pos           0                                          /*!< DWT SLEEPCNT: SLEEPCNT Position */

-#define DWT_SLEEPCNT_SLEEPCNT_Msk          (0xFFUL << DWT_SLEEPCNT_SLEEPCNT_Pos)       /*!< DWT SLEEPCNT: SLEEPCNT Mask */

-

-/* DWT LSU Count Register Definitions */

-#define DWT_LSUCNT_LSUCNT_Pos               0                                          /*!< DWT LSUCNT: LSUCNT Position */

-#define DWT_LSUCNT_LSUCNT_Msk              (0xFFUL << DWT_LSUCNT_LSUCNT_Pos)           /*!< DWT LSUCNT: LSUCNT Mask */

-

-/* DWT Folded-instruction Count Register Definitions */

-#define DWT_FOLDCNT_FOLDCNT_Pos             0                                          /*!< DWT FOLDCNT: FOLDCNT Position */

-#define DWT_FOLDCNT_FOLDCNT_Msk            (0xFFUL << DWT_FOLDCNT_FOLDCNT_Pos)         /*!< DWT FOLDCNT: FOLDCNT Mask */

-

-/* DWT Comparator Mask Register Definitions */

-#define DWT_MASK_MASK_Pos                   0                                          /*!< DWT MASK: MASK Position */

-#define DWT_MASK_MASK_Msk                  (0x1FUL << DWT_MASK_MASK_Pos)               /*!< DWT MASK: MASK Mask */

-

-/* DWT Comparator Function Register Definitions */

-#define DWT_FUNCTION_MATCHED_Pos           24                                          /*!< DWT FUNCTION: MATCHED Position */

-#define DWT_FUNCTION_MATCHED_Msk           (0x1UL << DWT_FUNCTION_MATCHED_Pos)         /*!< DWT FUNCTION: MATCHED Mask */

-

-#define DWT_FUNCTION_DATAVADDR1_Pos        16                                          /*!< DWT FUNCTION: DATAVADDR1 Position */

-#define DWT_FUNCTION_DATAVADDR1_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR1_Pos)      /*!< DWT FUNCTION: DATAVADDR1 Mask */

-

-#define DWT_FUNCTION_DATAVADDR0_Pos        12                                          /*!< DWT FUNCTION: DATAVADDR0 Position */

-#define DWT_FUNCTION_DATAVADDR0_Msk        (0xFUL << DWT_FUNCTION_DATAVADDR0_Pos)      /*!< DWT FUNCTION: DATAVADDR0 Mask */

-

-#define DWT_FUNCTION_DATAVSIZE_Pos         10                                          /*!< DWT FUNCTION: DATAVSIZE Position */

-#define DWT_FUNCTION_DATAVSIZE_Msk         (0x3UL << DWT_FUNCTION_DATAVSIZE_Pos)       /*!< DWT FUNCTION: DATAVSIZE Mask */

-

-#define DWT_FUNCTION_LNK1ENA_Pos            9                                          /*!< DWT FUNCTION: LNK1ENA Position */

-#define DWT_FUNCTION_LNK1ENA_Msk           (0x1UL << DWT_FUNCTION_LNK1ENA_Pos)         /*!< DWT FUNCTION: LNK1ENA Mask */

-

-#define DWT_FUNCTION_DATAVMATCH_Pos         8                                          /*!< DWT FUNCTION: DATAVMATCH Position */

-#define DWT_FUNCTION_DATAVMATCH_Msk        (0x1UL << DWT_FUNCTION_DATAVMATCH_Pos)      /*!< DWT FUNCTION: DATAVMATCH Mask */

-

-#define DWT_FUNCTION_CYCMATCH_Pos           7                                          /*!< DWT FUNCTION: CYCMATCH Position */

-#define DWT_FUNCTION_CYCMATCH_Msk          (0x1UL << DWT_FUNCTION_CYCMATCH_Pos)        /*!< DWT FUNCTION: CYCMATCH Mask */

-

-#define DWT_FUNCTION_EMITRANGE_Pos          5                                          /*!< DWT FUNCTION: EMITRANGE Position */

-#define DWT_FUNCTION_EMITRANGE_Msk         (0x1UL << DWT_FUNCTION_EMITRANGE_Pos)       /*!< DWT FUNCTION: EMITRANGE Mask */

-

-#define DWT_FUNCTION_FUNCTION_Pos           0                                          /*!< DWT FUNCTION: FUNCTION Position */

-#define DWT_FUNCTION_FUNCTION_Msk          (0xFUL << DWT_FUNCTION_FUNCTION_Pos)        /*!< DWT FUNCTION: FUNCTION Mask */

-

-/*@}*/ /* end of group CMSIS_DWT */

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_TPI     Trace Port Interface (TPI)

-    \brief      Type definitions for the Trace Port Interface (TPI)

-  @{

- */

-

-/** \brief  Structure type to access the Trace Port Interface Register (TPI).

- */

-typedef struct

-{

-  __IO uint32_t SSPSR;                   /*!< Offset: 0x000 (R/ )  Supported Parallel Port Size Register     */

-  __IO uint32_t CSPSR;                   /*!< Offset: 0x004 (R/W)  Current Parallel Port Size Register */

-       uint32_t RESERVED0[2];

-  __IO uint32_t ACPR;                    /*!< Offset: 0x010 (R/W)  Asynchronous Clock Prescaler Register */

-       uint32_t RESERVED1[55];

-  __IO uint32_t SPPR;                    /*!< Offset: 0x0F0 (R/W)  Selected Pin Protocol Register */

-       uint32_t RESERVED2[131];

-  __I  uint32_t FFSR;                    /*!< Offset: 0x300 (R/ )  Formatter and Flush Status Register */

-  __IO uint32_t FFCR;                    /*!< Offset: 0x304 (R/W)  Formatter and Flush Control Register */

-  __I  uint32_t FSCR;                    /*!< Offset: 0x308 (R/ )  Formatter Synchronization Counter Register */

-       uint32_t RESERVED3[759];

-  __I  uint32_t TRIGGER;                 /*!< Offset: 0xEE8 (R/ )  TRIGGER */

-  __I  uint32_t FIFO0;                   /*!< Offset: 0xEEC (R/ )  Integration ETM Data */

-  __I  uint32_t ITATBCTR2;               /*!< Offset: 0xEF0 (R/ )  ITATBCTR2 */

-       uint32_t RESERVED4[1];

-  __I  uint32_t ITATBCTR0;               /*!< Offset: 0xEF8 (R/ )  ITATBCTR0 */

-  __I  uint32_t FIFO1;                   /*!< Offset: 0xEFC (R/ )  Integration ITM Data */

-  __IO uint32_t ITCTRL;                  /*!< Offset: 0xF00 (R/W)  Integration Mode Control */

-       uint32_t RESERVED5[39];

-  __IO uint32_t CLAIMSET;                /*!< Offset: 0xFA0 (R/W)  Claim tag set */

-  __IO uint32_t CLAIMCLR;                /*!< Offset: 0xFA4 (R/W)  Claim tag clear */

-       uint32_t RESERVED7[8];

-  __I  uint32_t DEVID;                   /*!< Offset: 0xFC8 (R/ )  TPIU_DEVID */

-  __I  uint32_t DEVTYPE;                 /*!< Offset: 0xFCC (R/ )  TPIU_DEVTYPE */

-} TPI_Type;

-

-/* TPI Asynchronous Clock Prescaler Register Definitions */

-#define TPI_ACPR_PRESCALER_Pos              0                                          /*!< TPI ACPR: PRESCALER Position */

-#define TPI_ACPR_PRESCALER_Msk             (0x1FFFUL << TPI_ACPR_PRESCALER_Pos)        /*!< TPI ACPR: PRESCALER Mask */

-

-/* TPI Selected Pin Protocol Register Definitions */

-#define TPI_SPPR_TXMODE_Pos                 0                                          /*!< TPI SPPR: TXMODE Position */

-#define TPI_SPPR_TXMODE_Msk                (0x3UL << TPI_SPPR_TXMODE_Pos)              /*!< TPI SPPR: TXMODE Mask */

-

-/* TPI Formatter and Flush Status Register Definitions */

-#define TPI_FFSR_FtNonStop_Pos              3                                          /*!< TPI FFSR: FtNonStop Position */

-#define TPI_FFSR_FtNonStop_Msk             (0x1UL << TPI_FFSR_FtNonStop_Pos)           /*!< TPI FFSR: FtNonStop Mask */

-

-#define TPI_FFSR_TCPresent_Pos              2                                          /*!< TPI FFSR: TCPresent Position */

-#define TPI_FFSR_TCPresent_Msk             (0x1UL << TPI_FFSR_TCPresent_Pos)           /*!< TPI FFSR: TCPresent Mask */

-

-#define TPI_FFSR_FtStopped_Pos              1                                          /*!< TPI FFSR: FtStopped Position */

-#define TPI_FFSR_FtStopped_Msk             (0x1UL << TPI_FFSR_FtStopped_Pos)           /*!< TPI FFSR: FtStopped Mask */

-

-#define TPI_FFSR_FlInProg_Pos               0                                          /*!< TPI FFSR: FlInProg Position */

-#define TPI_FFSR_FlInProg_Msk              (0x1UL << TPI_FFSR_FlInProg_Pos)            /*!< TPI FFSR: FlInProg Mask */

-

-/* TPI Formatter and Flush Control Register Definitions */

-#define TPI_FFCR_TrigIn_Pos                 8                                          /*!< TPI FFCR: TrigIn Position */

-#define TPI_FFCR_TrigIn_Msk                (0x1UL << TPI_FFCR_TrigIn_Pos)              /*!< TPI FFCR: TrigIn Mask */

-

-#define TPI_FFCR_EnFCont_Pos                1                                          /*!< TPI FFCR: EnFCont Position */

-#define TPI_FFCR_EnFCont_Msk               (0x1UL << TPI_FFCR_EnFCont_Pos)             /*!< TPI FFCR: EnFCont Mask */

-

-/* TPI TRIGGER Register Definitions */

-#define TPI_TRIGGER_TRIGGER_Pos             0                                          /*!< TPI TRIGGER: TRIGGER Position */

-#define TPI_TRIGGER_TRIGGER_Msk            (0x1UL << TPI_TRIGGER_TRIGGER_Pos)          /*!< TPI TRIGGER: TRIGGER Mask */

-

-/* TPI Integration ETM Data Register Definitions (FIFO0) */

-#define TPI_FIFO0_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO0: ITM_ATVALID Position */

-#define TPI_FIFO0_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ITM_ATVALID_Pos)        /*!< TPI FIFO0: ITM_ATVALID Mask */

-

-#define TPI_FIFO0_ITM_bytecount_Pos        27                                          /*!< TPI FIFO0: ITM_bytecount Position */

-#define TPI_FIFO0_ITM_bytecount_Msk        (0x3UL << TPI_FIFO0_ITM_bytecount_Pos)      /*!< TPI FIFO0: ITM_bytecount Mask */

-

-#define TPI_FIFO0_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO0: ETM_ATVALID Position */

-#define TPI_FIFO0_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO0_ETM_ATVALID_Pos)        /*!< TPI FIFO0: ETM_ATVALID Mask */

-

-#define TPI_FIFO0_ETM_bytecount_Pos        24                                          /*!< TPI FIFO0: ETM_bytecount Position */

-#define TPI_FIFO0_ETM_bytecount_Msk        (0x3UL << TPI_FIFO0_ETM_bytecount_Pos)      /*!< TPI FIFO0: ETM_bytecount Mask */

-

-#define TPI_FIFO0_ETM2_Pos                 16                                          /*!< TPI FIFO0: ETM2 Position */

-#define TPI_FIFO0_ETM2_Msk                 (0xFFUL << TPI_FIFO0_ETM2_Pos)              /*!< TPI FIFO0: ETM2 Mask */

-

-#define TPI_FIFO0_ETM1_Pos                  8                                          /*!< TPI FIFO0: ETM1 Position */

-#define TPI_FIFO0_ETM1_Msk                 (0xFFUL << TPI_FIFO0_ETM1_Pos)              /*!< TPI FIFO0: ETM1 Mask */

-

-#define TPI_FIFO0_ETM0_Pos                  0                                          /*!< TPI FIFO0: ETM0 Position */

-#define TPI_FIFO0_ETM0_Msk                 (0xFFUL << TPI_FIFO0_ETM0_Pos)              /*!< TPI FIFO0: ETM0 Mask */

-

-/* TPI ITATBCTR2 Register Definitions */

-#define TPI_ITATBCTR2_ATREADY_Pos           0                                          /*!< TPI ITATBCTR2: ATREADY Position */

-#define TPI_ITATBCTR2_ATREADY_Msk          (0x1UL << TPI_ITATBCTR2_ATREADY_Pos)        /*!< TPI ITATBCTR2: ATREADY Mask */

-

-/* TPI Integration ITM Data Register Definitions (FIFO1) */

-#define TPI_FIFO1_ITM_ATVALID_Pos          29                                          /*!< TPI FIFO1: ITM_ATVALID Position */

-#define TPI_FIFO1_ITM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ITM_ATVALID_Pos)        /*!< TPI FIFO1: ITM_ATVALID Mask */

-

-#define TPI_FIFO1_ITM_bytecount_Pos        27                                          /*!< TPI FIFO1: ITM_bytecount Position */

-#define TPI_FIFO1_ITM_bytecount_Msk        (0x3UL << TPI_FIFO1_ITM_bytecount_Pos)      /*!< TPI FIFO1: ITM_bytecount Mask */

-

-#define TPI_FIFO1_ETM_ATVALID_Pos          26                                          /*!< TPI FIFO1: ETM_ATVALID Position */

-#define TPI_FIFO1_ETM_ATVALID_Msk          (0x3UL << TPI_FIFO1_ETM_ATVALID_Pos)        /*!< TPI FIFO1: ETM_ATVALID Mask */

-

-#define TPI_FIFO1_ETM_bytecount_Pos        24                                          /*!< TPI FIFO1: ETM_bytecount Position */

-#define TPI_FIFO1_ETM_bytecount_Msk        (0x3UL << TPI_FIFO1_ETM_bytecount_Pos)      /*!< TPI FIFO1: ETM_bytecount Mask */

-

-#define TPI_FIFO1_ITM2_Pos                 16                                          /*!< TPI FIFO1: ITM2 Position */

-#define TPI_FIFO1_ITM2_Msk                 (0xFFUL << TPI_FIFO1_ITM2_Pos)              /*!< TPI FIFO1: ITM2 Mask */

-

-#define TPI_FIFO1_ITM1_Pos                  8                                          /*!< TPI FIFO1: ITM1 Position */

-#define TPI_FIFO1_ITM1_Msk                 (0xFFUL << TPI_FIFO1_ITM1_Pos)              /*!< TPI FIFO1: ITM1 Mask */

-

-#define TPI_FIFO1_ITM0_Pos                  0                                          /*!< TPI FIFO1: ITM0 Position */

-#define TPI_FIFO1_ITM0_Msk                 (0xFFUL << TPI_FIFO1_ITM0_Pos)              /*!< TPI FIFO1: ITM0 Mask */

-

-/* TPI ITATBCTR0 Register Definitions */

-#define TPI_ITATBCTR0_ATREADY_Pos           0                                          /*!< TPI ITATBCTR0: ATREADY Position */

-#define TPI_ITATBCTR0_ATREADY_Msk          (0x1UL << TPI_ITATBCTR0_ATREADY_Pos)        /*!< TPI ITATBCTR0: ATREADY Mask */

-

-/* TPI Integration Mode Control Register Definitions */

-#define TPI_ITCTRL_Mode_Pos                 0                                          /*!< TPI ITCTRL: Mode Position */

-#define TPI_ITCTRL_Mode_Msk                (0x1UL << TPI_ITCTRL_Mode_Pos)              /*!< TPI ITCTRL: Mode Mask */

-

-/* TPI DEVID Register Definitions */

-#define TPI_DEVID_NRZVALID_Pos             11                                          /*!< TPI DEVID: NRZVALID Position */

-#define TPI_DEVID_NRZVALID_Msk             (0x1UL << TPI_DEVID_NRZVALID_Pos)           /*!< TPI DEVID: NRZVALID Mask */

-

-#define TPI_DEVID_MANCVALID_Pos            10                                          /*!< TPI DEVID: MANCVALID Position */

-#define TPI_DEVID_MANCVALID_Msk            (0x1UL << TPI_DEVID_MANCVALID_Pos)          /*!< TPI DEVID: MANCVALID Mask */

-

-#define TPI_DEVID_PTINVALID_Pos             9                                          /*!< TPI DEVID: PTINVALID Position */

-#define TPI_DEVID_PTINVALID_Msk            (0x1UL << TPI_DEVID_PTINVALID_Pos)          /*!< TPI DEVID: PTINVALID Mask */

-

-#define TPI_DEVID_MinBufSz_Pos              6                                          /*!< TPI DEVID: MinBufSz Position */

-#define TPI_DEVID_MinBufSz_Msk             (0x7UL << TPI_DEVID_MinBufSz_Pos)           /*!< TPI DEVID: MinBufSz Mask */

-

-#define TPI_DEVID_AsynClkIn_Pos             5                                          /*!< TPI DEVID: AsynClkIn Position */

-#define TPI_DEVID_AsynClkIn_Msk            (0x1UL << TPI_DEVID_AsynClkIn_Pos)          /*!< TPI DEVID: AsynClkIn Mask */

-

-#define TPI_DEVID_NrTraceInput_Pos          0                                          /*!< TPI DEVID: NrTraceInput Position */

-#define TPI_DEVID_NrTraceInput_Msk         (0x1FUL << TPI_DEVID_NrTraceInput_Pos)      /*!< TPI DEVID: NrTraceInput Mask */

-

-/* TPI DEVTYPE Register Definitions */

-#define TPI_DEVTYPE_SubType_Pos             0                                          /*!< TPI DEVTYPE: SubType Position */

-#define TPI_DEVTYPE_SubType_Msk            (0xFUL << TPI_DEVTYPE_SubType_Pos)          /*!< TPI DEVTYPE: SubType Mask */

-

-#define TPI_DEVTYPE_MajorType_Pos           4                                          /*!< TPI DEVTYPE: MajorType Position */

-#define TPI_DEVTYPE_MajorType_Msk          (0xFUL << TPI_DEVTYPE_MajorType_Pos)        /*!< TPI DEVTYPE: MajorType Mask */

-

-/*@}*/ /* end of group CMSIS_TPI */

-

-

-#if (__MPU_PRESENT == 1)

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_MPU     Memory Protection Unit (MPU)

-    \brief      Type definitions for the Memory Protection Unit (MPU)

-  @{

- */

-

-/** \brief  Structure type to access the Memory Protection Unit (MPU).

- */

-typedef struct

-{

-  __I  uint32_t TYPE;                    /*!< Offset: 0x000 (R/ )  MPU Type Register                              */

-  __IO uint32_t CTRL;                    /*!< Offset: 0x004 (R/W)  MPU Control Register                           */

-  __IO uint32_t RNR;                     /*!< Offset: 0x008 (R/W)  MPU Region RNRber Register                     */

-  __IO uint32_t RBAR;                    /*!< Offset: 0x00C (R/W)  MPU Region Base Address Register               */

-  __IO uint32_t RASR;                    /*!< Offset: 0x010 (R/W)  MPU Region Attribute and Size Register         */

-  __IO uint32_t RBAR_A1;                 /*!< Offset: 0x014 (R/W)  MPU Alias 1 Region Base Address Register       */

-  __IO uint32_t RASR_A1;                 /*!< Offset: 0x018 (R/W)  MPU Alias 1 Region Attribute and Size Register */

-  __IO uint32_t RBAR_A2;                 /*!< Offset: 0x01C (R/W)  MPU Alias 2 Region Base Address Register       */

-  __IO uint32_t RASR_A2;                 /*!< Offset: 0x020 (R/W)  MPU Alias 2 Region Attribute and Size Register */

-  __IO uint32_t RBAR_A3;                 /*!< Offset: 0x024 (R/W)  MPU Alias 3 Region Base Address Register       */

-  __IO uint32_t RASR_A3;                 /*!< Offset: 0x028 (R/W)  MPU Alias 3 Region Attribute and Size Register */

-} MPU_Type;

-

-/* MPU Type Register */

-#define MPU_TYPE_IREGION_Pos               16                                             /*!< MPU TYPE: IREGION Position */

-#define MPU_TYPE_IREGION_Msk               (0xFFUL << MPU_TYPE_IREGION_Pos)               /*!< MPU TYPE: IREGION Mask */

-

-#define MPU_TYPE_DREGION_Pos                8                                             /*!< MPU TYPE: DREGION Position */

-#define MPU_TYPE_DREGION_Msk               (0xFFUL << MPU_TYPE_DREGION_Pos)               /*!< MPU TYPE: DREGION Mask */

-

-#define MPU_TYPE_SEPARATE_Pos               0                                             /*!< MPU TYPE: SEPARATE Position */

-#define MPU_TYPE_SEPARATE_Msk              (1UL << MPU_TYPE_SEPARATE_Pos)                 /*!< MPU TYPE: SEPARATE Mask */

-

-/* MPU Control Register */

-#define MPU_CTRL_PRIVDEFENA_Pos             2                                             /*!< MPU CTRL: PRIVDEFENA Position */

-#define MPU_CTRL_PRIVDEFENA_Msk            (1UL << MPU_CTRL_PRIVDEFENA_Pos)               /*!< MPU CTRL: PRIVDEFENA Mask */

-

-#define MPU_CTRL_HFNMIENA_Pos               1                                             /*!< MPU CTRL: HFNMIENA Position */

-#define MPU_CTRL_HFNMIENA_Msk              (1UL << MPU_CTRL_HFNMIENA_Pos)                 /*!< MPU CTRL: HFNMIENA Mask */

-

-#define MPU_CTRL_ENABLE_Pos                 0                                             /*!< MPU CTRL: ENABLE Position */

-#define MPU_CTRL_ENABLE_Msk                (1UL << MPU_CTRL_ENABLE_Pos)                   /*!< MPU CTRL: ENABLE Mask */

-

-/* MPU Region Number Register */

-#define MPU_RNR_REGION_Pos                  0                                             /*!< MPU RNR: REGION Position */

-#define MPU_RNR_REGION_Msk                 (0xFFUL << MPU_RNR_REGION_Pos)                 /*!< MPU RNR: REGION Mask */

-

-/* MPU Region Base Address Register */

-#define MPU_RBAR_ADDR_Pos                   5                                             /*!< MPU RBAR: ADDR Position */

-#define MPU_RBAR_ADDR_Msk                  (0x7FFFFFFUL << MPU_RBAR_ADDR_Pos)             /*!< MPU RBAR: ADDR Mask */

-

-#define MPU_RBAR_VALID_Pos                  4                                             /*!< MPU RBAR: VALID Position */

-#define MPU_RBAR_VALID_Msk                 (1UL << MPU_RBAR_VALID_Pos)                    /*!< MPU RBAR: VALID Mask */

-

-#define MPU_RBAR_REGION_Pos                 0                                             /*!< MPU RBAR: REGION Position */

-#define MPU_RBAR_REGION_Msk                (0xFUL << MPU_RBAR_REGION_Pos)                 /*!< MPU RBAR: REGION Mask */

-

-/* MPU Region Attribute and Size Register */

-#define MPU_RASR_ATTRS_Pos                 16                                             /*!< MPU RASR: MPU Region Attribute field Position */

-#define MPU_RASR_ATTRS_Msk                 (0xFFFFUL << MPU_RASR_ATTRS_Pos)               /*!< MPU RASR: MPU Region Attribute field Mask */

-

-#define MPU_RASR_XN_Pos                    28                                             /*!< MPU RASR: ATTRS.XN Position */

-#define MPU_RASR_XN_Msk                    (1UL << MPU_RASR_XN_Pos)                       /*!< MPU RASR: ATTRS.XN Mask */

-

-#define MPU_RASR_AP_Pos                    24                                             /*!< MPU RASR: ATTRS.AP Position */

-#define MPU_RASR_AP_Msk                    (0x7UL << MPU_RASR_AP_Pos)                     /*!< MPU RASR: ATTRS.AP Mask */

-

-#define MPU_RASR_TEX_Pos                   19                                             /*!< MPU RASR: ATTRS.TEX Position */

-#define MPU_RASR_TEX_Msk                   (0x7UL << MPU_RASR_TEX_Pos)                    /*!< MPU RASR: ATTRS.TEX Mask */

-

-#define MPU_RASR_S_Pos                     18                                             /*!< MPU RASR: ATTRS.S Position */

-#define MPU_RASR_S_Msk                     (1UL << MPU_RASR_S_Pos)                        /*!< MPU RASR: ATTRS.S Mask */

-

-#define MPU_RASR_C_Pos                     17                                             /*!< MPU RASR: ATTRS.C Position */

-#define MPU_RASR_C_Msk                     (1UL << MPU_RASR_C_Pos)                        /*!< MPU RASR: ATTRS.C Mask */

-

-#define MPU_RASR_B_Pos                     16                                             /*!< MPU RASR: ATTRS.B Position */

-#define MPU_RASR_B_Msk                     (1UL << MPU_RASR_B_Pos)                        /*!< MPU RASR: ATTRS.B Mask */

-

-#define MPU_RASR_SRD_Pos                    8                                             /*!< MPU RASR: Sub-Region Disable Position */

-#define MPU_RASR_SRD_Msk                   (0xFFUL << MPU_RASR_SRD_Pos)                   /*!< MPU RASR: Sub-Region Disable Mask */

-

-#define MPU_RASR_SIZE_Pos                   1                                             /*!< MPU RASR: Region Size Field Position */

-#define MPU_RASR_SIZE_Msk                  (0x1FUL << MPU_RASR_SIZE_Pos)                  /*!< MPU RASR: Region Size Field Mask */

-

-#define MPU_RASR_ENABLE_Pos                 0                                             /*!< MPU RASR: Region enable bit Position */

-#define MPU_RASR_ENABLE_Msk                (1UL << MPU_RASR_ENABLE_Pos)                   /*!< MPU RASR: Region enable bit Disable Mask */

-

-/*@} end of group CMSIS_MPU */

-#endif

-

-

-#if (__FPU_PRESENT == 1)

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_FPU     Floating Point Unit (FPU)

-    \brief      Type definitions for the Floating Point Unit (FPU)

-  @{

- */

-

-/** \brief  Structure type to access the Floating Point Unit (FPU).

- */

-typedef struct

-{

-       uint32_t RESERVED0[1];

-  __IO uint32_t FPCCR;                   /*!< Offset: 0x004 (R/W)  Floating-Point Context Control Register               */

-  __IO uint32_t FPCAR;                   /*!< Offset: 0x008 (R/W)  Floating-Point Context Address Register               */

-  __IO uint32_t FPDSCR;                  /*!< Offset: 0x00C (R/W)  Floating-Point Default Status Control Register        */

-  __I  uint32_t MVFR0;                   /*!< Offset: 0x010 (R/ )  Media and FP Feature Register 0                       */

-  __I  uint32_t MVFR1;                   /*!< Offset: 0x014 (R/ )  Media and FP Feature Register 1                       */

-} FPU_Type;

-

-/* Floating-Point Context Control Register */

-#define FPU_FPCCR_ASPEN_Pos                31                                             /*!< FPCCR: ASPEN bit Position */

-#define FPU_FPCCR_ASPEN_Msk                (1UL << FPU_FPCCR_ASPEN_Pos)                   /*!< FPCCR: ASPEN bit Mask */

-

-#define FPU_FPCCR_LSPEN_Pos                30                                             /*!< FPCCR: LSPEN Position */

-#define FPU_FPCCR_LSPEN_Msk                (1UL << FPU_FPCCR_LSPEN_Pos)                   /*!< FPCCR: LSPEN bit Mask */

-

-#define FPU_FPCCR_MONRDY_Pos                8                                             /*!< FPCCR: MONRDY Position */

-#define FPU_FPCCR_MONRDY_Msk               (1UL << FPU_FPCCR_MONRDY_Pos)                  /*!< FPCCR: MONRDY bit Mask */

-

-#define FPU_FPCCR_BFRDY_Pos                 6                                             /*!< FPCCR: BFRDY Position */

-#define FPU_FPCCR_BFRDY_Msk                (1UL << FPU_FPCCR_BFRDY_Pos)                   /*!< FPCCR: BFRDY bit Mask */

-

-#define FPU_FPCCR_MMRDY_Pos                 5                                             /*!< FPCCR: MMRDY Position */

-#define FPU_FPCCR_MMRDY_Msk                (1UL << FPU_FPCCR_MMRDY_Pos)                   /*!< FPCCR: MMRDY bit Mask */

-

-#define FPU_FPCCR_HFRDY_Pos                 4                                             /*!< FPCCR: HFRDY Position */

-#define FPU_FPCCR_HFRDY_Msk                (1UL << FPU_FPCCR_HFRDY_Pos)                   /*!< FPCCR: HFRDY bit Mask */

-

-#define FPU_FPCCR_THREAD_Pos                3                                             /*!< FPCCR: processor mode bit Position */

-#define FPU_FPCCR_THREAD_Msk               (1UL << FPU_FPCCR_THREAD_Pos)                  /*!< FPCCR: processor mode active bit Mask */

-

-#define FPU_FPCCR_USER_Pos                  1                                             /*!< FPCCR: privilege level bit Position */

-#define FPU_FPCCR_USER_Msk                 (1UL << FPU_FPCCR_USER_Pos)                    /*!< FPCCR: privilege level bit Mask */

-

-#define FPU_FPCCR_LSPACT_Pos                0                                             /*!< FPCCR: Lazy state preservation active bit Position */

-#define FPU_FPCCR_LSPACT_Msk               (1UL << FPU_FPCCR_LSPACT_Pos)                  /*!< FPCCR: Lazy state preservation active bit Mask */

-

-/* Floating-Point Context Address Register */

-#define FPU_FPCAR_ADDRESS_Pos               3                                             /*!< FPCAR: ADDRESS bit Position */

-#define FPU_FPCAR_ADDRESS_Msk              (0x1FFFFFFFUL << FPU_FPCAR_ADDRESS_Pos)        /*!< FPCAR: ADDRESS bit Mask */

-

-/* Floating-Point Default Status Control Register */

-#define FPU_FPDSCR_AHP_Pos                 26                                             /*!< FPDSCR: AHP bit Position */

-#define FPU_FPDSCR_AHP_Msk                 (1UL << FPU_FPDSCR_AHP_Pos)                    /*!< FPDSCR: AHP bit Mask */

-

-#define FPU_FPDSCR_DN_Pos                  25                                             /*!< FPDSCR: DN bit Position */

-#define FPU_FPDSCR_DN_Msk                  (1UL << FPU_FPDSCR_DN_Pos)                     /*!< FPDSCR: DN bit Mask */

-

-#define FPU_FPDSCR_FZ_Pos                  24                                             /*!< FPDSCR: FZ bit Position */

-#define FPU_FPDSCR_FZ_Msk                  (1UL << FPU_FPDSCR_FZ_Pos)                     /*!< FPDSCR: FZ bit Mask */

-

-#define FPU_FPDSCR_RMode_Pos               22                                             /*!< FPDSCR: RMode bit Position */

-#define FPU_FPDSCR_RMode_Msk               (3UL << FPU_FPDSCR_RMode_Pos)                  /*!< FPDSCR: RMode bit Mask */

-

-/* Media and FP Feature Register 0 */

-#define FPU_MVFR0_FP_rounding_modes_Pos    28                                             /*!< MVFR0: FP rounding modes bits Position */

-#define FPU_MVFR0_FP_rounding_modes_Msk    (0xFUL << FPU_MVFR0_FP_rounding_modes_Pos)     /*!< MVFR0: FP rounding modes bits Mask */

-

-#define FPU_MVFR0_Short_vectors_Pos        24                                             /*!< MVFR0: Short vectors bits Position */

-#define FPU_MVFR0_Short_vectors_Msk        (0xFUL << FPU_MVFR0_Short_vectors_Pos)         /*!< MVFR0: Short vectors bits Mask */

-

-#define FPU_MVFR0_Square_root_Pos          20                                             /*!< MVFR0: Square root bits Position */

-#define FPU_MVFR0_Square_root_Msk          (0xFUL << FPU_MVFR0_Square_root_Pos)           /*!< MVFR0: Square root bits Mask */

-

-#define FPU_MVFR0_Divide_Pos               16                                             /*!< MVFR0: Divide bits Position */

-#define FPU_MVFR0_Divide_Msk               (0xFUL << FPU_MVFR0_Divide_Pos)                /*!< MVFR0: Divide bits Mask */

-

-#define FPU_MVFR0_FP_excep_trapping_Pos    12                                             /*!< MVFR0: FP exception trapping bits Position */

-#define FPU_MVFR0_FP_excep_trapping_Msk    (0xFUL << FPU_MVFR0_FP_excep_trapping_Pos)     /*!< MVFR0: FP exception trapping bits Mask */

-

-#define FPU_MVFR0_Double_precision_Pos      8                                             /*!< MVFR0: Double-precision bits Position */

-#define FPU_MVFR0_Double_precision_Msk     (0xFUL << FPU_MVFR0_Double_precision_Pos)      /*!< MVFR0: Double-precision bits Mask */

-

-#define FPU_MVFR0_Single_precision_Pos      4                                             /*!< MVFR0: Single-precision bits Position */

-#define FPU_MVFR0_Single_precision_Msk     (0xFUL << FPU_MVFR0_Single_precision_Pos)      /*!< MVFR0: Single-precision bits Mask */

-

-#define FPU_MVFR0_A_SIMD_registers_Pos      0                                             /*!< MVFR0: A_SIMD registers bits Position */

-#define FPU_MVFR0_A_SIMD_registers_Msk     (0xFUL << FPU_MVFR0_A_SIMD_registers_Pos)      /*!< MVFR0: A_SIMD registers bits Mask */

-

-/* Media and FP Feature Register 1 */

-#define FPU_MVFR1_FP_fused_MAC_Pos         28                                             /*!< MVFR1: FP fused MAC bits Position */

-#define FPU_MVFR1_FP_fused_MAC_Msk         (0xFUL << FPU_MVFR1_FP_fused_MAC_Pos)          /*!< MVFR1: FP fused MAC bits Mask */

-

-#define FPU_MVFR1_FP_HPFP_Pos              24                                             /*!< MVFR1: FP HPFP bits Position */

-#define FPU_MVFR1_FP_HPFP_Msk              (0xFUL << FPU_MVFR1_FP_HPFP_Pos)               /*!< MVFR1: FP HPFP bits Mask */

-

-#define FPU_MVFR1_D_NaN_mode_Pos            4                                             /*!< MVFR1: D_NaN mode bits Position */

-#define FPU_MVFR1_D_NaN_mode_Msk           (0xFUL << FPU_MVFR1_D_NaN_mode_Pos)            /*!< MVFR1: D_NaN mode bits Mask */

-

-#define FPU_MVFR1_FtZ_mode_Pos              0                                             /*!< MVFR1: FtZ mode bits Position */

-#define FPU_MVFR1_FtZ_mode_Msk             (0xFUL << FPU_MVFR1_FtZ_mode_Pos)              /*!< MVFR1: FtZ mode bits Mask */

-

-/*@} end of group CMSIS_FPU */

-#endif

-

-

-/** \ingroup  CMSIS_core_register

-    \defgroup CMSIS_CoreDebug       Core Debug Registers (CoreDebug)

-    \brief      Type definitions for the Core Debug Registers

-  @{

- */

-

-/** \brief  Structure type to access the Core Debug Register (CoreDebug).

- */

-typedef struct

-{

-  __IO uint32_t DHCSR;                   /*!< Offset: 0x000 (R/W)  Debug Halting Control and Status Register    */

-  __O  uint32_t DCRSR;                   /*!< Offset: 0x004 ( /W)  Debug Core Register Selector Register        */

-  __IO uint32_t DCRDR;                   /*!< Offset: 0x008 (R/W)  Debug Core Register Data Register            */

-  __IO uint32_t DEMCR;                   /*!< Offset: 0x00C (R/W)  Debug Exception and Monitor Control Register */

-} CoreDebug_Type;

-

-/* Debug Halting Control and Status Register */

-#define CoreDebug_DHCSR_DBGKEY_Pos         16                                             /*!< CoreDebug DHCSR: DBGKEY Position */

-#define CoreDebug_DHCSR_DBGKEY_Msk         (0xFFFFUL << CoreDebug_DHCSR_DBGKEY_Pos)       /*!< CoreDebug DHCSR: DBGKEY Mask */

-

-#define CoreDebug_DHCSR_S_RESET_ST_Pos     25                                             /*!< CoreDebug DHCSR: S_RESET_ST Position */

-#define CoreDebug_DHCSR_S_RESET_ST_Msk     (1UL << CoreDebug_DHCSR_S_RESET_ST_Pos)        /*!< CoreDebug DHCSR: S_RESET_ST Mask */

-

-#define CoreDebug_DHCSR_S_RETIRE_ST_Pos    24                                             /*!< CoreDebug DHCSR: S_RETIRE_ST Position */

-#define CoreDebug_DHCSR_S_RETIRE_ST_Msk    (1UL << CoreDebug_DHCSR_S_RETIRE_ST_Pos)       /*!< CoreDebug DHCSR: S_RETIRE_ST Mask */

-

-#define CoreDebug_DHCSR_S_LOCKUP_Pos       19                                             /*!< CoreDebug DHCSR: S_LOCKUP Position */

-#define CoreDebug_DHCSR_S_LOCKUP_Msk       (1UL << CoreDebug_DHCSR_S_LOCKUP_Pos)          /*!< CoreDebug DHCSR: S_LOCKUP Mask */

-

-#define CoreDebug_DHCSR_S_SLEEP_Pos        18                                             /*!< CoreDebug DHCSR: S_SLEEP Position */

-#define CoreDebug_DHCSR_S_SLEEP_Msk        (1UL << CoreDebug_DHCSR_S_SLEEP_Pos)           /*!< CoreDebug DHCSR: S_SLEEP Mask */

-

-#define CoreDebug_DHCSR_S_HALT_Pos         17                                             /*!< CoreDebug DHCSR: S_HALT Position */

-#define CoreDebug_DHCSR_S_HALT_Msk         (1UL << CoreDebug_DHCSR_S_HALT_Pos)            /*!< CoreDebug DHCSR: S_HALT Mask */

-

-#define CoreDebug_DHCSR_S_REGRDY_Pos       16                                             /*!< CoreDebug DHCSR: S_REGRDY Position */

-#define CoreDebug_DHCSR_S_REGRDY_Msk       (1UL << CoreDebug_DHCSR_S_REGRDY_Pos)          /*!< CoreDebug DHCSR: S_REGRDY Mask */

-

-#define CoreDebug_DHCSR_C_SNAPSTALL_Pos     5                                             /*!< CoreDebug DHCSR: C_SNAPSTALL Position */

-#define CoreDebug_DHCSR_C_SNAPSTALL_Msk    (1UL << CoreDebug_DHCSR_C_SNAPSTALL_Pos)       /*!< CoreDebug DHCSR: C_SNAPSTALL Mask */

-

-#define CoreDebug_DHCSR_C_MASKINTS_Pos      3                                             /*!< CoreDebug DHCSR: C_MASKINTS Position */

-#define CoreDebug_DHCSR_C_MASKINTS_Msk     (1UL << CoreDebug_DHCSR_C_MASKINTS_Pos)        /*!< CoreDebug DHCSR: C_MASKINTS Mask */

-

-#define CoreDebug_DHCSR_C_STEP_Pos          2                                             /*!< CoreDebug DHCSR: C_STEP Position */

-#define CoreDebug_DHCSR_C_STEP_Msk         (1UL << CoreDebug_DHCSR_C_STEP_Pos)            /*!< CoreDebug DHCSR: C_STEP Mask */

-

-#define CoreDebug_DHCSR_C_HALT_Pos          1                                             /*!< CoreDebug DHCSR: C_HALT Position */

-#define CoreDebug_DHCSR_C_HALT_Msk         (1UL << CoreDebug_DHCSR_C_HALT_Pos)            /*!< CoreDebug DHCSR: C_HALT Mask */

-

-#define CoreDebug_DHCSR_C_DEBUGEN_Pos       0                                             /*!< CoreDebug DHCSR: C_DEBUGEN Position */

-#define CoreDebug_DHCSR_C_DEBUGEN_Msk      (1UL << CoreDebug_DHCSR_C_DEBUGEN_Pos)         /*!< CoreDebug DHCSR: C_DEBUGEN Mask */

-

-/* Debug Core Register Selector Register */

-#define CoreDebug_DCRSR_REGWnR_Pos         16                                             /*!< CoreDebug DCRSR: REGWnR Position */

-#define CoreDebug_DCRSR_REGWnR_Msk         (1UL << CoreDebug_DCRSR_REGWnR_Pos)            /*!< CoreDebug DCRSR: REGWnR Mask */

-

-#define CoreDebug_DCRSR_REGSEL_Pos          0                                             /*!< CoreDebug DCRSR: REGSEL Position */

-#define CoreDebug_DCRSR_REGSEL_Msk         (0x1FUL << CoreDebug_DCRSR_REGSEL_Pos)         /*!< CoreDebug DCRSR: REGSEL Mask */

-

-/* Debug Exception and Monitor Control Register */

-#define CoreDebug_DEMCR_TRCENA_Pos         24                                             /*!< CoreDebug DEMCR: TRCENA Position */

-#define CoreDebug_DEMCR_TRCENA_Msk         (1UL << CoreDebug_DEMCR_TRCENA_Pos)            /*!< CoreDebug DEMCR: TRCENA Mask */

-

-#define CoreDebug_DEMCR_MON_REQ_Pos        19                                             /*!< CoreDebug DEMCR: MON_REQ Position */

-#define CoreDebug_DEMCR_MON_REQ_Msk        (1UL << CoreDebug_DEMCR_MON_REQ_Pos)           /*!< CoreDebug DEMCR: MON_REQ Mask */

-

-#define CoreDebug_DEMCR_MON_STEP_Pos       18                                             /*!< CoreDebug DEMCR: MON_STEP Position */

-#define CoreDebug_DEMCR_MON_STEP_Msk       (1UL << CoreDebug_DEMCR_MON_STEP_Pos)          /*!< CoreDebug DEMCR: MON_STEP Mask */

-

-#define CoreDebug_DEMCR_MON_PEND_Pos       17                                             /*!< CoreDebug DEMCR: MON_PEND Position */

-#define CoreDebug_DEMCR_MON_PEND_Msk       (1UL << CoreDebug_DEMCR_MON_PEND_Pos)          /*!< CoreDebug DEMCR: MON_PEND Mask */

-

-#define CoreDebug_DEMCR_MON_EN_Pos         16                                             /*!< CoreDebug DEMCR: MON_EN Position */

-#define CoreDebug_DEMCR_MON_EN_Msk         (1UL << CoreDebug_DEMCR_MON_EN_Pos)            /*!< CoreDebug DEMCR: MON_EN Mask */

-

-#define CoreDebug_DEMCR_VC_HARDERR_Pos     10                                             /*!< CoreDebug DEMCR: VC_HARDERR Position */

-#define CoreDebug_DEMCR_VC_HARDERR_Msk     (1UL << CoreDebug_DEMCR_VC_HARDERR_Pos)        /*!< CoreDebug DEMCR: VC_HARDERR Mask */

-

-#define CoreDebug_DEMCR_VC_INTERR_Pos       9                                             /*!< CoreDebug DEMCR: VC_INTERR Position */

-#define CoreDebug_DEMCR_VC_INTERR_Msk      (1UL << CoreDebug_DEMCR_VC_INTERR_Pos)         /*!< CoreDebug DEMCR: VC_INTERR Mask */

-

-#define CoreDebug_DEMCR_VC_BUSERR_Pos       8                                             /*!< CoreDebug DEMCR: VC_BUSERR Position */

-#define CoreDebug_DEMCR_VC_BUSERR_Msk      (1UL << CoreDebug_DEMCR_VC_BUSERR_Pos)         /*!< CoreDebug DEMCR: VC_BUSERR Mask */

-

-#define CoreDebug_DEMCR_VC_STATERR_Pos      7                                             /*!< CoreDebug DEMCR: VC_STATERR Position */

-#define CoreDebug_DEMCR_VC_STATERR_Msk     (1UL << CoreDebug_DEMCR_VC_STATERR_Pos)        /*!< CoreDebug DEMCR: VC_STATERR Mask */

-

-#define CoreDebug_DEMCR_VC_CHKERR_Pos       6                                             /*!< CoreDebug DEMCR: VC_CHKERR Position */

-#define CoreDebug_DEMCR_VC_CHKERR_Msk      (1UL << CoreDebug_DEMCR_VC_CHKERR_Pos)         /*!< CoreDebug DEMCR: VC_CHKERR Mask */

-

-#define CoreDebug_DEMCR_VC_NOCPERR_Pos      5                                             /*!< CoreDebug DEMCR: VC_NOCPERR Position */

-#define CoreDebug_DEMCR_VC_NOCPERR_Msk     (1UL << CoreDebug_DEMCR_VC_NOCPERR_Pos)        /*!< CoreDebug DEMCR: VC_NOCPERR Mask */

-

-#define CoreDebug_DEMCR_VC_MMERR_Pos        4                                             /*!< CoreDebug DEMCR: VC_MMERR Position */

-#define CoreDebug_DEMCR_VC_MMERR_Msk       (1UL << CoreDebug_DEMCR_VC_MMERR_Pos)          /*!< CoreDebug DEMCR: VC_MMERR Mask */

-

-#define CoreDebug_DEMCR_VC_CORERESET_Pos    0                                             /*!< CoreDebug DEMCR: VC_CORERESET Position */

-#define CoreDebug_DEMCR_VC_CORERESET_Msk   (1UL << CoreDebug_DEMCR_VC_CORERESET_Pos)      /*!< CoreDebug DEMCR: VC_CORERESET Mask */

-

-/*@} end of group CMSIS_CoreDebug */

-

-

-/** \ingroup    CMSIS_core_register

-    \defgroup   CMSIS_core_base     Core Definitions

-    \brief      Definitions for base addresses, unions, and structures.

-  @{

- */

-

-/* Memory mapping of Cortex-M4 Hardware */

-#define SCS_BASE            (0xE000E000UL)                            /*!< System Control Space Base Address  */

-#define ITM_BASE            (0xE0000000UL)                            /*!< ITM Base Address                   */

-#define DWT_BASE            (0xE0001000UL)                            /*!< DWT Base Address                   */

-#define TPI_BASE            (0xE0040000UL)                            /*!< TPI Base Address                   */

-#define CoreDebug_BASE      (0xE000EDF0UL)                            /*!< Core Debug Base Address            */

-#define SysTick_BASE        (SCS_BASE +  0x0010UL)                    /*!< SysTick Base Address               */

-#define NVIC_BASE           (SCS_BASE +  0x0100UL)                    /*!< NVIC Base Address                  */

-#define SCB_BASE            (SCS_BASE +  0x0D00UL)                    /*!< System Control Block Base Address  */

-

-#define SCnSCB              ((SCnSCB_Type    *)     SCS_BASE      )   /*!< System control Register not in SCB */

-#define SCB                 ((SCB_Type       *)     SCB_BASE      )   /*!< SCB configuration struct           */

-#define SysTick             ((SysTick_Type   *)     SysTick_BASE  )   /*!< SysTick configuration struct       */

-#define NVIC                ((NVIC_Type      *)     NVIC_BASE     )   /*!< NVIC configuration struct          */

-#define ITM                 ((ITM_Type       *)     ITM_BASE      )   /*!< ITM configuration struct           */

-#define DWT                 ((DWT_Type       *)     DWT_BASE      )   /*!< DWT configuration struct           */

-#define TPI                 ((TPI_Type       *)     TPI_BASE      )   /*!< TPI configuration struct           */

-#define CoreDebug           ((CoreDebug_Type *)     CoreDebug_BASE)   /*!< Core Debug configuration struct    */

-

-#if (__MPU_PRESENT == 1)

-  #define MPU_BASE          (SCS_BASE +  0x0D90UL)                    /*!< Memory Protection Unit             */

-  #define MPU               ((MPU_Type       *)     MPU_BASE      )   /*!< Memory Protection Unit             */

-#endif

-

-#if (__FPU_PRESENT == 1)

-  #define FPU_BASE          (SCS_BASE +  0x0F30UL)                    /*!< Floating Point Unit                */

-  #define FPU               ((FPU_Type       *)     FPU_BASE      )   /*!< Floating Point Unit                */

-#endif

-

-/*@} */

-

-

-

-/*******************************************************************************

- *                Hardware Abstraction Layer

-  Core Function Interface contains:

-  - Core NVIC Functions

-  - Core SysTick Functions

-  - Core Debug Functions

-  - Core Register Access Functions

- ******************************************************************************/

-/** \defgroup CMSIS_Core_FunctionInterface Functions and Instructions Reference

-*/

-

-

-

-/* ##########################   NVIC functions  #################################### */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_Core_NVICFunctions NVIC Functions

-    \brief      Functions that manage interrupts and exceptions via the NVIC.

-    @{

- */

-

-/** \brief  Set Priority Grouping

-

-  The function sets the priority grouping field using the required unlock sequence.

-  The parameter PriorityGroup is assigned to the field SCB->AIRCR [10:8] PRIGROUP field.

-  Only values from 0..7 are used.

-  In case of a conflict between priority grouping and available

-  priority bits (__NVIC_PRIO_BITS), the smallest possible priority group is set.

-

-    \param [in]      PriorityGroup  Priority grouping field.

- */

-__STATIC_INLINE void NVIC_SetPriorityGrouping(uint32_t PriorityGroup)

-{

-  uint32_t reg_value;

-  uint32_t PriorityGroupTmp = (PriorityGroup & (uint32_t)0x07);               /* only values 0..7 are used          */

-

-  reg_value  =  SCB->AIRCR;                                                   /* read old register configuration    */

-  reg_value &= ~(SCB_AIRCR_VECTKEY_Msk | SCB_AIRCR_PRIGROUP_Msk);             /* clear bits to change               */

-  reg_value  =  (reg_value                                 |

-                ((uint32_t)0x5FA << SCB_AIRCR_VECTKEY_Pos) |

-                (PriorityGroupTmp << 8));                                     /* Insert write key and priorty group */

-  SCB->AIRCR =  reg_value;

-}

-

-

-/** \brief  Get Priority Grouping

-

-  The function reads the priority grouping field from the NVIC Interrupt Controller.

-

-    \return                Priority grouping field (SCB->AIRCR [10:8] PRIGROUP field).

- */

-__STATIC_INLINE uint32_t NVIC_GetPriorityGrouping(void)

-{

-  return ((SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) >> SCB_AIRCR_PRIGROUP_Pos);   /* read priority grouping field */

-}

-

-

-/** \brief  Enable External Interrupt

-

-    The function enables a device-specific interrupt in the NVIC interrupt controller.

-

-    \param [in]      IRQn  External interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_EnableIRQ(IRQn_Type IRQn)

-{

-/*  NVIC->ISER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F));  enable interrupt */

-  NVIC->ISER[(uint32_t)((int32_t)IRQn) >> 5] = (uint32_t)(1 << ((uint32_t)((int32_t)IRQn) & (uint32_t)0x1F)); /* enable interrupt */

-}

-

-

-/** \brief  Disable External Interrupt

-

-    The function disables a device-specific interrupt in the NVIC interrupt controller.

-

-    \param [in]      IRQn  External interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_DisableIRQ(IRQn_Type IRQn)

-{

-  NVIC->ICER[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* disable interrupt */

-}

-

-

-/** \brief  Get Pending Interrupt

-

-    The function reads the pending register in the NVIC and returns the pending bit

-    for the specified interrupt.

-

-    \param [in]      IRQn  Interrupt number.

-

-    \return             0  Interrupt status is not pending.

-    \return             1  Interrupt status is pending.

- */

-__STATIC_INLINE uint32_t NVIC_GetPendingIRQ(IRQn_Type IRQn)

-{

-  return((uint32_t) ((NVIC->ISPR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if pending else 0 */

-}

-

-

-/** \brief  Set Pending Interrupt

-

-    The function sets the pending bit of an external interrupt.

-

-    \param [in]      IRQn  Interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_SetPendingIRQ(IRQn_Type IRQn)

-{

-  NVIC->ISPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* set interrupt pending */

-}

-

-

-/** \brief  Clear Pending Interrupt

-

-    The function clears the pending bit of an external interrupt.

-

-    \param [in]      IRQn  External interrupt number. Value cannot be negative.

- */

-__STATIC_INLINE void NVIC_ClearPendingIRQ(IRQn_Type IRQn)

-{

-  NVIC->ICPR[((uint32_t)(IRQn) >> 5)] = (1 << ((uint32_t)(IRQn) & 0x1F)); /* Clear pending interrupt */

-}

-

-

-/** \brief  Get Active Interrupt

-

-    The function reads the active register in NVIC and returns the active bit.

-

-    \param [in]      IRQn  Interrupt number.

-

-    \return             0  Interrupt status is not active.

-    \return             1  Interrupt status is active.

- */

-__STATIC_INLINE uint32_t NVIC_GetActive(IRQn_Type IRQn)

-{

-  return((uint32_t)((NVIC->IABR[(uint32_t)(IRQn) >> 5] & (1 << ((uint32_t)(IRQn) & 0x1F)))?1:0)); /* Return 1 if active else 0 */

-}

-

-

-/** \brief  Set Interrupt Priority

-

-    The function sets the priority of an interrupt.

-

-    \note The priority cannot be set for every core interrupt.

-

-    \param [in]      IRQn  Interrupt number.

-    \param [in]  priority  Priority to set.

- */

-__STATIC_INLINE void NVIC_SetPriority(IRQn_Type IRQn, uint32_t priority)

-{

-  if(IRQn < 0) {

-    SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff); } /* set Priority for Cortex-M  System Interrupts */

-  else {

-    NVIC->IP[(uint32_t)(IRQn)] = ((priority << (8 - __NVIC_PRIO_BITS)) & 0xff);    }        /* set Priority for device specific Interrupts  */

-}

-

-

-/** \brief  Get Interrupt Priority

-

-    The function reads the priority of an interrupt. The interrupt

-    number can be positive to specify an external (device specific)

-    interrupt, or negative to specify an internal (core) interrupt.

-

-

-    \param [in]   IRQn  Interrupt number.

-    \return             Interrupt Priority. Value is aligned automatically to the implemented

-                        priority bits of the microcontroller.

- */

-__STATIC_INLINE uint32_t NVIC_GetPriority(IRQn_Type IRQn)

-{

-

-  if(IRQn < 0) {

-    return((uint32_t)(SCB->SHP[((uint32_t)(IRQn) & 0xF)-4] >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for Cortex-M  system interrupts */

-  else {

-    return((uint32_t)(NVIC->IP[(uint32_t)(IRQn)]           >> (8 - __NVIC_PRIO_BITS)));  } /* get priority for device specific interrupts  */

-}

-

-

-/** \brief  Encode Priority

-

-    The function encodes the priority for an interrupt with the given priority group,

-    preemptive priority value, and subpriority value.

-    In case of a conflict between priority grouping and available

-    priority bits (__NVIC_PRIO_BITS), the samllest possible priority group is set.

-

-    \param [in]     PriorityGroup  Used priority group.

-    \param [in]   PreemptPriority  Preemptive priority value (starting from 0).

-    \param [in]       SubPriority  Subpriority value (starting from 0).

-    \return                        Encoded priority. Value can be used in the function \ref NVIC_SetPriority().

- */

-__STATIC_INLINE uint32_t NVIC_EncodePriority (uint32_t PriorityGroup, uint32_t PreemptPriority, uint32_t SubPriority)

-{

-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */

-  uint32_t PreemptPriorityBits;

-  uint32_t SubPriorityBits;

-

-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;

-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;

-

-  return (

-           ((PreemptPriority & ((1 << (PreemptPriorityBits)) - 1)) << SubPriorityBits) |

-           ((SubPriority     & ((1 << (SubPriorityBits    )) - 1)))

-         );

-}

-

-

-/** \brief  Decode Priority

-

-    The function decodes an interrupt priority value with a given priority group to

-    preemptive priority value and subpriority value.

-    In case of a conflict between priority grouping and available

-    priority bits (__NVIC_PRIO_BITS) the samllest possible priority group is set.

-

-    \param [in]         Priority   Priority value, which can be retrieved with the function \ref NVIC_GetPriority().

-    \param [in]     PriorityGroup  Used priority group.

-    \param [out] pPreemptPriority  Preemptive priority value (starting from 0).

-    \param [out]     pSubPriority  Subpriority value (starting from 0).

- */

-__STATIC_INLINE void NVIC_DecodePriority (uint32_t Priority, uint32_t PriorityGroup, uint32_t* pPreemptPriority, uint32_t* pSubPriority)

-{

-  uint32_t PriorityGroupTmp = (PriorityGroup & 0x07);          /* only values 0..7 are used          */

-  uint32_t PreemptPriorityBits;

-  uint32_t SubPriorityBits;

-

-  PreemptPriorityBits = ((7 - PriorityGroupTmp) > __NVIC_PRIO_BITS) ? __NVIC_PRIO_BITS : 7 - PriorityGroupTmp;

-  SubPriorityBits     = ((PriorityGroupTmp + __NVIC_PRIO_BITS) < 7) ? 0 : PriorityGroupTmp - 7 + __NVIC_PRIO_BITS;

-

-  *pPreemptPriority = (Priority >> SubPriorityBits) & ((1 << (PreemptPriorityBits)) - 1);

-  *pSubPriority     = (Priority                   ) & ((1 << (SubPriorityBits    )) - 1);

-}

-

-

-/** \brief  System Reset

-

-    The function initiates a system reset request to reset the MCU.

- */

-__STATIC_INLINE void NVIC_SystemReset(void)

-{

-  __DSB();                                                     /* Ensure all outstanding memory accesses included

-                                                                  buffered write are completed before reset */

-  SCB->AIRCR  = ((0x5FA << SCB_AIRCR_VECTKEY_Pos)      |

-                 (SCB->AIRCR & SCB_AIRCR_PRIGROUP_Msk) |

-                 SCB_AIRCR_SYSRESETREQ_Msk);                   /* Keep priority group unchanged */

-  __DSB();                                                     /* Ensure completion of memory access */

-  while(1);                                                    /* wait until reset */

-}

-

-/*@} end of CMSIS_Core_NVICFunctions */

-

-

-

-/* ##################################    SysTick function  ############################################ */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_Core_SysTickFunctions SysTick Functions

-    \brief      Functions that configure the System.

-  @{

- */

-

-#if (__Vendor_SysTickConfig == 0)

-

-/** \brief  System Tick Configuration

-

-    The function initializes the System Timer and its interrupt, and starts the System Tick Timer.

-    Counter is in free running mode to generate periodic interrupts.

-

-    \param [in]  ticks  Number of ticks between two interrupts.

-

-    \return          0  Function succeeded.

-    \return          1  Function failed.

-

-    \note     When the variable <b>__Vendor_SysTickConfig</b> is set to 1, then the

-    function <b>SysTick_Config</b> is not included. In this case, the file <b><i>device</i>.h</b>

-    must contain a vendor-specific implementation of this function.

-

- */

-__STATIC_INLINE uint32_t SysTick_Config(uint32_t ticks)

-{

-  if ((ticks - 1) > SysTick_LOAD_RELOAD_Msk)  return (1);      /* Reload value impossible */

-

-  SysTick->LOAD  = ticks - 1;                                  /* set reload register */

-  NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1);  /* set Priority for Systick Interrupt */

-  SysTick->VAL   = 0;                                          /* Load the SysTick Counter Value */

-  SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |

-                   SysTick_CTRL_TICKINT_Msk   |

-                   SysTick_CTRL_ENABLE_Msk;                    /* Enable SysTick IRQ and SysTick Timer */

-  return (0);                                                  /* Function successful */

-}

-

-#endif

-

-/*@} end of CMSIS_Core_SysTickFunctions */

-

-

-

-/* ##################################### Debug In/Output function ########################################### */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_core_DebugFunctions ITM Functions

-    \brief   Functions that access the ITM debug interface.

-  @{

- */

-

-extern volatile int32_t ITM_RxBuffer;                    /*!< External variable to receive characters.                         */

-#define                 ITM_RXBUFFER_EMPTY    0x5AA55AA5 /*!< Value identifying \ref ITM_RxBuffer is ready for next character. */

-

-

-/** \brief  ITM Send Character

-

-    The function transmits a character via the ITM channel 0, and

-    \li Just returns when no debugger is connected that has booked the output.

-    \li Is blocking when a debugger is connected, but the previous character sent has not been transmitted.

-

-    \param [in]     ch  Character to transmit.

-

-    \returns            Character to transmit.

- */

-__STATIC_INLINE uint32_t ITM_SendChar (uint32_t ch)

-{

-  if ((ITM->TCR & ITM_TCR_ITMENA_Msk)                  &&      /* ITM enabled */

-      (ITM->TER & (1UL << 0)        )                    )     /* ITM Port #0 enabled */

-  {

-    while (ITM->PORT[0].u32 == 0);

-    ITM->PORT[0].u8 = (uint8_t) ch;

-  }

-  return (ch);

-}

-

-

-/** \brief  ITM Receive Character

-

-    The function inputs a character via the external variable \ref ITM_RxBuffer.

-

-    \return             Received character.

-    \return         -1  No character pending.

- */

-__STATIC_INLINE int32_t ITM_ReceiveChar (void) {

-  int32_t ch = -1;                           /* no character available */

-

-  if (ITM_RxBuffer != ITM_RXBUFFER_EMPTY) {

-    ch = ITM_RxBuffer;

-    ITM_RxBuffer = ITM_RXBUFFER_EMPTY;       /* ready for next character */

-  }

-

-  return (ch);

-}

-

-

-/** \brief  ITM Check Character

-

-    The function checks whether a character is pending for reading in the variable \ref ITM_RxBuffer.

-

-    \return          0  No character available.

-    \return          1  Character available.

- */

-__STATIC_INLINE int32_t ITM_CheckChar (void) {

-

-  if (ITM_RxBuffer == ITM_RXBUFFER_EMPTY) {

-    return (0);                                 /* no character available */

-  } else {

-    return (1);                                 /*    character available */

-  }

-}

-

-/*@} end of CMSIS_core_DebugFunctions */

-

-#endif /* __CORE_CM4_H_DEPENDANT */

-

-#endif /* __CMSIS_GENERIC */

-

-#ifdef __cplusplus

-}

-#endif

diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
deleted file mode 100644
index af1831ee1..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
+++ /dev/null
@@ -1,673 +0,0 @@
-/**************************************************************************//**

- * @file     core_cm4_simd.h

- * @brief    CMSIS Cortex-M4 SIMD Header File

- * @version  V3.20

- * @date     25. February 2013

- *

- * @note

- *

- ******************************************************************************/

-/* Copyright (c) 2009 - 2013 ARM LIMITED

-

-   All rights reserved.

-   Redistribution and use in source and binary forms, with or without

-   modification, are permitted provided that the following conditions are met:

-   - Redistributions of source code must retain the above copyright

-     notice, this list of conditions and the following disclaimer.

-   - Redistributions in binary form must reproduce the above copyright

-     notice, this list of conditions and the following disclaimer in the

-     documentation and/or other materials provided with the distribution.

-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

-   ---------------------------------------------------------------------------*/

-

-

-#ifdef __cplusplus

- extern "C" {

-#endif

-

-#ifndef __CORE_CM4_SIMD_H

-#define __CORE_CM4_SIMD_H

-

-

-/*******************************************************************************

- *                Hardware Abstraction Layer

- ******************************************************************************/

-

-

-/* ###################  Compiler specific Intrinsics  ########################### */

-/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics

-  Access to dedicated SIMD instructions

-  @{

-*/

-

-#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/

-/* ARM armcc specific functions */

-

-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/

-#define __SADD8                           __sadd8

-#define __QADD8                           __qadd8

-#define __SHADD8                          __shadd8

-#define __UADD8                           __uadd8

-#define __UQADD8                          __uqadd8

-#define __UHADD8                          __uhadd8

-#define __SSUB8                           __ssub8

-#define __QSUB8                           __qsub8

-#define __SHSUB8                          __shsub8

-#define __USUB8                           __usub8

-#define __UQSUB8                          __uqsub8

-#define __UHSUB8                          __uhsub8

-#define __SADD16                          __sadd16

-#define __QADD16                          __qadd16

-#define __SHADD16                         __shadd16

-#define __UADD16                          __uadd16

-#define __UQADD16                         __uqadd16

-#define __UHADD16                         __uhadd16

-#define __SSUB16                          __ssub16

-#define __QSUB16                          __qsub16

-#define __SHSUB16                         __shsub16

-#define __USUB16                          __usub16

-#define __UQSUB16                         __uqsub16

-#define __UHSUB16                         __uhsub16

-#define __SASX                            __sasx

-#define __QASX                            __qasx

-#define __SHASX                           __shasx

-#define __UASX                            __uasx

-#define __UQASX                           __uqasx

-#define __UHASX                           __uhasx

-#define __SSAX                            __ssax

-#define __QSAX                            __qsax

-#define __SHSAX                           __shsax

-#define __USAX                            __usax

-#define __UQSAX                           __uqsax

-#define __UHSAX                           __uhsax

-#define __USAD8                           __usad8

-#define __USADA8                          __usada8

-#define __SSAT16                          __ssat16

-#define __USAT16                          __usat16

-#define __UXTB16                          __uxtb16

-#define __UXTAB16                         __uxtab16

-#define __SXTB16                          __sxtb16

-#define __SXTAB16                         __sxtab16

-#define __SMUAD                           __smuad

-#define __SMUADX                          __smuadx

-#define __SMLAD                           __smlad

-#define __SMLADX                          __smladx

-#define __SMLALD                          __smlald

-#define __SMLALDX                         __smlaldx

-#define __SMUSD                           __smusd

-#define __SMUSDX                          __smusdx

-#define __SMLSD                           __smlsd

-#define __SMLSDX                          __smlsdx

-#define __SMLSLD                          __smlsld

-#define __SMLSLDX                         __smlsldx

-#define __SEL                             __sel

-#define __QADD                            __qadd

-#define __QSUB                            __qsub

-

-#define __PKHBT(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0x0000FFFFUL) |  \

-                                           ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL)  )

-

-#define __PKHTB(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0xFFFF0000UL) |  \

-                                           ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL)  )

-

-#define __SMMLA(ARG1,ARG2,ARG3)          ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \

-                                                      ((int64_t)(ARG3) << 32)      ) >> 32))

-

-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/

-

-

-

-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/

-/* IAR iccarm specific functions */

-

-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/

-#include <cmsis_iar.h>

-

-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/

-

-

-

-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/

-/* TI CCS specific functions */

-

-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/

-#include <cmsis_ccs.h>

-

-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/

-

-

-

-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/

-/* GNU gcc specific functions */

-

-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)

-{

-  uint32_t result;

-

-  __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );

-  return(result);

-}

-

-#define __SSAT16(ARG1,ARG2) \

-({                          \

-  uint32_t __RES, __ARG1 = (ARG1); \

-  __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \

-  __RES; \

- })

-

-#define __USAT16(ARG1,ARG2) \

-({                          \

-  uint32_t __RES, __ARG1 = (ARG1); \

-  __ASM ("usat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \

-  __RES; \

- })

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1)

-{

-  uint32_t result;

-

-  __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1)

-{

-  uint32_t result;

-

-  __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD  (uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)

-{

-  uint32_t result;

-

-  __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)

-{

-  uint32_t result;

-

-  __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );

-  return(result);

-}

-

-#define __SMLALD(ARG1,ARG2,ARG3) \

-({ \

-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \

-  __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \

-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \

- })

-

-#define __SMLALDX(ARG1,ARG2,ARG3) \

-({ \

-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \

-  __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \

-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \

- })

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD  (uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)

-{

-  uint32_t result;

-

-  __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)

-{

-  uint32_t result;

-

-  __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );

-  return(result);

-}

-

-#define __SMLSLD(ARG1,ARG2,ARG3) \

-({ \

-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \

-  __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \

-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \

- })

-

-#define __SMLSLDX(ARG1,ARG2,ARG3) \

-({ \

-  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \

-  __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \

-  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \

- })

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL  (uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2)

-{

-  uint32_t result;

-

-  __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );

-  return(result);

-}

-

-#define __PKHBT(ARG1,ARG2,ARG3) \

-({                          \

-  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \

-  __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \

-  __RES; \

- })

-

-#define __PKHTB(ARG1,ARG2,ARG3) \

-({                          \

-  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \

-  if (ARG3 == 0) \

-    __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2)  ); \

-  else \

-    __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \

-  __RES; \

- })

-

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)

-{

- int32_t result;

-

- __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r"  (op1), "r" (op2), "r" (op3) );

- return(result);

-}

-

-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/

-

-

-

-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/

-/* TASKING carm specific functions */

-

-

-/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/

-/* not yet supported */

-/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/

-

-

-#endif

-

-/*@} end of group CMSIS_SIMD_intrinsics */

-

-

-#endif /* __CORE_CM4_SIMD_H */

-

-#ifdef __cplusplus

-}

-#endif

diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
deleted file mode 100644
index 139bc3c5e..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
+++ /dev/null
@@ -1,636 +0,0 @@
-/**************************************************************************//**

- * @file     core_cmFunc.h

- * @brief    CMSIS Cortex-M Core Function Access Header File

- * @version  V3.20

- * @date     25. February 2013

- *

- * @note

- *

- ******************************************************************************/

-/* Copyright (c) 2009 - 2013 ARM LIMITED

-

-   All rights reserved.

-   Redistribution and use in source and binary forms, with or without

-   modification, are permitted provided that the following conditions are met:

-   - Redistributions of source code must retain the above copyright

-     notice, this list of conditions and the following disclaimer.

-   - Redistributions in binary form must reproduce the above copyright

-     notice, this list of conditions and the following disclaimer in the

-     documentation and/or other materials provided with the distribution.

-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

-   ---------------------------------------------------------------------------*/

-

-

-#ifndef __CORE_CMFUNC_H

-#define __CORE_CMFUNC_H

-

-

-/* ###########################  Core Function Access  ########################### */

-/** \ingroup  CMSIS_Core_FunctionInterface

-    \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions

-  @{

- */

-

-#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/

-/* ARM armcc specific functions */

-

-#if (__ARMCC_VERSION < 400677)

-  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"

-#endif

-

-/* intrinsic void __enable_irq();     */

-/* intrinsic void __disable_irq();    */

-

-/** \brief  Get Control Register

-

-    This function returns the content of the Control Register.

-

-    \return               Control Register value

- */

-__STATIC_INLINE uint32_t __get_CONTROL(void)

-{

-  register uint32_t __regControl         __ASM("control");

-  return(__regControl);

-}

-

-

-/** \brief  Set Control Register

-

-    This function writes the given value to the Control Register.

-

-    \param [in]    control  Control Register value to set

- */

-__STATIC_INLINE void __set_CONTROL(uint32_t control)

-{

-  register uint32_t __regControl         __ASM("control");

-  __regControl = control;

-}

-

-

-/** \brief  Get IPSR Register

-

-    This function returns the content of the IPSR Register.

-

-    \return               IPSR Register value

- */

-__STATIC_INLINE uint32_t __get_IPSR(void)

-{

-  register uint32_t __regIPSR          __ASM("ipsr");

-  return(__regIPSR);

-}

-

-

-/** \brief  Get APSR Register

-

-    This function returns the content of the APSR Register.

-

-    \return               APSR Register value

- */

-__STATIC_INLINE uint32_t __get_APSR(void)

-{

-  register uint32_t __regAPSR          __ASM("apsr");

-  return(__regAPSR);

-}

-

-

-/** \brief  Get xPSR Register

-

-    This function returns the content of the xPSR Register.

-

-    \return               xPSR Register value

- */

-__STATIC_INLINE uint32_t __get_xPSR(void)

-{

-  register uint32_t __regXPSR          __ASM("xpsr");

-  return(__regXPSR);

-}

-

-

-/** \brief  Get Process Stack Pointer

-

-    This function returns the current value of the Process Stack Pointer (PSP).

-

-    \return               PSP Register value

- */

-__STATIC_INLINE uint32_t __get_PSP(void)

-{

-  register uint32_t __regProcessStackPointer  __ASM("psp");

-  return(__regProcessStackPointer);

-}

-

-

-/** \brief  Set Process Stack Pointer

-

-    This function assigns the given value to the Process Stack Pointer (PSP).

-

-    \param [in]    topOfProcStack  Process Stack Pointer value to set

- */

-__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)

-{

-  register uint32_t __regProcessStackPointer  __ASM("psp");

-  __regProcessStackPointer = topOfProcStack;

-}

-

-

-/** \brief  Get Main Stack Pointer

-

-    This function returns the current value of the Main Stack Pointer (MSP).

-

-    \return               MSP Register value

- */

-__STATIC_INLINE uint32_t __get_MSP(void)

-{

-  register uint32_t __regMainStackPointer     __ASM("msp");

-  return(__regMainStackPointer);

-}

-

-

-/** \brief  Set Main Stack Pointer

-

-    This function assigns the given value to the Main Stack Pointer (MSP).

-

-    \param [in]    topOfMainStack  Main Stack Pointer value to set

- */

-__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)

-{

-  register uint32_t __regMainStackPointer     __ASM("msp");

-  __regMainStackPointer = topOfMainStack;

-}

-

-

-/** \brief  Get Priority Mask

-

-    This function returns the current state of the priority mask bit from the Priority Mask Register.

-

-    \return               Priority Mask value

- */

-__STATIC_INLINE uint32_t __get_PRIMASK(void)

-{

-  register uint32_t __regPriMask         __ASM("primask");

-  return(__regPriMask);

-}

-

-

-/** \brief  Set Priority Mask

-

-    This function assigns the given value to the Priority Mask Register.

-

-    \param [in]    priMask  Priority Mask

- */

-__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)

-{

-  register uint32_t __regPriMask         __ASM("primask");

-  __regPriMask = (priMask);

-}

-

-

-#if       (__CORTEX_M >= 0x03)

-

-/** \brief  Enable FIQ

-

-    This function enables FIQ interrupts by clearing the F-bit in the CPSR.

-    Can only be executed in Privileged modes.

- */

-#define __enable_fault_irq                __enable_fiq

-

-

-/** \brief  Disable FIQ

-

-    This function disables FIQ interrupts by setting the F-bit in the CPSR.

-    Can only be executed in Privileged modes.

- */

-#define __disable_fault_irq               __disable_fiq

-

-

-/** \brief  Get Base Priority

-

-    This function returns the current value of the Base Priority register.

-

-    \return               Base Priority register value

- */

-__STATIC_INLINE uint32_t  __get_BASEPRI(void)

-{

-  register uint32_t __regBasePri         __ASM("basepri");

-  return(__regBasePri);

-}

-

-

-/** \brief  Set Base Priority

-

-    This function assigns the given value to the Base Priority register.

-

-    \param [in]    basePri  Base Priority value to set

- */

-__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)

-{

-  register uint32_t __regBasePri         __ASM("basepri");

-  __regBasePri = (basePri & 0xff);

-}

-

-

-/** \brief  Get Fault Mask

-

-    This function returns the current value of the Fault Mask register.

-

-    \return               Fault Mask register value

- */

-__STATIC_INLINE uint32_t __get_FAULTMASK(void)

-{

-  register uint32_t __regFaultMask       __ASM("faultmask");

-  return(__regFaultMask);

-}

-

-

-/** \brief  Set Fault Mask

-

-    This function assigns the given value to the Fault Mask register.

-

-    \param [in]    faultMask  Fault Mask value to set

- */

-__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)

-{

-  register uint32_t __regFaultMask       __ASM("faultmask");

-  __regFaultMask = (faultMask & (uint32_t)1);

-}

-

-#endif /* (__CORTEX_M >= 0x03) */

-

-

-#if       (__CORTEX_M == 0x04)

-

-/** \brief  Get FPSCR

-

-    This function returns the current value of the Floating Point Status/Control register.

-

-    \return               Floating Point Status/Control register value

- */

-__STATIC_INLINE uint32_t __get_FPSCR(void)

-{

-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)

-  register uint32_t __regfpscr         __ASM("fpscr");

-  return(__regfpscr);

-#else

-   return(0);

-#endif

-}

-

-

-/** \brief  Set FPSCR

-

-    This function assigns the given value to the Floating Point Status/Control register.

-

-    \param [in]    fpscr  Floating Point Status/Control value to set

- */

-__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)

-{

-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)

-  register uint32_t __regfpscr         __ASM("fpscr");

-  __regfpscr = (fpscr);

-#endif

-}

-

-#endif /* (__CORTEX_M == 0x04) */

-

-

-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/

-/* IAR iccarm specific functions */

-

-#include <cmsis_iar.h>

-

-

-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/

-/* TI CCS specific functions */

-

-#include <cmsis_ccs.h>

-

-

-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/

-/* GNU gcc specific functions */

-

-/** \brief  Enable IRQ Interrupts

-

-  This function enables IRQ interrupts by clearing the I-bit in the CPSR.

-  Can only be executed in Privileged modes.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)

-{

-  __ASM volatile ("cpsie i" : : : "memory");

-}

-

-

-/** \brief  Disable IRQ Interrupts

-

-  This function disables IRQ interrupts by setting the I-bit in the CPSR.

-  Can only be executed in Privileged modes.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)

-{

-  __ASM volatile ("cpsid i" : : : "memory");

-}

-

-

-/** \brief  Get Control Register

-

-    This function returns the content of the Control Register.

-

-    \return               Control Register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, control" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Set Control Register

-

-    This function writes the given value to the Control Register.

-

-    \param [in]    control  Control Register value to set

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)

-{

-  __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");

-}

-

-

-/** \brief  Get IPSR Register

-

-    This function returns the content of the IPSR Register.

-

-    \return               IPSR Register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, ipsr" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Get APSR Register

-

-    This function returns the content of the APSR Register.

-

-    \return               APSR Register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, apsr" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Get xPSR Register

-

-    This function returns the content of the xPSR Register.

-

-    \return               xPSR Register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, xpsr" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Get Process Stack Pointer

-

-    This function returns the current value of the Process Stack Pointer (PSP).

-

-    \return               PSP Register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)

-{

-  register uint32_t result;

-

-  __ASM volatile ("MRS %0, psp\n"  : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Set Process Stack Pointer

-

-    This function assigns the given value to the Process Stack Pointer (PSP).

-

-    \param [in]    topOfProcStack  Process Stack Pointer value to set

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)

-{

-  __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");

-}

-

-

-/** \brief  Get Main Stack Pointer

-

-    This function returns the current value of the Main Stack Pointer (MSP).

-

-    \return               MSP Register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)

-{

-  register uint32_t result;

-

-  __ASM volatile ("MRS %0, msp\n" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Set Main Stack Pointer

-

-    This function assigns the given value to the Main Stack Pointer (MSP).

-

-    \param [in]    topOfMainStack  Main Stack Pointer value to set

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)

-{

-  __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");

-}

-

-

-/** \brief  Get Priority Mask

-

-    This function returns the current state of the priority mask bit from the Priority Mask Register.

-

-    \return               Priority Mask value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, primask" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Set Priority Mask

-

-    This function assigns the given value to the Priority Mask Register.

-

-    \param [in]    priMask  Priority Mask

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)

-{

-  __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");

-}

-

-

-#if       (__CORTEX_M >= 0x03)

-

-/** \brief  Enable FIQ

-

-    This function enables FIQ interrupts by clearing the F-bit in the CPSR.

-    Can only be executed in Privileged modes.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)

-{

-  __ASM volatile ("cpsie f" : : : "memory");

-}

-

-

-/** \brief  Disable FIQ

-

-    This function disables FIQ interrupts by setting the F-bit in the CPSR.

-    Can only be executed in Privileged modes.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)

-{

-  __ASM volatile ("cpsid f" : : : "memory");

-}

-

-

-/** \brief  Get Base Priority

-

-    This function returns the current value of the Base Priority register.

-

-    \return               Base Priority register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Set Base Priority

-

-    This function assigns the given value to the Base Priority register.

-

-    \param [in]    basePri  Base Priority value to set

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)

-{

-  __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");

-}

-

-

-/** \brief  Get Fault Mask

-

-    This function returns the current value of the Fault Mask register.

-

-    \return               Fault Mask register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)

-{

-  uint32_t result;

-

-  __ASM volatile ("MRS %0, faultmask" : "=r" (result) );

-  return(result);

-}

-

-

-/** \brief  Set Fault Mask

-

-    This function assigns the given value to the Fault Mask register.

-

-    \param [in]    faultMask  Fault Mask value to set

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)

-{

-  __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");

-}

-

-#endif /* (__CORTEX_M >= 0x03) */

-

-

-#if       (__CORTEX_M == 0x04)

-

-/** \brief  Get FPSCR

-

-    This function returns the current value of the Floating Point Status/Control register.

-

-    \return               Floating Point Status/Control register value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)

-{

-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)

-  uint32_t result;

-

-  /* Empty asm statement works as a scheduling barrier */

-  __ASM volatile ("");

-  __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );

-  __ASM volatile ("");

-  return(result);

-#else

-   return(0);

-#endif

-}

-

-

-/** \brief  Set FPSCR

-

-    This function assigns the given value to the Floating Point Status/Control register.

-

-    \param [in]    fpscr  Floating Point Status/Control value to set

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)

-{

-#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)

-  /* Empty asm statement works as a scheduling barrier */

-  __ASM volatile ("");

-  __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");

-  __ASM volatile ("");

-#endif

-}

-

-#endif /* (__CORTEX_M == 0x04) */

-

-

-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/

-/* TASKING carm specific functions */

-

-/*

- * The CMSIS functions have been implemented as intrinsics in the compiler.

- * Please use "carm -?i" to get an up to date list of all instrinsics,

- * Including the CMSIS ones.

- */

-

-#endif

-

-/*@} end of CMSIS_Core_RegAccFunctions */

-

-

-#endif /* __CORE_CMFUNC_H */

diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
deleted file mode 100644
index 8946c2c49..000000000
--- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
+++ /dev/null
@@ -1,688 +0,0 @@
-/**************************************************************************//**

- * @file     core_cmInstr.h

- * @brief    CMSIS Cortex-M Core Instruction Access Header File

- * @version  V3.20

- * @date     05. March 2013

- *

- * @note

- *

- ******************************************************************************/

-/* Copyright (c) 2009 - 2013 ARM LIMITED

-

-   All rights reserved.

-   Redistribution and use in source and binary forms, with or without

-   modification, are permitted provided that the following conditions are met:

-   - Redistributions of source code must retain the above copyright

-     notice, this list of conditions and the following disclaimer.

-   - Redistributions in binary form must reproduce the above copyright

-     notice, this list of conditions and the following disclaimer in the

-     documentation and/or other materials provided with the distribution.

-   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

-   ---------------------------------------------------------------------------*/

-

-

-#ifndef __CORE_CMINSTR_H

-#define __CORE_CMINSTR_H

-

-

-/* ##########################  Core Instruction Access  ######################### */

-/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface

-  Access to dedicated instructions

-  @{

-*/

-

-#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/

-/* ARM armcc specific functions */

-

-#if (__ARMCC_VERSION < 400677)

-  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"

-#endif

-

-

-/** \brief  No Operation

-

-    No Operation does nothing. This instruction can be used for code alignment purposes.

- */

-#define __NOP                             __nop

-

-

-/** \brief  Wait For Interrupt

-

-    Wait For Interrupt is a hint instruction that suspends execution

-    until one of a number of events occurs.

- */

-#define __WFI                             __wfi

-

-

-/** \brief  Wait For Event

-

-    Wait For Event is a hint instruction that permits the processor to enter

-    a low-power state until one of a number of events occurs.

- */

-#define __WFE                             __wfe

-

-

-/** \brief  Send Event

-

-    Send Event is a hint instruction. It causes an event to be signaled to the CPU.

- */

-#define __SEV                             __sev

-

-

-/** \brief  Instruction Synchronization Barrier

-

-    Instruction Synchronization Barrier flushes the pipeline in the processor,

-    so that all instructions following the ISB are fetched from cache or

-    memory, after the instruction has been completed.

- */

-#define __ISB()                           __isb(0xF)

-

-

-/** \brief  Data Synchronization Barrier

-

-    This function acts as a special kind of Data Memory Barrier.

-    It completes when all explicit memory accesses before this instruction complete.

- */

-#define __DSB()                           __dsb(0xF)

-

-

-/** \brief  Data Memory Barrier

-

-    This function ensures the apparent order of the explicit memory operations before

-    and after the instruction, without ensuring their completion.

- */

-#define __DMB()                           __dmb(0xF)

-

-

-/** \brief  Reverse byte order (32 bit)

-

-    This function reverses the byte order in integer value.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-#define __REV                             __rev

-

-

-/** \brief  Reverse byte order (16 bit)

-

-    This function reverses the byte order in two unsigned short values.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-#ifndef __NO_EMBEDDED_ASM

-__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)

-{

-  rev16 r0, r0

-  bx lr

-}

-#endif

-

-/** \brief  Reverse byte order in signed short value

-

-    This function reverses the byte order in a signed short value with sign extension to integer.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-#ifndef __NO_EMBEDDED_ASM

-__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)

-{

-  revsh r0, r0

-  bx lr

-}

-#endif

-

-

-/** \brief  Rotate Right in unsigned value (32 bit)

-

-    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.

-

-    \param [in]    value  Value to rotate

-    \param [in]    value  Number of Bits to rotate

-    \return               Rotated value

- */

-#define __ROR                             __ror

-

-

-/** \brief  Breakpoint

-

-    This function causes the processor to enter Debug state.

-    Debug tools can use this to investigate system state when the instruction at a particular address is reached.

-

-    \param [in]    value  is ignored by the processor.

-                   If required, a debugger can use it to store additional information about the breakpoint.

- */

-#define __BKPT(value)                       __breakpoint(value)

-

-

-#if       (__CORTEX_M >= 0x03)

-

-/** \brief  Reverse bit order of value

-

-    This function reverses the bit order of the given value.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-#define __RBIT                            __rbit

-

-

-/** \brief  LDR Exclusive (8 bit)

-

-    This function performs a exclusive LDR command for 8 bit value.

-

-    \param [in]    ptr  Pointer to data

-    \return             value of type uint8_t at (*ptr)

- */

-#define __LDREXB(ptr)                     ((uint8_t ) __ldrex(ptr))

-

-

-/** \brief  LDR Exclusive (16 bit)

-

-    This function performs a exclusive LDR command for 16 bit values.

-

-    \param [in]    ptr  Pointer to data

-    \return        value of type uint16_t at (*ptr)

- */

-#define __LDREXH(ptr)                     ((uint16_t) __ldrex(ptr))

-

-

-/** \brief  LDR Exclusive (32 bit)

-

-    This function performs a exclusive LDR command for 32 bit values.

-

-    \param [in]    ptr  Pointer to data

-    \return        value of type uint32_t at (*ptr)

- */

-#define __LDREXW(ptr)                     ((uint32_t ) __ldrex(ptr))

-

-

-/** \brief  STR Exclusive (8 bit)

-

-    This function performs a exclusive STR command for 8 bit values.

-

-    \param [in]  value  Value to store

-    \param [in]    ptr  Pointer to location

-    \return          0  Function succeeded

-    \return          1  Function failed

- */

-#define __STREXB(value, ptr)              __strex(value, ptr)

-

-

-/** \brief  STR Exclusive (16 bit)

-

-    This function performs a exclusive STR command for 16 bit values.

-

-    \param [in]  value  Value to store

-    \param [in]    ptr  Pointer to location

-    \return          0  Function succeeded

-    \return          1  Function failed

- */

-#define __STREXH(value, ptr)              __strex(value, ptr)

-

-

-/** \brief  STR Exclusive (32 bit)

-

-    This function performs a exclusive STR command for 32 bit values.

-

-    \param [in]  value  Value to store

-    \param [in]    ptr  Pointer to location

-    \return          0  Function succeeded

-    \return          1  Function failed

- */

-#define __STREXW(value, ptr)              __strex(value, ptr)

-

-

-/** \brief  Remove the exclusive lock

-

-    This function removes the exclusive lock which is created by LDREX.

-

- */

-#define __CLREX                           __clrex

-

-

-/** \brief  Signed Saturate

-

-    This function saturates a signed value.

-

-    \param [in]  value  Value to be saturated

-    \param [in]    sat  Bit position to saturate to (1..32)

-    \return             Saturated value

- */

-#define __SSAT                            __ssat

-

-

-/** \brief  Unsigned Saturate

-

-    This function saturates an unsigned value.

-

-    \param [in]  value  Value to be saturated

-    \param [in]    sat  Bit position to saturate to (0..31)

-    \return             Saturated value

- */

-#define __USAT                            __usat

-

-

-/** \brief  Count leading zeros

-

-    This function counts the number of leading zeros of a data value.

-

-    \param [in]  value  Value to count the leading zeros

-    \return             number of leading zeros in value

- */

-#define __CLZ                             __clz

-

-#endif /* (__CORTEX_M >= 0x03) */

-

-

-

-#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/

-/* IAR iccarm specific functions */

-

-#include <cmsis_iar.h>

-

-

-#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/

-/* TI CCS specific functions */

-

-#include <cmsis_ccs.h>

-

-

-#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/

-/* GNU gcc specific functions */

-

-/* Define macros for porting to both thumb1 and thumb2.

- * For thumb1, use low register (r0-r7), specified by constrant "l"

- * Otherwise, use general registers, specified by constrant "r" */

-#if defined (__thumb__) && !defined (__thumb2__)

-#define __CMSIS_GCC_OUT_REG(r) "=l" (r)

-#define __CMSIS_GCC_USE_REG(r) "l" (r)

-#else

-#define __CMSIS_GCC_OUT_REG(r) "=r" (r)

-#define __CMSIS_GCC_USE_REG(r) "r" (r)

-#endif

-

-/** \brief  No Operation

-

-    No Operation does nothing. This instruction can be used for code alignment purposes.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)

-{

-  __ASM volatile ("nop");

-}

-

-

-/** \brief  Wait For Interrupt

-

-    Wait For Interrupt is a hint instruction that suspends execution

-    until one of a number of events occurs.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)

-{

-  __ASM volatile ("wfi");

-}

-

-

-/** \brief  Wait For Event

-

-    Wait For Event is a hint instruction that permits the processor to enter

-    a low-power state until one of a number of events occurs.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)

-{

-  __ASM volatile ("wfe");

-}

-

-

-/** \brief  Send Event

-

-    Send Event is a hint instruction. It causes an event to be signaled to the CPU.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)

-{

-  __ASM volatile ("sev");

-}

-

-

-/** \brief  Instruction Synchronization Barrier

-

-    Instruction Synchronization Barrier flushes the pipeline in the processor,

-    so that all instructions following the ISB are fetched from cache or

-    memory, after the instruction has been completed.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)

-{

-  __ASM volatile ("isb");

-}

-

-

-/** \brief  Data Synchronization Barrier

-

-    This function acts as a special kind of Data Memory Barrier.

-    It completes when all explicit memory accesses before this instruction complete.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)

-{

-  __ASM volatile ("dsb");

-}

-

-

-/** \brief  Data Memory Barrier

-

-    This function ensures the apparent order of the explicit memory operations before

-    and after the instruction, without ensuring their completion.

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)

-{

-  __ASM volatile ("dmb");

-}

-

-

-/** \brief  Reverse byte order (32 bit)

-

-    This function reverses the byte order in integer value.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)

-{

-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)

-  return __builtin_bswap32(value);

-#else

-  uint32_t result;

-

-  __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );

-  return(result);

-#endif

-}

-

-

-/** \brief  Reverse byte order (16 bit)

-

-    This function reverses the byte order in two unsigned short values.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)

-{

-  uint32_t result;

-

-  __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );

-  return(result);

-}

-

-

-/** \brief  Reverse byte order in signed short value

-

-    This function reverses the byte order in a signed short value with sign extension to integer.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)

-{

-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)

-  return (short)__builtin_bswap16(value);

-#else

-  uint32_t result;

-

-  __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );

-  return(result);

-#endif

-}

-

-

-/** \brief  Rotate Right in unsigned value (32 bit)

-

-    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.

-

-    \param [in]    value  Value to rotate

-    \param [in]    value  Number of Bits to rotate

-    \return               Rotated value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)

-{

-  return (op1 >> op2) | (op1 << (32 - op2)); 

-}

-

-

-/** \brief  Breakpoint

-

-    This function causes the processor to enter Debug state.

-    Debug tools can use this to investigate system state when the instruction at a particular address is reached.

-

-    \param [in]    value  is ignored by the processor.

-                   If required, a debugger can use it to store additional information about the breakpoint.

- */

-#define __BKPT(value)                       __ASM volatile ("bkpt "#value)

-

-

-#if       (__CORTEX_M >= 0x03)

-

-/** \brief  Reverse bit order of value

-

-    This function reverses the bit order of the given value.

-

-    \param [in]    value  Value to reverse

-    \return               Reversed value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)

-{

-  uint32_t result;

-

-   __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );

-   return(result);

-}

-

-

-/** \brief  LDR Exclusive (8 bit)

-

-    This function performs a exclusive LDR command for 8 bit value.

-

-    \param [in]    ptr  Pointer to data

-    \return             value of type uint8_t at (*ptr)

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)

-{

-    uint32_t result;

-

-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)

-   __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );

-#else

-    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not

-       accepted by assembler. So has to use following less efficient pattern.

-    */

-   __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );

-#endif

-   return(result);

-}

-

-

-/** \brief  LDR Exclusive (16 bit)

-

-    This function performs a exclusive LDR command for 16 bit values.

-

-    \param [in]    ptr  Pointer to data

-    \return        value of type uint16_t at (*ptr)

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)

-{

-    uint32_t result;

-

-#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)

-   __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );

-#else

-    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not

-       accepted by assembler. So has to use following less efficient pattern.

-    */

-   __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );

-#endif

-   return(result);

-}

-

-

-/** \brief  LDR Exclusive (32 bit)

-

-    This function performs a exclusive LDR command for 32 bit values.

-

-    \param [in]    ptr  Pointer to data

-    \return        value of type uint32_t at (*ptr)

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)

-{

-    uint32_t result;

-

-   __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );

-   return(result);

-}

-

-

-/** \brief  STR Exclusive (8 bit)

-

-    This function performs a exclusive STR command for 8 bit values.

-

-    \param [in]  value  Value to store

-    \param [in]    ptr  Pointer to location

-    \return          0  Function succeeded

-    \return          1  Function failed

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)

-{

-   uint32_t result;

-

-   __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );

-   return(result);

-}

-

-

-/** \brief  STR Exclusive (16 bit)

-

-    This function performs a exclusive STR command for 16 bit values.

-

-    \param [in]  value  Value to store

-    \param [in]    ptr  Pointer to location

-    \return          0  Function succeeded

-    \return          1  Function failed

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)

-{

-   uint32_t result;

-

-   __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );

-   return(result);

-}

-

-

-/** \brief  STR Exclusive (32 bit)

-

-    This function performs a exclusive STR command for 32 bit values.

-

-    \param [in]  value  Value to store

-    \param [in]    ptr  Pointer to location

-    \return          0  Function succeeded

-    \return          1  Function failed

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)

-{

-   uint32_t result;

-

-   __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );

-   return(result);

-}

-

-

-/** \brief  Remove the exclusive lock

-

-    This function removes the exclusive lock which is created by LDREX.

-

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)

-{

-  __ASM volatile ("clrex" ::: "memory");

-}

-

-

-/** \brief  Signed Saturate

-

-    This function saturates a signed value.

-

-    \param [in]  value  Value to be saturated

-    \param [in]    sat  Bit position to saturate to (1..32)

-    \return             Saturated value

- */

-#define __SSAT(ARG1,ARG2) \

-({                          \

-  uint32_t __RES, __ARG1 = (ARG1); \

-  __ASM ("ssat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \

-  __RES; \

- })

-

-

-/** \brief  Unsigned Saturate

-

-    This function saturates an unsigned value.

-

-    \param [in]  value  Value to be saturated

-    \param [in]    sat  Bit position to saturate to (0..31)

-    \return             Saturated value

- */

-#define __USAT(ARG1,ARG2) \

-({                          \

-  uint32_t __RES, __ARG1 = (ARG1); \

-  __ASM ("usat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \

-  __RES; \

- })

-

-

-/** \brief  Count leading zeros

-

-    This function counts the number of leading zeros of a data value.

-

-    \param [in]  value  Value to count the leading zeros

-    \return             number of leading zeros in value

- */

-__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)

-{

-   uint32_t result;

-

-  __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );

-  return(result);

-}

-

-#endif /* (__CORTEX_M >= 0x03) */

-

-

-

-

-#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/

-/* TASKING carm specific functions */

-

-/*

- * The CMSIS functions have been implemented as intrinsics in the compiler.

- * Please use "carm -?i" to get an up to date list of all intrinsics,

- * Including the CMSIS ones.

- */

-

-#endif

-

-/*@}*/ /* end of group CMSIS_Core_InstructionInterface */

-

-#endif /* __CORE_CMINSTR_H */

diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
deleted file mode 100644
index 6898bc27d..000000000
--- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
+++ /dev/null
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
deleted file mode 100755
index a0185eaa9..000000000
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
+++ /dev/null
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
deleted file mode 100755
index 94525528e..000000000
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
+++ /dev/null
diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/modules/mathlib/CMSIS/license.txt
deleted file mode 100644
index 31afac1ec..000000000
--- a/src/modules/mathlib/CMSIS/license.txt
+++ /dev/null
@@ -1,27 +0,0 @@
-All pre-built libraries are guided by the following license:

-

-Copyright (C) 2009-2012 ARM Limited. 

-All rights reserved.

-

-Redistribution and use in source and binary forms, with or without

-modification, are permitted provided that the following conditions are met:

- - Redistributions of source code must retain the above copyright

-   notice, this list of conditions and the following disclaimer.

- - Redistributions in binary form must reproduce the above copyright

-   notice, this list of conditions and the following disclaimer in the

-   documentation and/or other materials provided with the distribution.

- - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp
deleted file mode 100644
index f509f7081..000000000
--- a/src/modules/mathlib/math/Dcm.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Dcm.cpp
- *
- * math direction cosine matrix
- */
-
-#include <mathlib/math/test/test.hpp>
-
-#include "Dcm.hpp"
-#include "Quaternion.hpp"
-#include "EulerAngles.hpp"
-#include "Vector3.hpp"
-
-namespace math
-{
-
-Dcm::Dcm() :
-	Matrix(Matrix::identity(3))
-{
-}
-
-Dcm::Dcm(float c00, float c01, float c02,
-	 float c10, float c11, float c12,
-	 float c20, float c21, float c22) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	dcm(0, 0) = c00;
-	dcm(0, 1) = c01;
-	dcm(0, 2) = c02;
-	dcm(1, 0) = c10;
-	dcm(1, 1) = c11;
-	dcm(1, 2) = c12;
-	dcm(2, 0) = c20;
-	dcm(2, 1) = c21;
-	dcm(2, 2) = c22;
-}
-
-Dcm::Dcm(const float data[3][3]) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	/* set rotation matrix */
-	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
-		dcm(i, j) = data[i][j];
-}
-
-Dcm::Dcm(const float *data) :
-	Matrix(3, 3, data)
-{
-}
-
-Dcm::Dcm(const Quaternion &q) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	double a = q.getA();
-	double b = q.getB();
-	double c = q.getC();
-	double d = q.getD();
-	double aSq = a * a;
-	double bSq = b * b;
-	double cSq = c * c;
-	double dSq = d * d;
-	dcm(0, 0) = aSq + bSq - cSq - dSq;
-	dcm(0, 1) = 2.0 * (b * c - a * d);
-	dcm(0, 2) = 2.0 * (a * c + b * d);
-	dcm(1, 0) = 2.0 * (b * c + a * d);
-	dcm(1, 1) = aSq - bSq + cSq - dSq;
-	dcm(1, 2) = 2.0 * (c * d - a * b);
-	dcm(2, 0) = 2.0 * (b * d - a * c);
-	dcm(2, 1) = 2.0 * (a * b + c * d);
-	dcm(2, 2) = aSq - bSq - cSq + dSq;
-}
-
-Dcm::Dcm(const EulerAngles &euler) :
-	Matrix(3, 3)
-{
-	Dcm &dcm = *this;
-	double cosPhi = cos(euler.getPhi());
-	double sinPhi = sin(euler.getPhi());
-	double cosThe = cos(euler.getTheta());
-	double sinThe = sin(euler.getTheta());
-	double cosPsi = cos(euler.getPsi());
-	double sinPsi = sin(euler.getPsi());
-
-	dcm(0, 0) = cosThe * cosPsi;
-	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
-	dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
-
-	dcm(1, 0) = cosThe * sinPsi;
-	dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
-	dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
-
-	dcm(2, 0) = -sinThe;
-	dcm(2, 1) = sinPhi * cosThe;
-	dcm(2, 2) = cosPhi * cosThe;
-}
-
-Dcm::Dcm(const Dcm &right) :
-	Matrix(right)
-{
-}
-
-Dcm::~Dcm()
-{
-}
-
-int __EXPORT dcmTest()
-{
-	printf("Test DCM\t\t: ");
-	// default ctor
-	ASSERT(matrixEqual(Dcm(),
-			   Matrix::identity(3)));
-	// quaternion ctor
-	ASSERT(matrixEqual(
-		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
-		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
-			   0.2896295f,  0.9564251f, -0.0369570f,
-			   -0.1986693f,  0.0978434f,  0.9751703f)));
-	// euler angle ctor
-	ASSERT(matrixEqual(
-		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
-		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
-			   0.2896295f,  0.9564251f, -0.0369570f,
-			   -0.1986693f,  0.0978434f,  0.9751703f)));
-	// rotations
-	Vector3 vB(1, 2, 3);
-	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
-			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
-	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
-			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
-	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
-			   Dcm(EulerAngles(
-				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
-	printf("PASS\n");
-	return 0;
-}
-} // namespace math
diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/modules/mathlib/math/EulerAngles.cpp
deleted file mode 100644
index e733d23bb..000000000
--- a/src/modules/mathlib/math/EulerAngles.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "EulerAngles.hpp"
-#include "Quaternion.hpp"
-#include "Dcm.hpp"
-#include "Vector3.hpp"
-
-namespace math
-{
-
-EulerAngles::EulerAngles() :
-	Vector(3)
-{
-	setPhi(0.0f);
-	setTheta(0.0f);
-	setPsi(0.0f);
-}
-
-EulerAngles::EulerAngles(float phi, float theta, float psi) :
-	Vector(3)
-{
-	setPhi(phi);
-	setTheta(theta);
-	setPsi(psi);
-}
-
-EulerAngles::EulerAngles(const Quaternion &q) :
-	Vector(3)
-{
-	(*this) = EulerAngles(Dcm(q));
-}
-
-EulerAngles::EulerAngles(const Dcm &dcm) :
-	Vector(3)
-{
-	setTheta(asinf(-dcm(2, 0)));
-
-	if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
-		setPhi(0.0f);
-		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
-			      dcm(0, 2) + dcm(1, 1)) + getPhi());
-
-	} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
-		setPhi(0.0f);
-		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
-			      dcm(0, 2) + dcm(1, 1)) - getPhi());
-
-	} else {
-		setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
-		setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
-	}
-}
-
-EulerAngles::~EulerAngles()
-{
-}
-
-int __EXPORT eulerAnglesTest()
-{
-	printf("Test EulerAngles\t: ");
-	EulerAngles euler(0.1f, 0.2f, 0.3f);
-
-	// test ctor
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-	ASSERT(equal(euler.getPhi(), 0.1f));
-	ASSERT(equal(euler.getTheta(), 0.2f));
-	ASSERT(equal(euler.getPsi(), 0.3f));
-
-	// test dcm ctor
-	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-
-	// test quat ctor
-	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
-
-	// test assignment
-	euler.setPhi(0.4f);
-	euler.setTheta(0.5f);
-	euler.setPsi(0.6f);
-	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
-
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/modules/mathlib/math/EulerAngles.hpp
deleted file mode 100644
index 399eecfa7..000000000
--- a/src/modules/mathlib/math/EulerAngles.hpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-class Quaternion;
-class Dcm;
-
-class __EXPORT EulerAngles : public Vector
-{
-public:
-	EulerAngles();
-	EulerAngles(float phi, float theta, float psi);
-	EulerAngles(const Quaternion &q);
-	EulerAngles(const Dcm &dcm);
-	virtual ~EulerAngles();
-
-	// alias
-	void setPhi(float phi) { (*this)(0) = phi; }
-	void setTheta(float theta) { (*this)(1) = theta; }
-	void setPsi(float psi) { (*this)(2) = psi; }
-
-	// const accessors
-	const float &getPhi() const { return (*this)(0); }
-	const float &getTheta() const { return (*this)(1); }
-	const float &getPsi() const { return (*this)(2); }
-
-};
-
-int __EXPORT eulerAnglesTest();
-
-} // math
-
diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp
deleted file mode 100644
index d4c892d8a..000000000
--- a/src/modules/mathlib/math/Limits.cpp
+++ /dev/null
@@ -1,146 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Limits.cpp
- *
- * Limiting / constrain helper functions
- */
-
-
-#include <math.h>
-#include <stdint.h>
-
-#include "Limits.hpp"
-
-
-namespace math {
-
-
-float __EXPORT min(float val1, float val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-int __EXPORT min(int val1, int val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-unsigned __EXPORT min(unsigned val1, unsigned val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-double __EXPORT min(double val1, double val2)
-{
-	return (val1 < val2) ? val1 : val2;
-}
-
-float __EXPORT max(float val1, float val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-int __EXPORT max(int val1, int val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-unsigned __EXPORT max(unsigned val1, unsigned val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-double __EXPORT max(double val1, double val2)
-{
-	return (val1 > val2) ? val1 : val2;
-}
-
-
-float __EXPORT constrain(float val, float min, float max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-int __EXPORT constrain(int val, int min, int max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-double __EXPORT constrain(double val, double min, double max)
-{
-	return (val < min) ? min : ((val > max) ? max : val);
-}
-
-float __EXPORT radians(float degrees)
-{
-	return (degrees / 180.0f) * M_PI_F;
-}
-
-double __EXPORT radians(double degrees)
-{
-	return (degrees / 180.0) * M_PI;
-}
-
-float __EXPORT degrees(float radians)
-{
-	return (radians / M_PI_F) * 180.0f;
-}
-
-double __EXPORT degrees(double radians)
-{
-	return (radians / M_PI) * 180.0;
-}
-
-}
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/modules/mathlib/math/Matrix.cpp
deleted file mode 100644
index ebd1aeda3..000000000
--- a/src/modules/mathlib/math/Matrix.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Matrix.cpp
- *
- * matrix code
- */
-
-#include "test/test.hpp"
-#include <math.h>
-
-#include "Matrix.hpp"
-
-namespace math
-{
-
-static const float data_testA[] = {
-	1, 2, 3,
-	4, 5, 6
-};
-static Matrix testA(2, 3, data_testA);
-
-static const float data_testB[] = {
-	0, 1, 3,
-	7, -1, 2
-};
-static Matrix testB(2, 3, data_testB);
-
-static const float data_testC[] = {
-	0, 1,
-	2, 1,
-	3, 2
-};
-static Matrix testC(3, 2, data_testC);
-
-static const float data_testD[] = {
-	0, 1, 2,
-	2, 1, 4,
-	5, 2, 0
-};
-static Matrix testD(3, 3, data_testD);
-
-static const float data_testE[] = {
-	1, -1, 2,
-	0, 2, 3,
-	2, -1, 1
-};
-static Matrix testE(3, 3, data_testE);
-
-static const float data_testF[] = {
-	3.777e006f, 2.915e007f, 0.000e000f,
-	2.938e007f, 2.267e008f, 0.000e000f,
-	0.000e000f, 0.000e000f, 6.033e008f
-};
-static Matrix testF(3, 3, data_testF);
-
-int __EXPORT matrixTest()
-{
-	matrixAddTest();
-	matrixSubTest();
-	matrixMultTest();
-	matrixInvTest();
-	matrixDivTest();
-	return 0;
-}
-
-int matrixAddTest()
-{
-	printf("Test Matrix Add\t\t: ");
-	Matrix r = testA + testB;
-	float data_test[] = {
-		1.0f, 3.0f, 6.0f,
-		11.0f, 4.0f, 8.0f
-	};
-	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixSubTest()
-{
-	printf("Test Matrix Sub\t\t: ");
-	Matrix r = testA - testB;
-	float data_test[] = {
-		1.0f, 1.0f, 0.0f,
-		-3.0f, 6.0f, 4.0f
-	};
-	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixMultTest()
-{
-	printf("Test Matrix Mult\t: ");
-	Matrix r = testC * testB;
-	float data_test[] = {
-		7.0f, -1.0f,  2.0f,
-		7.0f,  1.0f,  8.0f,
-		14.0f,  1.0f, 13.0f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixInvTest()
-{
-	printf("Test Matrix Inv\t\t: ");
-	Matrix origF = testF;
-	Matrix r = testF.inverse();
-	float data_test[] = {
-		-0.0012518f,  0.0001610f, 0.0000000f,
-		0.0001622f, -0.0000209f, 0.0000000f,
-		0.0000000f,  0.0000000f, 1.6580e-9f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	// make sure F in unchanged
-	ASSERT(matrixEqual(origF, testF));
-	printf("PASS\n");
-	return 0;
-}
-
-int matrixDivTest()
-{
-	printf("Test Matrix Div\t\t: ");
-	Matrix r = testD / testE;
-	float data_test[] = {
-		0.2222222f, 0.5555556f, -0.1111111f,
-		0.0f,       1.0f,         1.0,
-		-4.1111111f, 1.2222222f,  4.5555556f
-	};
-	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
-
-bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
-{
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
-
-	} else if (a.getCols() != b.getCols()) {
-		printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
-		return false;
-	}
-
-	bool ret = true;
-
-	for (size_t i = 0; i < a.getRows(); i++)
-		for (size_t j = 0; j < a.getCols(); j++) {
-			if (!equal(a(i, j), b(i, j), eps)) {
-				printf("element mismatch (%d, %d)\n", i, j);
-				ret = false;
-			}
-		}
-
-	return ret;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/modules/mathlib/math/Quaternion.cpp
deleted file mode 100644
index 02fec4ca6..000000000
--- a/src/modules/mathlib/math/Quaternion.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Quaternion.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-
-#include "Quaternion.hpp"
-#include "Dcm.hpp"
-#include "EulerAngles.hpp"
-
-namespace math
-{
-
-Quaternion::Quaternion() :
-	Vector(4)
-{
-	setA(1.0f);
-	setB(0.0f);
-	setC(0.0f);
-	setD(0.0f);
-}
-
-Quaternion::Quaternion(float a, float b,
-		       float c, float d) :
-	Vector(4)
-{
-	setA(a);
-	setB(b);
-	setC(c);
-	setD(d);
-}
-
-Quaternion::Quaternion(const float *data) :
-	Vector(4, data)
-{
-}
-
-Quaternion::Quaternion(const Vector &v) :
-	Vector(v)
-{
-}
-
-Quaternion::Quaternion(const Dcm &dcm) :
-	Vector(4)
-{
-	// avoiding singularities by not using
-	// division equations
-	setA(0.5 * sqrt(1.0 +
-			double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
-	setB(0.5 * sqrt(1.0 +
-			double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
-	setC(0.5 * sqrt(1.0 +
-			double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
-	setD(0.5 * sqrt(1.0 +
-			double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
-}
-
-Quaternion::Quaternion(const EulerAngles &euler) :
-	Vector(4)
-{
-	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
-	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
-	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
-	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
-	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
-	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
-	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
-	     sinPhi_2 * sinTheta_2 * sinPsi_2);
-	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
-	     cosPhi_2 * sinTheta_2 * sinPsi_2);
-	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
-	     sinPhi_2 * cosTheta_2 * sinPsi_2);
-	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
-	     sinPhi_2 * sinTheta_2 * cosPsi_2);
-}
-
-Quaternion::Quaternion(const Quaternion &right) :
-	Vector(right)
-{
-}
-
-Quaternion::~Quaternion()
-{
-}
-
-Vector Quaternion::derivative(const Vector &w)
-{
-#ifdef QUATERNION_ASSERT
-	ASSERT(w.getRows() == 3);
-#endif
-	float dataQ[] = {
-		getA(), -getB(), -getC(), -getD(),
-		getB(),  getA(), -getD(),  getC(),
-		getC(),  getD(),  getA(), -getB(),
-		getD(), -getC(),  getB(),  getA()
-	};
-	Vector v(4);
-	v(0) = 0.0f;
-	v(1) = w(0);
-	v(2) = w(1);
-	v(3) = w(2);
-	Matrix Q(4, 4, dataQ);
-	return Q * v * 0.5f;
-}
-
-int __EXPORT quaternionTest()
-{
-	printf("Test Quaternion\t\t: ");
-	// test default ctor
-	Quaternion q;
-	ASSERT(equal(q.getA(), 1.0f));
-	ASSERT(equal(q.getB(), 0.0f));
-	ASSERT(equal(q.getC(), 0.0f));
-	ASSERT(equal(q.getD(), 0.0f));
-	// test float ctor
-	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
-	ASSERT(equal(q.getA(), 0.1825742f));
-	ASSERT(equal(q.getB(), 0.3651484f));
-	ASSERT(equal(q.getC(), 0.5477226f));
-	ASSERT(equal(q.getD(), 0.7302967f));
-	// test euler ctor
-	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
-	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
-	// test dcm ctor
-	q = Quaternion(Dcm());
-	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
-	// TODO test derivative
-	// test accessors
-	q.setA(0.1f);
-	q.setB(0.2f);
-	q.setC(0.3f);
-	q.setD(0.4f);
-	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp
deleted file mode 100644
index 68e741817..000000000
--- a/src/modules/mathlib/math/Vector2f.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector2f.cpp
- *
- * math vector
- */
-
-#include "test/test.hpp"
-
-#include "Vector2f.hpp"
-
-namespace math
-{
-
-Vector2f::Vector2f() :
-	Vector(2)
-{
-}
-
-Vector2f::Vector2f(const Vector &right) :
-	Vector(right)
-{
-#ifdef VECTOR_ASSERT
-	ASSERT(right.getRows() == 2);
-#endif
-}
-
-Vector2f::Vector2f(float x, float y) :
-	Vector(2)
-{
-	setX(x);
-	setY(y);
-}
-
-Vector2f::Vector2f(const float *data) :
-	Vector(2, data)
-{
-}
-
-Vector2f::~Vector2f()
-{
-}
-
-float Vector2f::cross(const Vector2f &b) const
-{
-	const Vector2f &a = *this;
-	return a(0)*b(1) - a(1)*b(0);
-}
-
-float Vector2f::operator %(const Vector2f &v) const
-{
-	return cross(v);
-}
-    
-float Vector2f::operator *(const Vector2f &v) const
-{
-    return dot(v);
-}
-
-int __EXPORT vector2fTest()
-{
-	printf("Test Vector2f\t\t: ");
-	// test float ctor
-	Vector2f v(1, 2);
-	ASSERT(equal(v(0), 1));
-	ASSERT(equal(v(1), 2));
-	printf("PASS\n");
-	return 0;
-}
-
-} // namespace math
diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp
deleted file mode 100644
index ecd62e81c..000000000
--- a/src/modules/mathlib/math/Vector2f.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector2f.hpp
- *
- * math 3 vector
- */
-
-#pragma once
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector2f :
-	public Vector
-{
-public:
-	Vector2f();
-	Vector2f(const Vector &right);
-	Vector2f(float x, float y);
-	Vector2f(const float *data);
-	virtual ~Vector2f();
-	float cross(const Vector2f &b) const;
-	float operator %(const Vector2f &v) const;
-    float operator *(const Vector2f &v) const;
-    inline Vector2f operator*(const float &right) const {
-		return Vector::operator*(right);
-	}
-
-	/**
-	 * accessors
-	 */
-	void setX(float x) { (*this)(0) = x; }
-	void setY(float y) { (*this)(1) = y; }
-	const float &getX() const { return (*this)(0); }
-	const float &getY() const { return (*this)(1); }
-};
-    
-class __EXPORT Vector2 :
-	public Vector2f
-{
-};
-
-int __EXPORT vector2fTest();
-} // math
-
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp
deleted file mode 100644
index 568d9669a..000000000
--- a/src/modules/mathlib/math/Vector3.hpp
+++ /dev/null
@@ -1,76 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector3.hpp
- *
- * math 3 vector
- */
-
-#pragma once
-
-#include "Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector3 :
-	public Vector
-{
-public:
-	Vector3();
-	Vector3(const Vector &right);
-	Vector3(float x, float y, float z);
-	Vector3(const float *data);
-	virtual ~Vector3();
-	Vector3 cross(const Vector3 &b) const;
-
-	/**
-	 * accessors
-	 */
-	void setX(float x) { (*this)(0) = x; }
-	void setY(float y) { (*this)(1) = y; }
-	void setZ(float z) { (*this)(2) = z; }
-	const float &getX() const { return (*this)(0); }
-	const float &getY() const { return (*this)(1); }
-	const float &getZ() const { return (*this)(2); }
-};
-    
-class __EXPORT Vector3f :
-	public Vector3
-{
-};
-
-int __EXPORT vector3Test();
-} // math
-
diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/modules/mathlib/math/arm/Matrix.hpp
deleted file mode 100644
index 715fd3a5e..000000000
--- a/src/modules/mathlib/math/arm/Matrix.hpp
+++ /dev/null
@@ -1,292 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../Matrix.hpp"
-
-// arm specific
-#include "../../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-class __EXPORT Matrix
-{
-public:
-	// constructor
-	Matrix(size_t rows, size_t cols) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 rows, cols,
-				 (float *)calloc(rows * cols, sizeof(float)));
-	}
-	Matrix(size_t rows, size_t cols, const float *data) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 rows, cols,
-				 (float *)malloc(rows * cols * sizeof(float)));
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Matrix() {
-		delete [] _matrix.pData;
-	}
-	// copy constructor (deep)
-	Matrix(const Matrix &right) :
-		_matrix() {
-		arm_mat_init_f32(&_matrix,
-				 right.getRows(), right.getCols(),
-				 (float *)malloc(right.getRows()*
-						 right.getCols()*sizeof(float)));
-		memcpy(getData(), right.getData(),
-		       getSize());
-	}
-	// assignment
-	inline Matrix &operator=(const Matrix &right) {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i, size_t j) {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	inline const float &operator()(size_t i, size_t j) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				float sig;
-				int exp;
-				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
-			}
-
-			printf("\n");
-		}
-	}
-	// boolean ops
-	inline bool operator==(const Matrix &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
-					return false;
-			}
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Matrix operator+(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_offset_f32((float *)getData(), right,
-			       (float *)result.getData(), getRows()*getCols());
-		return result;
-	}
-	inline Matrix operator-(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_offset_f32((float *)getData(), -right,
-			       (float *)result.getData(), getRows()*getCols());
-		return result;
-	}
-	inline Matrix operator*(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_mat_scale_f32(&_matrix, right,
-				  &(result._matrix));
-		return result;
-	}
-	inline Matrix operator/(float right) const {
-		Matrix result(getRows(), getCols());
-		arm_mat_scale_f32(&_matrix, 1.0f / right,
-				  &(result._matrix));
-		return result;
-	}
-	// vector ops
-	inline Vector operator*(const Vector &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix resultMat = (*this) *
-				   Matrix(right.getRows(), 1, right.getData());
-		return Vector(getRows(), resultMat.getData());
-	}
-	// matrix ops
-	inline Matrix operator+(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		arm_mat_add_f32(&_matrix, &(right._matrix),
-				&(result._matrix));
-		return result;
-	}
-	inline Matrix operator-(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		arm_mat_sub_f32(&_matrix, &(right._matrix),
-				&(result._matrix));
-		return result;
-	}
-	inline Matrix operator*(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix result(getRows(), right.getCols());
-		arm_mat_mult_f32(&_matrix, &(right._matrix),
-				 &(result._matrix));
-		return result;
-	}
-	inline Matrix operator/(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(right.getRows() == right.getCols());
-		ASSERT(getCols() == right.getCols());
-#endif
-		return (*this) * right.inverse();
-	}
-	// other functions
-	inline Matrix transpose() const {
-		Matrix result(getCols(), getRows());
-		arm_mat_trans_f32(&_matrix, &(result._matrix));
-		return result;
-	}
-	inline void swapRows(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t j = 0; j < getCols(); j++) {
-			float tmp = (*this)(a, j);
-			(*this)(a, j) = (*this)(b, j);
-			(*this)(b, j) = tmp;
-		}
-	}
-	inline void swapCols(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			float tmp = (*this)(i, a);
-			(*this)(i, a) = (*this)(i, b);
-			(*this)(i, b) = tmp;
-		}
-	}
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix inverse() const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == getCols());
-#endif
-		Matrix result(getRows(), getCols());
-		Matrix work = (*this);
-		arm_mat_inverse_f32(&(work._matrix),
-				    &(result._matrix));
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				(*this)(i, j) = val;
-			}
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _matrix.numRows; }
-	inline size_t getCols() const { return _matrix.numCols; }
-	inline static Matrix identity(size_t size) {
-		Matrix result(size, size);
-
-		for (size_t i = 0; i < size; i++) {
-			result(i, i) = 1.0f;
-		}
-
-		return result;
-	}
-	inline static Matrix zero(size_t size) {
-		Matrix result(size, size);
-		result.setAll(0.0f);
-		return result;
-	}
-	inline static Matrix zero(size_t m, size_t n) {
-		Matrix result(m, n);
-		result.setAll(0.0f);
-		return result;
-	}
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
-	inline float *getData() { return _matrix.pData; }
-	inline const float *getData() const { return _matrix.pData; }
-	inline void setData(float *data) { _matrix.pData = data; }
-private:
-	arm_matrix_instance_f32 _matrix;
-};
-
-} // namespace math
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp
deleted file mode 100644
index 4155800e8..000000000
--- a/src/modules/mathlib/math/arm/Vector.hpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../test/test.hpp"
-
-// arm specific
-#include "../../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-class __EXPORT Vector
-{
-public:
-	// constructor
-	Vector(size_t rows) :
-		_rows(rows),
-		_data((float *)calloc(rows, sizeof(float))) {
-	}
-	Vector(size_t rows, const float *data) :
-		_rows(rows),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Vector() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Vector(const Vector &right) :
-		_rows(right.getRows()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Vector &operator=(const Vector &right) {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i) {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	inline const float &operator()(size_t i) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			float sig;
-			int exp;
-			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
-		}
-
-		printf("\n");
-	}
-	// boolean ops
-	inline bool operator==(const Vector &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-				return false;
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Vector operator+(float right) const {
-		Vector result(getRows());
-		arm_offset_f32((float *)getData(),
-			       right, result.getData(),
-			       getRows());
-		return result;
-	}
-	inline Vector operator-(float right) const {
-		Vector result(getRows());
-		arm_offset_f32((float *)getData(),
-			       -right, result.getData(),
-			       getRows());
-		return result;
-	}
-	inline Vector operator*(float right) const {
-		Vector result(getRows());
-		arm_scale_f32((float *)getData(),
-			      right, result.getData(),
-			      getRows());
-		return result;
-	}
-	inline Vector operator/(float right) const {
-		Vector result(getRows());
-		arm_scale_f32((float *)getData(),
-			      1.0f / right, result.getData(),
-			      getRows());
-		return result;
-	}
-	// vector ops
-	inline Vector operator+(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-		arm_add_f32((float *)getData(),
-			    (float *)right.getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	inline Vector operator-(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-		arm_sub_f32((float *)getData(),
-			    (float *)right.getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	inline Vector operator-(void) const {
-		Vector result(getRows());
-		arm_negate_f32((float *)getData(),
-			    result.getData(),
-			    getRows());
-		return result;
-	}
-	// other functions
-	inline float dot(const Vector &right) const {
-		float result = 0;
-		arm_dot_prod_f32((float *)getData(),
-				 (float *)right.getData(),
-				 getRows(),
-				 &result);
-		return result;
-	}
-	inline float norm() const {
-		return sqrtf(dot(*this));
-	}
-	inline float length() const {
-		return norm();
-	}
-	inline Vector unit() const {
-		return (*this) / norm();
-	}
-	inline Vector normalized() const {
-		return unit();
-	}
-	inline void normalize() {
-		(*this) = (*this) / norm();
-	}
-	inline static Vector zero(size_t rows) {
-		Vector result(rows);
-		// calloc returns zeroed memory
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			(*this)(i) = val;
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-	inline const float *getData() const { return _data; }
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows(); }
-	inline float *getData() { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	float *_data;
-};
-
-} // math
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
deleted file mode 100644
index efb17225d..000000000
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
+++ /dev/null
@@ -1,77 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/// @file	LowPassFilter.cpp
-/// @brief	A class to implement a second order low pass filter 
-/// Author: Leonard Hall <LeonardTHall@gmail.com>
-
-#include "LowPassFilter2p.hpp"
-#include "math.h"
-
-namespace math
-{
-
-void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
-{
-    _cutoff_freq = cutoff_freq;
-    float fr = sample_freq/_cutoff_freq;
-    float ohm = tanf(M_PI_F/fr);
-    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
-    _b0 = ohm*ohm/c;
-    _b1 = 2.0f*_b0;
-    _b2 = _b0;
-    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
-    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
-}
-
-float LowPassFilter2p::apply(float sample)
-{
-    // do the filtering
-    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
-    if (isnan(delay_element_0) || isinf(delay_element_0)) {
-        // don't allow bad values to propogate via the filter
-        delay_element_0 = sample;
-    }
-    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
-    
-    _delay_element_2 = _delay_element_1;
-    _delay_element_1 = delay_element_0;
-
-    // return the value.  Should be no need to check limits
-    return output;
-}
-
-} // namespace math
-
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
deleted file mode 100644
index 208ec98d4..000000000
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
+++ /dev/null
@@ -1,78 +0,0 @@
-// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/// @file	LowPassFilter.h
-/// @brief	A class to implement a second order low pass filter 
-/// Author: Leonard Hall <LeonardTHall@gmail.com>
-/// Adapted for PX4 by Andrew Tridgell
-
-#pragma once
-
-namespace math
-{
-class __EXPORT LowPassFilter2p
-{
-public:
-    // constructor
-    LowPassFilter2p(float sample_freq, float cutoff_freq) {
-        // set initial parameters
-        set_cutoff_frequency(sample_freq, cutoff_freq);
-        _delay_element_1 = _delay_element_2 = 0;
-    }
-
-    // change parameters
-    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
-
-    // apply - Add a new raw value to the filter 
-    // and retrieve the filtered result
-    float apply(float sample);
-
-    // return the cutoff frequency
-    float get_cutoff_freq(void) const {
-        return _cutoff_freq;
-    }
-
-private:
-    float           _cutoff_freq; 
-    float           _a1;
-    float           _a2;
-    float           _b0;
-    float           _b1;
-    float           _b2;
-    float           _delay_element_1;        // buffered sample -1
-    float           _delay_element_2;        // buffered sample -2
-};
-
-} // namespace math
diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/modules/mathlib/math/generic/Matrix.hpp
deleted file mode 100644
index 5601a3447..000000000
--- a/src/modules/mathlib/math/generic/Matrix.hpp
+++ /dev/null
@@ -1,437 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Matrix.h
- *
- * matrix code
- */
-
-#pragma once
-
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-#include "../Matrix.hpp"
-
-namespace math
-{
-
-class __EXPORT Matrix
-{
-public:
-	// constructor
-	Matrix(size_t rows, size_t cols) :
-		_rows(rows),
-		_cols(cols),
-		_data((float *)calloc(rows *cols, sizeof(float))) {
-	}
-	Matrix(size_t rows, size_t cols, const float *data) :
-		_rows(rows),
-		_cols(cols),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Matrix() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Matrix(const Matrix &right) :
-		_rows(right.getRows()),
-		_cols(right.getCols()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Matrix &operator=(const Matrix &right) {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i, size_t j) {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	inline const float &operator()(size_t i, size_t j) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(i < getRows());
-		ASSERT(j < getCols());
-#endif
-		return getData()[i * getCols() + j];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				float sig;
-				int exp;
-				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
-			}
-
-			printf("\n");
-		}
-	}
-	// boolean ops
-	inline bool operator==(const Matrix &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
-					return false;
-			}
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Matrix operator+(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) + right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator-(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) - right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator*(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) * right;
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator/(const float &right) const {
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) / right;
-			}
-		}
-
-		return result;
-	}
-	// vector ops
-	inline Vector operator*(const Vector &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i) += (*this)(i, j) * right(j);
-			}
-		}
-
-		return result;
-	}
-	// matrix ops
-	inline Matrix operator+(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) + right(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator-(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == right.getRows());
-		ASSERT(getCols() == right.getCols());
-#endif
-		Matrix result(getRows(), getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(i, j) = (*this)(i, j) - right(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator*(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getCols() == right.getRows());
-#endif
-		Matrix result(getRows(), right.getCols());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < right.getCols(); j++) {
-				for (size_t k = 0; k < right.getRows(); k++) {
-					result(i, j) += (*this)(i, k) * right(k, j);
-				}
-			}
-		}
-
-		return result;
-	}
-	inline Matrix operator/(const Matrix &right) const {
-#ifdef MATRIX_ASSERT
-		ASSERT(right.getRows() == right.getCols());
-		ASSERT(getCols() == right.getCols());
-#endif
-		return (*this) * right.inverse();
-	}
-	// other functions
-	inline Matrix transpose() const {
-		Matrix result(getCols(), getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				result(j, i) = (*this)(i, j);
-			}
-		}
-
-		return result;
-	}
-	inline void swapRows(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t j = 0; j < getCols(); j++) {
-			float tmp = (*this)(a, j);
-			(*this)(a, j) = (*this)(b, j);
-			(*this)(b, j) = tmp;
-		}
-	}
-	inline void swapCols(size_t a, size_t b) {
-		if (a == b) return;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			float tmp = (*this)(i, a);
-			(*this)(i, a) = (*this)(i, b);
-			(*this)(i, b) = tmp;
-		}
-	}
-	/**
-	 * inverse based on LU factorization with partial pivotting
-	 */
-	Matrix inverse() const {
-#ifdef MATRIX_ASSERT
-		ASSERT(getRows() == getCols());
-#endif
-		size_t N = getRows();
-		Matrix L = identity(N);
-		const Matrix &A = (*this);
-		Matrix U = A;
-		Matrix P = identity(N);
-
-		//printf("A:\n"); A.print();
-
-		// for all diagonal elements
-		for (size_t n = 0; n < N; n++) {
-
-			// if diagonal is zero, swap with row below
-			if (fabsf(U(n, n)) < 1e-8f) {
-				//printf("trying pivot for row %d\n",n);
-				for (size_t i = 0; i < N; i++) {
-					if (i == n) continue;
-
-					//printf("\ttrying row %d\n",i);
-					if (fabsf(U(i, n)) > 1e-8f) {
-						//printf("swapped %d\n",i);
-						U.swapRows(i, n);
-						P.swapRows(i, n);
-					}
-				}
-			}
-
-#ifdef MATRIX_ASSERT
-			//printf("A:\n"); A.print();
-			//printf("U:\n"); U.print();
-			//printf("P:\n"); P.print();
-			//fflush(stdout);
-			ASSERT(fabsf(U(n, n)) > 1e-8f);
-#endif
-
-			// failsafe, return zero matrix
-			if (fabsf(U(n, n)) < 1e-8f) {
-				return Matrix::zero(n);
-			}
-
-			// for all rows below diagonal
-			for (size_t i = (n + 1); i < N; i++) {
-				L(i, n) = U(i, n) / U(n, n);
-
-				// add i-th row and n-th row
-				// multiplied by: -a(i,n)/a(n,n)
-				for (size_t k = n; k < N; k++) {
-					U(i, k) -= L(i, n) * U(n, k);
-				}
-			}
-		}
-
-		//printf("L:\n"); L.print();
-		//printf("U:\n"); U.print();
-
-		// solve LY=P*I for Y by forward subst
-		Matrix Y = P;
-
-		// for all columns of Y
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of L
-			for (size_t i = 0; i < N; i++) {
-				// for all columns of L
-				for (size_t j = 0; j < i; j++) {
-					// for all existing y
-					// subtract the component they
-					// contribute to the solution
-					Y(i, c) -= L(i, j) * Y(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				// Y(i,c) /= L(i,i);
-				// but L(i,i) = 1.0
-			}
-		}
-
-		//printf("Y:\n"); Y.print();
-
-		// solve Ux=y for x by back subst
-		Matrix X = Y;
-
-		// for all columns of X
-		for (size_t c = 0; c < N; c++) {
-			// for all rows of U
-			for (size_t k = 0; k < N; k++) {
-				// have to go in reverse order
-				size_t i = N - 1 - k;
-
-				// for all columns of U
-				for (size_t j = i + 1; j < N; j++) {
-					// for all existing x
-					// subtract the component they
-					// contribute to the solution
-					X(i, c) -= U(i, j) * X(j, c);
-				}
-
-				// divide by the factor
-				// on current
-				// term to be solved
-				X(i, c) /= U(i, i);
-			}
-		}
-
-		//printf("X:\n"); X.print();
-		return X;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			for (size_t j = 0; j < getCols(); j++) {
-				(*this)(i, j) = val;
-			}
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-	inline size_t getCols() const { return _cols; }
-	inline static Matrix identity(size_t size) {
-		Matrix result(size, size);
-
-		for (size_t i = 0; i < size; i++) {
-			result(i, i) = 1.0f;
-		}
-
-		return result;
-	}
-	inline static Matrix zero(size_t size) {
-		Matrix result(size, size);
-		result.setAll(0.0f);
-		return result;
-	}
-	inline static Matrix zero(size_t m, size_t n) {
-		Matrix result(m, n);
-		result.setAll(0.0f);
-		return result;
-	}
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
-	inline float *getData() { return _data; }
-	inline const float *getData() const { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	size_t _cols;
-	float *_data;
-};
-
-} // namespace math
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp
deleted file mode 100644
index 8cfdc676d..000000000
--- a/src/modules/mathlib/math/generic/Vector.hpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector.h
- *
- * math vector
- */
-
-#pragma once
-
-#include <inttypes.h>
-#include <assert.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdio.h>
-#include <math.h>
-
-#include "../Vector.hpp"
-
-namespace math
-{
-
-class __EXPORT Vector
-{
-public:
-	// constructor
-	Vector(size_t rows) :
-		_rows(rows),
-		_data((float *)calloc(rows, sizeof(float))) {
-	}
-	Vector(size_t rows, const float *data) :
-		_rows(rows),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), data, getSize());
-	}
-	// deconstructor
-	virtual ~Vector() {
-		delete [] getData();
-	}
-	// copy constructor (deep)
-	Vector(const Vector &right) :
-		_rows(right.getRows()),
-		_data((float *)malloc(getSize())) {
-		memcpy(getData(), right.getData(),
-		       right.getSize());
-	}
-	// assignment
-	inline Vector &operator=(const Vector &right) {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-
-		if (this != &right) {
-			memcpy(getData(), right.getData(),
-			       right.getSize());
-		}
-
-		return *this;
-	}
-	// element accessors
-	inline float &operator()(size_t i) {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	inline const float &operator()(size_t i) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(i < getRows());
-#endif
-		return getData()[i];
-	}
-	// output
-	inline void print() const {
-		for (size_t i = 0; i < getRows(); i++) {
-			float sig;
-			int exp;
-			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
-		}
-
-		printf("\n");
-	}
-	// boolean ops
-	inline bool operator==(const Vector &right) const {
-		for (size_t i = 0; i < getRows(); i++) {
-			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-				return false;
-		}
-
-		return true;
-	}
-	// scalar ops
-	inline Vector operator+(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) + right;
-		}
-
-		return result;
-	}
-	inline Vector operator-(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) - right;
-		}
-
-		return result;
-	}
-	inline Vector operator*(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) * right;
-		}
-
-		return result;
-	}
-	inline Vector operator/(const float &right) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) / right;
-		}
-
-		return result;
-	}
-	// vector ops
-	inline Vector operator+(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) + right(i);
-		}
-
-		return result;
-	}
-	inline Vector operator-(const Vector &right) const {
-#ifdef VECTOR_ASSERT
-		ASSERT(getRows() == right.getRows());
-#endif
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = (*this)(i) - right(i);
-		}
-
-		return result;
-	}
-	inline Vector operator-(void) const {
-		Vector result(getRows());
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result(i) = -((*this)(i));
-		}
-
-		return result;
-	}
-	// other functions
-	inline float dot(const Vector &right) const {
-		float result = 0;
-
-		for (size_t i = 0; i < getRows(); i++) {
-			result += (*this)(i) * (*this)(i);
-		}
-
-		return result;
-	}
-	inline float norm() const {
-		return sqrtf(dot(*this));
-	}
-	inline float length() const {
-		return norm();
-	}
-	inline Vector unit() const {
-		return (*this) / norm();
-	}
-	inline Vector normalized() const {
-		return unit();
-	}
-	inline void normalize() {
-		(*this) = (*this) / norm();
-	}
-	inline static Vector zero(size_t rows) {
-		Vector result(rows);
-		// calloc returns zeroed memory
-		return result;
-	}
-	inline void setAll(const float &val) {
-		for (size_t i = 0; i < getRows(); i++) {
-			(*this)(i) = val;
-		}
-	}
-	inline void set(const float *data) {
-		memcpy(getData(), data, getSize());
-	}
-	inline size_t getRows() const { return _rows; }
-protected:
-	inline size_t getSize() const { return sizeof(float) * getRows(); }
-	inline float *getData() { return _data; }
-	inline const float *getData() const { return _data; }
-	inline void setData(float *data) { _data = data; }
-private:
-	size_t _rows;
-	float *_data;
-};
-
-} // math
diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/modules/mathlib/math/nasa_rotation_def.pdf
deleted file mode 100644
index eb67a4bfc..000000000
--- a/src/modules/mathlib/math/nasa_rotation_def.pdf
+++ /dev/null
diff --git a/src/modules/mathlib/math/test/test.cpp b/src/modules/mathlib/math/test/test.cpp
deleted file mode 100644
index 2fa2f7e7c..000000000
--- a/src/modules/mathlib/math/test/test.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file test.cpp
- *
- * Test library code
- */
-
-#include <stdio.h>
-#include <math.h>
-#include <stdlib.h>
-
-#include "test.hpp"
-
-bool __EXPORT equal(float a, float b, float epsilon)
-{
-	float diff = fabsf(a - b);
-
-	if (diff > epsilon) {
-		printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
-		return false;
-
-	} else return true;
-}
-
-void __EXPORT float2SigExp(
-	const float &num,
-	float &sig,
-	int &exp)
-{
-	if (isnan(num) || isinf(num)) {
-		sig = 0.0f;
-		exp = -99;
-		return;
-	}
-
-	if (fabsf(num) < 1.0e-38f) {
-		sig = 0;
-		exp = 0;
-		return;
-	}
-
-	exp = log10f(fabsf(num));
-
-	if (exp > 0) {
-		exp = ceil(exp);
-
-	} else {
-		exp = floor(exp);
-	}
-
-	sig = num;
-
-	// cheap power since it is integer
-	if (exp > 0) {
-		for (int i = 0; i < abs(exp); i++) sig /= 10;
-
-	} else {
-		for (int i = 0; i < abs(exp); i++) sig *= 10;
-	}
-}
-
-
diff --git a/src/modules/mathlib/math/test_math.sce b/src/modules/mathlib/math/test_math.sce
deleted file mode 100644
index c3fba4729..000000000
--- a/src/modules/mathlib/math/test_math.sce
+++ /dev/null
@@ -1,63 +0,0 @@
-clc
-clear
-function out = float_truncate(in, digits)
-    out = round(in*10^digits)
-    out = out/10^digits
-endfunction
-
-phi = 0.1
-theta = 0.2
-psi = 0.3
-
-cosPhi = cos(phi)
-cosPhi_2 = cos(phi/2)
-sinPhi = sin(phi)
-sinPhi_2 = sin(phi/2)
-
-cosTheta = cos(theta)
-cosTheta_2 = cos(theta/2)
-sinTheta = sin(theta)
-sinTheta_2 = sin(theta/2)
-
-cosPsi = cos(psi)
-cosPsi_2 = cos(psi/2)
-sinPsi = sin(psi)
-sinPsi_2 = sin(psi/2)
-
-C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
-        cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
-        -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
-    
-disp(C_nb)
-//C_nb = float_truncate(C_nb,3)
-//disp(C_nb)
-
-theta = asin(-C_nb(3,1))
-phi = atan(C_nb(3,2), C_nb(3,3))
-psi = atan(C_nb(2,1), C_nb(1,1))
-printf('phi %f\n', phi)
-printf('theta %f\n', theta)
-printf('psi %f\n', psi)
-
-q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
-     sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
-     cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
-     cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
-     
-//q = float_truncate(q,3)
-
-a = q(1)
-b = q(2)
-c = q(3)
-d = q(4)
-printf('q: %f %f %f %f\n', a, b, c, d)
-a2 = a*a
-b2 = b*b
-c2 = c*c
-d2 = d*d
-
-C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
-         2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
-         2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
-
-disp(C2_nb)
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 919d01561..78e01cefb 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -64,6 +64,7 @@
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 #include "waypoints.h"
 #include "orb_topics.h"
@@ -181,102 +182,80 @@ set_hil_on_off(bool hil_enabled)
 }
 
 void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode)
+get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
 {
 	/* reset MAVLink mode bitfield */
-	*mavlink_mode = 0;
+	*mavlink_base_mode = 0;
+	*mavlink_custom_mode = 0;
 
-	/* set mode flags independent of system state */
+	/**
+	 * Set mode flags
+	 **/
 
 	/* HIL */
-	if (v_status.flag_hil_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	if (v_status.hil_state == HIL_STATE_ON) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
-	/* manual input */
-	if (v_status.flag_control_manual_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+	/* arming state */
+	if (v_status.arming_state == ARMING_STATE_ARMED
+			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	/* attitude or rate control */
-	if (v_status.flag_control_attitude_enabled ||
-	    v_status.flag_control_rates_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-	}
-
-	/* vector control */
-	if (v_status.flag_control_velocity_enabled ||
-	    v_status.flag_control_position_enabled) {
-		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-	}
-
-	/* autonomous mode */
-	if (v_status.state_machine == SYSTEM_STATE_AUTO) {
-		*mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
-	}
-
-	/* set arming state */
-	if (armed.armed) {
-		*mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-
-	} else {
-		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	switch (v_status.state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status.flag_preflight_gyro_calibration ||
-		    v_status.flag_preflight_mag_calibration ||
-		    v_status.flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
+	/* main state */
+	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+	union px4_custom_mode custom_mode;
+	custom_mode.data = 0;
+	if (v_status.main_state == MAIN_STATE_MANUAL) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+	} else if (v_status.main_state == MAIN_STATE_EASY) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
+		if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
-
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_AUTO:
+	}
+	*mavlink_custom_mode = custom_mode.data;
+
+	/**
+	 * Set mavlink state
+	 **/
+
+	/* set calibration state */
+	if (v_status.arming_state == ARMING_STATE_INIT
+			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
+			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
+		*mavlink_state = MAV_STATE_UNINIT;
+	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
 		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
+	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		*mavlink_state = MAV_STATE_CRITICAL;
+	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
+		*mavlink_state = MAV_STATE_STANDBY;
+	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
 		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		warnx("Unknown mavlink state");
+		*mavlink_state = MAV_STATE_CRITICAL;
 	}
-
 }
 
 
@@ -312,6 +291,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		orb_set_interval(subs->act_2_sub, min_interval);
 		orb_set_interval(subs->act_3_sub, min_interval);
 		orb_set_interval(subs->actuators_sub, min_interval);
+		orb_set_interval(subs->actuators_effective_sub, min_interval);
+		orb_set_interval(subs->spa_sub, min_interval);
+		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
 		break;
 
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
@@ -534,7 +516,7 @@ void mavlink_update_system(void)
 int mavlink_thread_main(int argc, char *argv[])
 {
 	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 5);
+	mavlink_logbuffer_init(&lb, 2);
 
 	int ch;
 	char *device_name = "/dev/ttyS1";
@@ -568,6 +550,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		default:
 			usage();
+			break;
 		}
 	}
 
@@ -674,23 +657,27 @@ int mavlink_thread_main(int argc, char *argv[])
 
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
 
 			/* switch HIL mode if required */
-			set_hil_on_off(v_status.flag_hil_enabled);
+			if (v_status.hil_state == HIL_STATE_ON)
+				set_hil_on_off(true);
+			else if (v_status.hil_state == HIL_STATE_OFF)
+				set_hil_on_off(false);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
 						    v_status.onboard_control_sensors_present,
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.load * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
@@ -751,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* Reset the UART flags to original state */
 	tcsetattr(uart, TCSANOW, &uart_config_original);
 
+	/* destroy log buffer */
+	mavlink_logbuffer_destroy(&lb);
+
 	thread_running = false;
 
 	exit(0);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46..af43542da 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
 #include <mqueue.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -67,6 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
+#include <commander/px4_custom_mode.h>
 
 __BEGIN_DECLS
 
@@ -101,6 +106,10 @@ static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
 static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
 
 static orb_advert_t cmd_pub = -1;
@@ -188,9 +197,11 @@ handle_message(mavlink_message_t *msg)
 		mavlink_set_mode_t new_mode;
 		mavlink_msg_set_mode_decode(msg, &new_mode);
 
+		union px4_custom_mode custom_mode;
+		custom_mode.data = new_mode.custom_mode;
 		/* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */
 		vcmd.param1 = new_mode.base_mode;
-		vcmd.param2 = new_mode.custom_mode;
+		vcmd.param2 = custom_mode.main_mode;
 		vcmd.param3 = 0;
 		vcmd.param4 = 0;
 		vcmd.param5 = 0;
@@ -412,12 +423,12 @@ handle_message(mavlink_message_t *msg)
 
 			/* airspeed from differential pressure, ambient pressure and temp */
 			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
 
 			float ias = calc_indicated_airspeed(imu.diff_pressure);
 			// XXX need to fix this
 			float tas = ias;
 
+			airspeed.timestamp = hrt_absolute_time();
 			airspeed.indicated_airspeed_m_s = ias;
 			airspeed.true_airspeed_m_s = tas;
 
@@ -428,7 +439,67 @@ handle_message(mavlink_message_t *msg)
 			}
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
-			/* publish */
+			/* individual sensor publications */
+			struct gyro_report gyro;
+			gyro.x_raw = imu.xgyro / mrad2rad;
+			gyro.y_raw = imu.ygyro / mrad2rad;
+			gyro.z_raw = imu.zgyro / mrad2rad;
+			gyro.x = imu.xgyro;
+			gyro.y = imu.ygyro;
+			gyro.z = imu.zgyro;
+			gyro.temperature = imu.temperature;
+			gyro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_gyro < 0) {
+				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			} else {
+				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+			}
+
+			struct accel_report accel;
+			accel.x_raw = imu.xacc / mg2ms2;
+			accel.y_raw = imu.yacc / mg2ms2;
+			accel.z_raw = imu.zacc / mg2ms2;
+			accel.x = imu.xacc;
+			accel.y = imu.yacc;
+			accel.z = imu.zacc;
+			accel.temperature = imu.temperature;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			struct mag_report mag;
+			mag.x_raw = imu.xmag / mga2ga;
+			mag.y_raw = imu.ymag / mga2ga;
+			mag.z_raw = imu.zmag / mga2ga;
+			mag.x = imu.xmag;
+			mag.y = imu.ymag;
+			mag.z = imu.zmag;
+			mag.timestamp = hrt_absolute_time();
+
+			if (pub_hil_mag < 0) {
+				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+			} else {
+				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+			}
+
+			struct baro_report baro;
+			baro.pressure = imu.abs_pressure;
+			baro.altitude = imu.pressure_alt;
+			baro.temperature = imu.temperature;
+			baro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_baro < 0) {
+				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+			} else {
+				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+			}
+
+			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
 			} else {
@@ -552,6 +623,22 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
+
+			struct accel_report accel;
+			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+			accel.x = hil_state.xacc;
+			accel.y = hil_state.yacc;
+			accel.z = hil_state.zacc;
+			accel.temperature = 25.0f;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index bfccb2d38..5d3d6a73c 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -39,7 +39,6 @@ MODULE_COMMAND	 = mavlink
 SRCS		 += mavlink.c \
 			   missionlib.c \
 			   mavlink_parameters.c \
-			   mavlink_log.c \
 			   mavlink_receiver.cpp \
 			   orb_listener.c \
 			   waypoints.c
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index edb8761b8..53d86ec00 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
+static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
+	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -272,19 +275,23 @@ l_vehicle_status(const struct listener *l)
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
 
 	/* enable or disable HIL */
-	set_hil_on_off(v_status.flag_hil_enabled);
+	if (v_status.hil_state == HIL_STATE_ON)
+		set_hil_on_off(true);
+	else if (v_status.hil_state == HIL_STATE_OFF)
+		set_hil_on_off(false);
 
 	/* translate the current syste state to mavlink state and mode */
 	uint8_t mavlink_state = 0;
-	uint8_t mavlink_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+	uint8_t mavlink_base_mode = 0;
+	uint32_t mavlink_custom_mode = 0;
+	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 	/* send heartbeat */
 	mavlink_msg_heartbeat_send(chan,
 				   mavlink_system.type,
 				   MAV_AUTOPILOT_PX4,
-				   mavlink_mode,
-				   v_status.state_machine,
+				   mavlink_base_mode,
+				   mavlink_custom_mode,
 				   mavlink_state);
 }
 
@@ -334,7 +341,7 @@ l_global_position(const struct listener *l)
 	int16_t vz = (int16_t)(global_pos.vz * 100.0f);
 
 	/* heading in degrees * 10, from 0 to 36.000) */
-	uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
+	uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f);
 
 	mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
 					     timestamp / 1000,
@@ -470,8 +477,9 @@ l_actuator_outputs(const struct listener *l)
 
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
-			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);
+			uint8_t mavlink_base_mode = 0;
+			uint32_t mavlink_custom_mode = 0;
+			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
 
 			/* HIL message as per MAVLink spec */
 
@@ -488,7 +496,7 @@ l_actuator_outputs(const struct listener *l)
 							      -1,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
@@ -502,7 +510,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      -1,
 							      -1,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
@@ -516,7 +524,7 @@ l_actuator_outputs(const struct listener *l)
 							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
 							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 
 			} else {
@@ -530,7 +538,7 @@ l_actuator_outputs(const struct listener *l)
 							      (act_outputs.output[5] - 1500.0f) / 500.0f,
 							      (act_outputs.output[6] - 1500.0f) / 500.0f,
 							      (act_outputs.output[7] - 1500.0f) / 500.0f,
-							      mavlink_mode,
+							      mavlink_base_mode,
 							      0);
 			}
 		}
@@ -562,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators_0.control_effective[0]);
+						   actuators_effective_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators_0.control_effective[1]);
+						   actuators_effective_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators_0.control_effective[2]);
+						   actuators_effective_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators_0.control_effective[3]);
+						   actuators_effective_0.control_effective[3]);
+	}
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0    ",
+						   actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1    ",
+						   actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2     ",
+						   actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -632,12 +666,12 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.alt;
-	float climb = global_pos.vz;
+	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+	float alt = global_pos.relative_alt;
+	float climb = -global_pos.vz;
 
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
-		((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
+	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 static void *
@@ -673,7 +707,7 @@ uorb_receive_thread(void *arg)
 
 		/* handle the poll result */
 		if (poll_ret == 0) {
-			mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s");
+			/* silent */
 
 		} else if (poll_ret < 0) {
 			mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data");
@@ -768,7 +802,10 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
+
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 73e278dc6..e2e630046 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -55,10 +55,12 @@
 #include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
@@ -75,8 +77,10 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
+	int armed_sub;
+	int actuators_effective_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h
index a4ff06a88..5e5ee8261 100644
--- a/src/modules/mavlink/util.h
+++ b/src/modules/mavlink/util.h
@@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode);
+extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode);
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index eea928a17..16a7c2d35 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
  */
 float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt)
 {
-	//TODO: implement for z once altidude contoller is implemented
-
 	static uint16_t counter;
 
-//	if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y);
 	if (seq < wpm->size) {
-		mavlink_mission_item_t *cur = &(wpm->waypoints[seq]);
+		mavlink_mission_item_t *wp = &(wpm->waypoints[seq]);
 
-		double current_x_rad = cur->x / 180.0 * M_PI;
-		double current_y_rad = cur->y / 180.0 * M_PI;
+		double current_x_rad = wp->x / 180.0 * M_PI;
+		double current_y_rad = wp->y / 180.0 * M_PI;
 		double x_rad = lat / 180.0 * M_PI;
 		double y_rad = lon / 180.0 * M_PI;
 
@@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float
 
 		const double radius_earth = 6371000.0;
 
-		return radius_earth * c;
+		float dxy = radius_earth * c;
+		float dz = alt - wp->z;
+
+		return sqrtf(dxy * dxy + dz * dz);
 
 	} else {
 		return -1.0f;
@@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			// XXX TODO
 		}
 
-		if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
-
+		if (dist >= 0.f && dist <= orbit) {
 			wpm->pos_reached = true;
-
 		}
-
-//		else
-//		{
-//			if(counter % 100 == 0)
-//				printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame);
-//		}
+		// check if required yaw reached
+		float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI);
+		float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw);
+		if (fabsf(yaw_err) < 0.05f) {
+			wpm->yaw_reached = true;
+		}
 	}
 
 	//check if the current waypoint was reached
-	if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) {
+	if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) {
 		if (wpm->current_active_wp_id < wpm->size) {
 			mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]);
 
@@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			bool time_elapsed = false;
 
-			if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
-				if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
-					time_elapsed = true;
-				}
-			} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
 				time_elapsed = true;
 			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
 				time_elapsed = true;
@@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio
 	// 	mavlink_wpm_send_setpoint(wpm->current_active_wp_id);
 	// }
 
-	check_waypoints_reached(now, global_position , local_position);
+	check_waypoints_reached(now, global_position, local_position);
 
 	return OK;
 }
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 20fb11b2c..e71344982 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -273,18 +273,18 @@ void mavlink_update_system(void)
 }
 
 void
-get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode)
 {
 	/* reset MAVLink mode bitfield */
 	*mavlink_mode = 0;
 
 	/* set mode flags independent of system state */
-	if (v_status->flag_control_manual_enabled) {
+	if (control_mode->flag_control_manual_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 	}
 
-	if (v_status->flag_hil_enabled) {
+	if (control_mode->flag_system_hil_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 	}
 
@@ -295,61 +295,67 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct
 		*mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
-	switch (v_status->state_machine) {
-	case SYSTEM_STATE_PREFLIGHT:
-		if (v_status->flag_preflight_gyro_calibration ||
-		    v_status->flag_preflight_mag_calibration ||
-		    v_status->flag_preflight_accel_calibration) {
-			*mavlink_state = MAV_STATE_CALIBRATING;
-		} else {
-			*mavlink_state = MAV_STATE_UNINIT;
-		}
-		break;
-
-	case SYSTEM_STATE_STANDBY:
-		*mavlink_state = MAV_STATE_STANDBY;
-		break;
-
-	case SYSTEM_STATE_GROUND_READY:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		break;
-
-	case SYSTEM_STATE_MANUAL:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
-		break;
-
-	case SYSTEM_STATE_STABILIZED:
-		*mavlink_state = MAV_STATE_ACTIVE;
-		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
-		break;
-
-	case SYSTEM_STATE_AUTO:
-		*mavlink_state = MAV_STATE_ACTIVE;
+	if (control_mode->flag_control_velocity_enabled) {
 		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
-		break;
-
-	case SYSTEM_STATE_MISSION_ABORT:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_LANDING:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_EMCY_CUTOFF:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_GROUND_ERROR:
-		*mavlink_state = MAV_STATE_EMERGENCY;
-		break;
-
-	case SYSTEM_STATE_REBOOT:
-		*mavlink_state = MAV_STATE_POWEROFF;
-		break;
+	} else {
+		*mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED;
 	}
 
+//	switch (v_status->state_machine) {
+//	case SYSTEM_STATE_PREFLIGHT:
+//		if (v_status->flag_preflight_gyro_calibration ||
+//		    v_status->flag_preflight_mag_calibration ||
+//		    v_status->flag_preflight_accel_calibration) {
+//			*mavlink_state = MAV_STATE_CALIBRATING;
+//		} else {
+//			*mavlink_state = MAV_STATE_UNINIT;
+//		}
+//		break;
+//
+//	case SYSTEM_STATE_STANDBY:
+//		*mavlink_state = MAV_STATE_STANDBY;
+//		break;
+//
+//	case SYSTEM_STATE_GROUND_READY:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		break;
+//
+//	case SYSTEM_STATE_MANUAL:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_STABILIZED:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_AUTO:
+//		*mavlink_state = MAV_STATE_ACTIVE;
+//		*mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
+//		break;
+//
+//	case SYSTEM_STATE_MISSION_ABORT:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_EMCY_LANDING:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_EMCY_CUTOFF:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_GROUND_ERROR:
+//		*mavlink_state = MAV_STATE_EMERGENCY;
+//		break;
+//
+//	case SYSTEM_STATE_REBOOT:
+//		*mavlink_state = MAV_STATE_POWEROFF;
+//		break;
+//	}
+
 }
 
 /**
@@ -361,7 +367,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	char *device_name = "/dev/ttyS1";
 	baudrate = 57600;
 
+	/* XXX this is never written? */
 	struct vehicle_status_s v_status;
+	struct vehicle_control_mode_s control_mode;
 	struct actuator_armed_s armed;
 
 	/* work around some stupidity in task_create's argv handling */
@@ -430,19 +438,19 @@ int mavlink_thread_main(int argc, char *argv[])
 			/* translate the current system state to mavlink state and mode */
 			uint8_t mavlink_state = 0;
 			uint8_t mavlink_mode = 0;
-			get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode);
+			get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode);
 
 			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);
+			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state);
 
 			/* send status (values already copied in the section above) */
 			mavlink_msg_sys_status_send(chan,
 						    v_status.onboard_control_sensors_present,
 						    v_status.onboard_control_sensors_enabled,
 						    v_status.onboard_control_sensors_health,
-						    v_status.load,
-						    v_status.voltage_battery * 1000.0f,
-						    v_status.current_battery * 1000.0f,
+						    v_status.load * 1000.0f,
+						    v_status.battery_voltage * 1000.0f,
+						    v_status.battery_current * 1000.0f,
 						    v_status.battery_remaining,
 						    v_status.drop_rate_comm,
 						    v_status.errors_comm,
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 68d49c24b..0236e6126 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -327,4 +327,4 @@ receive_start(int uart)
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
 	return thread;
-}
\ No newline at end of file
+}
diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h
index f18f56243..1b49c9ce4 100644
--- a/src/modules/mavlink_onboard/orb_topics.h
+++ b/src/modules/mavlink_onboard/orb_topics.h
@@ -52,9 +52,11 @@
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_rc_input.h>
@@ -69,7 +71,7 @@ struct mavlink_subscriptions {
 	int act_3_sub;
 	int gps_sub;
 	int man_control_sp_sub;
-	int armed_sub;
+	int safety_sub;
 	int actuators_sub;
 	int local_pos_sub;
 	int spa_sub;
diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h
index 38a4db372..c84b6fd26 100644
--- a/src/modules/mavlink_onboard/util.h
+++ b/src/modules/mavlink_onboard/util.h
@@ -50,5 +50,6 @@ extern volatile bool thread_should_exit;
 /**
  * Translate the custom state into standard mavlink modes and state.
  */
-extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed,
+extern void
+get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed,
 	uint8_t *mavlink_state, uint8_t *mavlink_mode);
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 99f25cfe9..04582f2a4 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -38,6 +39,7 @@
  * Implementation of multirotor attitude control main loop.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -57,12 +59,13 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <drivers/drv_gyro.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/parameter_update.h>
@@ -74,23 +77,20 @@
 #include "multirotor_attitude_control.h"
 #include "multirotor_rate_control.h"
 
-PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost.
-
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
 static bool thread_should_exit;
 static int mc_task;
 static bool motor_test_mode = false;
-
-static orb_advert_t actuator_pub;
-
-static struct vehicle_status_s state;
+static const float min_takeoff_throttle = 0.3f;
+static const float yaw_deadzone = 0.01f;
 
 static int
 mc_thread_main(int argc, char *argv[])
 {
 	/* declare and safely initialize all structs */
-	memset(&state, 0, sizeof(state));
+	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 vehicle_attitude_setpoint_s att_sp;
@@ -103,7 +103,8 @@ mc_thread_main(int argc, char *argv[])
 	memset(&offboard_sp, 0, sizeof(offboard_sp));
 	struct vehicle_rates_setpoint_s rates_sp;
 	memset(&rates_sp, 0, sizeof(rates_sp));
-
+	struct vehicle_status_s status;
+	memset(&status, 0, sizeof(status));
 	struct actuator_controls_s actuators;
 	memset(&actuators, 0, sizeof(actuators));
 
@@ -112,9 +113,11 @@ mc_thread_main(int argc, char *argv[])
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/*
 	 * Do not rate-limit the loop to prevent aliasing
@@ -134,10 +137,9 @@ mc_thread_main(int argc, char *argv[])
 		actuators.control[i] = 0.0f;
 	}
 
-	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime");
@@ -145,23 +147,11 @@ mc_thread_main(int argc, char *argv[])
 	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err");
 
 	/* welcome user */
-	printf("[multirotor_att_control] starting\n");
+	warnx("starting");
 
 	/* store last control mode to detect mode switches */
-	bool flag_control_manual_enabled = false;
-	bool flag_control_attitude_enabled = false;
-	bool flag_system_armed = false;
-
-	/* store if yaw position or yaw speed has been changed */
 	bool control_yaw_position = true;
-
-	/* store if we stopped a yaw movement */
-	bool first_time_after_yaw_speed_control = true;
-
-	/* prepare the handle for the failsafe throttle */
-	param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR");
-	float failsafe_throttle = 0.0f;
-
+	bool reset_yaw_sp = true;
 
 	while (!thread_should_exit) {
 
@@ -183,7 +173,6 @@ mc_thread_main(int argc, char *argv[])
 				orb_copy(ORB_ID(parameter_update), param_sub, &update);
 
 				/* update parameters */
-				// XXX no params here yet
 			}
 
 			/* only run controller if attitude changed */
@@ -193,10 +182,10 @@ mc_thread_main(int argc, char *argv[])
 
 				/* get a local copy of system state */
 				bool updated;
-				orb_check(state_sub, &updated);
+				orb_check(control_mode_sub, &updated);
 
 				if (updated) {
-					orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
 				}
 
 				/* get a local copy of manual setpoint */
@@ -212,19 +201,32 @@ mc_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp);
 				}
 
+				/* get a local copy of status */
+				orb_check(status_sub, &updated);
+
+				if (updated) {
+					orb_copy(ORB_ID(vehicle_status), status_sub, &status);
+				}
+
 				/* get a local copy of the current sensor values */
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
+				/* set flag to safe value */
+				control_yaw_position = true;
+
+				/* reset yaw setpoint if not armed */
+				if (!control_mode.flag_armed) {
+					reset_yaw_sp = true;
+				}
 
-				/** STEP 1: Define which input is the dominating control input */
-				if (state.flag_control_offboard_enabled) {
+				/* define which input is the dominating control input */
+				if (control_mode.flag_control_offboard_enabled) {
 					/* offboard inputs */
 					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
 						rates_sp.roll = offboard_sp.p1;
 						rates_sp.pitch = offboard_sp.p2;
 						rates_sp.yaw = offboard_sp.p3;
 						rates_sp.thrust = offboard_sp.p4;
-//						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
 						rates_sp.timestamp = hrt_absolute_time();
 						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 
@@ -233,108 +235,57 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.pitch_body = offboard_sp.p2;
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
-//						printf("thrust_att=%8.4f\n",offboard_sp.p4);
 						att_sp.timestamp = hrt_absolute_time();
-						/* STEP 2: publish the result to the vehicle actuators */
+						/* publish the result to the vehicle actuators */
 						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 					}
 
+					/* reset yaw setpoint after offboard control */
+					reset_yaw_sp = true;
+
+				} else if (control_mode.flag_control_manual_enabled) {
+					/* manual input */
+					if (control_mode.flag_control_attitude_enabled) {
+						/* control attitude, update attitude setpoint depending on mode */
+						if (att_sp.thrust < 0.1f) {
+							/* no thrust, don't try to control yaw */
+							rates_sp.yaw = 0.0f;
+							control_yaw_position = false;
+
+							if (status.condition_landed) {
+								/* reset yaw setpoint if on ground */
+								reset_yaw_sp = true;
+							}
 
-				} else if (state.flag_control_manual_enabled) {
-
-					if (state.flag_control_attitude_enabled) {
-
-						/* initialize to current yaw if switching to manual or att control */
-						if (state.flag_control_attitude_enabled != flag_control_attitude_enabled ||
-						    state.flag_control_manual_enabled != flag_control_manual_enabled ||
-						    state.flag_system_armed != flag_system_armed) {
-							att_sp.yaw_body = att.yaw;
-						}
-
-						static bool rc_loss_first_time = true;
-
-						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (state.rc_signal_lost) {
-							/* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */
-							param_get(failsafe_throttle_handle, &failsafe_throttle);
-							att_sp.roll_body = 0.0f;
-							att_sp.pitch_body = 0.0f;
-
-							/*
-							 * Only go to failsafe throttle if last known throttle was
-							 * high enough to create some lift to make hovering state likely.
-							 *
-							 * This is to prevent that someone landing, but not disarming his
-							 * multicopter (throttle = 0) does not make it jump up in the air
-							 * if shutting down his remote.
-							 */
-							if (isfinite(manual.throttle) && manual.throttle > 0.2f) {
-								att_sp.thrust = failsafe_throttle;
+						} else {
+							/* only move yaw setpoint if manual input is != 0 */
+							if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) {
+								/* control yaw rate */
+								control_yaw_position = false;
+								rates_sp.yaw = manual.yaw;
+								reset_yaw_sp = true;	// has no effect on control, just for beautiful log
 
 							} else {
-								att_sp.thrust = 0.0f;
+								control_yaw_position = true;
 							}
+						}
 
-							/* keep current yaw, do not attempt to go to north orientation,
-							 * since if the pilot regains RC control, he will be lost regarding
-							 * the current orientation.
-							 */
-							if (rc_loss_first_time)
-								att_sp.yaw_body = att.yaw;
-
-							rc_loss_first_time = false;
-
-						} else {
-							rc_loss_first_time = true;
-
+						if (!control_mode.flag_control_velocity_enabled) {
+							/* update attitude setpoint if not in position control mode */
 							att_sp.roll_body = manual.roll;
 							att_sp.pitch_body = manual.pitch;
 
-							/* set attitude if arming */
-							if (!flag_control_attitude_enabled && state.flag_system_armed) {
-								att_sp.yaw_body = att.yaw;
+							if (!control_mode.flag_control_climb_rate_enabled) {
+								/* pass throttle directly if not in altitude control mode */
+								att_sp.thrust = manual.throttle;
 							}
-
-							/* act if stabilization is active or if the (nonsense) direct pass through mode is set */
-							if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS ||
-							    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
-								if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) {
-									rates_sp.yaw = manual.yaw;
-									control_yaw_position = false;
-
-								} else {
-									/*
-									 * This mode SHOULD be the default mode, which is:
-									 * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS
-									 *
-									 * However, we fall back to this setting for all other (nonsense)
-									 * settings as well.
-									 */
-
-									/* only move setpoint if manual input is != 0 */
-									if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) {
-										rates_sp.yaw = manual.yaw;
-										control_yaw_position = false;
-										first_time_after_yaw_speed_control = true;
-
-									} else {
-										if (first_time_after_yaw_speed_control) {
-											att_sp.yaw_body = att.yaw;
-											first_time_after_yaw_speed_control = false;
-										}
-
-										control_yaw_position = true;
-									}
-								}
-							}
-
-							att_sp.thrust = manual.throttle;
-							att_sp.timestamp = hrt_absolute_time();
 						}
 
-						/* STEP 2: publish the controller output */
-						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+						/* reset yaw setpint to current position if needed */
+						if (reset_yaw_sp) {
+							att_sp.yaw_body = att.yaw;
+							reset_yaw_sp = false;
+						}
 
 						if (motor_test_mode) {
 							printf("testmode");
@@ -342,65 +293,89 @@ mc_thread_main(int argc, char *argv[])
 							att_sp.pitch_body = 0.0f;
 							att_sp.yaw_body = 0.0f;
 							att_sp.thrust = 0.1f;
-							att_sp.timestamp = hrt_absolute_time();
-							/* STEP 2: publish the result to the vehicle actuators */
-							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 						}
 
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+
 					} else {
-						/* manual rate inputs, from RC control or joystick */
-						if (state.flag_control_rates_enabled &&
-						    state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) {
+						/* manual rate inputs (ACRO), from RC control or joystick */
+						if (control_mode.flag_control_rates_enabled) {
 							rates_sp.roll = manual.roll;
-
 							rates_sp.pitch = manual.pitch;
 							rates_sp.yaw = manual.yaw;
 							rates_sp.thrust = manual.throttle;
 							rates_sp.timestamp = hrt_absolute_time();
 						}
+
+						/* reset yaw setpoint after ACRO */
+						reset_yaw_sp = true;
+					}
+
+				} else {
+					if (!control_mode.flag_control_auto_enabled) {
+						/* no control, try to stay on place */
+						if (!control_mode.flag_control_velocity_enabled) {
+							/* no velocity control, reset attitude setpoint */
+							att_sp.roll_body = 0.0f;
+							att_sp.pitch_body = 0.0f;
+							att_sp.timestamp = hrt_absolute_time();
+							orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+						}
 					}
 
+					/* reset yaw setpoint after non-manual control */
+					reset_yaw_sp = true;
 				}
 
-				/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
-				if (state.flag_control_attitude_enabled) {
-					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position);
+				/* check if we should we reset integrals */
+				bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f;	// TODO use landed status instead of throttle
 
+				/* run attitude controller if needed */
+				if (control_mode.flag_control_attitude_enabled) {
+					multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral);
 					orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 				}
 
 				/* measure in what intervals the controller runs */
 				perf_count(mc_interval_perf);
 
-				float gyro[3];
+				/* run rates controller if needed */
+				if (control_mode.flag_control_rates_enabled) {
+					/* get current rate setpoint */
+					bool rates_sp_updated = false;
+					orb_check(rates_sp_sub, &rates_sp_updated);
 
-				/* get current rate setpoint */
-				bool rates_sp_valid = false;
-				orb_check(rates_sp_sub, &rates_sp_valid);
+					if (rates_sp_updated) {
+						orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+					}
 
-				if (rates_sp_valid) {
-					orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+					/* apply controller */
+					float rates[3];
+					rates[0] = att.rollspeed;
+					rates[1] = att.pitchspeed;
+					rates[2] = att.yawspeed;
+					multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral);
+
+				} else {
+					/* rates controller disabled, set actuators to zero for safety */
+					actuators.control[0] = 0.0f;
+					actuators.control[1] = 0.0f;
+					actuators.control[2] = 0.0f;
+					actuators.control[3] = 0.0f;
 				}
 
-				/* apply controller */
-				gyro[0] = att.rollspeed;
-				gyro[1] = att.pitchspeed;
-				gyro[2] = att.yawspeed;
-
-				multirotor_control_rates(&rates_sp, gyro, &actuators);
+				actuators.timestamp = hrt_absolute_time();
 				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
-				/* update state */
-				flag_control_attitude_enabled = state.flag_control_attitude_enabled;
-				flag_control_manual_enabled = state.flag_control_manual_enabled;
-				flag_system_armed = state.flag_system_armed;
-
 				perf_end(mc_loop_perf);
 			} /* end of poll call for attitude updates */
 		} /* end of poll return value check */
 	}
 
-	printf("[multirotor att control] stopping, disarming motors.\n");
+	warnx("stopping, disarming motors");
 
 	/* kill all outputs */
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
@@ -410,7 +385,7 @@ mc_thread_main(int argc, char *argv[])
 
 
 	close(att_sub);
-	close(state_sub);
+	close(control_mode_sub);
 	close(manual_sub);
 	close(actuator_pub);
 	close(att_sp_pub);
@@ -467,11 +442,11 @@ int multirotor_att_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		mc_task = task_spawn_cmd("multirotor_att_control",
-				     SCHED_DEFAULT,
-				     SCHED_PRIORITY_MAX - 15,
-				     2048,
-				     mc_thread_main,
-				     NULL);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 15,
+					 2048,
+					 mc_thread_main,
+					 NULL);
 		exit(0);
 	}
 
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 8f19c6a4b..c78232f11 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position)
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral)
 {
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
@@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f);
 	}
 
-	/* reset integral if on ground */
-	if (att_sp->thrust < 0.1f) {
+	/* reset integrals if needed */
+	if (reset_integral) {
 		pid_reset_integral(&pitch_controller);
 		pid_reset_integral(&roll_controller);
+		//TODO pid_reset_integral(&yaw_controller);
 	}
 
-
 	/* calculate current control outputs */
 
 	/* control pitch (forward) output */
@@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 
 	if (control_yaw_position) {
 		/* control yaw rate */
+		// TODO use pid lib
 
 		/* positive error: rotate to right, negative error, rotate to left (NED frame) */
 		// yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw);
@@ -246,6 +247,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	}
 
 	rates_sp->thrust = att_sp->thrust;
+    //need to update the timestamp now that we've touched rates_sp
+    rates_sp->timestamp = hrt_absolute_time();
 
 	motor_skip_counter++;
 }
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index e78f45c47..431a435f7 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -60,6 +60,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position);
+				 const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index e58d357d5..0a336be47 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 }
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators)
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
 	}
 
-	/* reset integral if on ground */
-	if (rate_sp->thrust < 0.01f) {
+	/* reset integrals if needed */
+	if (reset_integral) {
 		pid_reset_integral(&pitch_rate_controller);
 		pid_reset_integral(&roll_rate_controller);
+		// TODO pid_reset_integral(&yaw_rate_controller);
 	}
 
 	/* control pitch (forward) output */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 362b5ed86..ca7794c59 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -59,6 +59,6 @@
 #include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-			      const float rates[], struct actuator_controls_s *actuators);
+			      const float rates[], struct actuator_controls_s *actuators, bool reset_integral);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk
index d04847745..bc4b48fb4 100644
--- a/src/modules/multirotor_pos_control/module.mk
+++ b/src/modules/multirotor_pos_control/module.mk
@@ -38,4 +38,5 @@
 MODULE_COMMAND	= multirotor_pos_control
 
 SRCS		= multirotor_pos_control.c \
-		  multirotor_pos_control_params.c
+		  multirotor_pos_control_params.c \
+		  thrust_pid.c
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index f39d11438..a25448af2 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -35,13 +35,14 @@
 /**
  * @file multirotor_pos_control.c
  *
- * Skeleton for multirotor position controller
+ * Multirotor position controller
  */
 
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 #include <stdbool.h>
 #include <unistd.h>
 #include <fcntl.h>
@@ -52,15 +53,22 @@
 #include <sys/prctl.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.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_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/pid/pid.h>
+#include <mavlink/mavlink_log.h>
 
 #include "multirotor_pos_control_params.h"
+#include "thrust_pid.h"
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -79,12 +87,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]);
  */
 static void usage(const char *reason);
 
-static void
-usage(const char *reason)
+static float scale_control(float ctl, float end, float dz);
+
+static float norm(float x, float y);
+
+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");
+
+	fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n");
 	exit(1);
 }
 
@@ -92,9 +104,9 @@ usage(const char *reason)
  * 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().
+ * to task_spawn().
  */
 int multirotor_pos_control_main(int argc, char *argv[])
 {
@@ -104,32 +116,36 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("multirotor pos control already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
 
+		warnx("start");
 		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("multirotor pos control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 60,
-					 4096,
-					 multirotor_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_spawn_cmd("multirotor_pos_control",
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 60,
+					     4096,
+					     multirotor_pos_control_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "stop")) {
+		warnx("stop");
 		thread_should_exit = true;
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tmultirotor pos control app is running\n");
+			warnx("app is running");
+
 		} else {
-			printf("\tmultirotor pos control app not started\n");
+			warnx("app not started");
 		}
+
 		exit(0);
 	}
 
@@ -137,98 +153,509 @@ int multirotor_pos_control_main(int argc, char *argv[])
 	exit(1);
 }
 
-static int
-multirotor_pos_control_thread_main(int argc, char *argv[])
+static float scale_control(float ctl, float end, float dz)
+{
+	if (ctl > dz) {
+		return (ctl - dz) / (end - dz);
+
+	} else if (ctl < -dz) {
+		return (ctl + dz) / (end - dz);
+
+	} else {
+		return 0.0f;
+	}
+}
+
+static float norm(float x, float y)
+{
+	return sqrtf(x * x + y * y);
+}
+
+static int multirotor_pos_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
-	printf("[multirotor pos control] Control started, taking over position control\n");
+	warnx("started");
+	static int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[mpc] started");
 
 	/* structures */
-	struct vehicle_status_s state;
+	struct vehicle_control_mode_s control_mode;
+	memset(&control_mode, 0, sizeof(control_mode));
 	struct vehicle_attitude_s att;
-	//struct vehicle_global_position_setpoint_s global_pos_sp;
-	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_vicon_position_s local_pos;
-	struct manual_control_setpoint_s manual;
+	memset(&att, 0, sizeof(att));
 	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct vehicle_local_position_setpoint_s local_pos_sp;
+	memset(&local_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_position_setpoint_s global_pos_sp;
+	memset(&global_pos_sp, 0, sizeof(local_pos_sp));
+	struct vehicle_global_velocity_setpoint_s global_vel_sp;
+	memset(&global_vel_sp, 0, sizeof(global_vel_sp));
 
 	/* subscribe to attitude, motor setpoints and system state */
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 
-	/* publish attitude setpoint */
+	/* publish setpoint */
+	orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp);
+	orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
+	bool global_pos_sp_reproject = false;
+	bool global_pos_sp_valid = false;
+	bool local_pos_sp_valid = false;
+	bool reset_sp_z = true;
+	bool reset_sp_xy = true;
+	bool reset_int_z = true;
+	bool reset_int_z_manual = false;
+	bool reset_int_xy = true;
+	bool was_armed = false;
+	bool reset_integral = true;
+	bool reset_auto_pos = true;
+
+	hrt_abstime t_prev = 0;
+	const float alt_ctl_dz = 0.2f;
+	const float pos_ctl_dz = 0.05f;
+	const float takeoff_alt_default = 10.0f;
+	float ref_alt = 0.0f;
+	hrt_abstime ref_alt_t = 0;
+	uint64_t local_ref_timestamp = 0;
+
+	PID_t xy_pos_pids[2];
+	PID_t xy_vel_pids[2];
+	PID_t z_pos_pid;
+	thrust_pid_t z_vel_pid;
+
 	thread_running = true;
 
-	int loopcounter = 0;
-
-	struct multirotor_position_control_params p;
-	struct multirotor_position_control_param_handles h;
-	parameters_init(&h);
-	parameters_update(&h, &p);
-
-
-	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
-		/* get a local copy of local position setpoint */
-		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-
-		if (loopcounter == 500) {
-			parameters_update(&h, &p);
-			loopcounter = 0;
+	struct multirotor_position_control_params params;
+	struct multirotor_position_control_param_handles params_h;
+	parameters_init(&params_h);
+	parameters_update(&params_h, &params);
+
+	for (int i = 0; i < 2; i++) {
+		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
+		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+	}
+
+	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
+	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
+
+	while (!thread_should_exit) {
+
+		bool param_updated;
+		orb_check(param_sub, &param_updated);
+
+		if (param_updated) {
+			/* clear updated flag */
+			struct parameter_update_s ps;
+			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
+			/* update params */
+			parameters_update(&params_h, &params);
+
+			for (int i = 0; i < 2; i++) {
+				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
+				/* use integral_limit_out = tilt_max / 2 */
+				float i_limit;
+
+				if (params.xy_vel_i == 0.0f) {
+					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
+
+				} else {
+					i_limit = 1.0f;	// not used really
+				}
+
+				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
+			}
+
+			pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+			thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
+		}
+
+		bool updated;
+
+		orb_check(control_mode_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
+		}
+
+		orb_check(global_pos_sp_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp);
+			global_pos_sp_valid = true;
+			global_pos_sp_reproject = true;
 		}
 
-		// if (state.state_machine == SYSTEM_STATE_AUTO) {
-			
-			// XXX IMPLEMENT POSITION CONTROL HERE
-
-			float dT = 1.0f / 50.0f;
-
-			float x_setpoint = 0.0f;
-
-			// XXX enable switching between Vicon and local position estimate
-			/* local pos is the Vicon position */
-
-			// XXX just an example, lacks rotation around world-body transformation
-			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p;
-			att_sp.roll_body = 0.0f;
-			att_sp.yaw_body = 0.0f;
-			att_sp.thrust = 0.3f;
-			att_sp.timestamp = hrt_absolute_time();
-
-			/* publish new attitude setpoint */
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
-			/* set setpoint to current position */
-			// XXX select pos reset channel on remote
-			/* reset setpoint to current position  (position hold) */
-			// if (1 == 2) {
-			// 	local_pos_sp.x = local_pos.x;
-			// 	local_pos_sp.y = local_pos.y;
-			// 	local_pos_sp.z = local_pos.z;
-			// 	local_pos_sp.yaw = att.yaw;
-			// }
-		// }
+		hrt_abstime t = hrt_absolute_time();
+		float dt;
+
+		if (t_prev != 0) {
+			dt = (t - t_prev) * 0.000001f;
+
+		} else {
+			dt = 0.0f;
+		}
+
+		if (control_mode.flag_armed && !was_armed) {
+			/* reset setpoints and integrals on arming */
+			reset_sp_z = true;
+			reset_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+		}
+
+		was_armed = control_mode.flag_armed;
+
+		t_prev = t;
+
+		if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) {
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
+			orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+
+			float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f;
+			float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f;
+			float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f };
+
+			if (control_mode.flag_control_manual_enabled) {
+				/* manual control */
+				/* check for reference point updates and correct setpoint */
+				if (local_pos.ref_timestamp != ref_alt_t) {
+					if (ref_alt_t != 0) {
+						/* home alt changed, don't follow large ground level changes in manual flight */
+						local_pos_sp.z += local_pos.ref_alt - ref_alt;
+					}
+
+					ref_alt_t = local_pos.ref_timestamp;
+					ref_alt = local_pos.ref_alt;
+					// TODO also correct XY setpoint
+				}
+
+				/* reset setpoints to current position if needed */
+				if (control_mode.flag_control_altitude_enabled) {
+					if (reset_sp_z) {
+						reset_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double)-local_pos_sp.z);
+					}
+
+					/* move altitude setpoint with throttle stick */
+					float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+
+					if (z_sp_ctl != 0.0f) {
+						sp_move_rate[2] = -z_sp_ctl * params.z_vel_max;
+						local_pos_sp.z += sp_move_rate[2] * dt;
+
+						if (local_pos_sp.z > local_pos.z + z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z + z_sp_offs_max;
+
+						} else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) {
+							local_pos_sp.z = local_pos.z - z_sp_offs_max;
+						}
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_sp_xy) {
+						reset_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
+					}
+
+					/* move position setpoint with roll/pitch stick */
+					float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz);
+					float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz);
+
+					if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) {
+						/* calculate direction and increment of control in NED frame */
+						float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl);
+						float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max;
+						sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed;
+						local_pos_sp.x += sp_move_rate[0] * dt;
+						local_pos_sp.y += sp_move_rate[1] * dt;
+						/* limit maximum setpoint from position offset and preserve direction
+						 * fail safe, should not happen in normal operation */
+						float pos_vec_x = local_pos_sp.x - local_pos.x;
+						float pos_vec_y = local_pos_sp.y - local_pos.y;
+						float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max;
+
+						if (pos_vec_norm > 1.0f) {
+							local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm;
+							local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm;
+						}
+					}
+				}
+
+				local_pos_sp.yaw = att_sp.yaw_body;
+
+				/* local position setpoint is valid and can be used for loiter after position controlled mode */
+				local_pos_sp_valid = control_mode.flag_control_position_enabled;
+
+				reset_auto_pos = true;
+
+				/* force reprojection of global setpoint after manual mode */
+				global_pos_sp_reproject = true;
+
+			} else if (control_mode.flag_control_auto_enabled) {
+				/* AUTO mode, use global setpoint */
+				if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) {
+					reset_auto_pos = true;
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+					if (reset_auto_pos) {
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.z = -takeoff_alt_default;
+						local_pos_sp.yaw = att.yaw;
+						local_pos_sp_valid = true;
+						att_sp.yaw_body = att.yaw;
+						reset_auto_pos = false;
+						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double)-local_pos_sp.z);
+					}
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
+					// TODO
+					reset_auto_pos = true;
+
+				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) {
+					/* init local projection using local position ref */
+					if (local_pos.ref_timestamp != local_ref_timestamp) {
+						global_pos_sp_reproject = true;
+						local_ref_timestamp = local_pos.ref_timestamp;
+						double lat_home = local_pos.ref_lat * 1e-7;
+						double lon_home = local_pos.ref_lon * 1e-7;
+						map_projection_init(lat_home, lon_home);
+						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
+					}
+
+					if (global_pos_sp_reproject) {
+						/* update global setpoint projection */
+						global_pos_sp_reproject = false;
+
+						if (global_pos_sp_valid) {
+							/* global position setpoint valid, use it */
+							double sp_lat = global_pos_sp.lat * 1e-7;
+							double sp_lon = global_pos_sp.lon * 1e-7;
+							/* project global setpoint to local setpoint */
+							map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y));
+
+							if (global_pos_sp.altitude_is_relative) {
+								local_pos_sp.z = -global_pos_sp.altitude;
+
+							} else {
+								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
+							}
+
+							local_pos_sp.yaw = global_pos_sp.yaw;
+							att_sp.yaw_body = global_pos_sp.yaw;
+
+							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
+
+						} else {
+							if (!local_pos_sp_valid) {
+								/* local position setpoint is invalid,
+								 * use current position as setpoint for loiter */
+								local_pos_sp.x = local_pos.x;
+								local_pos_sp.y = local_pos.y;
+								local_pos_sp.z = local_pos.z;
+								local_pos_sp.yaw = att.yaw;
+								local_pos_sp_valid = true;
+							}
+
+							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
+						}
+					}
+
+					reset_auto_pos = true;
+				}
+
+				if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) {
+					global_pos_sp_reproject = true;
+				}
+
+				/* reset setpoints after AUTO mode */
+				reset_sp_xy = true;
+				reset_sp_z = true;
+
+			} else {
+				/* no control, loiter or stay on ground */
+				if (local_pos.landed) {
+					/* landed: move setpoint down */
+					/* in air: hold altitude */
+					if (local_pos_sp.z < 5.0f) {
+						/* set altitude setpoint to 5m under ground,
+						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
+						local_pos_sp.z = 5.0f;
+						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double)-local_pos_sp.z);
+					}
+
+					reset_sp_z = true;
+
+				} else {
+					/* in air: hold altitude */
+					if (reset_sp_z) {
+						reset_sp_z = false;
+						local_pos_sp.z = local_pos.z;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double)-local_pos_sp.z);
+					}
+				}
+
+				if (control_mode.flag_control_position_enabled) {
+					if (reset_sp_xy) {
+						reset_sp_xy = false;
+						local_pos_sp.x = local_pos.x;
+						local_pos_sp.y = local_pos.y;
+						local_pos_sp.yaw = att.yaw;
+						local_pos_sp_valid = true;
+						att_sp.yaw_body = att.yaw;
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
+					}
+				}
+			}
+
+			/* publish local position setpoint */
+			orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp);
+
+			/* run position & altitude controllers, calculate velocity setpoint */
+			if (control_mode.flag_control_altitude_enabled) {
+				global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2];
+
+			} else {
+				reset_sp_z = true;
+				global_vel_sp.vz = 0.0f;
+			}
+
+			if (control_mode.flag_control_position_enabled) {
+				/* calculate velocity set point in NED frame */
+				global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0];
+				global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1];
+
+				/* limit horizontal speed */
+				float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max;
+
+				if (xy_vel_sp_norm > 1.0f) {
+					global_vel_sp.vx /= xy_vel_sp_norm;
+					global_vel_sp.vy /= xy_vel_sp_norm;
+				}
+
+			} else {
+				reset_sp_xy = true;
+				global_vel_sp.vx = 0.0f;
+				global_vel_sp.vy = 0.0f;
+			}
+
+			/* publish new velocity setpoint */
+			orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp);
+			// TODO subscribe to velocity setpoint if altitude/position control disabled
+
+			if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) {
+				/* run velocity controllers, calculate thrust vector with attitude-thrust compensation */
+				float thrust_sp[3] = { 0.0f, 0.0f, 0.0f };
+
+				if (control_mode.flag_control_climb_rate_enabled) {
+					if (reset_int_z) {
+						reset_int_z = false;
+						float i = params.thr_min;
+
+						if (reset_int_z_manual) {
+							i = manual.throttle;
+
+							if (i < params.thr_min) {
+								i = params.thr_min;
+
+							} else if (i > params.thr_max) {
+								i = params.thr_max;
+							}
+						}
+
+						thrust_pid_set_integral(&z_vel_pid, -i);
+						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
+					}
+
+					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
+					att_sp.thrust = -thrust_sp[2];
+
+				} else {
+					/* reset thrust integral when altitude control enabled */
+					reset_int_z = true;
+				}
+
+				if (control_mode.flag_control_velocity_enabled) {
+					/* calculate thrust set point in NED frame */
+					if (reset_int_xy) {
+						reset_int_xy = false;
+						pid_reset_integral(&xy_vel_pids[0]);
+						pid_reset_integral(&xy_vel_pids[1]);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos integral");
+					}
+
+					thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt);
+					thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt);
+
+					/* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */
+					/* limit horizontal part of thrust */
+					float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]);
+					/* assuming that vertical component of thrust is g,
+					 * horizontal component = g * tan(alpha) */
+					float tilt = atanf(norm(thrust_sp[0], thrust_sp[1]));
+
+					if (tilt > params.tilt_max) {
+						tilt = params.tilt_max;
+					}
+
+					/* convert direction to body frame */
+					thrust_xy_dir -= att.yaw;
+					/* calculate roll and pitch */
+					att_sp.roll_body = sinf(thrust_xy_dir) * tilt;
+					att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body);
+
+				} else {
+					reset_int_xy = true;
+				}
+
+				att_sp.timestamp = hrt_absolute_time();
+
+				/* publish new attitude setpoint */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+			}
+
+		} else {
+			/* position controller disabled, reset setpoints */
+			reset_sp_z = true;
+			reset_sp_xy = true;
+			reset_int_z = true;
+			reset_int_xy = true;
+			global_pos_sp_reproject = true;
+			reset_auto_pos = true;
+		}
+
+		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
+		reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled;
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
-		loopcounter++;
-
 	}
 
-	printf("[multirotor pos control] ending now...\n");
+	warnx("stopped");
+	mavlink_log_info(mavlink_fd, "[mpc] stopped");
 
 	thread_running = false;
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 6b73dc405..9c1ef2edb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,29 +33,76 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.c
- * 
- * Parameters for EKF filter
+ * @file multirotor_pos_control_params.c
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include "multirotor_pos_control_params.h"
 
-/* Extended Kalman Filter covariances */
-
 /* controller parameters */
-PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f);
+PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f);
+PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f);
+PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f);
+PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
-	/* PID parameters */
-	h->p 	=	param_find("MC_POS_P");
+	h->thr_min 	=	param_find("MPC_THR_MIN");
+	h->thr_max 	=	param_find("MPC_THR_MAX");
+	h->z_p 	=	param_find("MPC_Z_P");
+	h->z_d 	=	param_find("MPC_Z_D");
+	h->z_vel_p 	=	param_find("MPC_Z_VEL_P");
+	h->z_vel_i 	=	param_find("MPC_Z_VEL_I");
+	h->z_vel_d 	=	param_find("MPC_Z_VEL_D");
+	h->z_vel_max 	=	param_find("MPC_Z_VEL_MAX");
+	h->xy_p 	=	param_find("MPC_XY_P");
+	h->xy_d 	=	param_find("MPC_XY_D");
+	h->xy_vel_p 	=	param_find("MPC_XY_VEL_P");
+	h->xy_vel_i 	=	param_find("MPC_XY_VEL_I");
+	h->xy_vel_d 	=	param_find("MPC_XY_VEL_D");
+	h->xy_vel_max 	=	param_find("MPC_XY_VEL_MAX");
+	h->tilt_max 	=	param_find("MPC_TILT_MAX");
+
+	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 multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
-	param_get(h->p, &(p->p));
+	param_get(h->thr_min, &(p->thr_min));
+	param_get(h->thr_max, &(p->thr_max));
+	param_get(h->z_p, &(p->z_p));
+	param_get(h->z_d, &(p->z_d));
+	param_get(h->z_vel_p, &(p->z_vel_p));
+	param_get(h->z_vel_i, &(p->z_vel_i));
+	param_get(h->z_vel_d, &(p->z_vel_d));
+	param_get(h->z_vel_max, &(p->z_vel_max));
+	param_get(h->xy_p, &(p->xy_p));
+	param_get(h->xy_d, &(p->xy_d));
+	param_get(h->xy_vel_p, &(p->xy_vel_p));
+	param_get(h->xy_vel_i, &(p->xy_vel_i));
+	param_get(h->xy_vel_d, &(p->xy_vel_d));
+	param_get(h->xy_vel_max, &(p->xy_vel_max));
+	param_get(h->tilt_max, &(p->tilt_max));
+
+	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/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 274f6c22a..3ec85a364 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -1,8 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,23 +33,55 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_position_control_params.h
- * 
- * Parameters for position controller
+ * @file multirotor_pos_control_params.h
+ *
+ * Parameters for multirotor_pos_control
  */
 
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
-	float p;
-	float i;
-	float d;
+	float thr_min;
+	float thr_max;
+	float z_p;
+	float z_d;
+	float z_vel_p;
+	float z_vel_i;
+	float z_vel_d;
+	float z_vel_max;
+	float xy_p;
+	float xy_d;
+	float xy_vel_p;
+	float xy_vel_i;
+	float xy_vel_d;
+	float xy_vel_max;
+	float tilt_max;
+
+	float rc_scale_pitch;
+	float rc_scale_roll;
+	float rc_scale_yaw;
 };
 
 struct multirotor_position_control_param_handles {
-	param_t p;
-	param_t i;
-	param_t d;
+	param_t thr_min;
+	param_t thr_max;
+	param_t z_p;
+	param_t z_d;
+	param_t z_vel_p;
+	param_t z_vel_i;
+	param_t z_vel_d;
+	param_t z_vel_max;
+	param_t xy_p;
+	param_t xy_d;
+	param_t xy_vel_p;
+	param_t xy_vel_i;
+	param_t xy_vel_d;
+	param_t xy_vel_max;
+	param_t tilt_max;
+
+	param_t rc_scale_pitch;
+	param_t rc_scale_roll;
+	param_t rc_scale_yaw;
 };
 
 /**
diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c
deleted file mode 100644
index 9c53a5bf6..000000000
--- a/src/modules/multirotor_pos_control/position_control.c
+++ /dev/null
@@ -1,235 +0,0 @@
-// /****************************************************************************
-//  *
-//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
-//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
-//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
-//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
-//  *           @author Martin Rutschmann <rutmarti@student.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 multirotor_position_control.c
-//  * Implementation of the position control for a multirotor VTOL
-//  */
-
-// #include <stdio.h>
-// #include <stdlib.h>
-// #include <stdio.h>
-// #include <stdint.h>
-// #include <math.h>
-// #include <stdbool.h>
-// #include <float.h>
-// #include <systemlib/pid/pid.h>
-
-// #include "multirotor_position_control.h"
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-// {
-// 	static PID_t distance_controller;
-
-// 	static int read_ret;
-// 	static global_data_position_t position_estimated;
-
-// 	static uint16_t counter;
-
-// 	static bool initialized;
-// 	static uint16_t pm_counter;
-
-// 	static float lat_next;
-// 	static float lon_next;
-
-// 	static float pitch_current;
-
-// 	static float thrust_total;
-
-
-// 	if (initialized == false) {
-
-// 		pid_init(&distance_controller,
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-// 			 PID_MODE_DERIVATIV_CALC, 150);//150
-
-// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 		thrust_total = 0.0f;
-
-// 		/* Position initialization */
-// 		/* Wait for new position estimate */
-// 		do {
-// 			read_ret = read_lock_position(&position_estimated);
-// 		} while (read_ret != 0);
-
-// 		lat_next = position_estimated.lat;
-// 		lon_next = position_estimated.lon;
-
-// 		/* attitude initialization */
-// 		global_data_lock(&global_data_attitude->access_conf);
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-
-// 		initialized = true;
-// 	}
-
-// 	/* load new parameters with 10Hz */
-// 	if (counter % 50 == 0) {
-// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-// 			/* check whether new parameters are available */
-// 			if (global_data_parameter_storage->counter > pm_counter) {
-// 				pid_set_parameters(&distance_controller,
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-// //
-// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-// 				pm_counter = global_data_parameter_storage->counter;
-// 				printf("Position controller changed pid parameters\n");
-// 			}
-// 		}
-
-// 		global_data_unlock(&global_data_parameter_storage->access_conf);
-// 	}
-
-
-// 	/* Wait for new position estimate */
-// 	do {
-// 		read_ret = read_lock_position(&position_estimated);
-// 	} while (read_ret != 0);
-
-// 	/* Get next waypoint */ //TODO: add local copy
-
-// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-// 		lat_next = global_data_position_setpoint->x;
-// 		lon_next = global_data_position_setpoint->y;
-// 		global_data_unlock(&global_data_position_setpoint->access_conf);
-// 	}
-
-// 	/* Get distance to waypoint */
-// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-// //	if(counter % 5 == 0)
-// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
-
-// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
-// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
-
-// 	if (counter % 5 == 0)
-// 		printf("bearing: %.4f\n", bearing);
-
-// 	/* Calculate speed in direction of bearing (needed for controller) */
-// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-// //	if(counter % 5 == 0)
-// //		printf("speed_norm: %.4f\n", speed_norm);
-// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
-
-// 	/* Control Thrust in bearing direction  */
-// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
-
-// //	if(counter % 5 == 0)
-// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
-
-// 	/* Get total thrust (from remote for now) */
-// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-// 		global_data_unlock(&global_data_rc_channels->access_conf);
-// 	}
-
-// 	const float max_gas = 500.0f;
-// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
-// 	thrust_total += max_gas / 2.0f;
-
-
-// 	if (horizontal_thrust > thrust_total) {
-// 		horizontal_thrust = thrust_total;
-
-// 	} else if (horizontal_thrust < -thrust_total) {
-// 		horizontal_thrust = -thrust_total;
-// 	}
-
-
-
-// 	//TODO: maybe we want to add a speed controller later...
-
-// 	/* Calclulate thrust in east and north direction */
-// 	float thrust_north = cosf(bearing) * horizontal_thrust;
-// 	float thrust_east = sinf(bearing) * horizontal_thrust;
-
-// 	if (counter % 10 == 0) {
-// 		printf("thrust north: %.4f\n", thrust_north);
-// 		printf("thrust east: %.4f\n", thrust_east);
-// 		fflush(stdout);
-// 	}
-
-// 	/* Get current attitude */
-// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-// 		pitch_current = global_data_attitude->pitch;
-// 		global_data_unlock(&global_data_attitude->access_conf);
-// 	}
-
-// 	/* Get desired pitch & roll */
-// 	float pitch_desired = 0.0f;
-// 	float roll_desired = 0.0f;
-
-// 	if (thrust_total != 0) {
-// 		float pitch_fraction = -thrust_north / thrust_total;
-// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
-
-// 		if (roll_fraction < -1) {
-// 			roll_fraction = -1;
-
-// 		} else if (roll_fraction > 1) {
-// 			roll_fraction = 1;
-// 		}
-
-// //		if(counter % 5 == 0)
-// //		{
-// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-// //			fflush(stdout);
-// //		}
-
-// 		pitch_desired = asinf(pitch_fraction);
-// 		roll_desired = asinf(roll_fraction);
-// 	}
-
-// 	att_sp.roll = roll_desired;
-// 	att_sp.pitch = pitch_desired;
-
-// 	counter++;
-// }
diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h
deleted file mode 100644
index 2144ebc34..000000000
--- a/src/modules/multirotor_pos_control/position_control.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.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 multirotor_position_control.h
- * Definition of the position control for a multirotor VTOL
- */
-
-// #ifndef POSITION_CONTROL_H_
-// #define POSITION_CONTROL_H_
-
-// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
-//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
-//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
-
-// #endif /* POSITION_CONTROL_H_ */
diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c
new file mode 100644
index 000000000..b985630ae
--- /dev/null
+++ b/src/modules/multirotor_pos_control/thrust_pid.c
@@ -0,0 +1,189 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file thrust_pid.c
+ *
+ * Implementation of thrust control PID.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include "thrust_pid.h"
+#include <math.h>
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->limit_min = limit_min;
+	pid->limit_max = limit_max;
+	pid->mode = mode;
+	pid->dt_min = dt_min;
+	pid->last_output = 0.0f;
+	pid->sp = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->integral = 0.0f;
+}
+
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max)
+{
+	int ret = 0;
+
+	if (isfinite(kp)) {
+		pid->kp = kp;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(ki)) {
+		pid->ki = ki;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(kd)) {
+		pid->kd = kd;
+
+	} else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_min)) {
+		pid->limit_min = limit_min;
+
+	}  else {
+		ret = 1;
+	}
+
+	if (isfinite(limit_max)) {
+		pid->limit_max = limit_max;
+
+	}  else {
+		ret = 1;
+	}
+
+	return ret;
+}
+
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22)
+{
+	/* Alternative integral component calculation
+	 *
+	 * start:
+	 * error = setpoint - current_value
+	 * integral = integral + (Ki * error * dt)
+	 * derivative = (error - previous_error) / dt
+	 * previous_error = error
+	 * output = (Kp * error) + integral + (Kd * derivative)
+	 * wait(dt)
+	 * goto start
+	 */
+
+	if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) {
+		return pid->last_output;
+	}
+
+	float i, d;
+	pid->sp = sp;
+
+	// Calculated current error value
+	float error = pid->sp - val;
+
+	// Calculate or measured current error derivative
+	if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
+
+	} else {
+		d = 0.0f;
+	}
+
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
+	/* calculate the error integral */
+	i = pid->integral + (pid->ki * error * dt);
+
+	/* attitude-thrust compensation
+	 * r22 is (2, 2) componet of rotation matrix for current attitude */
+	float att_comp;
+
+	if (r22 > 0.8f)
+		att_comp = 1.0f / r22;
+	else if (r22 > 0.0f)
+		att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f;
+	else
+		att_comp = 1.0f;
+
+	/* calculate output */
+	float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp;
+
+	/* check for saturation */
+	if (output < pid->limit_min || output > pid->limit_max) {
+		/* saturated, recalculate output with old integral */
+		output = (error * pid->kp) + pid->integral + (d * pid->kd);
+
+	} else {
+		if (isfinite(i)) {
+			pid->integral = i;
+		}
+	}
+
+	if (isfinite(output)) {
+		if (output > pid->limit_max) {
+			output = pid->limit_max;
+
+		} else if (output < pid->limit_min) {
+			output = pid->limit_min;
+		}
+
+		pid->last_output = output;
+	}
+
+	return pid->last_output;
+}
+
+__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i)
+{
+	pid->integral = i;
+}
diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/multirotor_pos_control/thrust_pid.h
index fb778dd66..5e169c1ba 100644
--- a/src/modules/mathlib/math/Limits.hpp
+++ b/src/modules/multirotor_pos_control/thrust_pid.h
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,56 +33,44 @@
  ****************************************************************************/
 
 /**
- * @file Limits.hpp
+ * @file thrust_pid.h
  *
- * Limiting / constrain helper functions
+ * Definition of thrust control PID interface.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#pragma once
+#ifndef THRUST_PID_H_
+#define THRUST_PID_H_
 
-#include <nuttx/config.h>
 #include <stdint.h>
 
-namespace math {
-
-
-float __EXPORT min(float val1, float val2);
-
-int __EXPORT min(int val1, int val2);
-
-unsigned __EXPORT min(unsigned val1, unsigned val2);
-
-uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
-
-double __EXPORT min(double val1, double val2);
-
-float __EXPORT max(float val1, float val2);
-
-int __EXPORT max(int val1, int val2);
-
-unsigned __EXPORT max(unsigned val1, unsigned val2);
-
-uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
-
-double __EXPORT max(double val1, double val2);
-
-
-float __EXPORT constrain(float val, float min, float max);
-
-int __EXPORT constrain(int val, int min, int max);
-
-unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
-
-uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
-
-double __EXPORT constrain(double val, double min, double max);
-
-float __EXPORT radians(float degrees);
-
-double __EXPORT radians(double degrees);
-
-float __EXPORT degrees(float radians);
-
-double __EXPORT degrees(double radians);
-
-}
\ No newline at end of file
+__BEGIN_DECLS
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */
+#define THRUST_PID_MODE_DERIVATIV_CALC	0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */
+#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP	1
+
+typedef struct {
+	float kp;
+	float ki;
+	float kd;
+	float sp;
+	float integral;
+	float error_previous;
+	float last_output;
+	float limit_min;
+	float limit_max;
+	float dt_min;
+	uint8_t mode;
+} thrust_pid_t;
+
+__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min);
+__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max);
+__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22);
+__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i);
+
+__END_DECLS
+
+#endif /* THRUST_PID_H_ */
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
new file mode 100644
index 000000000..13328edb4
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -0,0 +1,31 @@
+/*
+ * inertial_filter.c
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include "inertial_filter.h"
+
+void inertial_filter_predict(float dt, float x[3])
+{
+	x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
+	x[1] += x[2] * dt;
+}
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+{
+	float ewdt = w * dt;
+	if (ewdt > 1.0f)
+		ewdt = 1.0f;	// prevent over-correcting
+	ewdt *= e;
+	x[i] += ewdt;
+
+	if (i == 0) {
+		x[1] += w * ewdt;
+		x[2] += w * w * ewdt / 3.0;
+
+	} else if (i == 1) {
+		x[2] += w * ewdt;
+	}
+}
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
new file mode 100644
index 000000000..761c17097
--- /dev/null
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -0,0 +1,13 @@
+/*
+ * inertial_filter.h
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ */
+
+#include <stdbool.h>
+#include <drivers/drv_hrt.h>
+
+void inertial_filter_predict(float dt, float x[3]);
+
+void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/modules/position_estimator_inav/module.mk
index 0cc1b559d..939d76849 100644
--- a/src/modules/mathlib/CMSIS/library.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -32,15 +32,10 @@
 ############################################################################
 
 #
-# ARM CMSIS DSP library
+# Makefile to build position_estimator_inav
 #
 
-ifeq ($(CONFIG_ARCH),CORTEXM4F)
-PREBUILT_LIB		:= libarm_cortexM4lf_math.a
-else ifeq ($(CONFIG_ARCH),CORTEXM4)
-PREBUILT_LIB		:= libarm_cortexM4l_math.a
-else ifeq ($(CONFIG_ARCH),CORTEXM3)
-PREBUILT_LIB		:= libarm_cortexM3l_math.a
-else
-$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library)
-endif
+MODULE_COMMAND	 	= position_estimator_inav
+SRCS		 	= position_estimator_inav_main.c \
+			position_estimator_inav_params.c \
+			inertial_filter.c
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
new file mode 100644
index 000000000..88057b323
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -0,0 +1,653 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file position_estimator_inav_main.c
+ * Model-identification based position estimator for multirotors
+ */
+
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <string.h>
+#include <nuttx/config.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/optical_flow.h>
+#include <mavlink/mavlink_log.h>
+#include <poll.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+
+#include "position_estimator_inav_params.h"
+#include "inertial_filter.h"
+
+static bool thread_should_exit = false; /**< Deamon exit flag */
+static bool thread_running = false; /**< Deamon status flag */
+static int position_estimator_inav_task; /**< Handle of deamon task / thread */
+static bool verbose_mode = false;
+
+static const hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
+static const hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
+static const uint32_t updates_counter_len = 1000000;
+static const uint32_t pub_interval = 4000;	// limit publish rate to 250 Hz
+
+__EXPORT int position_estimator_inav_main(int argc, char *argv[]);
+
+int position_estimator_inav_thread_main(int argc, char *argv[]);
+
+static void usage(const char *reason);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr,
+		"usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	exit(1);
+}
+
+/**
+ * The position_estimator_inav_thread 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_create().
+ */
+int position_estimator_inav_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+		if (thread_running) {
+			printf("position_estimator_inav already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		verbose_mode = false;
+
+		if (argc > 1)
+			if (!strcmp(argv[2], "-v"))
+				verbose_mode = true;
+
+		thread_should_exit = false;
+		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
+					       SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       position_estimator_inav_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("\tposition_estimator_inav is running\n");
+
+		} else {
+			printf("\tposition_estimator_inav not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+int position_estimator_inav_thread_main(int argc, char *argv[])
+{
+	warnx("started.");
+	int mavlink_fd;
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(mavlink_fd, "[inav] started");
+
+	/* initialize values */
+	float x_est[3] = { 0.0f, 0.0f, 0.0f };
+	float y_est[3] = { 0.0f, 0.0f, 0.0f };
+	float z_est[3] = { 0.0f, 0.0f, 0.0f };
+
+	int baro_init_cnt = 0;
+	int baro_init_num = 200;
+	float baro_alt0 = 0.0f; /* to determine while start up */
+	float alt_avg = 0.0f;
+	bool landed = true;
+	hrt_abstime landed_time = 0;
+	bool flag_armed = false;
+
+	uint32_t accel_counter = 0;
+	uint32_t baro_counter = 0;
+
+	/* declare and safely initialize all structs */
+	struct actuator_controls_effective_s actuator;
+	memset(&actuator, 0, sizeof(actuator));
+	struct actuator_armed_s armed;
+	memset(&armed, 0, sizeof(armed));
+	struct sensor_combined_s sensor;
+	memset(&sensor, 0, sizeof(sensor));
+	struct vehicle_gps_position_s gps;
+	memset(&gps, 0, sizeof(gps));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_local_position_s local_pos;
+	memset(&local_pos, 0, sizeof(local_pos));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+
+	/* subscribe */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
+	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+
+	/* advertise */
+	orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
+	orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+
+	struct position_estimator_inav_params params;
+	struct position_estimator_inav_param_handles pos_inav_param_handles;
+	/* initialize parameter handles */
+	parameters_init(&pos_inav_param_handles);
+
+	/* first parameters read at start up */
+	struct parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_update); /* read from param topic to clear updated flag */
+	/* first parameters update */
+	parameters_update(&pos_inav_param_handles, &params);
+
+	struct pollfd fds_init[1] = {
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+	};
+
+	/* wait for initial baro value */
+	bool wait_baro = true;
+
+	thread_running = true;
+
+	while (wait_baro && !thread_should_exit) {
+		int ret = poll(fds_init, 1, 1000);
+
+		if (ret < 0) {
+			/* poll error */
+			errx(1, "subscriptions poll error on init.");
+
+		} else if (ret > 0) {
+			if (fds_init[0].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (wait_baro && sensor.baro_counter > baro_counter) {
+					baro_counter = sensor.baro_counter;
+
+					/* mean calculation over several measurements */
+					if (baro_init_cnt < baro_init_num) {
+						baro_alt0 += sensor.baro_alt_meter;
+						baro_init_cnt++;
+
+					} else {
+						wait_baro = false;
+						baro_alt0 /= (float) baro_init_cnt;
+						warnx("init baro: alt = %.3f", baro_alt0);
+						mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
+						local_pos.ref_alt = baro_alt0;
+						local_pos.ref_timestamp = hrt_absolute_time();
+						local_pos.z_valid = true;
+						local_pos.v_z_valid = true;
+						local_pos.global_z = true;
+					}
+				}
+			}
+		}
+	}
+
+	bool ref_xy_inited = false;
+	hrt_abstime ref_xy_init_start = 0;
+	const hrt_abstime ref_xy_init_delay = 5000000;	// wait for 5s after 3D fix
+
+	hrt_abstime t_prev = 0;
+
+	uint16_t accel_updates = 0;
+	uint16_t baro_updates = 0;
+	uint16_t gps_updates = 0;
+	uint16_t attitude_updates = 0;
+	uint16_t flow_updates = 0;
+
+	hrt_abstime updates_counter_start = hrt_absolute_time();
+	hrt_abstime pub_last = hrt_absolute_time();
+
+	/* acceleration in NED frame */
+	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
+
+	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
+	float accel_corr[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float accel_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
+	float baro_corr = 0.0f;		// D
+	float gps_corr[2][2] = {
+		{ 0.0f, 0.0f },		// N (pos, vel)
+		{ 0.0f, 0.0f },		// E (pos, vel)
+	};
+	float sonar_corr = 0.0f;
+	float sonar_corr_filtered = 0.0f;
+	float flow_corr[] = { 0.0f, 0.0f };	// X, Y
+
+	float sonar_prev = 0.0f;
+	hrt_abstime sonar_time = 0;
+
+	/* main loop */
+	struct pollfd fds[7] = {
+		{ .fd = parameter_update_sub, .events = POLLIN },
+		{ .fd = actuator_sub, .events = POLLIN },
+		{ .fd = armed_sub, .events = POLLIN },
+		{ .fd = vehicle_attitude_sub, .events = POLLIN },
+		{ .fd = sensor_combined_sub, .events = POLLIN },
+		{ .fd = optical_flow_sub, .events = POLLIN },
+		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
+	};
+
+	if (!thread_should_exit) {
+		warnx("main loop started.");
+	}
+
+	while (!thread_should_exit) {
+		int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		hrt_abstime t = hrt_absolute_time();
+
+		if (ret < 0) {
+			/* poll error */
+			warnx("subscriptions poll error.");
+			thread_should_exit = true;
+			continue;
+
+		} else if (ret > 0) {
+			/* parameter update */
+			if (fds[0].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub,
+					 &update);
+				/* update parameters */
+				parameters_update(&pos_inav_param_handles, &params);
+			}
+
+			/* actuator */
+			if (fds[1].revents & POLLIN) {
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
+			}
+
+			/* armed */
+			if (fds[2].revents & POLLIN) {
+				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+			}
+
+			/* vehicle attitude */
+			if (fds[3].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+				attitude_updates++;
+			}
+
+			/* sensor combined */
+			if (fds[4].revents & POLLIN) {
+				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
+				if (sensor.accelerometer_counter > accel_counter) {
+					if (att.R_valid) {
+						/* correct accel bias, now only for Z */
+						sensor.accelerometer_m_s2[2] -= accel_bias[2];
+
+						/* transform acceleration vector from body frame to NED frame */
+						for (int i = 0; i < 3; i++) {
+							accel_NED[i] = 0.0f;
+
+							for (int j = 0; j < 3; j++) {
+								accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+							}
+						}
+
+						accel_corr[0] = accel_NED[0] - x_est[2];
+						accel_corr[1] = accel_NED[1] - y_est[2];
+						accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+
+					} else {
+						memset(accel_corr, 0, sizeof(accel_corr));
+					}
+
+					accel_counter = sensor.accelerometer_counter;
+					accel_updates++;
+				}
+
+				if (sensor.baro_counter > baro_counter) {
+					baro_corr = - sensor.baro_alt_meter - z_est[0];
+					baro_counter = sensor.baro_counter;
+					baro_updates++;
+				}
+			}
+
+			/* optical flow */
+			if (fds[5].revents & POLLIN) {
+				orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+
+				if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) {
+					if (flow.ground_distance_m != sonar_prev) {
+						sonar_time = t;
+						sonar_prev = flow.ground_distance_m;
+						sonar_corr = -flow.ground_distance_m - z_est[0];
+						sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
+
+						if (fabsf(sonar_corr) > params.sonar_err) {
+							// correction is too large: spike or new ground level?
+							if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
+								// spike detected, ignore
+								sonar_corr = 0.0f;
+
+							} else {
+								// new ground level
+								baro_alt0 += sonar_corr;
+								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
+								local_pos.ref_alt = baro_alt0;
+								local_pos.ref_timestamp = hrt_absolute_time();
+								z_est[0] += sonar_corr;
+								sonar_corr = 0.0f;
+								sonar_corr_filtered = 0.0f;
+							}
+						}
+					}
+
+				} else {
+					sonar_corr = 0.0f;
+				}
+
+				flow_updates++;
+			}
+
+			/* vehicle GPS position */
+			if (fds[6].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
+
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
+					/* initialize reference position if needed */
+					if (!ref_xy_inited) {
+						/* require EPH < 10m */
+						if (gps.eph_m < 10.0f) {
+							if (ref_xy_init_start == 0) {
+								ref_xy_init_start = t;
+
+							} else if (t > ref_xy_init_start + ref_xy_init_delay) {
+								ref_xy_inited = true;
+								/* reference GPS position */
+								double lat = gps.lat * 1e-7;
+								double lon = gps.lon * 1e-7;
+
+								local_pos.ref_lat = gps.lat;
+								local_pos.ref_lon = gps.lon;
+								local_pos.ref_timestamp = t;
+
+								/* initialize projection */
+								map_projection_init(lat, lon);
+								warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
+								mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
+							}
+						} else {
+							ref_xy_init_start = 0;
+						}
+					}
+
+					if (ref_xy_inited) {
+						/* project GPS lat lon to plane */
+						float gps_proj[2];
+						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+						/* calculate correction for position */
+						gps_corr[0][0] = gps_proj[0] - x_est[0];
+						gps_corr[1][0] = gps_proj[1] - y_est[0];
+
+						/* calculate correction for velocity */
+						if (gps.vel_ned_valid) {
+							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
+							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+
+						} else {
+							gps_corr[0][1] = 0.0f;
+							gps_corr[1][1] = 0.0f;
+						}
+					}
+
+				} else {
+					/* no GPS lock */
+					memset(gps_corr, 0, sizeof(gps_corr));
+					ref_xy_init_start = 0;
+				}
+
+				gps_updates++;
+			}
+		}
+
+		/* end of poll return value check */
+
+		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		t_prev = t;
+
+		/* reset ground level on arm */
+		if (armed.armed && !flag_armed) {
+			baro_alt0 -= z_est[0];
+			z_est[0] = 0.0f;
+			local_pos.ref_alt = baro_alt0;
+			local_pos.ref_timestamp = hrt_absolute_time();
+			mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0);
+		}
+
+		/* accel bias correction, now only for Z
+		 * not strictly correct, but stable and works */
+		accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt;
+
+		/* inertial filter prediction for altitude */
+		inertial_filter_predict(dt, z_est);
+
+		/* inertial filter correction for altitude */
+		baro_alt0 += sonar_corr * params.w_alt_sonar * dt;
+		inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro);
+		inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
+		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
+
+		bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
+		bool flow_valid = false;	// TODO implement opt flow
+
+		/* try to estimate xy even if no absolute position source available,
+		 * if using optical flow velocity will be correct in this case */
+		bool can_estimate_xy = gps_valid || flow_valid;
+
+		if (can_estimate_xy) {
+			/* inertial filter prediction for position */
+			inertial_filter_predict(dt, x_est);
+			inertial_filter_predict(dt, y_est);
+
+			/* inertial filter correction for position */
+			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
+			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
+
+			if (gps_valid) {
+				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+
+				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
+					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+				}
+			}
+		}
+
+		/* detect land */
+		alt_avg += (z_est[0] - alt_avg) * dt / params.land_t;
+		float alt_disp = z_est[0] - alt_avg;
+		alt_disp = alt_disp * alt_disp;
+		float land_disp2 = params.land_disp * params.land_disp;
+		/* get actual thrust output */
+		float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
+
+		if (landed) {
+			if (alt_disp > land_disp2 && thrust > params.land_thr) {
+				landed = false;
+				landed_time = 0;
+			}
+
+		} else {
+			if (alt_disp < land_disp2 && thrust < params.land_thr) {
+				if (landed_time == 0) {
+					landed_time = t;    // land detected first time
+
+				} else {
+					if (t > landed_time + params.land_t * 1000000.0f) {
+						landed = true;
+						landed_time = 0;
+					}
+				}
+
+			} else {
+				landed_time = 0;
+			}
+		}
+
+		if (verbose_mode) {
+			/* print updates rate */
+			if (t > updates_counter_start + updates_counter_len) {
+				float updates_dt = (t - updates_counter_start) * 0.000001f;
+				warnx(
+					"updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s",
+					accel_updates / updates_dt,
+					baro_updates / updates_dt,
+					gps_updates / updates_dt,
+					attitude_updates / updates_dt,
+					flow_updates / updates_dt);
+				updates_counter_start = t;
+				accel_updates = 0;
+				baro_updates = 0;
+				gps_updates = 0;
+				attitude_updates = 0;
+				flow_updates = 0;
+			}
+		}
+
+		if (t > pub_last + pub_interval) {
+			pub_last = t;
+			/* publish local position */
+			local_pos.timestamp = t;
+			local_pos.xy_valid = can_estimate_xy && gps_valid;
+			local_pos.v_xy_valid = can_estimate_xy;
+			local_pos.global_xy = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.x = x_est[0];
+			local_pos.vx = x_est[1];
+			local_pos.y = y_est[0];
+			local_pos.vy = y_est[1];
+			local_pos.z = z_est[0];
+			local_pos.vz = z_est[1];
+			local_pos.landed = landed;
+			local_pos.yaw = att.yaw;
+
+			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
+
+			/* publish global position */
+			global_pos.valid = local_pos.global_xy;
+
+			if (local_pos.global_xy) {
+				double est_lat, est_lon;
+				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
+				global_pos.lat = (int32_t)(est_lat * 1e7);
+				global_pos.lon = (int32_t)(est_lon * 1e7);
+				global_pos.time_gps_usec = gps.time_gps_usec;
+			}
+
+			/* set valid values even if position is not valid */
+			if (local_pos.v_xy_valid) {
+				global_pos.vx = local_pos.vx;
+				global_pos.vy = local_pos.vy;
+			}
+
+			if (local_pos.z_valid) {
+				global_pos.relative_alt = -local_pos.z;
+			}
+
+			if (local_pos.global_z) {
+				global_pos.alt = local_pos.ref_alt - local_pos.z;
+			}
+
+			if (local_pos.v_z_valid) {
+				global_pos.vz = local_pos.vz;
+			}
+			global_pos.yaw = local_pos.yaw;
+
+			global_pos.timestamp = t;
+
+			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
+		}
+		flag_armed = armed.armed;
+	}
+
+	warnx("exiting.");
+	mavlink_log_info(mavlink_fd, "[inav] exiting");
+	thread_running = false;
+	return 0;
+}
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
new file mode 100644
index 000000000..4f9ddd009
--- /dev/null
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file position_estimator_inav_params.c
+ *
+ * Parameters for position_estimator_inav
+ */
+
+#include "position_estimator_inav_params.h"
+
+PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
+PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
+PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f);
+PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
+
+int parameters_init(struct position_estimator_inav_param_handles *h)
+{
+	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
+	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
+	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
+	h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
+	h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
+	h->w_pos_acc = param_find("INAV_W_POS_ACC");
+	h->w_pos_flow = param_find("INAV_W_POS_FLOW");
+	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
+	h->flow_k = param_find("INAV_FLOW_K");
+	h->sonar_filt = param_find("INAV_SONAR_FILT");
+	h->sonar_err = param_find("INAV_SONAR_ERR");
+	h->land_t = param_find("INAV_LAND_T");
+	h->land_disp = param_find("INAV_LAND_DISP");
+	h->land_thr = param_find("INAV_LAND_THR");
+
+	return OK;
+}
+
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
+{
+	param_get(h->w_alt_baro, &(p->w_alt_baro));
+	param_get(h->w_alt_acc, &(p->w_alt_acc));
+	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
+	param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
+	param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
+	param_get(h->w_pos_acc, &(p->w_pos_acc));
+	param_get(h->w_pos_flow, &(p->w_pos_flow));
+	param_get(h->w_acc_bias, &(p->w_acc_bias));
+	param_get(h->flow_k, &(p->flow_k));
+	param_get(h->sonar_filt, &(p->sonar_filt));
+	param_get(h->sonar_err, &(p->sonar_err));
+	param_get(h->land_t, &(p->land_t));
+	param_get(h->land_disp, &(p->land_disp));
+	param_get(h->land_thr, &(p->land_thr));
+
+	return OK;
+}
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index df8970d3a..61570aea7 100644
--- a/src/modules/mathlib/math/Dcm.hpp
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
+ *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,78 +32,56 @@
  *
  ****************************************************************************/
 
-/**
- * @file Dcm.hpp
+/*
+ * @file position_estimator_inav_params.h
  *
- * math direction cosine matrix
+ * Parameters for Position Estimator
  */
 
-//#pragma once
-
-#include "Vector.hpp"
-#include "Matrix.hpp"
-
-namespace math
-{
+#include <systemlib/param/param.h>
+
+struct position_estimator_inav_params {
+	float w_alt_baro;
+	float w_alt_acc;
+	float w_alt_sonar;
+	float w_pos_gps_p;
+	float w_pos_gps_v;
+	float w_pos_acc;
+	float w_pos_flow;
+	float w_acc_bias;
+	float flow_k;
+	float sonar_filt;
+	float sonar_err;
+	float land_t;
+	float land_disp;
+	float land_thr;
+};
 
-class Quaternion;
-class EulerAngles;
+struct position_estimator_inav_param_handles {
+	param_t w_alt_baro;
+	param_t w_alt_acc;
+	param_t w_alt_sonar;
+	param_t w_pos_gps_p;
+	param_t w_pos_gps_v;
+	param_t w_pos_acc;
+	param_t w_pos_flow;
+	param_t w_acc_bias;
+	param_t flow_k;
+	param_t sonar_filt;
+	param_t sonar_err;
+	param_t land_t;
+	param_t land_disp;
+	param_t land_thr;
+};
 
 /**
- * This is a Tait Bryan, Body 3-2-1 sequence.
- * (yaw)-(pitch)-(roll)
- * The Dcm transforms a vector in the body frame
- * to the navigation frame, typically represented
- * as C_nb. C_bn can be obtained through use
- * of the transpose() method.
+ * Initialize all parameter handles and values
+ *
  */
-class __EXPORT Dcm : public Matrix
-{
-public:
-	/**
-	 * default ctor
-	 */
-	Dcm();
-
-	/**
-	 * scalar ctor
-	 */
-	Dcm(float c00, float c01, float c02,
-	    float c10, float c11, float c12,
-	    float c20, float c21, float c22);
-
-	/**
-	 * data ctor
-	 */
-	Dcm(const float *data);
-
-	/**
-	 * array ctor
-	 */
-	Dcm(const float data[3][3]);
-
-	/**
-	 * quaternion ctor
-	 */
-	Dcm(const Quaternion &q);
-
-	/**
-	 * euler angles ctor
-	 */
-	Dcm(const EulerAngles &euler);
-
-	/**
-	 * copy ctor (deep)
-	 */
-	Dcm(const Dcm &right);
-
-	/**
-	 * dtor
-	 */
-	virtual ~Dcm();
-};
-
-int __EXPORT dcmTest();
-
-} // math
+int parameters_init(struct position_estimator_inav_param_handles *h);
 
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index fbd82a4c6..796c6cd9f 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -59,14 +59,14 @@ static perf_counter_t c_gather_ppm;
 void
 controls_init(void)
 {
-	/* DSM input */
+	/* DSM input (USART1) */
 	dsm_init("/dev/ttyS0");
 
-	/* S.bus input */
+	/* S.bus input (USART3) */
 	sbus_init("/dev/ttyS2");
 
 	/* default to a 1:1 input map, all enabled */
-	for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+	for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
 
 		r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS]    = 0;
@@ -124,7 +124,7 @@ controls_tick() {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 	perf_end(c_gather_ppm);
 
-	ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
+	ASSERT(r_raw_rc_count <= PX4IO_CONTROL_CHANNELS);
 
 	/*
 	 * In some cases we may have received a frame, but input has still
@@ -197,7 +197,7 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
-				ASSERT(mapped < MAX_CONTROL_CHANNELS);
+				ASSERT(mapped < PX4IO_CONTROL_CHANNELS);
 
 				/* invert channel if pitch - pulling the lever down means pitching up by convention */
 				if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
@@ -209,7 +209,7 @@ controls_tick() {
 		}
 
 		/* set un-assigned controls to zero */
-		for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+		for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
 			if (!(assigned_channels & (1 << i)))
 				r_rc_values[i] = 0;
 		}
@@ -321,8 +321,8 @@ ppm_input(uint16_t *values, uint16_t *num_values)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > MAX_CONTROL_CHANNELS)
-			*num_values = MAX_CONTROL_CHANNELS;
+		if (*num_values > PX4IO_CONTROL_CHANNELS)
+			*num_values = PX4IO_CONTROL_CHANNELS;
 
 		for (unsigned i = 0; i < *num_values; i++)
 			values[i] = ppm_buffer[i];
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index 745cdfa40..206e27db5 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -53,13 +53,13 @@
 #define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
 #define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-static int dsm_fd = -1;				/**<File handle to the DSM UART*/
-static hrt_abstime dsm_last_rx_time;		/**<Timestamp when we last received*/
-static hrt_abstime dsm_last_frame_time;		/**<Timestamp for start of last dsm frame*/
-static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**<DSM dsm frame receive buffer*/
-static unsigned dsm_partial_frame_count;	/**<Count of bytes received for current dsm frame*/
-static unsigned dsm_channel_shift;		/**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
-static unsigned dsm_frame_drops;		/**<Count of incomplete DSM frames*/
+static int dsm_fd = -1;						/**< File handle to the DSM UART */
+static hrt_abstime dsm_last_rx_time;		/**< Timestamp when we last received */
+static hrt_abstime dsm_last_frame_time;		/**< Timestamp for start of last dsm frame */
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**< DSM dsm frame receive buffer */
+static unsigned dsm_partial_frame_count;	/**< Count of bytes received for current dsm frame */
+static unsigned dsm_channel_shift;			/**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */
+static unsigned dsm_frame_drops;			/**< Count of incomplete DSM frames */
 
 /**
  * Attempt to decode a single channel raw channel datum
@@ -249,6 +249,10 @@ dsm_bind(uint16_t cmd, int pulses)
 	if (dsm_fd < 0)
 		return;
 
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	// XXX implement
+	#warning DSM BIND NOT IMPLEMENTED ON PX4IO V2
+#else
 	switch (cmd) {
 
 	case dsm_bind_power_down:
@@ -288,6 +292,7 @@ dsm_bind(uint16_t cmd, int pulses)
 		break;
 
 	}
+#endif
 }
 
 /**
diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 4485daa5b..10aeb5c9f 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -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
@@ -69,6 +69,7 @@ static void		i2c_rx_setup(void);
 static void		i2c_tx_setup(void);
 static void		i2c_rx_complete(void);
 static void		i2c_tx_complete(void);
+static void		i2c_dump(void);
 
 static DMA_HANDLE	rx_dma;
 static DMA_HANDLE	tx_dma;
@@ -92,7 +93,7 @@ enum {
 } direction;
 
 void
-i2c_init(void)
+interface_init(void)
 {
 	debug("i2c init");
 
@@ -148,12 +149,18 @@ i2c_init(void)
 #endif
 }
 
+void
+interface_tick()
+{
+}
+
 
 /*
   reset the I2C bus
   used to recover from lockups
  */
-void i2c_reset(void)
+void
+i2c_reset(void)
 {
 	rCR1 |= I2C_CR1_SWRST;
 	rCR1 = 0;
@@ -330,7 +337,7 @@ i2c_tx_complete(void)
 	i2c_tx_setup();
 }
 
-void
+static void
 i2c_dump(void)
 {
 	debug("CR1   0x%08x  CR2   0x%08x", rCR1,  rCR2);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index a2193b526..deed25836 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -59,6 +59,12 @@ extern "C" {
  */
 #define FMU_INPUT_DROP_LIMIT_US		200000
 
+/*
+ * Time that the ESCs need to initialize
+ */
+ #define ESC_INIT_TIME_US		1000000
+ #define ESC_RAMP_TIME_US		2000000
+
 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */
 #define ROLL     0
 #define PITCH    1
@@ -68,6 +74,17 @@ extern "C" {
 
 /* current servo arm/disarm state */
 static bool mixer_servos_armed = false;
+static bool should_arm = false;
+static bool should_always_enable_pwm = false;
+static uint64_t esc_init_time;
+
+enum esc_state_e {
+	ESC_OFF,
+	ESC_INIT,
+	ESC_RAMP,
+	ESC_ON
+};
+static esc_state_e esc_state;
 
 /* selected control values and count for mixing */
 enum mixer_source {
@@ -98,7 +115,7 @@ mixer_tick(void)
 		if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
 			isr_debug(1, "AP RX timeout");
 		}
-		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK);
 		r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
 
 	} else {
@@ -112,12 +129,11 @@ mixer_tick(void)
 	 * Decide which set of controls we're using.
 	 */
 
-	/* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */
+	/* do not mix if RAW_PWM mode is on and FMU is good */
 	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) &&
-	        !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+	        (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
 
-		/* don't actually mix anything - we already have raw PWM values or
-		 not a valid mixer. */
+		/* don't actually mix anything - we already have raw PWM values */
 		source = MIX_NONE;
 
 	} else {
@@ -132,7 +148,8 @@ mixer_tick(void)
 
 		if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
 		     (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+		     (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) &&
+		     !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) {
 
 		 	/* if allowed, mix from RC inputs directly */
 			source = MIX_OVERRIDE;
@@ -154,7 +171,7 @@ mixer_tick(void)
 	if (source == MIX_FAILSAFE) {
 
 		/* copy failsafe values to the servo outputs */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
 			r_page_servos[i] = r_page_servo_failsafe[i];
 
 			/* safe actuators for FMU feedback */
@@ -164,11 +181,53 @@ mixer_tick(void)
 
 	} else if (source != MIX_NONE) {
 
-		float	outputs[IO_SERVO_COUNT];
+		float	outputs[PX4IO_SERVO_COUNT];
 		unsigned mixed;
 
+		uint16_t ramp_promille;
+
+		/* update esc init state, but only if we are truely armed and not just PWM enabled */
+		if (mixer_servos_armed && should_arm) {
+
+			switch (esc_state) {
+
+				/* after arming, some ESCs need an initalization period, count the time from here */
+				case ESC_OFF:
+					esc_init_time = hrt_absolute_time();
+					esc_state = ESC_INIT;
+				break;
+
+				/* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */
+				case ESC_INIT:
+					if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) {
+						esc_state = ESC_RAMP;
+					}
+				break;
+
+				/* then ramp until the min speed is reached */
+				case ESC_RAMP:
+					if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) {
+						esc_state = ESC_ON;
+					}
+				break;
+
+				case ESC_ON:
+				default:
+
+				break;
+			}
+		} else {
+			esc_state = ESC_OFF;
+		}
+
+		/* do the calculations during the ramp for all at once */
+		if(esc_state == ESC_RAMP) {
+			ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
+		}
+
+
 		/* mix */
-		mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
 		/* scale to PWM and update the servo outputs as required */
 		for (unsigned i = 0; i < mixed; i++) {
@@ -176,11 +235,29 @@ mixer_tick(void)
 			/* save actuator values for FMU readback */
 			r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
 
-			/* scale to servo output */
-			r_page_servos[i] = (outputs[i] * 600.0f) + 1500;
-
+			switch (esc_state) {
+				case ESC_INIT:
+					r_page_servos[i] = (outputs[i] * 600 + 1500);
+				break;
+
+				case ESC_RAMP:
+					r_page_servos[i] = (outputs[i]
+					 * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000
+					 + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000);
+				break;
+
+				case ESC_ON:
+					r_page_servos[i] = (outputs[i]
+					 * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2
+					 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2);
+				break;
+
+				case ESC_OFF:
+				default:
+				break;
+			}
 		}
-		for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 			r_page_servos[i] = 0;
 	}
 
@@ -193,30 +270,46 @@ mixer_tick(void)
 	 * XXX correct behaviour for failsafe may require an additional case
 	 * here.
 	 */
-	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
-		/* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
-		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
-		/* FMU is available or FMU is not available but override is an option */
-		((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
+	should_arm = (
+		/* IO initialised without error */   (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+		/* and IO is armed */ 		  && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+		/* and FMU is armed */ 		  && (
+							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
+		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
+		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+						     )
 	);
 
-	if (should_arm && !mixer_servos_armed) {
+	should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
+						&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK);
+
+	if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) {
 		/* need to arm, but not armed */
 		up_pwm_servo_arm(true);
 		mixer_servos_armed = true;
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED;
+		isr_debug(5, "> PWM enabled");
 
-	} else if (!should_arm && mixer_servos_armed) {
+	} else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) {
 		/* armed but need to disarm */
 		up_pwm_servo_arm(false);
 		mixer_servos_armed = false;
+		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED);
+		isr_debug(5, "> PWM disabled");
+
 	}
 
-	if (mixer_servos_armed) {
+	if (mixer_servos_armed && should_arm) {
 		/* update the servo outputs. */
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
+
+	} else if (mixer_servos_armed && should_always_enable_pwm) {
+		/* set the idle servo outputs. */
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
+			up_pwm_servo_set(i, r_page_servo_idle[i]);
 	}
 }
 
@@ -265,9 +358,8 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	/* do not allow a mixer change while outputs armed */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 		return;
 	}
 
@@ -344,16 +436,17 @@ mixer_set_failsafe()
 	 * Check if a custom failsafe value has been written,
 	 * or if the mixer is not ok and bail out.
 	 */
+
 	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ||
 		!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
 		return;
 
 	/* set failsafe defaults to the values for all inputs = 0 */
-	float	outputs[IO_SERVO_COUNT];
+	float	outputs[PX4IO_SERVO_COUNT];
 	unsigned mixed;
 
 	/* mix */
-	mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+	mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 
 	/* scale to PWM and update the servo outputs as required */
 	for (unsigned i = 0; i < mixed; i++) {
@@ -364,7 +457,7 @@ mixer_set_failsafe()
 	}
 
 	/* disable the rest of the outputs */
-	for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+	for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
 		r_page_servo_failsafe[i] = 0;
 
 }
diff --git a/src/modules/px4iofirmware/module.mk b/src/modules/px4iofirmware/module.mk
index 6379366f4..59f470a94 100644
--- a/src/modules/px4iofirmware/module.mk
+++ b/src/modules/px4iofirmware/module.mk
@@ -3,17 +3,22 @@
 SRCS		= adc.c \
 		  controls.c \
 		  dsm.c \
-		  i2c.c \
 		  px4io.c \
 		  registers.c \
 		  safety.c \
 		  sbus.c \
 		  ../systemlib/up_cxxinitialize.c \
-		  ../systemlib/hx_stream.c \
 		  ../systemlib/perf_counter.c \
 		  mixer.cpp \
 		  ../systemlib/mixer/mixer.cpp \
 		  ../systemlib/mixer/mixer_group.cpp \
 		  ../systemlib/mixer/mixer_multirotor.cpp \
 		  ../systemlib/mixer/mixer_simple.cpp \
-		  
\ No newline at end of file
+
+ifeq ($(BOARD),px4io-v1)
+SRCS		+= i2c.c
+endif
+ifeq ($(BOARD),px4io-v2)
+SRCS		+= serial.c \
+		   ../systemlib/hx_stream.c
+endif
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 88d8cc87c..f5fa0ba75 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -36,7 +36,7 @@
 /**
  * @file protocol.h
  *
- * PX4IO I2C interface protocol.
+ * PX4IO interface protocol.
  *
  * Communication is performed via writes to and reads from 16-bit virtual
  * registers organised into pages of 255 registers each.
@@ -45,7 +45,7 @@
  * respectively. Subsequent reads and writes increment the offset within
  * the page. 
  *
- * Most pages are readable or writable but not both.
+ * Some pages are read- or write-only.
  *
  * Note that some pages may permit offset values greater than 255, which
  * can only be achieved by long writes. The offset does not wrap.
@@ -62,12 +62,11 @@
  * Note that the implementation of readable pages prefers registers within
  * readable pages to be densely packed. Page numbers do not need to be
  * packed.
+ *
+ * Definitions marked 1 are only valid on PX4IOv1 boards. Likewise, 
+ * [2] denotes definitions specific to the PX4IOv2 board.
  */
 
-#define PX4IO_CONTROL_CHANNELS			8
-#define PX4IO_INPUT_CHANNELS			12
-#define PX4IO_RELAY_CHANNELS			4
-
 /* Per C, this is safe for all 2's complement systems */
 #define REG_TO_SIGNED(_reg)	((int16_t)(_reg))
 #define SIGNED_TO_REG(_signed)	((uint16_t)(_signed))
@@ -75,10 +74,12 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
+#define PX4IO_PROTOCOL_VERSION		4
+
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0
-#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* magic numbers TBD */
-#define PX4IO_P_CONFIG_SOFTWARE_VERSION		1	/* magic numbers TBD */
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION		0	/* PX4IO_PROTOCOL_VERSION */
+#define PX4IO_P_CONFIG_HARDWARE_VERSION		1	/* magic numbers TBD */
 #define PX4IO_P_CONFIG_BOOTLOADER_VERSION	2	/* get this how? */
 #define PX4IO_P_CONFIG_MAX_TRANSFER		3	/* maximum I2C transfer size */
 #define PX4IO_P_CONFIG_CONTROL_COUNT		4	/* hardcoded max control count supported */
@@ -93,7 +94,7 @@
 #define PX4IO_P_STATUS_CPULOAD			1
 
 #define PX4IO_P_STATUS_FLAGS			2	 /* monitoring flags */
-#define PX4IO_P_STATUS_FLAGS_ARMED		(1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED	(1 << 0) /* arm-ok and locally armed */
 #define PX4IO_P_STATUS_FLAGS_OVERRIDE		(1 << 1) /* in manual override */
 #define PX4IO_P_STATUS_FLAGS_RC_OK		(1 << 2) /* RC input is valid */
 #define PX4IO_P_STATUS_FLAGS_RC_PPM		(1 << 3) /* PPM input is valid */
@@ -105,19 +106,24 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
-#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
+#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
-#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
 #define PX4IO_P_STATUS_ALARMS_TEMPERATURE	(1 << 1) /* board temperature is high */
-#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* servo current limit was exceeded */
-#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT	(1 << 2) /* [1] servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT	(1 << 3) /* [1] accessory current limit was exceeded */
 #define PX4IO_P_STATUS_ALARMS_FMU_LOST		(1 << 4) /* timed out waiting for controls from FMU */
 #define PX4IO_P_STATUS_ALARMS_RC_LOST		(1 << 5) /* timed out waiting for RC input */
 #define PX4IO_P_STATUS_ALARMS_PWM_ERROR		(1 << 6) /* PWM configuration or output was bad */
+#define PX4IO_P_STATUS_ALARMS_VSERVO_FAULT	(1 << 7) /* [2] VServo was out of the valid range (2.5 - 5.5 V) */
 
-#define PX4IO_P_STATUS_VBATT			4	/* battery voltage in mV */
-#define PX4IO_P_STATUS_IBATT			5	/* battery current (raw ADC) */
+#define PX4IO_P_STATUS_VBATT			4	/* [1] battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT			5	/* [1] battery current (raw ADC) */
+#define PX4IO_P_STATUS_VSERVO			6	/* [2] servo rail voltage in mV */
+#define PX4IO_P_STATUS_VRSSI			7	/* [2] RSSI voltage */
+#define PX4IO_P_STATUS_PRSSI			8	/* [2] RSSI PWM value */
 
 /* array of post-mix actuator outputs, -10000..10000 */
 #define PX4IO_PAGE_ACTUATORS		2		/* 0..CONFIG_ACTUATOR_COUNT-1 */
@@ -143,7 +149,7 @@
 #define PX4IO_RATE_MAP_BASE			0	/* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
 
 /* setup page */
-#define PX4IO_PAGE_SETUP			100
+#define PX4IO_PAGE_SETUP		50
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
@@ -152,37 +158,40 @@
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
 #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM	(1 << 3) /* use custom failsafe values, not 0 values of mixer */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
+#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
+#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
-#define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
 
-/* px4io relay bit definitions */
-#define PX4IO_RELAY1	(1<<0)
-#define PX4IO_RELAY2	(1<<1)
-#define PX4IO_ACC1		(1<<2)
-#define PX4IO_ACC2		(1<<3)
+#define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_RELAYS_POWER1		(1<<0)	/* hardware rev [1] power relay 1 */
+#define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
+#define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
+#define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
 
-#define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
-#define PX4IO_P_SETUP_DSM				7	/* DSM bind state */
-enum {                                      /* DSM bind states */
+#define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_VSERVO_SCALE		6	/* hardware rev [2] servo voltage correction factor (float) */
+#define PX4IO_P_SETUP_DSM			7	/* DSM bind state */
+enum {							/* DSM bind states */
 	dsm_bind_power_down = 0,
 	dsm_bind_power_up,
 	dsm_bind_set_rx_out,
 	dsm_bind_send_pulses,
 	dsm_bind_reinit_uart
 };
+ 					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			101	/* 0..CONFIG_CONTROL_COUNT */
+#define PX4IO_PAGE_CONTROLS		51		/* 0..CONFIG_CONTROL_COUNT */
 
 /* raw text load to the mixer parser - ignores offset */
-#define PX4IO_PAGE_MIXERLOAD			102
+#define PX4IO_PAGE_MIXERLOAD		52
 
 /* R/C channel config */
-#define PX4IO_PAGE_RC_CONFIG			103	/* R/C input configuration */
+#define PX4IO_PAGE_RC_CONFIG		53		/* R/C input configuration */
 #define PX4IO_P_RC_CONFIG_MIN			0	/* lowest input value */
 #define PX4IO_P_RC_CONFIG_CENTER		1	/* center input value */
 #define PX4IO_P_RC_CONFIG_MAX			2	/* highest input value */
@@ -194,10 +203,23 @@ enum {                                      /* DSM bind states */
 #define PX4IO_P_RC_CONFIG_STRIDE		6	/* spacing between channel config data */
 
 /* PWM output - overrides mixer */
-#define PX4IO_PAGE_DIRECT_PWM			104	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_DIRECT_PWM		54		/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /* PWM failsafe values - zero disables the output */
-#define PX4IO_PAGE_FAILSAFE_PWM			105	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+#define PX4IO_PAGE_FAILSAFE_PWM		55		/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* Debug and test page - not used in normal operation */
+#define PX4IO_PAGE_TEST			127
+#define PX4IO_P_TEST_LED			0	/* set the amber LED on/off */
+
+/* PWM minimum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MIN_PWM		106	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM maximum values for certain ESCs */
+#define PX4IO_PAGE_CONTROL_MAX_PWM		107	/* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM idle values that are active, even when SAFETY_SAFE */
+#define PX4IO_PAGE_IDLE_PWM		108	/* 0..CONFIG_ACTUATOR_COUNT-1 */
 
 /**
  * As-needed mixer data upload.
@@ -218,3 +240,81 @@ struct px4io_mixdata {
 };
 #pragma pack(pop)
 
+/**
+ * Serial protocol encapsulation.
+ */
+
+#define PKT_MAX_REGS	32 // by agreement w/FMU
+
+#pragma pack(push, 1)
+struct IOPacket {
+	uint8_t 	count_code;
+	uint8_t 	crc;
+	uint8_t 	page;
+	uint8_t 	offset;
+	uint16_t	regs[PKT_MAX_REGS];
+};
+#pragma pack(pop)
+
+#define PKT_CODE_READ		0x00	/* FMU->IO read transaction */
+#define PKT_CODE_WRITE		0x40	/* FMU->IO write transaction */
+#define PKT_CODE_SUCCESS	0x00	/* IO->FMU success reply */
+#define PKT_CODE_CORRUPT	0x40	/* IO->FMU bad packet reply */
+#define PKT_CODE_ERROR		0x80	/* IO->FMU register op error reply */
+
+#define PKT_CODE_MASK		0xc0
+#define PKT_COUNT_MASK		0x3f
+
+#define PKT_COUNT(_p)	((_p).count_code & PKT_COUNT_MASK)
+#define PKT_CODE(_p)	((_p).count_code & PKT_CODE_MASK)
+#define PKT_SIZE(_p)	((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))
+
+static const uint8_t crc8_tab[256] __attribute__((unused)) =
+{
+	0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
+	0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
+	0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
+	0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
+	0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
+	0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
+	0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
+	0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
+	0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
+	0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
+	0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
+	0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
+	0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
+	0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
+	0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
+	0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
+	0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
+	0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
+	0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
+	0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
+	0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
+	0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
+	0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
+	0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
+	0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
+	0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
+	0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
+	0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
+	0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
+	0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
+	0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
+	0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3
+};
+
+static uint8_t crc_packet(struct IOPacket *pkt) __attribute__((unused));
+static uint8_t
+crc_packet(struct IOPacket *pkt)
+{
+	uint8_t *end = (uint8_t *)(&pkt->regs[PKT_COUNT(*pkt)]);
+	uint8_t *p = (uint8_t *)pkt;
+	uint8_t c = 0;
+
+	while (p < end)
+		c = crc8_tab[c ^ *(p++)];
+
+	return c;
+}
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index bc8dfc116..e70b3fe88 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -64,11 +64,6 @@ struct sys_state_s 	system_state;
 
 static struct hrt_call serial_dma_call;
 
-#ifdef CONFIG_STM32_I2C1
-/* store i2c reset count XXX this should be a register, together with other error counters */
-volatile uint32_t i2c_loop_resets = 0;
-#endif
-
 /*
  * a set of debug buffers to allow us to send debug information from ISRs
  */
@@ -147,8 +142,10 @@ user_start(int argc, char *argv[])
 	LED_BLUE(false);
 	LED_SAFETY(false);
 
-	/* turn on servo power */
+	/* turn on servo power (if supported) */
+#ifdef POWER_SERVO
 	POWER_SERVO(true);
+#endif
 
 	/* start the safety switch handler */
 	safety_init();
@@ -159,10 +156,11 @@ user_start(int argc, char *argv[])
 	/* initialise the control inputs */
 	controls_init();
 
-#ifdef CONFIG_STM32_I2C1
-	/* start the i2c handler */
-	i2c_init();
-#endif
+	/* start the FMU interface */
+	interface_init();
+
+	/* add a performance counter for the interface */
+	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
 
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
@@ -205,6 +203,11 @@ user_start(int argc, char *argv[])
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);
 
+		/* kick the interface */
+		perf_begin(interface_perf);
+		interface_tick();
+		perf_end(interface_perf);
+
 		/* kick the mixer */
 		perf_begin(mixer_perf);
 		mixer_tick();
@@ -223,12 +226,11 @@ user_start(int argc, char *argv[])
 
 			struct mallinfo minfo = mallinfo();
 
-			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", 
+			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", 
 				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
 				  (unsigned)r_status_flags,
 				  (unsigned)r_setup_arming,
 				  (unsigned)r_setup_features,
-				  (unsigned)i2c_loop_resets,
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 57cffcc23..dea67043e 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -42,15 +42,16 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-#include <drivers/boards/px4io/px4io_internal.h>
+#include <board_config.h>
 
 #include "protocol.h"
 
 /*
  * Constants and limits.
  */
-#define MAX_CONTROL_CHANNELS	12
-#define IO_SERVO_COUNT		8
+#define PX4IO_SERVO_COUNT		8
+#define PX4IO_CONTROL_CHANNELS		8
+#define PX4IO_INPUT_CHANNELS		12
 
 /*
  * Debug logging
@@ -77,6 +78,9 @@ extern volatile uint16_t	r_page_setup[];		/* PX4IO_PAGE_SETUP */
 extern volatile uint16_t	r_page_controls[];	/* PX4IO_PAGE_CONTROLS */
 extern uint16_t			r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
 extern uint16_t			r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
+extern uint16_t			r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */
+extern uint16_t			r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */
+extern uint16_t			r_page_servo_idle[];	/* PX4IO_PAGE_IDLE_PWM */
 
 /*
  * Register aliases.
@@ -119,32 +123,43 @@ extern struct sys_state_s system_state;
 /*
  * GPIO handling.
  */
-#define LED_BLUE(_s)		stm32_gpiowrite(GPIO_LED1, !(_s))
-#define LED_AMBER(_s)		stm32_gpiowrite(GPIO_LED2, !(_s))
-#define LED_SAFETY(_s)		stm32_gpiowrite(GPIO_LED3, !(_s))
+#define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
+#define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
+#define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
+
+# define PX4IO_RELAY_CHANNELS		4
+# define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
+# define POWER_ACC1(_s)			stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
+# define POWER_ACC2(_s)			stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
+# define POWER_RELAY1(_s)		stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
+# define POWER_RELAY2(_s)		stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+# define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
+# define OVERCURRENT_SERVO		(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VBATT			4
+# define ADC_IN5			5
 
-#define POWER_SERVO(_s)		stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s))
-#ifdef GPIO_ACC1_PWR_EN
-    #define POWER_ACC1(_s)		stm32_gpiowrite(GPIO_ACC1_PWR_EN, (_s))
-#endif
-#ifdef GPIO_ACC2_PWR_EN
-    #define POWER_ACC2(_s)		stm32_gpiowrite(GPIO_ACC2_PWR_EN, (_s))
-#endif
-#ifdef GPIO_RELAY1_EN
-    #define POWER_RELAY1(_s)	stm32_gpiowrite(GPIO_RELAY1_EN, (_s))
 #endif
-#ifdef GPIO_RELAY2_EN
-    #define POWER_RELAY2(_s)	stm32_gpiowrite(GPIO_RELAY2_EN, (_s))
+
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+
+# define PX4IO_RELAY_CHANNELS		0
+# define POWER_SPEKTRUM(_s)		stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, (_s))
+
+# define VDD_SERVO_FAULT		(!stm32_gpioread(GPIO_SERVO_FAULT_DETECT))
+
+# define PX4IO_ADC_CHANNEL_COUNT	2
+# define ADC_VSERVO			4
+# define ADC_RSSI			5
+
 #endif
 
-#define OVERCURRENT_ACC		(!stm32_gpioread(GPIO_ACC_OC_DETECT))
-#define OVERCURRENT_SERVO	(!stm32_gpioread(GPIO_SERVO_OC_DETECT))
 #define BUTTON_SAFETY		stm32_gpioread(GPIO_BTN_SAFETY)
 
-#define ADC_VBATT		4
-#define ADC_IN5			5
-#define ADC_CHANNEL_COUNT	2
-
 /*
  * Mixer
  */
@@ -156,17 +171,16 @@ extern void	mixer_handle_text(const void *buffer, size_t length);
  */
 extern void	safety_init(void);
 
-#ifdef CONFIG_STM32_I2C1
 /**
  * FMU communications
  */
-extern void	i2c_init(void);
-#endif
+extern void	interface_init(void);
+extern void	interface_tick(void);
 
 /**
  * Register space
  */
-extern void	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int	registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
 extern int	registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
 
 /**
@@ -191,10 +205,5 @@ extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
 
-/* send a debug message to the console */
+/** send a debug message to the console */
 extern void	isr_debug(uint8_t level, const char *fmt, ...);
-
-#ifdef CONFIG_STM32_I2C1
-void		i2c_dump(void);
-void		i2c_reset(void);
-#endif
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a922362b6..9c95fd1c5 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -44,6 +44,7 @@
 #include <string.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
 
 #include "px4io.h"
 #include "protocol.h"
@@ -57,14 +58,18 @@ static void	pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt
  * Static configuration parameters.
  */
 static const uint16_t	r_page_config[] = {
-	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= 1,	/* XXX hardcoded magic number */
-	[PX4IO_P_CONFIG_SOFTWARE_VERSION]	= 1,	/* XXX hardcoded magic number */
+	[PX4IO_P_CONFIG_PROTOCOL_VERSION]	= PX4IO_PROTOCOL_VERSION,
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V2
+	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 2,
+#else
+	[PX4IO_P_CONFIG_HARDWARE_VERSION]	= 1,
+#endif
 	[PX4IO_P_CONFIG_BOOTLOADER_VERSION]	= 3,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_MAX_TRANSFER]		= 64,	/* XXX hardcoded magic number */
 	[PX4IO_P_CONFIG_CONTROL_COUNT]		= PX4IO_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= IO_SERVO_COUNT,
-	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= MAX_CONTROL_CHANNELS,
-	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= ADC_CHANNEL_COUNT,
+	[PX4IO_P_CONFIG_ACTUATOR_COUNT]		= PX4IO_SERVO_COUNT,
+	[PX4IO_P_CONFIG_RC_INPUT_COUNT]		= PX4IO_CONTROL_CHANNELS,
+	[PX4IO_P_CONFIG_ADC_INPUT_COUNT]	= PX4IO_ADC_CHANNEL_COUNT,
 	[PX4IO_P_CONFIG_RELAY_COUNT]		= PX4IO_RELAY_CHANNELS,
 };
 
@@ -79,7 +84,10 @@ uint16_t		r_page_status[] = {
 	[PX4IO_P_STATUS_FLAGS]			= 0,
 	[PX4IO_P_STATUS_ALARMS]			= 0,
 	[PX4IO_P_STATUS_VBATT]			= 0,
-	[PX4IO_P_STATUS_IBATT]			= 0
+	[PX4IO_P_STATUS_IBATT]			= 0,
+	[PX4IO_P_STATUS_VSERVO]			= 0,
+	[PX4IO_P_STATUS_VRSSI]			= 0,
+	[PX4IO_P_STATUS_PRSSI]			= 0
 };
 
 /**
@@ -87,14 +95,14 @@ uint16_t		r_page_status[] = {
  *
  * Post-mixed actuator values.
  */
-uint16_t 		r_page_actuators[IO_SERVO_COUNT];
+uint16_t 		r_page_actuators[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 3
  *
  * Servo PWM values
  */
-uint16_t		r_page_servos[IO_SERVO_COUNT];
+uint16_t		r_page_servos[PX4IO_SERVO_COUNT];
 
 /**
  * PAGE 4
@@ -104,7 +112,7 @@ uint16_t		r_page_servos[IO_SERVO_COUNT];
 uint16_t		r_page_raw_rc_input[] =
 {
 	[PX4IO_P_RAW_RC_COUNT]			= 0,
-	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -114,7 +122,7 @@ uint16_t		r_page_raw_rc_input[] =
  */
 uint16_t		r_page_rc_input[] = {
 	[PX4IO_P_RC_VALID]			= 0,
-	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+	[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + PX4IO_CONTROL_CHANNELS)] = 0
 };
 
 /**
@@ -138,7 +146,11 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_DEFAULTRATE]		= 50,
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#ifdef ADC_VSERVO
+	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
+#else
 	[PX4IO_P_SETUP_VBATT_SCALE]		= 10000,
+#endif
 	[PX4IO_P_SETUP_SET_DEBUG]		= 0,
 };
 
@@ -146,8 +158,11 @@ volatile uint16_t	r_page_setup[] =
 #define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
 					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
-					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
-#define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK | \
+					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
+					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
+					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)
+#define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
 /**
@@ -166,7 +181,7 @@ volatile uint16_t	r_page_controls[PX4IO_CONTROL_CHANNELS];
  *
  * R/C channel input configuration.
  */
-uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+uint16_t		r_page_rc_input_config[PX4IO_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
 
 /* valid options */
 #define PX4IO_P_RC_CONFIG_OPTIONS_VALID	(PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
@@ -182,9 +197,33 @@ uint16_t		r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE
  * 
  * Disable pulses as default.
  */
-uint16_t		r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 };
+uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0 };
 
-void
+/**
+ * PAGE 106
+ *
+ * minimum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+/**
+ * PAGE 107
+ *
+ * maximum PWM values when armed
+ *
+ */
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 };
+
+/**
+ * PAGE 108
+ *
+ * idle PWM values for difficult ESCs
+ *
+ */
+uint16_t		r_page_servo_idle[PX4IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 };
+
+int
 registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
 {
 
@@ -233,7 +272,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_FAILSAFE_PWM:
 
 		/* copy channel data */
-		while ((offset < IO_SERVO_COUNT) && (num_values > 0)) {
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			r_page_servo_failsafe[offset] = *values;
@@ -247,6 +286,75 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		}
 		break;
 
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_min[offset] = 900;
+
+			else if (*values > 1200)
+				r_page_servo_control_min[offset] = 1200;
+			else if (*values < 900)
+				r_page_servo_control_min[offset] = 900;
+			else
+				r_page_servo_control_min[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+	
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_control_max[offset] = 2100;
+
+			else if (*values > 2100)
+				r_page_servo_control_max[offset] = 2100;
+			else if (*values < 1800)
+				r_page_servo_control_max[offset] = 1800;
+			else
+				r_page_servo_control_max[offset] = *values;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
+	case PX4IO_PAGE_IDLE_PWM:
+
+		/* copy channel data */
+		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+
+			if (*values == 0)
+				/* set to default */
+				r_page_servo_idle[offset] = 0;
+
+			else if (*values < 900)
+				r_page_servo_idle[offset] = 900;
+			else if (*values > 2100)
+				r_page_servo_idle[offset] = 2100;
+			else
+				r_page_servo_idle[offset] = *values;
+
+			/* flag the failsafe values as custom */
+			r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE;
+
+			offset++;
+			num_values--;
+			values++;
+		}
+		break;
+
 		/* handle text going to the mixer parser */
 	case PX4IO_PAGE_MIXERLOAD:
 		mixer_handle_text(values, num_values * sizeof(*values));
@@ -260,11 +368,13 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 		/* iterate individual registers, set each in turn */
 		while (num_values--) {
 			if (registers_set_one(page, offset, *values))
-				break;
+				return -1;
 			offset++;
 			values++;
 		}
+		break;
 	}
+	return 0;
 }
 
 static int
@@ -317,8 +427,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
-				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+
+			// XXX do not reset IO's safety state by FMU for now
+			// if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+			// 	r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			// }
+
+			if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) {
+				r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 			}
 
 			r_setup_arming = value;
@@ -349,10 +465,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-			POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
-			POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
-			POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
-			POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
+#ifdef POWER_RELAY1
+			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
+#endif
+#ifdef POWER_RELAY2
+			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
+#endif
+#ifdef POWER_ACC1
+			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
+#endif
+#ifdef POWER_ACC2
+			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
+#endif
+			break;
+
+		case PX4IO_P_SETUP_VBATT_SCALE:
+			r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value;
 			break;
 
 		case PX4IO_P_SETUP_SET_DEBUG:
@@ -371,9 +499,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 
 	case PX4IO_PAGE_RC_CONFIG: {
 
-		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
-		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+		/**
+		 * do not allow a RC config change while outputs armed
+		 */
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
+			(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ||
+			(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 			break;
 		}
 
@@ -381,7 +512,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
 		uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
 
-		if (channel >= MAX_CONTROL_CHANNELS)
+		if (channel >= PX4IO_CONTROL_CHANNELS)
 			return -1;
 
 		/* disable the channel until we have a chance to sanity-check it */
@@ -401,6 +532,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
 
+			/* clear any existing RC disabled flag */
+			r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED);
+
 			/* set all options except the enabled option */
 			conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
 
@@ -426,7 +560,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 				if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
 					count++;
 				}
-				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+				if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_CONTROL_CHANNELS) {
 					count++;
 				}
 
@@ -446,6 +580,14 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		/* case PX4IO_RC_PAGE_CONFIG */
 	}
 
+	case PX4IO_PAGE_TEST:
+		switch (offset) {
+		case PX4IO_P_TEST_LED:
+			LED_AMBER(value & 1);
+			break;
+		}
+		break;
+
 	default:
 		return -1;
 	}
@@ -482,6 +624,7 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 
 		/* PX4IO_P_STATUS_ALARMS maintained externally */
 
+#ifdef ADC_VBATT
 		/* PX4IO_P_STATUS_VBATT */
 		{
 			/*
@@ -515,7 +658,8 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
 			}
 		}
-
+#endif
+#ifdef ADC_IBATT
 		/* PX4IO_P_STATUS_IBATT */
 		{
 			/*
@@ -525,26 +669,62 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 			  FMU sort it out, with user selectable
 			  configuration for their sensor
 			 */
-			unsigned counts = adc_measure(ADC_IN5);
+			unsigned counts = adc_measure(ADC_IBATT);
 			if (counts != 0xffff) {
 				r_page_status[PX4IO_P_STATUS_IBATT] = counts;
 			}
 		}
+#endif
+#ifdef ADC_VSERVO
+		/* PX4IO_P_STATUS_VSERVO */
+		{
+			/*
+			 * Coefficients here derived by measurement of the 5-16V
+			 * range on one unit:
+			 *
+			 * XXX pending measurements
+			 *
+			 * slope = xxx
+			 * intercept = xxx
+			 *
+			 * Intercept corrected for best results @ 5.0V.
+			 */
+			unsigned counts = adc_measure(ADC_VSERVO);
+			if (counts != 0xffff) {
+				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
+
+				r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
+			}
+		}
+#endif
+		/* XXX PX4IO_P_STATUS_VRSSI */
+		/* XXX PX4IO_P_STATUS_PRSSI */
 
 		SELECT_PAGE(r_page_status);
 		break;
 
 	case PX4IO_PAGE_RAW_ADC_INPUT:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
+#ifdef ADC_VBATT
 		r_page_scratch[0] = adc_measure(ADC_VBATT);
-		r_page_scratch[1] = adc_measure(ADC_IN5);
-
+#endif
+#ifdef ADC_IBATT
+		r_page_scratch[1] = adc_measure(ADC_IBATT);
+#endif
+
+#ifdef ADC_VSERVO
+		r_page_scratch[0] = adc_measure(ADC_VSERVO);
+#endif
+#ifdef ADC_RSSI
+		r_page_scratch[1] = adc_measure(ADC_RSSI);
+#endif
 		SELECT_PAGE(r_page_scratch);
 		break;
 
 	case PX4IO_PAGE_PWM_INFO:
 		memset(r_page_scratch, 0, sizeof(r_page_scratch));
-		for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
 
 		SELECT_PAGE(r_page_scratch);
@@ -587,6 +767,15 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 	case PX4IO_PAGE_FAILSAFE_PWM:
 		SELECT_PAGE(r_page_servo_failsafe);
 		break;
+	case PX4IO_PAGE_CONTROL_MIN_PWM:
+		SELECT_PAGE(r_page_servo_control_min);
+		break;
+	case PX4IO_PAGE_CONTROL_MAX_PWM:
+		SELECT_PAGE(r_page_servo_control_max);
+		break;
+	case PX4IO_PAGE_IDLE_PWM:
+		SELECT_PAGE(r_page_servo_idle);
+		break;
 
 	default:
 		return -1;
@@ -616,7 +805,7 @@ static void
 pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
 {
 	for (unsigned pass = 0; pass < 2; pass++) {
-		for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
+		for (unsigned group = 0; group < PX4IO_SERVO_COUNT; group++) {
 
 			/* get the channel mask for this rate group */
 			uint32_t mask = up_pwm_servo_get_rate_group(group);
diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c
index 4dbecc274..95335f038 100644
--- a/src/modules/px4iofirmware/safety.c
+++ b/src/modules/px4iofirmware/safety.c
@@ -110,7 +110,7 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) &&
 		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -118,18 +118,18 @@ safety_check_button(void *arg)
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* switch to armed state */
-			r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
-	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
 
 		} else if (counter == ARM_COUNTER_THRESHOLD) {
 			/* change to disarmed state and notify the FMU */
-			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
 			counter++;
 		}
 
@@ -140,7 +140,7 @@ safety_check_button(void *arg)
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
 	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
-	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+	if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
 		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
new file mode 100644
index 000000000..94d7407df
--- /dev/null
+++ b/src/modules/px4iofirmware/serial.c
@@ -0,0 +1,352 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012,2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file serial.c
+ *
+ * Serial communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+#include <unistd.h>
+#include <termios.h>
+#include <fcntl.h>
+#include <string.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+
+/* XXX might be able to prune these */
+#include <chip.h>
+#include <up_internal.h>
+#include <up_arch.h>
+#include <stm32.h>
+#include <systemlib/perf_counter.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+static perf_counter_t	pc_txns;
+static perf_counter_t	pc_errors;
+static perf_counter_t	pc_ore;
+static perf_counter_t	pc_fe;
+static perf_counter_t	pc_ne;
+static perf_counter_t	pc_idle;
+static perf_counter_t	pc_badidle;
+static perf_counter_t	pc_regerr;
+static perf_counter_t	pc_crcerr;
+
+static void		rx_handle_packet(void);
+static void		rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg);
+static DMA_HANDLE	tx_dma;
+static DMA_HANDLE	rx_dma;
+
+static int		serial_interrupt(int irq, void *context);
+static void		dma_reset(void);
+
+/* if we spend this many ticks idle, reset the DMA */
+static unsigned		idle_ticks;
+
+static struct IOPacket	dma_packet;
+
+/* serial register accessors */
+#define REG(_x)		(*(volatile uint32_t *)(PX4FMU_SERIAL_BASE + _x))
+#define rSR		REG(STM32_USART_SR_OFFSET)
+#define rDR		REG(STM32_USART_DR_OFFSET)
+#define rBRR		REG(STM32_USART_BRR_OFFSET)
+#define rCR1		REG(STM32_USART_CR1_OFFSET)
+#define rCR2		REG(STM32_USART_CR2_OFFSET)
+#define rCR3		REG(STM32_USART_CR3_OFFSET)
+#define rGTPR		REG(STM32_USART_GTPR_OFFSET)
+
+void
+interface_init(void)
+{
+	pc_txns = perf_alloc(PC_ELAPSED, "txns");
+	pc_errors = perf_alloc(PC_COUNT, "errors");
+	pc_ore = perf_alloc(PC_COUNT, "overrun");
+	pc_fe = perf_alloc(PC_COUNT, "framing");
+	pc_ne = perf_alloc(PC_COUNT, "noise");
+	pc_idle = perf_alloc(PC_COUNT, "idle");
+	pc_badidle = perf_alloc(PC_COUNT, "badidle");
+	pc_regerr = perf_alloc(PC_COUNT, "regerr");
+	pc_crcerr = perf_alloc(PC_COUNT, "crcerr");
+
+	/* allocate DMA */
+	tx_dma = stm32_dmachannel(PX4FMU_SERIAL_TX_DMA);
+	rx_dma = stm32_dmachannel(PX4FMU_SERIAL_RX_DMA);
+
+	/* configure pins for serial use */
+	stm32_configgpio(PX4FMU_SERIAL_TX_GPIO);
+	stm32_configgpio(PX4FMU_SERIAL_RX_GPIO);
+
+	/* reset and configure the UART */
+	rCR1 = 0;
+	rCR2 = 0;
+	rCR3 = 0;
+
+	/* clear status/errors */
+	(void)rSR;
+	(void)rDR;
+
+	/* configure line speed */
+	uint32_t usartdiv32 = PX4FMU_SERIAL_CLOCK / (PX4FMU_SERIAL_BITRATE / 2);
+	uint32_t mantissa = usartdiv32 >> 5;
+	uint32_t fraction = (usartdiv32 - (mantissa << 5) + 1) >> 1;
+	rBRR = (mantissa << USART_BRR_MANT_SHIFT) | (fraction << USART_BRR_FRAC_SHIFT);
+
+	/* connect our interrupt */
+	irq_attach(PX4FMU_SERIAL_VECTOR, serial_interrupt);
+	up_enable_irq(PX4FMU_SERIAL_VECTOR);
+
+	/* enable UART and error/idle interrupts */
+	rCR3 = USART_CR3_EIE;
+	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
+
+#if 0	/* keep this for signal integrity testing */
+	for (;;) {
+		while (!(rSR & USART_SR_TXE))
+			;
+		rDR = 0xfa;
+		while (!(rSR & USART_SR_TXE))
+			;
+		rDR = 0xa0;
+	}
+#endif
+
+	/* configure RX DMA and return to listening state */
+	dma_reset();
+
+	debug("serial init");
+}
+
+void
+interface_tick()
+{
+	/* XXX look for stuck/damaged DMA and reset? */
+	if (idle_ticks++ > 100) {
+		dma_reset();
+		idle_ticks = 0;
+	}
+}
+
+static void
+rx_handle_packet(void)
+{
+	/* check packet CRC */
+	uint8_t crc = dma_packet.crc;
+	dma_packet.crc = 0;
+	if (crc != crc_packet(&dma_packet)) {
+		perf_count(pc_crcerr);
+
+		/* send a CRC error reply */
+		dma_packet.count_code = PKT_CODE_CORRUPT;
+		dma_packet.page = 0xff;
+		dma_packet.offset = 0xff;
+
+		return;
+	}
+
+	if (PKT_CODE(dma_packet) == PKT_CODE_WRITE) {
+
+		/* it's a blind write - pass it on */
+		if (registers_set(dma_packet.page, dma_packet.offset, &dma_packet.regs[0], PKT_COUNT(dma_packet))) {
+			perf_count(pc_regerr);
+			dma_packet.count_code = PKT_CODE_ERROR;
+		} else {
+			dma_packet.count_code = PKT_CODE_SUCCESS;
+		}
+		return;
+	} 
+
+	if (PKT_CODE(dma_packet) == PKT_CODE_READ) {
+
+		/* it's a read - get register pointer for reply */
+		unsigned count;
+		uint16_t *registers;
+
+		if (registers_get(dma_packet.page, dma_packet.offset, &registers, &count) < 0) {
+			perf_count(pc_regerr);
+			dma_packet.count_code = PKT_CODE_ERROR;
+		} else {
+			/* constrain reply to requested size */
+			if (count > PKT_MAX_REGS)
+				count = PKT_MAX_REGS;
+			if (count > PKT_COUNT(dma_packet))
+				count = PKT_COUNT(dma_packet);
+
+			/* copy reply registers into DMA buffer */
+			memcpy((void *)&dma_packet.regs[0], registers, count * 2);
+			dma_packet.count_code = count | PKT_CODE_SUCCESS;
+		}
+		return;
+	}
+
+	/* send a bad-packet error reply */
+	dma_packet.count_code = PKT_CODE_CORRUPT;
+	dma_packet.page = 0xff;
+	dma_packet.offset = 0xfe;
+}
+
+static void
+rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
+{
+	/*
+	 * We are here because DMA completed, or UART reception stopped and
+	 * we think we have a packet in the buffer.
+	 */
+	perf_begin(pc_txns);
+
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
+	/* reset the idle counter */
+	idle_ticks = 0;
+
+	/* handle the received packet */
+	rx_handle_packet();
+
+	/* re-set DMA for reception first, so we are ready to receive before we start sending */
+	dma_reset();
+
+	/* send the reply to the just-processed request */
+	dma_packet.crc = 0;
+	dma_packet.crc = crc_packet(&dma_packet);
+	stm32_dmasetup(
+		tx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		PKT_SIZE(dma_packet),
+		DMA_CCR_DIR		|
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS);
+	stm32_dmastart(tx_dma, NULL, NULL, false);
+	rCR3 |= USART_CR3_DMAT;
+
+	perf_end(pc_txns);
+}
+
+static int
+serial_interrupt(int irq, void *context)
+{
+	static bool abort_on_idle = false;
+
+	uint32_t sr = rSR;	/* get UART status register */
+	(void)rDR;		/* required to clear any of the interrupt status that brought us here */
+
+	if (sr & (USART_SR_ORE |	/* overrun error - packet was too big for DMA or DMA was too slow */
+		USART_SR_NE |		/* noise error - we have lost a byte due to noise */
+		USART_SR_FE)) {		/* framing error - start/stop bit lost or line break */
+
+		perf_count(pc_errors);
+		if (sr & USART_SR_ORE)
+			perf_count(pc_ore);
+		if (sr & USART_SR_NE)
+			perf_count(pc_ne);
+		if (sr & USART_SR_FE)
+			perf_count(pc_fe);
+
+		/* send a line break - this will abort transmission/reception on the other end */
+		rCR1 |= USART_CR1_SBK;
+
+		/* when the line goes idle, abort rather than look at the packet */
+		abort_on_idle = true;
+	}
+
+	if (sr & USART_SR_IDLE) {
+
+		/* 
+		 * If we saw an error, don't bother looking at the packet - it should have
+		 * been aborted by the sender and will definitely be bad. Get the DMA reconfigured
+		 * ready for their retry.
+		 */
+		if (abort_on_idle) {
+
+			abort_on_idle = false;
+			dma_reset();
+			return 0;
+		}
+
+		/*
+		 * The sender has stopped sending - this is probably the end of a packet.
+		 * Check the received length against the length in the header to see if 
+		 * we have something that looks like a packet.
+		 */
+		unsigned length = sizeof(dma_packet) - stm32_dmaresidual(rx_dma);
+		if ((length < 1) || (length < PKT_SIZE(dma_packet))) {
+
+			/* it was too short - possibly truncated */
+			perf_count(pc_badidle);
+			return 0;
+		}
+
+		/*
+		 * Looks like we received a packet. Stop the DMA and go process the
+		 * packet.
+		 */
+		perf_count(pc_idle);
+		stm32_dmastop(rx_dma);
+		rx_dma_callback(rx_dma, DMA_STATUS_TCIF, NULL);
+	}
+
+	return 0;
+}
+
+static void
+dma_reset(void)
+{
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+	(void)rSR;
+	(void)rDR;
+	(void)rDR;
+
+	/* kill any pending DMA */
+	stm32_dmastop(tx_dma);
+	stm32_dmastop(rx_dma);
+
+	/* reset the RX side */
+	stm32_dmasetup(
+		rx_dma,
+		(uint32_t)&rDR,
+		(uint32_t)&dma_packet,
+		sizeof(dma_packet),
+		DMA_CCR_MINC		|
+		DMA_CCR_PSIZE_8BITS	|
+		DMA_CCR_MSIZE_8BITS);
+
+	/* start receive DMA ready for the next packet */
+	stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);
+	rCR3 |= USART_CR3_DMAR;
+}
+
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ba7cdd91c..e83fb7dd3 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -75,6 +75,7 @@
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
+#include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -94,7 +95,6 @@
 		log_msgs_written++; \
 	} else { \
 		log_msgs_skipped++; \
-		/*printf("skip\n");*/ \
 	}
 
 #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \
@@ -102,9 +102,6 @@
 	fds[fdsc_count].events = POLLIN; \
 	fdsc_count++;
 
-
-//#define SDLOG2_DEBUG
-
 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 */
@@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("sdlog2 already running\n");
+			warnx("already running");
 			/* this is not an error */
 			exit(0);
 		}
@@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 		if (!thread_running) {
-			printf("\tsdlog2 is not started\n");
+			warnx("not started");
 		}
 
 		main_thread_should_exit = true;
@@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[])
 			sdlog2_status();
 
 		} else {
-			printf("\tsdlog2 not started\n");
+			warnx("not started\n");
 		}
 
 		exit(0);
@@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg)
 		/* only get pointer to thread-safe data, do heavy I/O a few lines down */
 		int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part);
 
-#ifdef SDLOG2_DEBUG
-		int rp = logbuf->read_ptr;
-		int wp = logbuf->write_ptr;
-#endif
-
 		/* continue */
 		pthread_mutex_unlock(&logbuffer_mutex);
 
@@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg)
 			n = write(log_file, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;
-#ifdef SDLOG2_DEBUG
-			printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait);
-#endif
 
 			if (n < 0) {
 				main_thread_should_exit = true;
@@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg)
 
 		} else {
 			n = 0;
-#ifdef SDLOG2_DEBUG
-			printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit);
-#endif
 			/* exit only with empty buffer */
 			if (main_thread_should_exit || logwriter_should_exit) {
-#ifdef SDLOG2_DEBUG
-				printf("break logwriter thread\n");
-#endif
 				break;
 			}
 			should_wait = true;
@@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg)
 	fsync(log_file);
 	close(log_file);
 
-#ifdef SDLOG2_DEBUG
-	printf("logwriter thread exit\n");
-#endif
-
 	return OK;
 }
 
@@ -604,15 +583,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
-	const char *converter_in = "/etc/logging/conv.zip";
-	char* converter_out = malloc(150);
-	sprintf(converter_out, "%s/conv.zip", folder_path);
-
-	if (file_copy(converter_in, converter_out)) {
-		errx(1, "unable to copy conversion scripts, exiting.");
-	}
-	free(converter_out);
-
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -623,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "can't allocate log buffer, exiting.");
 	}
 
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 19;
-	/* Sanity check variable and index */
-	ssize_t fdsc_count = 0;
-	/* file descriptors to wait for */
-	struct pollfd fds[fdsc];
-
 	struct vehicle_status_s buf_status;
 	memset(&buf_status, 0, sizeof(buf_status));
 
@@ -655,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
 		struct esc_status_s esc;
+		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -678,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int rc_sub;
 		int airspeed_sub;
 		int esc_sub;
+		int global_vel_sp_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -703,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_GPOS_s log_GPOS;
 			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
+			struct log_GVSP_s log_GVSP;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -710,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 #pragma pack(pop)
 	memset(&log_msg.body, 0, sizeof(log_msg.body));
 
+	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+	/* number of messages */
+	const ssize_t fdsc = 20;
+	/* Sanity check variable and index */
+	ssize_t fdsc_count = 0;
+	/* file descriptors to wait for */
+	struct pollfd fds[fdsc];
+
 	/* --- VEHICLE COMMAND --- */
 	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
 	fds[fdsc_count].fd = subs.cmd_sub;
@@ -824,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- GLOBAL VELOCITY SETPOINT --- */
+	subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint));
+	fds[fdsc_count].fd = subs.global_vel_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -911,15 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_STAT_MSG;
-				log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine;
-				log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode;
-				log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode;
-				log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode;
-				log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed;
-				log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery;
-				log_msg.body.log_STAT.battery_current = buf_status.current_battery;
+				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
+				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
+				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
+				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
+				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
-				log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
+				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
+				log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
 				LOGBUFFER_WRITE_AND_COUNT(STAT);
 			}
 
@@ -1065,10 +1043,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.vx = buf.local_pos.vx;
 				log_msg.body.log_LPOS.vy = buf.local_pos.vy;
 				log_msg.body.log_LPOS.vz = buf.local_pos.vz;
-				log_msg.body.log_LPOS.hdg = buf.local_pos.hdg;
-				log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat;
-				log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon;
-				log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
+				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
+				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
+				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
 
@@ -1175,14 +1152,18 @@ int sdlog2_thread_main(int argc, char *argv[])
 				}
 			}
 
-#ifdef SDLOG2_DEBUG
-				printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
+			/* --- GLOBAL VELOCITY SETPOINT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp);
+				log_msg.msg_type = LOG_GVSP_MSG;
+				log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx;
+				log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy;
+				log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz;
+				LOGBUFFER_WRITE_AND_COUNT(GVSP);
+			}
+
 			/* signal the other thread new data, but not yet unlock */
 			if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) {
-#ifdef SDLOG2_DEBUG
-				printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
-#endif
 				/* only request write if several packets can be written at once */
 				pthread_cond_signal(&logbuffer_cond);
 			}
@@ -1202,6 +1183,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 	pthread_mutex_destroy(&logbuffer_mutex);
 	pthread_cond_destroy(&logbuffer_cond);
 
+	free(lb.data);
+
 	warnx("exiting.");
 
 	thread_running = false;
@@ -1265,7 +1248,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return OK;
+	return ret;
 }
 
 void handle_command(struct vehicle_command_s *cmd)
@@ -1297,8 +1280,10 @@ void handle_command(struct vehicle_command_s *cmd)
 
 void handle_status(struct vehicle_status_s *status)
 {
-	if (status->flag_system_armed != flag_system_armed) {
-		flag_system_armed = status->flag_system_armed;
+	// TODO use flag from actuator_armed here?
+	bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
+	if (armed != flag_system_armed) {
+		flag_system_armed = armed;
 
 		if (flag_system_armed) {
 			sdlog2_start_log();
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 934e4dec8..4eeb65a87 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -106,10 +106,9 @@ struct log_LPOS_s {
 	float vx;
 	float vy;
 	float vz;
-	float hdg;
-	int32_t home_lat;
-	int32_t home_lon;
-	float home_alt;
+	int32_t ref_lat;
+	int32_t ref_lon;
+	float ref_alt;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -149,15 +148,14 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
-	uint8_t state;
-	uint8_t flight_mode;
-	uint8_t manual_control_mode;
-	uint8_t manual_sas_mode;
-	uint8_t armed;
+	uint8_t main_state;
+	uint8_t navigation_state;
+	uint8_t arming_state;
 	float battery_voltage;
 	float battery_current;
 	float battery_remaining;
 	uint8_t battery_warning;
+	uint8_t landed;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -244,6 +242,14 @@ struct log_ESC_s {
 	uint16_t esc_setpoint_raw;
 };
 
+/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */
+#define LOG_GVSP_MSG 19
+struct log_GVSP_s {
+	float vx;
+	float vy;
+	float vz;
+};
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -254,11 +260,11 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
+	LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
 	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(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"),
+	LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
@@ -266,7 +272,8 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 6c8e514b6..992abf2cc 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -68,7 +68,11 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 1667);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
+
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
 
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
@@ -154,36 +158,42 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
-PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_FLOAT(RC15_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC15_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
+
+
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
 
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#else
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 /* PX4IOAR: 0.00838095238 */
 /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
+#endif
 
 PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
 PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
 
-PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5);
-PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5);
+PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6);
+PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0);
-PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera yaw / azimuth */
 PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera pitch / tilt */
-PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera trigger */
-PARAM_DEFINE_INT32(RC_MAP_AUX4, 0);	/**< default function: camera roll */
-PARAM_DEFINE_INT32(RC_MAP_AUX5, 0);	/**< default function: payload drop */
 
-PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f);
-PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f);
+PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
+PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
+PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 42268b971..e98c4d548 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -74,7 +75,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/differential_pressure.h>
@@ -138,6 +139,77 @@
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+    ROTATION_NONE                = 0,
+    ROTATION_YAW_45              = 1,
+    ROTATION_YAW_90              = 2,
+    ROTATION_YAW_135             = 3,
+    ROTATION_YAW_180             = 4,
+    ROTATION_YAW_225             = 5,
+    ROTATION_YAW_270             = 6,
+    ROTATION_YAW_315             = 7,
+    ROTATION_ROLL_180            = 8,
+    ROTATION_ROLL_180_YAW_45     = 9,
+    ROTATION_ROLL_180_YAW_90     = 10,
+    ROTATION_ROLL_180_YAW_135    = 11,
+    ROTATION_PITCH_180           = 12,
+    ROTATION_ROLL_180_YAW_225    = 13,
+    ROTATION_ROLL_180_YAW_270    = 14,
+    ROTATION_ROLL_180_YAW_315    = 15,
+    ROTATION_ROLL_90             = 16,
+    ROTATION_ROLL_90_YAW_45      = 17,
+    ROTATION_ROLL_90_YAW_90      = 18,
+    ROTATION_ROLL_90_YAW_135     = 19,
+    ROTATION_ROLL_270            = 20,
+    ROTATION_ROLL_270_YAW_45     = 21,
+    ROTATION_ROLL_270_YAW_90     = 22,
+    ROTATION_ROLL_270_YAW_135    = 23,
+    ROTATION_PITCH_90            = 24,
+    ROTATION_PITCH_270           = 25,
+    ROTATION_MAX
+};
+
+typedef struct
+{
+    uint16_t roll;
+    uint16_t pitch;
+    uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+    {  0,   0,   0 },
+    {  0,   0,  45 },
+    {  0,   0,  90 },
+    {  0,   0, 135 },
+    {  0,   0, 180 },
+    {  0,   0, 225 },
+    {  0,   0, 270 },
+    {  0,   0, 315 },
+    {180,   0,   0 },
+    {180,   0,  45 },
+    {180,   0,  90 },
+    {180,   0, 135 },
+    {  0, 180,   0 },
+    {180,   0, 225 },
+    {180,   0, 270 },
+    {180,   0, 315 },
+    { 90,   0,   0 },
+    { 90,   0,  45 },
+    { 90,   0,  90 },
+    { 90,   0, 135 },
+    {270,   0,   0 },
+    {270,   0,  45 },
+    {270,   0,  90 },
+    {270,   0, 135 },
+    {  0,  90,   0 },
+    {  0, 270,   0 }
+};
+
+/**
  * Sensor app start / stop handling function
  *
  * @ingroup apps
@@ -189,9 +261,9 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
+	int		_vcontrol_mode_sub;			/**< vehicle control mode subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
 
@@ -210,13 +282,16 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
+	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	bool		_mag_is_external;		/**< true if the active mag is on an external board */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -226,21 +301,22 @@ private:
 		float accel_offset[3];
 		float accel_scale[3];
 		float diff_pres_offset_pa;
+		float diff_pres_analog_enabled;
 
-		int rc_type;
+		int board_rotation;
+		int external_mag_rotation;
 
 		int rc_map_roll;
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
 
-		int rc_map_manual_override_sw;
-		int rc_map_auto_mode_sw;
+		int rc_map_mode_sw;
+		int rc_map_return_sw;
+		int rc_map_assisted_sw;
+		int rc_map_mission_sw;
 
-		int rc_map_manual_mode_sw;
-		int rc_map_sas_mode_sw;
-		int rc_map_rtl_sw;
-		int rc_map_offboard_ctrl_mode_sw;
+//		int rc_map_offboard_ctrl_mode_sw;
 
 		int rc_map_flaps;
 
@@ -265,10 +341,6 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
-		param_t rc_type;
-
-		param_t rc_demix;
 
 		param_t gyro_offset[3];
 		param_t gyro_scale[3];
@@ -277,19 +349,19 @@ private:
 		param_t mag_offset[3];
 		param_t mag_scale[3];
 		param_t diff_pres_offset_pa;
+		param_t diff_pres_analog_enabled;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
 
-		param_t rc_map_manual_override_sw;
-		param_t rc_map_auto_mode_sw;
+		param_t rc_map_mode_sw;
+		param_t rc_map_return_sw;
+		param_t rc_map_assisted_sw;
+		param_t rc_map_mission_sw;
 
-		param_t rc_map_manual_mode_sw;
-		param_t rc_map_sas_mode_sw;
-		param_t rc_map_rtl_sw;
-		param_t rc_map_offboard_ctrl_mode_sw;
+//		param_t rc_map_offboard_ctrl_mode_sw;
 
 		param_t rc_map_flaps;
 
@@ -306,6 +378,9 @@ private:
 
 		param_t battery_voltage_scaling;
 
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -315,6 +390,11 @@ private:
 	int		parameters_update();
 
 	/**
+	 * Get the rotation matrices
+	 */
+	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+	/**
 	 * Do accel-related initialisation.
 	 */
 	void		accel_init();
@@ -380,9 +460,9 @@ private:
 	void		diff_pres_poll(struct sensor_combined_s &raw);
 
 	/**
-	 * Check for changes in vehicle status.
+	 * Check for changes in vehicle control mode.
 	 */
-	void		vehicle_status_poll();
+	void		vehicle_control_mode_poll();
 
 	/**
 	 * Check for changes in parameters.
@@ -437,7 +517,7 @@ Sensors::Sensors() :
 	_mag_sub(-1),
 	_rc_sub(-1),
 	_baro_sub(-1),
-	_vstatus_sub(-1),
+	_vcontrol_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
 
@@ -450,7 +530,11 @@ Sensors::Sensors() :
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+	_board_rotation(3,3),
+	_external_mag_rotation(3,3),
+	_mag_is_external(false)
 {
 
 	/* basic r/c parameters */
@@ -479,8 +563,6 @@ Sensors::Sensors() :
 
 	}
 
-	_parameter_handles.rc_type = param_find("RC_TYPE");
-
 	/* mandatory input switched, mapped to channels 1-4 per default */
 	_parameter_handles.rc_map_roll 	= param_find("RC_MAP_ROLL");
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
@@ -488,16 +570,16 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
-	_parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW");
-	_parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
+	_parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW");
 
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW");
-	_parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW");
-	_parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW");
-	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
+	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+
+//	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -537,9 +619,14 @@ Sensors::Sensors() :
 
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
+	_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -573,7 +660,9 @@ int
 Sensors::parameters_update()
 {
 	bool rc_valid = true;
-
+    float tmpScaleFactor = 0.0f;
+    float tmpRevFactor = 0.0f;
+    
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
@@ -583,29 +672,27 @@ Sensors::parameters_update()
 		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
 		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
 
-		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
-
+		tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
+        
 		/* handle blowup in the scaling factor calculation */
-		if (!isfinite(_parameters.scaling_factor[i]) ||
-		    _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
-		    _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
-
+		if (!isfinite(tmpScaleFactor) ||
+		    (tmpRevFactor < 0.000001f) ||
+		    (tmpRevFactor > 0.2f) ) {
+			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
 			/* scaling factors do not make sense, lock them down */
-			_parameters.scaling_factor[i] = 0;
+			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
 		}
-
+        else {
+            _parameters.scaling_factor[i] = tmpScaleFactor;
+        }
 	}
 
 	/* handle wrong values */
 	if (!rc_valid)
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
 
-	/* remote control type */
-	if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) {
-		warnx("Failed getting remote control type");
-	}
-
 	/* channel mapping */
 	if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) {
 		warnx("Failed getting roll chan index");
@@ -623,54 +710,35 @@ Sensors::parameters_update()
 		warnx("Failed getting throttle chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) {
-		warnx("Failed getting override sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) {
-		warnx("Failed getting auto mode sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
-		warnx("Failed getting flaps chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) {
-		warnx("Failed getting manual mode sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) {
-		warnx("Failed getting rtl sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) {
-		warnx("Failed getting sas mode sw chan index");
-	}
-
-	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
-		warnx("Failed getting offboard control mode sw chan index");
+	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
+		warnx("Failed getting mode sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) {
-		warnx("Failed getting mode aux 1 index");
+	if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) {
+		warnx("Failed getting return sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) {
-		warnx("Failed getting mode aux 2 index");
+	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+		warnx("Failed getting assisted sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) {
-		warnx("Failed getting mode aux 3 index");
+	if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) {
+		warnx("Failed getting mission sw chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) {
-		warnx("Failed getting mode aux 4 index");
+	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+		warnx("Failed getting flaps chan index");
 	}
 
-	if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) {
-		warnx("Failed getting mode aux 5 index");
-	}
+//	if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
+//		warnx("Failed getting offboard control mode sw chan index");
+//	}
 
+	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
+	param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
+	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
+	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
+	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
 	param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll));
 	param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch));
 	param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw));
@@ -682,15 +750,14 @@ Sensors::parameters_update()
 	_rc.function[PITCH] = _parameters.rc_map_pitch - 1;
 	_rc.function[YAW] = _parameters.rc_map_yaw - 1;
 
-	_rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1;
-	_rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1;
+	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
+	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
+	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
+	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
-	_rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1;
-	_rc.function[RTL] = _parameters.rc_map_rtl_sw - 1;
-	_rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1;
-	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
+//	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
 
 	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
 	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
@@ -725,16 +792,41 @@ Sensors::parameters_update()
 
 	/* Airspeed offset */
 	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
+	param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
 		warnx("Failed updating voltage scaling param");
 	}
 
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
 void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3,3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		(*rot_matrix)(i,j) = R(i, j);
+}
+
+void
 Sensors::accel_init()
 {
 	int	fd;
@@ -757,7 +849,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#else
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -765,6 +857,9 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
+		#else
+			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
 		#endif
 
 		warnx("using system accel");
@@ -799,11 +894,11 @@ Sensors::gyro_init()
 
 		#else
 
-		/* set the gyro internal sampling rate up to at leat 800Hz */
-		ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		/* set the gyro internal sampling rate up to at least 760Hz */
+		ioctl(fd, GYROIOCSSAMPLERATE, 760);
 
-		/* set the driver to poll at 800Hz */
-		ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		/* set the driver to poll at 760Hz */
+		ioctl(fd, SENSORIOCSPOLLRATE, 760);
 
 		#endif
 
@@ -816,6 +911,7 @@ void
 Sensors::mag_init()
 {
 	int	fd;
+	int	ret;
 
 	fd = open(MAG_DEVICE_PATH, 0);
 
@@ -824,11 +920,33 @@ Sensors::mag_init()
 		errx(1, "FATAL: no magnetometer found");
 	}
 
-	/* set the mag internal poll rate to at least 150Hz */
-	ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	/* try different mag sampling rates */
 
-	/* set the driver to poll at 150Hz */
-	ioctl(fd, SENSORIOCSPOLLRATE, 150);
+
+	ret = ioctl(fd, MAGIOCSSAMPLERATE, 150);
+	if (ret == OK) {
+		/* set the pollrate accordingly */
+		ioctl(fd, SENSORIOCSPOLLRATE, 150);
+	} else {
+		ret = ioctl(fd, MAGIOCSSAMPLERATE, 100);
+		/* if the slower sampling rate still fails, something is wrong */
+		if (ret == OK) {
+			/* set the driver to poll also at the slower rate */
+			ioctl(fd, SENSORIOCSPOLLRATE, 100);
+		} else {
+			errx(1, "FATAL: mag sampling rate could not be set");
+		}
+	}
+
+	
+
+	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
+	if (ret < 0)
+		errx(1, "FATAL: unknown if magnetometer is external or onboard");
+	else if (ret == 1)
+		_mag_is_external = true;
+	else
+		_mag_is_external = false;
 
 	close(fd);
 }
@@ -842,7 +960,7 @@ Sensors::baro_init()
 
 	if (fd < 0) {
 		warn("%s", BARO_DEVICE_PATH);
-		warnx("No barometer found, ignoring");
+		errx(1, "FATAL: No barometer found");
 	}
 
 	/* set the driver to poll at 150Hz */
@@ -874,9 +992,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
-		raw.accelerometer_m_s2[1] = accel_report.y;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		vect = _board_rotation*vect;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -897,9 +1018,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		vect = _board_rotation*vect;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -920,9 +1044,16 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
-		raw.magnetometer_ga[1] = mag_report.y;
-		raw.magnetometer_ga[2] = mag_report.z;
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+
+		if (_mag_is_external)
+			vect = _external_mag_rotation*vect;
+		else
+			vect = _board_rotation*vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;
@@ -977,21 +1108,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
 }
 
 void
-Sensors::vehicle_status_poll()
+Sensors::vehicle_control_mode_poll()
 {
-	struct vehicle_status_s vstatus;
-	bool vstatus_updated;
+	struct vehicle_control_mode_s vcontrol_mode;
+	bool vcontrol_mode_updated;
 
-	/* Check HIL state if vehicle status has changed */
-	orb_check(_vstatus_sub, &vstatus_updated);
+	/* Check HIL state if vehicle control mode has changed */
+	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
 
-	if (vstatus_updated) {
+	if (vcontrol_mode_updated) {
 
-		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus);
+		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
 
 		/* switching from non-HIL to HIL mode */
 		//printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled);
-		if (vstatus.flag_hil_enabled && !_hil_enabled) {
+		if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) {
 			_hil_enabled = true;
 			_publishing = false;
 
@@ -1144,9 +1275,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
-					 * a valid voltage from a connected sensor
+					 * a valid voltage from a connected sensor. Also assume a non-
+					 * zero offset from the sensor if its connected.
 					 */
-					if (voltage > 0.4f) {
+					if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
 
 						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
 
@@ -1194,10 +1326,11 @@ Sensors::ppm_poll()
 		manual_control.yaw = NAN;
 		manual_control.throttle = NAN;
 
-		manual_control.manual_mode_switch = NAN;
-		manual_control.manual_sas_switch = NAN;
-		manual_control.return_to_launch_switch = NAN;
-		manual_control.auto_offboard_input_switch = NAN;
+		manual_control.mode_switch = NAN;
+		manual_control.return_switch = NAN;
+		manual_control.assisted_switch = NAN;
+		manual_control.mission_switch = NAN;
+//		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
 		manual_control.aux1 = NAN;
@@ -1297,11 +1430,17 @@ Sensors::ppm_poll()
 			manual_control.yaw *= _parameters.rc_scale_yaw;
 		}
 
-		/* override switch input */
-		manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled);
-
 		/* mode switch input */
-		manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled);
+		manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
+
+		/* land switch input */
+		manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+
+		/* assisted switch input */
+		manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+
+		/* mission switch input */
+		manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 
 		/* flaps */
 		if (_rc.function[FLAPS] >= 0) {
@@ -1313,21 +1452,17 @@ Sensors::ppm_poll()
 			}
 		}
 
-		if (_rc.function[MANUAL_MODE] >= 0) {
-			manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled);
-		}
-
-		if (_rc.function[SAS_MODE] >= 0) {
-			manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled);
+		if (_rc.function[MODE] >= 0) {
+			manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
 		}
 
-		if (_rc.function[RTL] >= 0) {
-			manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled);
+		if (_rc.function[MISSION] >= 0) {
+			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
-		if (_rc.function[OFFBOARD_MODE] >= 0) {
-			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
-		}
+//		if (_rc.function[OFFBOARD_MODE] >= 0) {
+//			manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
+//		}
 
 		/* aux functions, only assign if valid mapping is present */
 		if (_rc.function[AUX_1] >= 0) {
@@ -1400,12 +1535,12 @@ Sensors::task_main()
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
 	/* rate limit vehicle status updates to 5Hz */
-	orb_set_interval(_vstatus_sub, 200);
+	orb_set_interval(_vcontrol_mode_sub, 200);
 
 	/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
 	orb_set_interval(_gyro_sub, 4);
@@ -1461,7 +1596,7 @@ Sensors::task_main()
 		perf_begin(_loop_perf);
 
 		/* check vehicle status for changes to publication state */
-		vehicle_status_poll();
+		vehicle_control_mode_poll();
 
 		/* check parameters for updates */
 		parameter_update_poll();
diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index e01cc4dda..310fbf60f 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -42,7 +42,7 @@
 
 #include <stdio.h>
 #include <math.h>
-#include "conversions.h"
+#include <geo/geo.h>
 #include "airspeed.h"
 
 
@@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
 float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
 	float density = get_air_density(static_pressure, temperature_celsius);
+
 	if (density < 0.0001f || !isfinite(density)) {
-	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-//	 printf("[airspeed] Invalid air density, using density at sea level\n");
+		density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
 
-	if(pressure_difference > 0) {
+	if (pressure_difference > 0) {
 		return sqrtf((2.0f*(pressure_difference)) / density);
-	} else
-	{
+	} else {
 		return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
 	}
 }
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1..8dccaab9c 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
@@ -85,6 +85,14 @@
   */
  __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
 
+ /**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5..9105d83cb 100644
--- a/src/modules/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
@@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[])
 
 	return u.w;
 }
-
-void rot2quat(const float R[9], float Q[4])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-	int32_t idx;
-
-	/* conversion of rotation matrix to quaternion
-	 * choose the largest component to begin with */
-	q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
-	q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
-	q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
-	q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
-
-	idx = 0;
-
-	if (q0_2 < q1_2) {
-		q0_2 = q1_2;
-
-		idx = 1;
-	}
-
-	if (q0_2 < q2_2) {
-		q0_2 = q2_2;
-		idx = 2;
-	}
-
-	if (q0_2 < q3_2) {
-		q0_2 = q3_2;
-		idx = 3;
-	}
-
-	q0_2 = sqrtf(q0_2);
-
-	/* solve for the remaining three components */
-	if (idx == 0) {
-		q1_2 = q0_2;
-		q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
-
-	} else if (idx == 1) {
-		q2_2 = q0_2;
-		q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
-
-	} else if (idx == 2) {
-		q3_2 = q0_2;
-		q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
-
-	} else {
-		q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
-		q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
-		q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
-	}
-
-	/* return values */
-	Q[0] = q1_2;
-	Q[1] = q2_2;
-	Q[2] = q3_2;
-	Q[3] = q0_2;
-}
-
-void quat2rot(const float Q[4], float R[9])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-
-	memset(&R[0], 0, 9U * sizeof(float));
-
-	q0_2 = Q[0] * Q[0];
-	q1_2 = Q[1] * Q[1];
-	q2_2 = Q[2] * Q[2];
-	q3_2 = Q[3] * Q[3];
-
-	R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
-	R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
-	R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
-	R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
-	R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
-	R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
-	R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
-	R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
-	R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
-}
-
-float get_air_density(float static_pressure, float temperature_celsius)
-{
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-}
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 064426f21..dc383e770 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,7 +43,6 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
@@ -57,34 +56,6 @@ __BEGIN_DECLS
  */
 __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
 
-/**
- * Converts a 3 x 3 rotation matrix to an unit quaternion.
- *
- * All orientations are expressed in NED frame.
- *
- * @param R rotation matrix to convert
- * @param Q quaternion to write back to
- */
-__EXPORT void rot2quat(const float R[9], float Q[4]);
-
-/**
- * Converts an unit quaternion to a 3 x 3 rotation matrix.
- *
- * All orientations are expressed in NED frame.
- *
- * @param Q quaternion to convert
- * @param R rotation matrix to write back to
- */
-__EXPORT void quat2rot(const float Q[4], float R[9]);
-
-/**
- * Calculates air density.
- *
- * @param static_pressure ambient pressure in millibar
- * @param temperature_celcius air / ambient temperature in celcius
- */
-__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
-
 __END_DECLS
 
 #endif /* CONVERSIONS_H_ */
diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c
deleted file mode 100644
index 6463e6489..000000000
--- a/src/modules/systemlib/geo/geo.c
+++ /dev/null
@@ -1,438 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 geo.c
- *
- * Geo / math functions to perform geodesic calculations
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <systemlib/geo/geo.h>
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <pthread.h>
-#include <stdio.h>
-#include <math.h>
-#include <stdbool.h>
-
-
-/* values for map projection */
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-{
-	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	phi_1 = lat_0 / 180.0 * M_PI;
-	lambda_0 = lon_0 / 180.0 * M_PI;
-
-	sin_phi_1 = sin(phi_1);
-	cos_phi_1 = cos(phi_1);
-
-	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
-	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
-
-	double lat1 = phi_1;
-	double lon1 = lambda_0;
-
-	double lat2 = phi_1 + 0.5 / 180 * M_PI;
-	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
-	double sin_lat_2 = sin(lat2);
-	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
-
-	/* 2) calculate distance rho on plane */
-	double k_bar = 0;
-	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
-	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
-	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
-	scale = d / rho;
-
-}
-
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	double phi = lat / 180.0 * M_PI;
-	double lambda = lon / 180.0 * M_PI;
-
-	double sin_phi = sin(phi);
-	double cos_phi = cos(phi);
-
-	double k_bar = 0;
-	/* using small angle approximation (formula in comment is without aproximation) */
-	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	/* using small angle approximation (formula in comment is without aproximation) */
-	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
-	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
-
-//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
-}
-
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-
-	double x_descaled = x / scale;
-	double y_descaled = y / scale;
-
-	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
-	double sin_c = sin(c);
-	double cos_c = cos(c);
-
-	double lat_sphere = 0;
-
-	if (c != 0)
-		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
-	else
-		lat_sphere = asin(cos_c * sin_phi_1);
-
-//	printf("lat_sphere = %.10f\n",lat_sphere);
-
-	double lon_sphere = 0;
-
-	if (phi_1  == M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
-
-	} else if (phi_1 == -M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
-
-	} else {
-
-		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
-		//using small angle approximation
-//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-//    	if(denominator != 0)
-//    	{
-//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-//    	}
-//    	else
-//    	{
-//    	...
-//    	}
-	}
-
-//	printf("lon_sphere = %.10f\n",lon_sphere);
-
-	*lat = lat_sphere * 180.0 / M_PI;
-	*lon = lon_sphere * 180.0 / M_PI;
-
-}
-
-
-__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
-{
-	double lat_now_rad = lat_now / 180.0d * M_PI;
-	double lon_now_rad = lon_now / 180.0d * M_PI;
-	double lat_next_rad = lat_next / 180.0d * M_PI;
-	double lon_next_rad = lon_next / 180.0d * M_PI;
-
-
-	double d_lat = lat_next_rad - lat_now_rad;
-	double d_lon = lon_next_rad - lon_now_rad;
-
-	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
-	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
-
-	const double radius_earth = 6371000.0d;
-	return radius_earth * c; 
-}
-
-__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
-{
-	double lat_now_rad = lat_now * M_DEG_TO_RAD;
-	double lon_now_rad = lon_now * M_DEG_TO_RAD;
-	double lat_next_rad = lat_next * M_DEG_TO_RAD;
-	double lon_next_rad = lon_next * M_DEG_TO_RAD;
-
-	double d_lat = lat_next_rad - lat_now_rad;
-	double d_lon = lon_next_rad - lon_now_rad;
-
-	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
-
-	theta = _wrap_pi(theta);
-
-	return theta;
-}
-
-// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
-
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end)
-{
-// This function returns the distance to the nearest point on the track line.  Distance is positive if current
-// position is right of the track and negative if left of the track as seen from a point on the track line
-// headed towards the end point.
-
-	float dist_to_end;
-	float bearing_end;
-	float bearing_track;
-	float bearing_diff;
-
-	int return_value = ERROR;	// Set error flag, cleared when valid result calculated.
-	crosstrack_error->past_end = false;
-	crosstrack_error->distance = 0.0f;
-	crosstrack_error->bearing = 0.0f;
-
-	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_value;
-
-	bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
-	bearing_diff = bearing_track - bearing_end;
-	bearing_diff = _wrap_pi(bearing_diff);
-
-	// Return past_end = true if past end point of line
-	if (bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) {
-		crosstrack_error->past_end = true;
-		return_value = OK;
-		return return_value;
-	}
-
-	dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
-
-	if (sin(bearing_diff) >= 0) {
-		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
-
-	} else {
-		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
-	}
-
-	return_value = OK;
-
-	return return_value;
-
-}
-
-
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
-		float radius, float arc_start_bearing, float arc_sweep)
-{
-	// This function returns the distance to the nearest point on the track arc.  Distance is positive if current
-	// position is right of the arc and negative if left of the arc as seen from the closest point on the arc and
-	// headed towards the end point.
-
-	// Determine if the current position is inside or outside the sector between the line from the center
-	// to the arc start and the line from the center to the arc end
-	float	bearing_sector_start;
-	float	bearing_sector_end;
-	float	bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-	bool	in_sector;
-
-	int return_value = ERROR;		// Set error flag, cleared when valid result calculated.
-	crosstrack_error->past_end = false;
-	crosstrack_error->distance = 0.0f;
-	crosstrack_error->bearing = 0.0f;
-
-	// Return error if arguments are bad
-	if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
-
-
-	if (arc_sweep >= 0) {
-		bearing_sector_start = arc_start_bearing;
-		bearing_sector_end = arc_start_bearing + arc_sweep;
-
-		if (bearing_sector_end > 2.0f * M_PI_F) bearing_sector_end -= M_TWOPI_F;
-
-	} else {
-		bearing_sector_end = arc_start_bearing;
-		bearing_sector_start = arc_start_bearing - arc_sweep;
-
-		if (bearing_sector_start < 0.0f) bearing_sector_start += M_TWOPI_F;
-	}
-
-	in_sector = false;
-
-	// Case where sector does not span zero
-	if (bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true;
-
-	// Case where sector does span zero
-	if (bearing_sector_end < bearing_sector_start && (bearing_now > bearing_sector_start || bearing_now < bearing_sector_end)) in_sector = true;
-
-	// If in the sector then calculate distance and bearing to closest point
-	if (in_sector) {
-		crosstrack_error->past_end = false;
-		float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
-
-		if (dist_to_center <= radius) {
-			crosstrack_error->distance = radius - dist_to_center;
-			crosstrack_error->bearing = bearing_now + M_PI_F;
-
-		} else {
-			crosstrack_error->distance = dist_to_center - radius;
-			crosstrack_error->bearing = bearing_now;
-		}
-
-		// If out of the sector then calculate dist and bearing to start or end point
-
-	} else {
-
-		// Use the approximation  that 111,111 meters in the y direction is 1 degree (of latitude)
-		// and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to
-		// calculate the position of the start and end points.  We should not be doing this often
-		// as this function generally will not be called repeatedly when we are out of the sector.
-
-		// TO DO - this is messed up and won't compile
-		float start_disp_x = radius * sin(arc_start_bearing);
-		float start_disp_y = radius * cos(arc_start_bearing);
-		float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep));
-		float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep));
-		float lon_start = lon_now + start_disp_x / 111111.0d;
-		float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d;
-		float lon_end = lon_now + end_disp_x / 111111.0d;
-		float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d;
-		float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-		float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-
-
-		if (dist_to_start < dist_to_end) {
-			crosstrack_error->distance = dist_to_start;
-			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-
-		} else {
-			crosstrack_error->past_end = true;
-			crosstrack_error->distance = dist_to_end;
-			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
-		}
-
-	}
-
-	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
-	return_value = OK;
-	return return_value;
-}
-
-__EXPORT float _wrap_pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing) || bearing == 0) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing > M_PI_F && c < 30) {
-		bearing -= M_TWOPI_F;
-		c++;
-	}
-
-	c = 0;
-
-	while (bearing <=  -M_PI_F && c < 30) {
-		bearing += M_TWOPI_F;
-		c++;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_2pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing >= M_TWOPI_F) {
-		bearing = bearing - M_TWOPI_F;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + M_TWOPI_F;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_180(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <=  -180.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_360(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing >= 360.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
deleted file mode 100644
index dadec51ec..000000000
--- a/src/modules/systemlib/geo/geo.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.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 geo.h
- *
- * Definition of geo / math functions to perform geodesic calculations
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
- */
-
-#pragma once
-
-__BEGIN_DECLS
-
-#include <stdbool.h>
-
-#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
-#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
-
-/* compatibility aliases */
-#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
-#define GRAVITY_MSS		CONSTANTS_ONE_G
-
-// XXX remove
-struct crosstrack_error_s {
-	bool past_end;		// Flag indicating we are past the end of the line/arc segment
-	float distance;		// Distance in meters to closest point on line/arc
-	float bearing;		// Bearing in radians to closest point on line/arc
-} ;
-
-/**
- * Initializes the map transformation.
- *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_init(double lat_0, double lon_0);
-
-/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
-
-/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
-
-/**
- * Returns the distance to the next waypoint in meters.
- *
- * @param lat_now current position in degrees (47.1234567°, not 471234567°)
- * @param lon_now current position in degrees (8.1234567°, not 81234567°)
- * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
- * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-
-/**
- * Returns the bearing to the next waypoint in radians.
- *
- * @param lat_now current position in degrees (47.1234567°, not 471234567°)
- * @param lon_now current position in degrees (8.1234567°, not 81234567°)
- * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
- * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-
-__EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
-
-__EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
-		float radius, float arc_start_bearing, float arc_sweep);
-
-__EXPORT float _wrap_180(float bearing);
-__EXPORT float _wrap_360(float bearing);
-__EXPORT float _wrap_pi(float bearing);
-__EXPORT float _wrap_2pi(float bearing);
-
-__END_DECLS
diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 192195856..03ca71375 100644
--- a/src/modules/mavlink/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -46,28 +46,33 @@
 
 #include <mavlink/mavlink_log.h>
 
-static FILE* text_recorder_fd = NULL;
-
-void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
+__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
 {
 	lb->size  = size;
 	lb->start = 0;
 	lb->count = 0;
 	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
-	text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w");
 }
 
-int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
+__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb)
+{
+	lb->size  = 0;
+	lb->start = 0;
+	lb->count = 0;
+	free(lb->elems);
+}
+
+__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
 {
 	return lb->count == (int)lb->size;
 }
 
-int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
+__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb)
 {
 	return lb->count == 0;
 }
 
-void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
+__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem)
 {
 	int end = (lb->start + lb->count) % lb->size;
 	memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage));
@@ -80,19 +85,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 	}
 }
 
-int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
+__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
 {
 	if (!mavlink_logbuffer_is_empty(lb)) {
 		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
 		lb->start = (lb->start + 1) % lb->size;
 		--lb->count;
 
-		if (text_recorder_fd) {
-			fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd);
-			fputc("\n", text_recorder_fd);
-			fsync(text_recorder_fd);
-		}
-
 		return 0;
 
 	} else {
@@ -100,7 +99,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa
 	}
 }
 
-void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
+__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...)
 {
 	va_list ap;
 	va_start(ap, fmt);
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index b470c1227..843cda722 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -45,7 +45,8 @@ SRCS		 = err.c \
 		   getopt_long.c \
 		   up_cxxinitialize.c \
 		   pid/pid.c \
-		   geo/geo.c \
 		   systemlib.c \
 		   airspeed.c \
-		   system_params.c
+		   system_params.c \
+		   mavlink_log.c \
+		   rc_check.c
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
new file mode 100644
index 000000000..9d47d8000
--- /dev/null
+++ b/src/modules/systemlib/rc_check.c
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_check.c
+ *
+ * RC calibration check
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <fcntl.h>
+
+#include <systemlib/rc_check.h>
+#include <systemlib/param/param.h>
+#include <mavlink/mavlink_log.h>
+#include <uORB/topics/rc_channels.h>
+
+int rc_calibration_check(void) {
+
+	char nbuf[20];
+	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
+	_parameter_handles_rev, _parameter_handles_dz;
+
+	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	float param_min, param_max, param_trim, param_rev, param_dz;
+
+	/* first check channel mappings */
+			/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+
+
+	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
+		/* should the channel be enabled? */
+		uint8_t count = 0;
+
+		/* min values */
+		sprintf(nbuf, "RC%d_MIN", i + 1);
+		_parameter_handles_min = param_find(nbuf);
+		param_get(_parameter_handles_min, &param_min);
+
+		/* trim values */
+		sprintf(nbuf, "RC%d_TRIM", i + 1);
+		_parameter_handles_trim = param_find(nbuf);
+		param_get(_parameter_handles_trim, &param_trim);
+
+		/* max values */
+		sprintf(nbuf, "RC%d_MAX", i + 1);
+		_parameter_handles_max = param_find(nbuf);
+		param_get(_parameter_handles_max, &param_max);
+
+		/* channel reverse */
+		sprintf(nbuf, "RC%d_REV", i + 1);
+		_parameter_handles_rev = param_find(nbuf);
+		param_get(_parameter_handles_rev, &param_rev);
+
+		/* channel deadzone */
+		sprintf(nbuf, "RC%d_DZ", i + 1);
+		_parameter_handles_dz = param_find(nbuf);
+		param_get(_parameter_handles_dz, &param_dz);
+
+		/* assert min..center..max ordering */
+		if (param_min < 500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_max > 2500) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_trim < param_min) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+		if (param_trim > param_max) {
+			count++;
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+		}
+
+		/* assert deadzone is sane */
+		if (param_dz > 500) {
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1);
+			/* give system time to flush error message in case there are more */
+			usleep(100000);
+			count++;
+		}
+
+		/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
+		/* sanity checks pass, enable channel */
+		if (count) {
+			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			usleep(100000);
+		}
+	}
+}
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
new file mode 100644
index 000000000..e2238d151
--- /dev/null
+++ b/src/modules/systemlib/rc_check.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_check.h
+ *
+ * RC calibration check
+ */
+
+#pragma once
+
+ __BEGIN_DECLS
+
+/**
+ * Check the RC calibration
+ *
+ * @return			0 / OK if RC calibration is ok, index + 1 of the first
+ *				channel that failed else (so 1 == first channel failed)
+ */
+__EXPORT int	rc_calibration_check(void);
+
+__END_DECLS
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index 3283aad8a..57a751e1c 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -43,13 +43,29 @@
 #include <fcntl.h>
 #include <sched.h>
 #include <signal.h>
-#include <sys/stat.h>
 #include <unistd.h>
 #include <float.h>
 #include <string.h>
 
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include <stm32_pwr.h>
+
 #include "systemlib.h"
 
+void
+systemreset(bool to_bootloader)
+{
+	if (to_bootloader) {
+		stm32_pwr_enablebkp();
+
+		/* XXX wow, this is evil - write a magic number into backup register zero */
+		*(uint32_t *)0x40002850 = 0xb007b007;
+	}
+	up_systemreset();
+}
+
 static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
 
 void killall()
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 0194b5e52..3728f2067 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
 #include <float.h>
 #include <stdint.h>
 
-/** Reboots the board */
-extern void up_systemreset(void) noreturn_function;
-
 __BEGIN_DECLS
 
+/** Reboots the board */
+__EXPORT void systemreset(bool to_bootloader) noreturn_function;
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
diff --git a/src/modules/test/foo.c b/src/modules/test/foo.c
deleted file mode 100644
index ff6af031f..000000000
--- a/src/modules/test/foo.c
+++ /dev/null
@@ -1,4 +0,0 @@
-int test_main(void)
-{
-	return 0;
-}
\ No newline at end of file
diff --git a/src/modules/test/module.mk b/src/modules/test/module.mk
deleted file mode 100644
index abf80eedf..000000000
--- a/src/modules/test/module.mk
+++ /dev/null
@@ -1,4 +0,0 @@
-
-MODULE_NAME	 = test
-SRCS		 = foo.c
-
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 301cfa255..1eb63a799 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -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
@@ -81,6 +81,9 @@ ORB_DEFINE(home_position, struct home_position_s);
 #include "topics/vehicle_status.h"
 ORB_DEFINE(vehicle_status, struct vehicle_status_s);
 
+#include "topics/safety.h"
+ORB_DEFINE(safety, struct safety_s);
+
 #include "topics/battery_status.h"
 ORB_DEFINE(battery_status, struct battery_status_s);
 
@@ -102,6 +105,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s);
 #include "topics/vehicle_command.h"
 ORB_DEFINE(vehicle_command, struct vehicle_command_s);
 
+#include "topics/vehicle_control_mode.h"
+ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s);
+
 #include "topics/vehicle_local_position_setpoint.h"
 ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
 
@@ -114,6 +120,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
 #include "topics/vehicle_global_position_set_triplet.h"
 ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
 
+#include "topics/vehicle_global_velocity_setpoint.h"
+ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s);
+
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
 
@@ -123,6 +132,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
+#include "topics/vehicle_control_debug.h"
+ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s);
+
 #include "topics/offboard_control_setpoint.h"
 ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
 
@@ -150,6 +162,8 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
 ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+
+#include "topics/actuator_armed.h"
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
 
 /* actuator controls, as set by actuators / mixers after limiting */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
new file mode 100644
index 000000000..6e944ffee
--- /dev/null
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_armed.h
+ *
+ * Actuator armed topic
+ *
+ */
+
+#ifndef TOPIC_ACTUATOR_ARMED_H
+#define TOPIC_ACTUATOR_ARMED_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/** global 'actuator output is live' control. */
+struct actuator_armed_s {
+
+	uint64_t	timestamp;
+	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
+	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+};
+
+ORB_DECLARE(actuator_armed);
+
+#endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index a27095be5..e768ab2f6 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -52,6 +52,9 @@
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
 
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
+
 /**
  * @addtogroup topics
  * @{
@@ -72,16 +75,4 @@ ORB_DECLARE(actuator_controls_1);
 ORB_DECLARE(actuator_controls_2);
 ORB_DECLARE(actuator_controls_3);
 
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS	ORB_ID(actuator_controls_0)
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
-	bool	armed;		/**< Set to true if system is armed */
-	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-};
-
-ORB_DECLARE(actuator_armed);
-
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 261a8a4ad..eac6d6e98 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -56,17 +56,18 @@ struct manual_control_setpoint_s {
 	float yaw;				/**< rudder / yaw rate / yaw */
 	float throttle;				/**< throttle / collective thrust / altitude */
 
-	float manual_override_switch;		/**< manual override mode (mandatory) */
-	float auto_mode_switch;			/**< auto mode switch (mandatory) */
+	float mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
+	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
+	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
+	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
 
 	/**
 	 * Any of the channels below may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
 	 */
-	float manual_mode_switch;		/**< manual mode (man, sas, alt) switch (optional) */
-	float manual_sas_switch;		/**< sas mode (rates / attitude) switch (optional) */
-	float return_to_launch_switch;		/**< return to launch switch (0 = disabled, 1 = enabled) */
-	float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
+
+	// XXX needed or parameter?
+	//float auto_offboard_input_switch;	/**< controller setpoint source (0 = onboard, 1 = offboard) */
 
 	float flaps;				/**< flap position */
 
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index e69335b3d..5a8580143 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -48,9 +48,12 @@
 /**
  * The number of RC channel inputs supported.
  * Current (Q1/2013) radios support up to 18 channels,
- * leaving at a sane value of 14.
+ * leaving at a sane value of 15.
+ * This number can be greater then number of RC channels,
+ * because single RC channel can be mapped to multiple
+ * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAX   14
+#define RC_CHANNELS_MAX   15
 
 /** 
  * This defines the mapping of the RC functions.
@@ -63,18 +66,17 @@ enum RC_CHANNELS_FUNCTION
   ROLL     = 1,
   PITCH    = 2,
   YAW      = 3,
-  OVERRIDE = 4,
-  AUTO_MODE = 5,
-  MANUAL_MODE = 6,
-  SAS_MODE = 7,
-  RTL = 8,
-  OFFBOARD_MODE = 9,
-  FLAPS   = 10,
-  AUX_1   = 11,
-  AUX_2   = 12,
-  AUX_3   = 13,
-  AUX_4   = 14,
-  AUX_5   = 15,
+  MODE = 4,
+  RETURN = 5,
+  ASSISTED = 6,
+  MISSION = 7,
+  OFFBOARD_MODE = 8,
+  FLAPS   = 9,
+  AUX_1   = 10,
+  AUX_2   = 11,
+  AUX_3   = 12,
+  AUX_4   = 13,
+  AUX_5   = 14,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/src/modules/mathlib/mathlib.h b/src/modules/uORB/topics/safety.h
index 40ffb22bc..a5d21cd4a 100644
--- a/src/modules/mathlib/mathlib.h
+++ b/src/modules/uORB/topics/safety.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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,28 +32,26 @@
  ****************************************************************************/
 
 /**
- * @file mathlib.h
+ * @file safety.h
  *
- * Common header for mathlib exports.
+ * Safety topic to pass safety state from px4io driver to commander
+ * This concerns only the safety button of the px4io but has nothing to do
+ * with arming/disarming.
  */
 
-#ifdef __cplusplus
+#ifndef TOPIC_SAFETY_H
+#define TOPIC_SAFETY_H
 
-#pragma once
+#include <stdint.h>
+#include "../uORB.h"
 
-#include "math/Dcm.hpp"
-#include "math/EulerAngles.hpp"
-#include "math/Matrix.hpp"
-#include "math/Quaternion.hpp"
-#include "math/Vector.hpp"
-#include "math/Vector3.hpp"
-#include "math/Vector2f.hpp"
-#include "math/Limits.hpp"
+struct safety_s {
 
-#endif
+	uint64_t	timestamp;
+	bool	safety_switch_available;	/**< Set to true if a safety switch is connected */
+	bool	safety_off;			/**< Set to true if safety is off */
+};
 
-#ifdef CONFIG_ARCH_ARM
-
-#include "CMSIS/Include/arm_math.h"
+ORB_DECLARE(safety);
 
 #endif
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Vector.cpp b/src/modules/uORB/topics/vehicle_control_debug.h
index 35158a396..6184284a4 100644
--- a/src/modules/mathlib/math/Vector.cpp
+++ b/src/modules/uORB/topics/vehicle_control_debug.h
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,69 +33,55 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
- *
- * math vector
+ * @file vehicle_control_debug.h
+ * For debugging purposes to log PID parts of controller
  */
 
-#include "test/test.hpp"
+#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_
+#define TOPIC_VEHICLE_CONTROL_DEBUG_H_
 
-#include "Vector.hpp"
+#include <stdint.h>
+#include "../uORB.h"
 
-namespace math
+/**
+ * @addtogroup topics
+ * @{
+ */
+struct vehicle_control_debug_s
 {
+	uint64_t timestamp; /**< in microseconds since system start */
 
-static const float data_testA[] = {1, 3};
-static const float data_testB[] = {4, 1};
+	float roll_p;		/**< roll P control part		*/
+	float roll_i;		/**< roll I control part 		*/
+	float roll_d;		/**< roll D control part 		*/
 
-static Vector testA(2, data_testA);
-static Vector testB(2, data_testB);
+	float roll_rate_p;	/**< roll rate P control part		*/
+	float roll_rate_i;	/**< roll rate I control part 		*/
+	float roll_rate_d;	/**< roll rate D control part 		*/
 
-int __EXPORT vectorTest()
-{
-	vectorAddTest();
-	vectorSubTest();
-	return 0;
-}
+	float pitch_p;		/**< pitch P control part		*/
+	float pitch_i;		/**< pitch I control part 		*/
+	float pitch_d;		/**< pitch D control part 		*/
 
-int vectorAddTest()
-{
-	printf("Test Vector Add\t\t: ");
-	Vector r = testA + testB;
-	float data_test[] = {5.0f, 4.0f};
-	ASSERT(vectorEqual(Vector(2, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
+	float pitch_rate_p;	/**< pitch rate P control part		*/
+	float pitch_rate_i;	/**< pitch rate I control part 		*/
+	float pitch_rate_d;	/**< pitch rate D control part 		*/
 
-int vectorSubTest()
-{
-	printf("Test Vector Sub\t\t: ");
-	Vector r(2);
-	r = testA - testB;
-	float data_test[] = { -3.0f, 2.0f};
-	ASSERT(vectorEqual(Vector(2, data_test), r));
-	printf("PASS\n");
-	return 0;
-}
+	float yaw_p;		/**< yaw P control part			*/
+	float yaw_i;		/**< yaw I control part 		*/
+	float yaw_d;		/**< yaw D control part 		*/
 
-bool vectorEqual(const Vector &a, const Vector &b, float eps)
-{
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
-	}
+	float yaw_rate_p;	/**< yaw rate P control part		*/
+	float yaw_rate_i;	/**< yaw rate I control part 		*/
+	float yaw_rate_d;	/**< yaw rate D control part 		*/
 
-	bool ret = true;
+}; /**< vehicle_control_debug */
 
-	for (size_t i = 0; i < a.getRows(); i++) {
-		if (!equal(a(i), b(i), eps)) {
-			printf("element mismatch (%d)\n", i);
-			ret = false;
-		}
-	}
+ /**
+ * @}
+ */
 
-	return ret;
-}
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_debug);
 
-} // namespace math
+#endif
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
new file mode 100644
index 000000000..093c6775d
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.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 vehicle_control_mode.h
+ * Definition of the vehicle_control_mode uORB topic.
+ * 
+ * All control apps should depend their actions based on the flags set here.
+ */
+
+#ifndef VEHICLE_CONTROL_MODE
+#define VEHICLE_CONTROL_MODE
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+struct vehicle_control_mode_s
+{
+	uint16_t counter;   /**< incremented by the writing thread every time new data is stored */
+	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+	bool flag_armed;
+
+	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
+
+	// XXX needs yet to be set by state machine helper
+	bool flag_system_hil_enabled;
+
+	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
+	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
+	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
+	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
+	bool flag_control_position_enabled;		/**< true if position is controlled */
+	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
+	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
+
+	bool flag_control_auto_enabled;		// TEMP
+	uint8_t auto_state;	// TEMP navigation state for AUTO modes
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_control_mode);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index f036c7223..0fc0ed5c9 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -62,18 +62,17 @@
 struct vehicle_global_position_s
 {
 	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							  */
+	uint64_t time_gps_usec; /**< GPS timestamp in microseconds							   */
 	bool valid;				/**< true if position satisfies validity criteria of estimator */
 
-	int32_t lat;			/**< Latitude in 1E7 degrees							LOGME */
-	int32_t lon;			/**< Longitude in 1E7 degrees							LOGME */
-	float alt;				/**< Altitude in meters									LOGME */
-	float relative_alt;		/**< Altitude above home position in meters, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in ENU				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in ENU				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in ENU				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
-
+	int32_t lat;			/**< Latitude in 1E7 degrees							 	   */
+	int32_t lon;			/**< Longitude in 1E7 degrees							 	   */
+	float alt;				/**< Altitude in meters									 	   */
+	float relative_alt;		/**< Altitude above home position in meters, 				   */
+	float vx; 				/**< Ground X velocity, m/s in NED				 			   */
+	float vy;				/**< Ground Y velocity, m/s in NED							   */
+	float vz;				/**< Ground Z velocity, m/s	in NED 							   */
+	float yaw; 				/**< Compass heading in radians -PI..+PI.					   */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
new file mode 100644
index 000000000..73961cdfe
--- /dev/null
+++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_global_velocity_setpoint.h
+ * Definition of the global velocity setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_global_velocity_setpoint_s
+{
+	float vx;		/**< in m/s NED			  		*/
+	float vy;		/**< in m/s NED			  		*/
+	float vz;		/**< in m/s NED			  		*/
+}; /**< Velocity setpoint in NED frame */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_velocity_setpoint);
+
+#endif
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 76eddeacd..31a0e632b 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -54,27 +54,29 @@
  */
 struct vehicle_local_position_s
 {
-	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
-	bool valid;				/**< true if position satisfies validity criteria of estimator */
-
-	float x;				/**< X positin in meters in NED earth-fixed frame */
-	float y;				/**< X positin in meters in NED earth-fixed frame */
-	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
-	float absolute_alt;			/**< Altitude as defined by pressure / GPS, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in NED				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in NED				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
-
-	// TODO Add covariances here
-
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
+	bool xy_valid;			/**< true if x and y are valid */
+	bool z_valid;			/**< true if z is valid */
+	bool v_xy_valid;		/**< true if vy and vy are valid */
+	bool v_z_valid;			/**< true if vz is valid */
+	/* Position in local NED frame */
+	float x;				/**< X position in meters in NED earth-fixed frame */
+	float y;				/**< X position in meters in NED earth-fixed frame */
+	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
+	/* Velocity in NED frame */
+	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
+	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
+	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
+	/* Heading */
+	float yaw;
 	/* Reference position in GPS / WGS84 frame */
-	uint64_t home_timestamp;/**< Time when home position was set						  */
-	int32_t home_lat;		/**< Latitude in 1E7 degrees							LOGME */
-	int32_t home_lon;		/**< Longitude in 1E7 degrees							LOGME */
-	float home_alt;			/**< Altitude in meters									LOGME */
-	float home_hdg; 		/**< Compass heading in radians -PI..+PI.					  */
-
+	bool global_xy;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
+	bool global_z;			/**< true if z is valid and has valid global reference (ref_alt) */
+	uint64_t ref_timestamp;	/**< Time when reference position was set */
+	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
+	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
+	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
+	bool landed;			/**< true if vehicle is landed */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 94068a9ac..1c184d3a7 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,22 +54,67 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/* State Machine */
-typedef enum
-{
-	SYSTEM_STATE_PREFLIGHT = 0,
-	SYSTEM_STATE_STANDBY = 1,
-	SYSTEM_STATE_GROUND_READY = 2,
-	SYSTEM_STATE_MANUAL = 3,
-	SYSTEM_STATE_STABILIZED = 4,
-	SYSTEM_STATE_AUTO = 5,
-	SYSTEM_STATE_MISSION_ABORT = 6,
-	SYSTEM_STATE_EMCY_LANDING = 7,
-	SYSTEM_STATE_EMCY_CUTOFF = 8,
-	SYSTEM_STATE_GROUND_ERROR = 9,
-	SYSTEM_STATE_REBOOT= 10,
-
-} commander_state_machine_t;
+/**
+ * @addtogroup topics @{
+ */
+
+/* main state machine */
+typedef enum {
+	MAIN_STATE_MANUAL = 0,
+	MAIN_STATE_SEATBELT,
+	MAIN_STATE_EASY,
+	MAIN_STATE_AUTO,
+} main_state_t;
+
+/* navigation state machine */
+typedef enum {
+	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
+	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
+	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
+	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
+	NAVIGATION_STATE_AUTO_READY,	// AUTO, landed, reeady for takeoff
+	NAVIGATION_STATE_AUTO_TAKEOFF,	// detect takeoff using land detector and switch to desired AUTO mode
+	NAVIGATION_STATE_AUTO_LOITER,	// pause mission
+	NAVIGATION_STATE_AUTO_MISSION,	// fly mission
+	NAVIGATION_STATE_AUTO_RTL,		// Return To Launch, when home position switch to LAND
+	NAVIGATION_STATE_AUTO_LAND		// land and switch to AUTO_READY when landed (detect using land detector)
+} navigation_state_t;
+
+typedef enum {
+	ARMING_STATE_INIT = 0,
+	ARMING_STATE_STANDBY,
+	ARMING_STATE_ARMED,
+	ARMING_STATE_ARMED_ERROR,
+	ARMING_STATE_STANDBY_ERROR,
+	ARMING_STATE_REBOOT,
+	ARMING_STATE_IN_AIR_RESTORE
+} arming_state_t;
+
+typedef enum {
+	HIL_STATE_OFF = 0,
+	HIL_STATE_ON
+} hil_state_t;
+
+typedef enum {
+	MODE_SWITCH_MANUAL = 0,
+	MODE_SWITCH_ASSISTED,
+	MODE_SWITCH_AUTO
+} mode_switch_pos_t;
+
+typedef enum {
+	ASSISTED_SWITCH_SEATBELT = 0,
+	ASSISTED_SWITCH_EASY
+} assisted_switch_pos_t;
+
+typedef enum {
+	RETURN_SWITCH_NONE = 0,
+	RETURN_SWITCH_RETURN
+} return_switch_pos_t;
+
+typedef enum {
+	MISSION_SWITCH_NONE = 0,
+	MISSION_SWITCH_MISSION
+} mission_switch_pos_t;
 
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
@@ -82,26 +127,6 @@ enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
 
-enum VEHICLE_FLIGHT_MODE {
-	VEHICLE_FLIGHT_MODE_MANUAL = 0,		/**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */
-	VEHICLE_FLIGHT_MODE_STAB,		/**< attitude or rate stabilization plus velocity or position stabilization */
-	VEHICLE_FLIGHT_MODE_HOLD,		/**< hold current position (hover or loiter around position when switched) */
-	VEHICLE_FLIGHT_MODE_AUTO		/**< attitude or rate stabilization plus absolute position control and waypoints */
-};
-
-enum VEHICLE_MANUAL_CONTROL_MODE {
-	VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0,	/**< no attitude control, direct stick input mixing (only fixed wing) */
-	VEHICLE_MANUAL_CONTROL_MODE_RATES,	/**< body rates control mode */
-	VEHICLE_MANUAL_CONTROL_MODE_SAS		/**< stability augmented system (SAS) mode */
-};
-
-enum VEHICLE_MANUAL_SAS_MODE {
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0,	/**< roll, pitch and yaw absolute */
-	VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE,	/**< roll and pitch absolute, yaw rate */
-	VEHICLE_MANUAL_SAS_MODE_SIMPLE,				/**< simple mode (includes altitude hold) */
-	VEHICLE_MANUAL_SAS_MODE_ALTITUDE			/**< altitude hold */
-};
-
 /**
  * Should match 1:1 MAVLink's MAV_TYPE ENUM
  */
@@ -128,9 +153,9 @@ enum VEHICLE_TYPE {
 };
 
 enum VEHICLE_BATTERY_WARNING {
-    VEHICLE_BATTERY_WARNING_NONE = 0,    /**< no battery low voltage warning active */
-    VEHICLE_BATTERY_WARNING_WARNING,        /**< warning of low voltage 1. stage */
-    VEHICLE_BATTERY_WARNING_ALERT            /**< aleting of low voltage 2. stage */
+    VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
+    VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
+    VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
 };
 
 /**
@@ -149,55 +174,54 @@ struct vehicle_status_s
 
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
-	//uint64_t failsave_highlevel_begin; TO BE COMPLETED
 
-	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
-	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
-	enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode;	/**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
-	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;	/**< current stabilization mode */
+	main_state_t main_state;				/**< main state machine */
+	navigation_state_t navigation_state;	/**< navigation state machine */
+	arming_state_t arming_state;			/**< current arming state */
+	hil_state_t hil_state;					/**< current hil state */
+
 	int32_t system_type;				/**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
 	int32_t	system_id;				/**< system id, inspired by MAVLink's system ID field */
 	int32_t component_id;				/**< subsystem / component id, inspired by MAVLink's component ID field */
 
-	/* system flags - these represent the state predicates */
-
-	bool flag_system_armed;				/**< true is motors / actuators are armed */
-	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
-	bool flag_hil_enabled;				/**< true if hardware in the loop simulation is enabled */
-
-	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
-	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if speed (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-
-	bool flag_preflight_gyro_calibration;		/**< true if gyro calibration is requested */
-	bool flag_preflight_mag_calibration;		/**< true if mag calibration is requested */
-	bool flag_preflight_accel_calibration;
-	bool flag_preflight_airspeed_calibration;
+	bool is_rotary_wing;
+
+	mode_switch_pos_t mode_switch;
+	return_switch_pos_t return_switch;
+	assisted_switch_pos_t assisted_switch;
+	mission_switch_pos_t mission_switch;
+
+	bool condition_battery_voltage_valid;
+	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */
+	bool condition_system_sensors_initialized;
+	bool condition_system_returned_to_home;
+	bool condition_auto_mission_available;
+	bool condition_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+	bool condition_launch_position_valid;		/**< indicates a valid launch position */
+	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
+	bool condition_local_position_valid;
+	bool condition_local_altitude_valid;
+	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
+	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */
 
 	bool rc_signal_found_once;
-	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
-	bool rc_signal_cutting_off;			/**< true if RC reception is weak / cutting off */
-	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
+	bool rc_signal_lost;				/**< true if RC reception lost */
 
 	bool offboard_control_signal_found_once;
 	bool offboard_control_signal_lost;
 	bool offboard_control_signal_weak;
 	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
 
-	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
-	//bool failsave_highlevel;
-
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;
 	uint32_t onboard_control_sensors_health;
-	float load;
-	float voltage_battery;
-	float current_battery;
+	
+	float load;					/**< processor load from 0 to 1 */
+	float battery_voltage;
+	float battery_current;
 	float battery_remaining;
+
 	enum VEHICLE_BATTERY_WARNING battery_warning;    /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
 	uint16_t drop_rate_comm;
 	uint16_t errors_comm;
@@ -205,15 +229,6 @@ struct vehicle_status_s
 	uint16_t errors_count2;
 	uint16_t errors_count3;
 	uint16_t errors_count4;
-
-	bool flag_global_position_valid;		/**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
-	bool flag_local_position_valid;
-	bool flag_vector_flight_mode_ok;		/**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
-	bool flag_auto_flight_mode_ok;			/**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
-	bool flag_external_manual_override_ok;		/**< external override non-fatal for system. Only true for fixed wing */
-	bool flag_valid_launch_position;		/**< indicates a valid launch position */
-	bool flag_valid_home_position;			/**< indicates a valid home position (a valid home position is not always a valid launch) */
-	bool flag_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 };
 
 /**
diff --git a/src/modules/mathlib/module.mk b/src/modules/unit_test/module.mk
index 2146a1413..f00b0f592 100644
--- a/src/modules/mathlib/module.mk
+++ b/src/modules/unit_test/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 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,31 +32,8 @@
 ############################################################################
 
 #
-# Math library
+# Makefile to build the unit test library.
 #
-SRCS		 = math/test/test.cpp \
-		   math/Vector.cpp \
-		   math/Vector2f.cpp \
-		   math/Vector3.cpp \
-		   math/EulerAngles.cpp \
-		   math/Quaternion.cpp \
-		   math/Dcm.cpp \
-		   math/Matrix.cpp \
-		   math/Limits.cpp
 
-#
-# In order to include .config we first have to save off the
-# current makefile name, since app.mk needs it.
-#
-APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
--include $(TOPDIR)/.config
+SRCS		 = unit_test.cpp
 
-ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
-INCLUDE_DIRS	+= math/arm
-SRCS		+= math/arm/Vector.cpp \
-		   math/arm/Matrix.cpp
-else
-#INCLUDE_DIRS	+= math/generic
-#SRCS		+= math/generic/Vector.cpp \
-#		   math/generic/Matrix.cpp
-endif
diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp
new file mode 100644
index 000000000..64ee544a2
--- /dev/null
+++ b/src/modules/unit_test/unit_test.cpp
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file unit_test.cpp
+ * A unit test library.
+ *
+ */
+
+#include "unit_test.h"
+
+#include <systemlib/err.h>
+
+
+UnitTest::UnitTest()
+{
+}
+
+UnitTest::~UnitTest()
+{
+}
+
+void
+UnitTest::print_results(const char* result)
+{
+	if (result != 0) {
+        	warnx("Failed: %s:%d", mu_last_test(), mu_line());
+        	warnx("%s", result);
+    	} else {
+        	warnx("ALL TESTS PASSED");
+        	warnx("  Tests run : %d", mu_tests_run());
+        	warnx("  Assertion : %d", mu_assertion());
+    	}
+}
diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h
new file mode 100644
index 000000000..3020734f4
--- /dev/null
+++ b/src/modules/unit_test/unit_test.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file unit_test.h
+ * A unit test library based on MinUnit (http://www.jera.com/techinfo/jtns/jtn002.html).
+ *
+ */
+
+#ifndef UNIT_TEST_H_
+#define UNIT_TEST_
+
+#include <systemlib/err.h>
+
+
+class __EXPORT UnitTest
+{
+
+public:
+#define xstr(s) str(s)
+#define str(s) #s
+#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; }
+
+INLINE_GLOBAL(int, mu_tests_run)
+INLINE_GLOBAL(int, mu_assertion)
+INLINE_GLOBAL(int, mu_line)
+INLINE_GLOBAL(const char*, mu_last_test)
+
+#define mu_assert(message, test)                                           \
+    do                                                                     \
+    {                                                                      \
+        if (!(test))                                                       \
+            return __FILE__ ":" xstr(__LINE__) " " message " (" #test ")"; \
+        else                                                               \
+            mu_assertion()++;                                              \
+    } while (0)
+
+
+#define mu_run_test(test)                                                  \
+do                                                                         \
+{                                                                          \
+    const char *message;                                                   \
+    mu_last_test() = #test;                                                \
+    mu_line() = __LINE__;                                                  \
+    message = test();                                                      \
+    mu_tests_run()++;                                                      \
+    if (message)                                                           \
+        return message;                                                    \
+} while (0)
+
+
+public:
+	UnitTest();
+    virtual ~UnitTest();
+
+    virtual const char*     run_tests() = 0;
+    virtual void            print_results(const char* result);
+};
+
+
+
+#endif /* UNIT_TEST_H_ */