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

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

128 files changed, 1369 insertions, 7170 deletions

diff --git a/.gitignore b/.gitignore
index c992dbf5a..afd63e06a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -38,3 +38,4 @@ tags
 .tags_sorted_by_file
 .pydevproject
 .ropeproject
+*.orig
diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg
index 1ef05458f..682c63e47 100644
--- a/Documentation/rc_mode_switch.odg
+++ b/Documentation/rc_mode_switch.odg
diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf
index b1a468d17..e795f4870 100644
--- a/Documentation/rc_mode_switch.pdf
+++ b/Documentation/rc_mode_switch.pdf
diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
new file mode 100644
index 000000000..6179855f6
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
@@ -0,0 +1,35 @@
+#!nsh
+#
+# Steadidrone QU4D
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+# Lorenz Meier <lm@inf.ethz.ch>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	# TODO tune roll/pitch separately
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.13
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.004
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
+	param set MC_YAW_P 0.5
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+
+	param set BAT_N_CELLS 4
+fi
+
+set MIXER FMU_quad_w
+
+set PWM_MIN 1210
+set PWM_MAX 2100
+
+set PWM_OUTPUTS 1234
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 5ec735d39..b365bd642 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -195,6 +195,11 @@ then
 	sh /etc/init.d/10016_3dr_iris
 fi
 
+if param compare SYS_AUTOSTART 10017
+then
+	sh /etc/init.d/10017_steadidrone_qu4d
+fi
+
 #
 # Hexa Coaxial
 #
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
index 4db62607a..a8c6dc811 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -32,9 +32,9 @@ then
 	param set MPC_Z_VEL_D 0.0
 	param set MPC_Z_VEL_MAX 3
 	param set MPC_Z_FF 0.5
-	param set MPC_TILT_MAX 1.0
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
 	param set MPC_LAND_SPEED 1.0
-	param set MPC_LAND_TILT 0.3
 fi
 
 set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 756ee8ef8..8c413e087 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -420,16 +420,6 @@ then
 	mavlink start $MAVLINK_FLAGS
 	
 	#
-	# Start the datamanager
-	#
-	dataman start
-	
-	#
-	# Start the navigator
-	#
-	navigator start
-	
-	#
 	# Sensors, Logging, GPS
 	#
 	sh /etc/init.d/rc.sensors
@@ -551,6 +541,16 @@ then
 	fi
 
 	#
+	# Start the datamanager
+	#
+	dataman start
+
+	#
+	# Start the navigator
+	#
+	navigator start
+
+	#
 	# Generic setup (autostart ID not found)
 	#
 	if [ $VEHICLE_TYPE == none ]
diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh
index 0b6743013..5995d428e 100755
--- a/Tools/fix_code_style.sh
+++ b/Tools/fix_code_style.sh
@@ -16,5 +16,6 @@ astyle \
     --ignore-exclude-errors-x	\
     --lineend=linux		\
     --exclude=EASTL		\
-    --add-brackets \
+    --add-brackets		\
+    --max-code-length=120	\
     $*
diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 127418f1f..c599b118f 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -504,8 +504,8 @@ CONFIG_MTD_BYTE_WRITE=y
 #
 # USART1 Configuration
 #
-CONFIG_USART1_RXBUFSIZE=512
-CONFIG_USART1_TXBUFSIZE=512
+CONFIG_USART1_RXBUFSIZE=256
+CONFIG_USART1_TXBUFSIZE=128
 CONFIG_USART1_BAUD=57600
 CONFIG_USART1_BITS=8
 CONFIG_USART1_PARITY=0
@@ -528,8 +528,8 @@ CONFIG_USART2_OFLOWCONTROL=y
 #
 # UART5 Configuration
 #
-CONFIG_UART5_RXBUFSIZE=512
-CONFIG_UART5_TXBUFSIZE=512
+CONFIG_UART5_RXBUFSIZE=256
+CONFIG_UART5_TXBUFSIZE=128
 CONFIG_UART5_BAUD=57600
 CONFIG_UART5_BITS=8
 CONFIG_UART5_PARITY=0
@@ -540,8 +540,8 @@ CONFIG_UART5_2STOP=0
 #
 # USART6 Configuration
 #
-CONFIG_USART6_RXBUFSIZE=512
-CONFIG_USART6_TXBUFSIZE=512
+CONFIG_USART6_RXBUFSIZE=128
+CONFIG_USART6_TXBUFSIZE=64
 CONFIG_USART6_BAUD=57600
 CONFIG_USART6_BITS=8
 CONFIG_USART6_PARITY=0
diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h
index 4b9c90638..17e77918b 100755
--- a/nuttx-configs/px4io-v2/include/board.h
+++ b/nuttx-configs/px4io-v2/include/board.h
@@ -103,8 +103,6 @@
 #define GPIO_USART2_RTS	0xffffffff
 #undef GPIO_USART2_CK
 #define GPIO_USART2_CK 	0xffffffff
-#undef GPIO_USART3_TX
-#define GPIO_USART3_TX 	0xffffffff
 #undef GPIO_USART3_CK
 #define GPIO_USART3_CK 	0xffffffff
 #undef GPIO_USART3_CTS
diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig
index 4111e70eb..e6563e587 100755
--- a/nuttx-configs/px4io-v2/nsh/defconfig
+++ b/nuttx-configs/px4io-v2/nsh/defconfig
@@ -104,9 +104,9 @@ CONFIG_ARMV7M_CMNVECTOR=y
 #   CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled
 #
 CONFIG_STM32_DFU=n
-CONFIG_STM32_JTAG_FULL_ENABLE=y
+CONFIG_STM32_JTAG_FULL_ENABLE=n
 CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n
-CONFIG_STM32_JTAG_SW_ENABLE=n
+CONFIG_STM32_JTAG_SW_ENABLE=y
 
 #
 # Individual subsystems can be enabled:
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp
index b75c2297f..5c502f682 100644
--- a/src/drivers/blinkm/blinkm.cpp
+++ b/src/drivers/blinkm/blinkm.cpp
@@ -194,6 +194,26 @@ private:
 
 	bool systemstate_run;
 
+	int vehicle_status_sub_fd;
+	int vehicle_control_mode_sub_fd;
+	int vehicle_gps_position_sub_fd;
+	int actuator_armed_sub_fd;
+	int safety_sub_fd;
+
+	int num_of_cells;
+	int detected_cells_runcount;
+
+	int t_led_color[8];
+	int t_led_blink;
+	int led_thread_runcount;
+	int led_interval;
+
+	bool topic_initialized;
+	bool detected_cells_blinked;
+	bool led_thread_ready;
+
+	int num_of_used_sats;
+
 	void 			setLEDColor(int ledcolor);
 	static void		led_trampoline(void *arg);
 	void			led();
@@ -265,7 +285,22 @@ BlinkM::BlinkM(int bus, int blinkm) :
 	led_color_7(LED_OFF),
 	led_color_8(LED_OFF),
 	led_blink(LED_NOBLINK),
-	systemstate_run(false)
+	systemstate_run(false),
+	vehicle_status_sub_fd(-1),
+	vehicle_control_mode_sub_fd(-1),
+	vehicle_gps_position_sub_fd(-1),
+	actuator_armed_sub_fd(-1),
+	safety_sub_fd(-1),
+	num_of_cells(0),
+	detected_cells_runcount(0),
+	t_led_color({0}),
+	t_led_blink(0),
+	led_thread_runcount(0),
+	led_interval(1000),
+	topic_initialized(false),
+	detected_cells_blinked(false),
+	led_thread_ready(true),
+	num_of_used_sats(0)
 {
 	memset(&_work, 0, sizeof(_work));
 }
@@ -382,31 +417,6 @@ void
 BlinkM::led()
 {
 
-	static int vehicle_status_sub_fd;
-	static int vehicle_control_mode_sub_fd;
-	static int vehicle_gps_position_sub_fd;
-	static int actuator_armed_sub_fd;
-	static int safety_sub_fd;
-
-	static int num_of_cells = 0;
-	static int detected_cells_runcount = 0;
-
-	static int t_led_color[8] = { 0, 0, 0, 0, 0, 0, 0, 0};
-	static int t_led_blink = 0;
-	static int led_thread_runcount=0;
-	static int led_interval = 1000;
-
-	static int no_data_vehicle_status = 0;
-	static int no_data_vehicle_control_mode = 0;
-	static int no_data_actuator_armed = 0;
-	static int no_data_vehicle_gps_position = 0;
-
-	static bool topic_initialized = false;
-	static bool detected_cells_blinked = false;
-	static bool led_thread_ready = true;
-
-	int num_of_used_sats = 0;
-
 	if(!topic_initialized) {
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
 		orb_set_interval(vehicle_status_sub_fd, 250);
@@ -494,6 +504,11 @@ BlinkM::led()
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
 
+		int no_data_vehicle_status = 0;
+		int no_data_vehicle_control_mode = 0;
+		int no_data_actuator_armed = 0;
+		int no_data_vehicle_gps_position = 0;
+
 		if (new_data_vehicle_status) {
 			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub_fd, &vehicle_status_raw);
 			no_data_vehicle_status = 0;
@@ -638,11 +653,11 @@ BlinkM::led()
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
 							/* indicate main control state */
-							if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
+							if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL)
 								led_color_4 = LED_GREEN;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL)
 								led_color_4 = LED_YELLOW;
 							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
 								led_color_4 = LED_WHITE;
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 0a047f38f..55cc077fb 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -122,7 +122,7 @@ private:
 	actuator_controls_s _controls;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	static int	control_callback(uintptr_t handle,
 			uint8_t control_group,
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index cdc9d3106..fddba806e 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -158,6 +158,7 @@ private:
 	int			_class_instance;
 
 	orb_advert_t		_mag_topic;
+	orb_advert_t		_subsystem_pub;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
@@ -324,7 +325,9 @@ HMC5883::HMC5883(int bus) :
 	_reports(nullptr),
 	_range_scale(0), /* default range scale from counts to gauss */
 	_range_ga(1.3f),
+	_collect_phase(false),
 	_mag_topic(-1),
+	_subsystem_pub(-1),
 	_class_instance(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
@@ -1137,13 +1140,12 @@ int HMC5883::check_calibration()
 			true,
 			_calibrated,
 			SUBSYSTEM_TYPE_MAG};
-		static orb_advert_t pub = -1;
 
 		if (!(_pub_blocked)) {
-			if (pub > 0) {
-				orb_publish(ORB_ID(subsystem_info), pub, &info);
+			if (_subsystem_pub > 0) {
+				orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info);
 			} else {
-				pub = orb_advertise(ORB_ID(subsystem_info), &info);
+				_subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info);
 			}
 		}
 	}
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 90a744015..1e779e8dc 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -51,6 +51,8 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
+#include <drivers/drv_hrt.h>
+
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
@@ -62,7 +64,6 @@ static int _airspeed_sub = -1;
 static int _esc_sub = -1;
 
 static orb_advert_t _esc_pub;
-struct esc_status_s _esc;
 
 static bool _home_position_set = false;
 static double _home_lat = 0.0d;
@@ -82,8 +83,6 @@ init_sub_messages(void)
 void 
 init_pub_messages(void)
 {
-	memset(&_esc, 0, sizeof(_esc));
-	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
 }
 
 void
@@ -106,23 +105,26 @@ publish_gam_message(const uint8_t *buffer)
 	size_t size = sizeof(msg);
 	memset(&msg, 0, size);
 	memcpy(&msg, buffer, size);
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+
+	// Publish it.
+	esc.timestamp = hrt_absolute_time();
+	esc.esc_count = 1;
+	esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
+
+	esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
+	esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+	esc.esc[0].esc_temperature = msg.temperature1 - 20; 
+	esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
+	esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
 
 	/* announce the esc if needed, just publish else */
 	if (_esc_pub > 0) {
-		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+		orb_publish(ORB_ID(esc_status), _esc_pub, &esc);
 	} else {
-		_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &esc);
 	}
-
-	// Publish it.
-	_esc.esc_count = 1;
-	_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
-
-	_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
-	_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
-	_esc.esc[0].esc_temperature = msg.temperature1 - 20; 
-	_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
-	_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
 }
 
 void 
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 3cb9abc49..5954c40da 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -92,8 +92,20 @@
 #define MOTOR_SPINUP_COUNTER			30
 #define ESC_UORB_PUBLISH_DELAY		500000
 
-
-
+struct MotorData_t {
+	unsigned int Version;                        // the version of the BL (0 = old)
+	unsigned int SetPoint;                       // written by attitude controller
+	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	float SetPoint_PX4; 			     // Values from PX4
+	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
+	unsigned int ReadMode;                       // select data to read
+	unsigned short RawPwmValue;							// length of PWM pulse
+	// the following bytes must be exactly in that order!
+	unsigned int Current;                        // in 0.1 A steps, read back from BL
+	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
+	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
+	unsigned int RoundCount;
+};
 
 class MK : public device::I2C
 {
@@ -154,8 +166,10 @@ private:
 
 	actuator_controls_s _controls;
 
+	MotorData_t Motor[MAX_MOTORS];
+
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
@@ -195,24 +209,6 @@ const int blctrlAddr_px4[]  = { 0, 0, 0, 0, 0, 0, 0, 0};
 
 int addrTranslator[] = {0, 0, 0, 0, 0, 0, 0, 0};
 
-struct MotorData_t {
-	unsigned int Version;                        // the version of the BL (0 = old)
-	unsigned int SetPoint;                       // written by attitude controller
-	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
-	float SetPoint_PX4; 			     // Values from PX4
-	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
-	unsigned int ReadMode;                       // select data to read
-	unsigned short RawPwmValue;							// length of PWM pulse
-	// the following bytes must be exactly in that order!
-	unsigned int Current;                        // in 0.1 A steps, read back from BL
-	unsigned int MaxPWM;                         // read back from BL is less than 255 if BL is in current limit
-	unsigned int Temperature;            // old BL-Ctrl will return a 255 here, the new version the temp. in
-	unsigned int RoundCount;
-};
-
-MotorData_t Motor[MAX_MOTORS];
-
-
 namespace
 {
 
diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 0ef056273..3fe1b0abc 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -526,6 +526,7 @@ void
 MS5611::cycle()
 {
 	int ret;
+	unsigned dummy;
 
 	/* collection phase? */
 	if (_collect_phase) {
@@ -542,6 +543,8 @@ MS5611::cycle()
 			} else {
 				//log("collection error %d", ret);
 			}
+			/* issue a reset command to the sensor */
+			_interface->ioctl(IOCTL_RESET, dummy);
 			/* reset the collection state machine and try again */
 			start_cycle();
 			return;
@@ -573,6 +576,8 @@ MS5611::cycle()
 	ret = measure();
 	if (ret != OK) {
 		//log("measure error %d", ret);
+		/* issue a reset command to the sensor */
+		_interface->ioctl(IOCTL_RESET, dummy);
 		/* reset the collection state machine and try again */
 		start_cycle();
 		return;
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 7258d5f9e..4d72ead9b 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -120,19 +120,25 @@ private:
 	uint32_t	_pwm_alt_rate_channels;
 	unsigned	_current_update_rate;
 	int		_task;
-	int		_t_actuators;
-	int		_t_actuator_armed;
-	orb_advert_t	_t_outputs;
+	int		_armed_sub;
+	orb_advert_t	_outputs_pub;
+	actuator_armed_s	_armed;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
 
 	volatile bool	_task_should_exit;
-	bool		_armed;
+	bool		_servo_armed;
 	bool		_pwm_on;
 
 	MixerGroup	*_mixers;
 
-	actuator_controls_s _controls;
+	uint32_t	_groups_required;
+	uint32_t	_groups_subscribed;
+	int		_control_subs[NUM_ACTUATOR_CONTROL_GROUPS];
+	actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS];
+	orb_id_t	_control_topics[NUM_ACTUATOR_CONTROL_GROUPS];
+	pollfd	_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS];
+	unsigned	_poll_fds_num;
 
 	pwm_limit_t	_pwm_limit;
 	uint16_t	_failsafe_pwm[_max_actuators];
@@ -143,13 +149,13 @@ private:
 	unsigned	_num_disarmed_set;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	static int	control_callback(uintptr_t handle,
 					 uint8_t control_group,
 					 uint8_t control_index,
 					 float &input);
-
+	void	subscribe();
 	int		set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
 	int		pwm_ioctl(file *filp, int cmd, unsigned long arg);
 
@@ -216,15 +222,18 @@ PX4FMU::PX4FMU() :
 	_pwm_alt_rate_channels(0),
 	_current_update_rate(0),
 	_task(-1),
-	_t_actuators(-1),
-	_t_actuator_armed(-1),
-	_t_outputs(0),
+	_control_subs({-1}),
+	_poll_fds_num(0),
+	_armed_sub(-1),
+	_outputs_pub(-1),
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
-	_armed(false),
+	_servo_armed(false),
 	_pwm_on(false),
 	_mixers(nullptr),
+	_groups_required(0),
+	_groups_subscribed(0),
 	_failsafe_pwm({0}),
 	      _disarmed_pwm({0}),
 	      _num_failsafe_set(0),
@@ -235,6 +244,14 @@ PX4FMU::PX4FMU() :
 		_max_pwm[i] = PWM_DEFAULT_MAX;
 	}
 
+	_control_topics[0] = ORB_ID(actuator_controls_0);
+	_control_topics[1] = ORB_ID(actuator_controls_1);
+	_control_topics[2] = ORB_ID(actuator_controls_2);
+	_control_topics[3] = ORB_ID(actuator_controls_3);
+
+	memset(_controls, 0, sizeof(_controls));
+	memset(_poll_fds, 0, sizeof(_poll_fds));
+
 	_debug_enabled = true;
 }
 
@@ -448,32 +465,42 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels)
 }
 
 void
+PX4FMU::subscribe()
+{
+	/* subscribe/unsubscribe to required actuator control groups */
+	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+	_poll_fds_num = 0;
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (sub_groups & (1 << i)) {
+			warnx("subscribe to actuator_controls_%d", i);
+			_control_subs[i] = orb_subscribe(_control_topics[i]);
+		}
+		if (unsub_groups & (1 << i)) {
+			warnx("unsubscribe from actuator_controls_%d", i);
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+
+		if (_control_subs[i] > 0) {
+			_poll_fds[_poll_fds_num].fd = _control_subs[i];
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num++;
+		}
+	}
+}
+
+void
 PX4FMU::task_main()
 {
-	/*
-	 * Subscribe to the appropriate PWM output topic based on whether we are the
-	 * primary PWM output or not.
-	 */
-	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
-				     ORB_ID(actuator_controls_1));
 	/* force a reset of the update rate */
 	_current_update_rate = 0;
 
-	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
-	orb_set_interval(_t_actuator_armed, 200);		/* 5Hz update rate */
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	/* advertise the mixed control outputs */
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	/* advertise the mixed control outputs */
-	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
-				   &outputs);
-
-	pollfd fds[2];
-	fds[0].fd = _t_actuators;
-	fds[0].events = POLLIN;
-	fds[1].fd = _t_actuator_armed;
-	fds[1].events = POLLIN;
 
 #ifdef HRT_PPM_CHANNEL
 	// rc input, published to ORB
@@ -491,6 +518,12 @@ PX4FMU::task_main()
 
 	/* loop until killed */
 	while (!_task_should_exit) {
+		if (_groups_subscribed != _groups_required) {
+			subscribe();
+			_groups_subscribed = _groups_required;
+			/* force setting update rate */
+			_current_update_rate = 0;
+		}
 
 		/*
 		 * Adjust actuator topic update rate to keep up with
@@ -515,20 +548,23 @@ PX4FMU::task_main()
 			}
 
 			debug("adjusted actuator update interval to %ums", update_rate_in_ms);
-			orb_set_interval(_t_actuators, update_rate_in_ms);
+			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					orb_set_interval(_control_subs[i], update_rate_in_ms);
+				}
+			}
 
 			// set to current max rate, even if we are actually checking slower/faster
 			_current_update_rate = max_rate;
 		}
 
 		/* sleep waiting for data, stopping to check for PPM
-		 * input at 100Hz */
-		int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS);
+		 * input at 50Hz */
+		int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS);
 
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 
 		} else if (ret == 0) {
@@ -537,89 +573,98 @@ PX4FMU::task_main()
 
 		} else {
 
-			/* do we have a control update? */
-			if (fds[0].revents & POLLIN) {
+			/* get controls for required topics */
+			unsigned poll_id = 0;
+			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					if (_poll_fds[poll_id].revents & POLLIN) {
+						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+					}
+					poll_id++;
+				}
+			}
 
-				/* get controls - must always do this to avoid spinning */
-				orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls);
+			/* can we mix? */
+			if (_mixers != nullptr) {
 
-				/* can we mix? */
-				if (_mixers != nullptr) {
+				unsigned num_outputs;
 
-					unsigned num_outputs;
+				switch (_mode) {
+				case MODE_2PWM:
+					num_outputs = 2;
+					break;
 
-					switch (_mode) {
-					case MODE_2PWM:
-						num_outputs = 2;
-						break;
+				case MODE_4PWM:
+					num_outputs = 4;
+					break;
 
-					case MODE_4PWM:
-						num_outputs = 4;
-						break;
+				case MODE_6PWM:
+					num_outputs = 6;
+					break;
 
-					case MODE_6PWM:
-						num_outputs = 6;
-						break;
+				default:
+					num_outputs = 0;
+					break;
+				}
 
-					default:
-						num_outputs = 0;
-						break;
+				/* do mixing */
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				/* iterate actuators */
+				for (unsigned i = 0; i < num_outputs; i++) {
+					/* last resort: catch NaN, INF and out-of-band errors */
+					if (i >= outputs.noutputs ||
+						!isfinite(outputs.output[i]) ||
+						outputs.output[i] < -1.0f ||
+						outputs.output[i] > 1.0f) {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = -1.0f;
 					}
+				}
 
-					/* do mixing */
-					outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
-					outputs.timestamp = hrt_absolute_time();
-
-					/* iterate actuators */
-					for (unsigned i = 0; i < num_outputs; i++) {
-						/* last resort: catch NaN, INF and out-of-band errors */
-						if (i >= outputs.noutputs ||
-						    !isfinite(outputs.output[i]) ||
-						    outputs.output[i] < -1.0f ||
-						    outputs.output[i] > 1.0f) {
-							/*
-							 * Value is NaN, INF or out of band - set to the minimum value.
-							 * This will be clearly visible on the servo status and will limit the risk of accidentally
-							 * spinning motors. It would be deadly in flight.
-							 */
-							outputs.output[i] = -1.0f;
-						}
-					}
+				uint16_t pwm_limited[num_outputs];
 
-					uint16_t pwm_limited[num_outputs];
+				pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
 
-					pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit);
+				/* output to the servos */
+				for (unsigned i = 0; i < num_outputs; i++) {
+					up_pwm_servo_set(i, pwm_limited[i]);
+				}
 
-					/* output to the servos */
-					for (unsigned i = 0; i < num_outputs; i++) {
-						up_pwm_servo_set(i, pwm_limited[i]);
-					}
+				/* publish mixed control outputs */
+				if (_outputs_pub < 0) {
+					_outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs);
+				} else {
 
-					/* and publish for anyone that cares to see */
-					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
+					orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs);
 				}
 			}
+		}
 
-			/* how about an arming update? */
-			if (fds[1].revents & POLLIN) {
-				actuator_armed_s aa;
+		/* check arming state */
+		bool updated = false;
+		orb_check(_armed_sub, &updated);
 
-				/* get new value */
-				orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa);
+		if (updated) {
+			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-				/* update the armed status and check that we're not locked down */
-				bool set_armed = aa.armed && !aa.lockdown;
+			/* update the armed status and check that we're not locked down */
+			bool set_armed = _armed.armed && !_armed.lockdown;
 
-				if (_armed != set_armed)
-					_armed = set_armed;
+			if (_servo_armed != set_armed)
+				_servo_armed = set_armed;
 
-				/* update PWM status if armed or if disarmed PWM values are set */
-				bool pwm_on = (aa.armed || _num_disarmed_set > 0);
+			/* update PWM status if armed or if disarmed PWM values are set */
+			bool pwm_on = (_armed.armed || _num_disarmed_set > 0);
 
-				if (_pwm_on != pwm_on) {
-					_pwm_on = pwm_on;
-					up_pwm_servo_arm(pwm_on);
-				}
+			if (_pwm_on != pwm_on) {
+				_pwm_on = pwm_on;
+				up_pwm_servo_arm(pwm_on);
 			}
 		}
 
@@ -661,8 +706,13 @@ PX4FMU::task_main()
 
 	}
 
-	::close(_t_actuators);
-	::close(_t_actuator_armed);
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (_control_subs > 0) {
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+	}
+	::close(_armed_sub);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -684,7 +734,7 @@ PX4FMU::control_callback(uintptr_t handle,
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-	input = controls->control[control_index];
+	input = controls[control_group].control[control_index];
 	return 0;
 }
 
@@ -1053,6 +1103,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		if (_mixers != nullptr) {
 			delete _mixers;
 			_mixers = nullptr;
+			_groups_required = 0;
 		}
 
 		break;
@@ -1061,18 +1112,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
 			SimpleMixer *mixer = new SimpleMixer(control_callback,
-							     (uintptr_t)&_controls, mixinfo);
+							     (uintptr_t)_controls, mixinfo);
 
 			if (mixer->check()) {
 				delete mixer;
+				_groups_required = 0;
 				ret = -EINVAL;
 
 			} else {
 				if (_mixers == nullptr)
 					_mixers = new MixerGroup(control_callback,
-								 (uintptr_t)&_controls);
+								 (uintptr_t)_controls);
 
 				_mixers->add_mixer(mixer);
+				_mixers->groups_required(_groups_required);
 			}
 
 			break;
@@ -1083,9 +1136,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			unsigned buflen = strnlen(buf, 1024);
 
 			if (_mixers == nullptr)
-				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
+				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
 
 			if (_mixers == nullptr) {
+				_groups_required = 0;
 				ret = -ENOMEM;
 
 			} else {
@@ -1096,7 +1150,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					debug("mixer load failed with %d", ret);
 					delete _mixers;
 					_mixers = nullptr;
+					_groups_required = 0;
 					ret = -EINVAL;
+				} else {
+
+					_mixers->groups_required(_groups_required);
 				}
 			}
 
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 8458c2fdb..aec6dd3b7 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -683,6 +683,25 @@ PX4IO::init()
 
 		/* send command to arm system via command API */
 		vehicle_command_s cmd;
+		/* send this to itself */
+		param_t sys_id_param = param_find("MAV_SYS_ID");
+		param_t comp_id_param = param_find("MAV_COMP_ID");
+
+		int32_t sys_id;
+		int32_t comp_id;
+
+		if (param_get(sys_id_param, &sys_id)) {
+			errx(1, "PRM SYSID");
+		}
+
+		if (param_get(comp_id_param, &comp_id)) {
+			errx(1, "PRM CMPID");
+		}
+
+		cmd.target_system = sys_id;
+		cmd.target_component = comp_id;
+		cmd.source_system = sys_id;
+		cmd.source_component = comp_id;
 		/* request arming */
 		cmd.param1 = 1.0f;
 		cmd.param2 = 0;
@@ -692,10 +711,7 @@ PX4IO::init()
 		cmd.param6 = 0;
 		cmd.param7 = 0;
 		cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
-		// cmd.target_system = status.system_id;
-		// cmd.target_component = status.component_id;
-		// cmd.source_system = status.system_id;
-		// cmd.source_component = status.component_id;
+
 		/* ask to confirm command */
 		cmd.confirmation =  1;
 
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index b7c9b89a4..5bb550279 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -94,7 +94,7 @@
 #elif HRT_TIMER == 3
 # define HRT_TIMER_BASE		STM32_TIM3_BASE
 # define HRT_TIMER_POWER_REG	STM32_RCC_APB1ENR
-# define HRT_TIMER_POWER_BIT	RCC_APB2ENR_TIM3EN
+# define HRT_TIMER_POWER_BIT	RCC_APB1ENR_TIM3EN
 # define HRT_TIMER_VECTOR	STM32_IRQ_TIM3
 # define HRT_TIMER_CLOCK	STM32_APB1_TIM3_CLKIN
 # if CONFIG_STM32_TIM3
@@ -354,6 +354,9 @@ __EXPORT uint16_t ppm_frame_length = 0;
 __EXPORT unsigned ppm_decoded_channels = 0;
 __EXPORT uint64_t ppm_last_valid_decode = 0;
 
+#define PPM_DEBUG 0
+
+#if PPM_DEBUG
 /* PPM edge history */
 __EXPORT uint16_t ppm_edge_history[32];
 unsigned ppm_edge_next;
@@ -361,6 +364,7 @@ unsigned ppm_edge_next;
 /* PPM pulse history */
 __EXPORT uint16_t ppm_pulse_history[32];
 unsigned ppm_pulse_next;
+#endif
 
 static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
 
@@ -455,10 +459,12 @@ hrt_ppm_decode(uint32_t status)
 	/* how long since the last edge? - this handles counter wrapping implicitely. */
 	width = count - ppm.last_edge;
 
+#if PPM_DEBUG
 	ppm_edge_history[ppm_edge_next++] = width;
 
 	if (ppm_edge_next >= 32)
 		ppm_edge_next = 0;
+#endif
 
 	/*
 	 * if this looks like a start pulse, then push the last set of values
@@ -546,10 +552,12 @@ hrt_ppm_decode(uint32_t status)
 		interval = count - ppm.last_mark;
 		ppm.last_mark = count;
 
+#if PPM_DEBUG
 		ppm_pulse_history[ppm_pulse_next++] = interval;
 
 		if (ppm_pulse_next >= 32)
 			ppm_pulse_next = 0;
+#endif
 
 		/* if the mark-mark timing is out of bounds, abandon the frame */
 		if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 1cbdf9bf8..7180048ff 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
 		int32_t board_rotation_int;
 		param_get(board_rotation_h, &(board_rotation_int));
 		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-		math::Matrix<3,3> board_rotation;
+		math::Matrix<3, 3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
+		math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
 		math::Vector<3> accel_offs_vec(&accel_offs[0]);
-		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
-		math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
-		math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+		math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
+		math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
+		math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
 
 		accel_scale.x_offset = accel_offs_rotated(0);
 		accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			}
 		}
 
-		if (old_done_count != done_count)
+		if (old_done_count != done_count) {
 			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
+		}
 
-		if (done)
+		if (done) {
 			break;
+		}
 
 		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
 				 (!data_collected[0]) ? "x+ " : "",
@@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
 
-				if (d > still_thr2 * 8.0f)
+				if (d > still_thr2 * 8.0f) {
 					d = still_thr2 * 8.0f;
+				}
 
-				if (d > accel_disp[i])
+				if (d > accel_disp[i]) {
 					accel_disp[i] = d;
+				}
 			}
 
 			/* still detector with hysteresis */
@@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 
 	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 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 0;        // [ g, 0, 0 ]
+	}
 
 	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 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 1;        // [ -g, 0, 0 ]
+	}
 
 	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 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 2;        // [ 0, g, 0 ]
+	}
 
 	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 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 3;        // [ 0, -g, 0 ]
+	}
 
 	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 ]
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
+		return 4;        // [ 0, 0, g ]
+	}
 
 	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 ]
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
+		return 5;        // [ 0, 0, -g ]
+	}
 
 	mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
 
@@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 			struct sensor_combined_s sensor;
 			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
-			for (int i = 0; i < 3; i++)
+			for (int i = 0; i < 3; i++) {
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
+			}
 
 			count++;
 
@@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 			continue;
 		}
 
-		if (errcount > samples_num / 10)
+		if (errcount > samples_num / 10) {
 			return ERROR;
+		}
 	}
 
 	for (int i = 0; i < 3; i++) {
@@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
 		    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.0f)
-		return ERROR;	// Singular matrix
+	if (det == 0.0f) {
+		return ERROR;        // Singular matrix
+	}
 
 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
 	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 	/* calculate inverse matrix for A */
 	float mat_A_inv[3][3];
 
-	if (mat_invert3(mat_A, mat_A_inv) != OK)
+	if (mat_invert3(mat_A, mat_A_inv) != OK) {
 		return ERROR;
+	}
 
 	/* copy results to accel_T */
 	for (int i = 0; i < 3; i++) {
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index c8c7a42e7..5d21d89d0 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd)
 
 	bool paramreset_successful = false;
 	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+
 		} else {
 			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
+
 		close(fd);
 	}
 
@@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd)
 			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;
 
-			if (calibration_counter % (calibration_count / 20) == 0)
+			if (calibration_counter % (calibration_count / 20) == 0) {
 				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+			}
 
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 53ed34f46..17d3d3dcd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -117,9 +117,10 @@ extern struct system_load_s system_load;
 #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 100000 /**< consider the local or global position estimate invalid after 100ms */
-#define RC_TIMEOUT 500000
-#define DIFFPRESS_TIMEOUT 2000000
+#define POSITION_TIMEOUT		(600 * 1000)		/**< consider the local or global position estimate invalid after 600ms */
+#define FAILSAFE_DEFAULT_TIMEOUT	(3 * 1000 * 1000)	/**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
+#define RC_TIMEOUT			500000
+#define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
 #define PRINT_MODE_REJECT_INTERVAL	2000000
@@ -220,7 +221,7 @@ 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);
 
-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
 
 /**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -232,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 
 int commander_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		usage("missing command");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
@@ -260,8 +262,9 @@ int commander_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "stop")) {
 
-		if (!thread_running)
+		if (!thread_running) {
 			errx(0, "commander already stopped");
+		}
 
 		thread_should_exit = true;
 
@@ -303,8 +306,9 @@ int commander_main(int argc, char *argv[])
 
 void usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}
 
 	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
@@ -363,20 +367,22 @@ void print_status()
 
 static orb_advert_t status_pub;
 
-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
 {
-    transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-    
-    // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
-    // output appropriate error messages if the state cannot transition.
-    arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
-    if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
-        mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
-    } else if (arming_res == TRANSITION_DENIED) {
-        tune_negative(true);
-    }
-    
-    return arming_res;
+	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+	// output appropriate error messages if the state cannot transition.
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+
+	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+		mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+
+	} else if (arming_res == TRANSITION_DENIED) {
+		tune_negative(true);
+	}
+
+	return arming_res;
 }
 
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
@@ -416,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 			}
 
-			if (hil_ret == OK)
+			if (hil_ret == OK) {
 				ret = true;
+			}
 
-            // Transition the arming state
-            arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+			// Transition the arming state
+			arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
 
-			if (arming_res == TRANSITION_CHANGED)
+			if (arming_res == TRANSITION_CHANGED) {
 				ret = true;
+			}
 
 			/* set main state */
 			transition_result_t main_res = TRANSITION_DENIED;
@@ -434,13 +442,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 					/* 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_ALTCTL) {
+					/* ALTCTL */
+					main_res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
-				} 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_POSCTL) {
+					/* POSCTL */
+					main_res = main_state_transition(status, MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
@@ -455,8 +463,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 				} 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);
+						/* POSCTL */
+						main_res = main_state_transition(status, MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
 						/* MANUAL */
@@ -465,8 +473,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 			}
 
-			if (main_res == TRANSITION_CHANGED)
+			if (main_res == TRANSITION_CHANGED) {
 				ret = true;
+			}
 
 			if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -479,19 +488,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-            // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
-            // We use an float epsilon delta to test float equality.
-            if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+			// We use an float epsilon delta to test float equality.
+			if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
 				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
-            } else {
-                transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
-                if (arming_res == TRANSITION_DENIED) {
-                    mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-                    result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-                } else {
-                    result = VEHICLE_CMD_RESULT_ACCEPTED;
-                }
-            }
+
+			} else {
+
+				// Flick to inair restore first if this comes from an onboard system
+				if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
+					status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+				}
+
+				transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+
+				if (arming_res == TRANSITION_DENIED) {
+					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				}
+			}
 		}
 		break;
 
@@ -519,7 +537,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;
 
-		/* Flight termination */
+	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
 
 			//XXX: to enable the parachute, a param needs to be set
@@ -539,6 +557,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
+
 			if (use_current) {
 				/* use current position */
 				if (status->condition_global_position_valid) {
@@ -582,6 +601,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			}
 		}
 		break;
+
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -595,6 +615,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
+
 	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
 		answer_command(*cmd, result);
@@ -628,8 +649,8 @@ int commander_thread_main(int argc, char *argv[])
 
 	char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "SEATBELT";
-	main_states_str[2] = "EASY";
+	main_states_str[1] = "ALTCTL";
+	main_states_str[2] = "POSCTL";
 	main_states_str[3] = "AUTO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
@@ -741,8 +762,9 @@ int commander_thread_main(int argc, char *argv[])
 	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;
+	hrt_abstime last_idle_time = 0;
+	hrt_abstime start_time = 0;
+	hrt_abstime last_auto_state_valid = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
@@ -771,6 +793,9 @@ int commander_thread_main(int argc, char *argv[])
 	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));
+	/* Init EPH and EPV */
+	global_position.eph = 1000.0f;
+	global_position.epv = 1000.0f;
 
 	/* Subscribe to local position data */
 	int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
@@ -873,6 +898,7 @@ int commander_thread_main(int argc, char *argv[])
 				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
 			}
+
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
@@ -913,6 +939,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* disarm if safety is now on and still armed */
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
 				}
@@ -930,23 +957,31 @@ int commander_thread_main(int argc, char *argv[])
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
 		bool eph_epv_good;
+
 		if (status.condition_global_position_valid) {
 			if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
 				eph_epv_good = false;
+
+			} else {
+				eph_epv_good = true;
 			}
 
 		} else {
 			if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
 				eph_epv_good = true;
+
+			} else {
+				eph_epv_good = false;
 			}
 		}
+
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
 
 		/* check if GPS fix is ok */
 
 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
-			(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+		    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
 
 			home.lat = global_position.lat;
 			home.lon = global_position.lon;
@@ -981,6 +1016,7 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
+
 		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
@@ -994,6 +1030,7 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
+
 		} else {
 			if (!published_condition_landed_fw) {
 				status.condition_landed = false; // Fixedwing does not have a landing detector currently
@@ -1063,8 +1100,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* 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
+			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;
 
@@ -1140,22 +1178,22 @@ int commander_thread_main(int argc, char *argv[])
 
 			status.rc_signal_lost = false;
 
-			transition_result_t res;	// store all transitions results here
+			transition_result_t arming_res;	// store all transitions results here
 
 			/* arm/disarm by RC */
-			res = TRANSITION_NOT_CHANGED;
+			arming_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
+			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST 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.condition_landed) &&
-			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
+			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 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);
+					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
 					stick_off_counter = 0;
 
 				} else {
@@ -1168,7 +1206,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (status.arming_state == ARMING_STATE_STANDBY &&
-			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
+			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
 						print_reject_arm("NOT ARMING: Press safety switch first.");
@@ -1177,7 +1215,7 @@ int commander_thread_main(int argc, char *argv[])
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
-						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 					}
 
 					stick_on_counter = 0;
@@ -1190,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[])
 				stick_on_counter = 0;
 			}
 
-			if (res == TRANSITION_CHANGED) {
+			if (arming_res == TRANSITION_CHANGED) {
 				if (status.arming_state == ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");
 
@@ -1198,24 +1236,24 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 				}
 
-			} else if (res == TRANSITION_DENIED) {
+			} else if (arming_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}
 
 			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
 				/* recover from failsafe */
-				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+				(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 			}
 
 			/* evaluate the main state machine according to mode switches */
-			res = set_main_state_rc(&status, &sp_man);
+			transition_result_t main_res = set_main_state_rc(&status, &sp_man);
 
 			/* play tune on mode change only if armed, blink LED always */
-			if (res == TRANSITION_CHANGED) {
+			if (main_res == TRANSITION_CHANGED) {
 				tune_positive(armed.armed);
 
-			} else if (res == TRANSITION_DENIED) {
+			} else if (main_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
@@ -1228,7 +1266,8 @@ int commander_thread_main(int argc, char *argv[])
 				status.set_nav_state_timestamp = hrt_absolute_time();
 
 			} else {
-				/* MISSION switch */
+
+				/* LOITER switch */
 				if (sp_man.loiter_switch == SWITCH_POS_ON) {
 					/* stick is in LOITER position */
 					status.set_nav_state = NAV_STATE_LOITER;
@@ -1240,7 +1279,7 @@ int commander_thread_main(int argc, char *argv[])
 					status.set_nav_state_timestamp = hrt_absolute_time();
 
 				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
+					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
 					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
 					status.set_nav_state = NAV_STATE_MISSION;
 					status.set_nav_state_timestamp = hrt_absolute_time();
@@ -1257,34 +1296,39 @@ int commander_thread_main(int argc, char *argv[])
 			if (armed.armed) {
 				if (status.main_state == MAIN_STATE_AUTO) {
 					/* check if AUTO mode still allowed */
-					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+					transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);
+
+					if (auto_res == TRANSITION_NOT_CHANGED) {
+						last_auto_state_valid = hrt_absolute_time();
+					}
 
-					if (res == TRANSITION_DENIED) {
+					/* still invalid state after the timeout interval, execute failsafe */
+					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
 						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
-						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+						auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
-						if (res == TRANSITION_DENIED) {
+						if (auto_res == TRANSITION_DENIED) {
 							/* LAND not allowed, set TERMINATION state */
-							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
 					}
 
 				} else {
 					/* failsafe for manual modes */
-					transition_result_t res = TRANSITION_DENIED;
+					transition_result_t manual_res = TRANSITION_DENIED;
 
 					if (!status.condition_landed) {
 						/* vehicle is not landed, try to perform RTL */
-						res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
 					}
 
-					if (res == TRANSITION_DENIED) {
+					if (manual_res == TRANSITION_DENIED) {
 						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
-						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
 
-						if (res == TRANSITION_DENIED) {
+						if (manual_res == TRANSITION_DENIED) {
 							/* LAND not allowed, set TERMINATION state */
-							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
 					}
 				}
@@ -1292,14 +1336,14 @@ int commander_thread_main(int argc, char *argv[])
 			} else {
 				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
 					/* reset failsafe when disarmed */
-					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+					(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 				}
 			}
 		}
 
 		// TODO remove this hack
-		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
+		/* flight termination in manual mode if assist switch is on POSCTL position */
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive(armed.armed);
 			}
@@ -1313,8 +1357,9 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
+			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
 				status_changed = true;
+			}
 		}
 
 		/* check which state machines for changes, clear "changed" flag */
@@ -1331,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
 			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
-				(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+			    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
 
 				// TODO remove code duplication
 				home.lat = global_position.lat;
@@ -1353,6 +1398,7 @@ int commander_thread_main(int argc, char *argv[])
 				status.condition_home_position_valid = true;
 			}
 		}
+
 		was_armed = armed.armed;
 
 		if (main_state_changed) {
@@ -1516,21 +1562,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 	} else if (actuator_armed->ready_to_arm) {
 		/* ready to arm, blink at 1Hz */
-		if (leds_counter % 20 == 0)
+		if (leds_counter % 20 == 0) {
 			led_toggle(LED_BLUE);
+		}
 
 	} else {
 		/* not ready to arm, blink at 10Hz */
-		if (leds_counter % 2 == 0)
+		if (leds_counter % 2 == 0) {
 			led_toggle(LED_BLUE);
+		}
 	}
 
 #endif
 
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
 	if (status->load > 0.95f) {
-		if (leds_counter % 2 == 0)
+		if (leds_counter % 2 == 0) {
 			led_toggle(LED_AMBER);
+		}
 
 	} else {
 		led_off(LED_AMBER);
@@ -1555,26 +1604,26 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		// TRANSITION_DENIED is not possible here
 		break;
 
-	case SWITCH_POS_MIDDLE:		// ASSISTED
-		if (sp_man->assisted_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status, MAIN_STATE_EASY);
+	case SWITCH_POS_MIDDLE:		// ASSIST
+		if (sp_man->posctl_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_POSCTL);
 
 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}
 
-			// else fallback to SEATBELT
-			print_reject_mode(status, "EASY");
+			// else fallback to ALTCTL
+			print_reject_mode(status, "POSCTL");
 		}
 
-		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
 		}
 
-		if (sp_man->assisted_switch != SWITCH_POS_ON) {
-			print_reject_mode(status, "SEATBELT");
+		if (sp_man->posctl_switch != SWITCH_POS_ON) {
+			print_reject_mode(status, "ALTCTL");
 		}
 
 		// else fallback to MANUAL
@@ -1589,9 +1638,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			break;	// changed successfully or already in this state
 		}
 
-		// else fallback to SEATBELT (EASY likely will not work too)
+		// else fallback to ALTCTL (POSCTL likely will not work too)
 		print_reject_mode(status, "AUTO");
-		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+		res = main_state_transition(status, MAIN_STATE_ALTCTL);
 
 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
@@ -1637,7 +1686,7 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = false;
 			break;
 
-		case MAIN_STATE_SEATBELT:
+		case MAIN_STATE_ALTCTL:
 			control_mode.flag_control_manual_enabled = true;
 			control_mode.flag_control_auto_enabled = false;
 			control_mode.flag_control_rates_enabled = true;
@@ -1648,7 +1697,7 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = false;
 			break;
 
-		case MAIN_STATE_EASY:
+		case MAIN_STATE_POSCTL:
 			control_mode.flag_control_manual_enabled = true;
 			control_mode.flag_control_auto_enabled = false;
 			control_mode.flag_control_rates_enabled = true;
@@ -1744,7 +1793,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-			tune_positive(true);
+		tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
@@ -1794,8 +1843,9 @@ void *commander_low_prio_loop(void *arg)
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
 		/* timed out - periodic check for thread_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1810,8 +1860,9 @@ void *commander_low_prio_loop(void *arg)
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
 		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
-		    cmd.command == VEHICLE_CMD_DO_SET_SERVO)
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
 			continue;
+		}
 
 		/* only handle low-priority commands here */
 		switch (cmd.command) {
@@ -1889,6 +1940,7 @@ void *commander_low_prio_loop(void *arg)
 					/* airspeed calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_airspeed_calibration(mavlink_fd);
+
 				} else if ((int)(cmd.param4) == 0) {
 					/* RC calibration ended - have we been in one worth confirming? */
 					if (status.rc_input_blocked) {
@@ -1903,10 +1955,12 @@ void *commander_low_prio_loop(void *arg)
 
 				}
 
-				if (calib_ret == OK)
+				if (calib_ret == OK) {
 					tune_positive(true);
-				else
+
+				} else {
 					tune_negative(true);
+				}
 
 				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 
@@ -1926,11 +1980,13 @@ void *commander_low_prio_loop(void *arg)
 						mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
 
 						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0)
+						if (ret < 0) {
 							ret = -ret;
+						}
 
-						if (ret < 1000)
+						if (ret < 1000) {
 							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+						}
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1946,11 +2002,13 @@ void *commander_low_prio_loop(void *arg)
 						mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
 
 						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0)
+						if (ret < 0) {
 							ret = -ret;
+						}
 
-						if (ret < 1000)
+						if (ret < 1000) {
 							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+						}
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1960,8 +2018,8 @@ void *commander_low_prio_loop(void *arg)
 			}
 
 		case VEHICLE_CMD_START_RX_PAIR:
-		/* handled in the IO driver */
-		break;
+			/* handled in the IO driver */
+			break;
 
 		default:
 			/* don't answer on unsupported commands, it will be done in main loop */
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 0fd3c9e9e..86f77cdb8 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -113,17 +113,22 @@ void buzzer_deinit()
 	close(buzzer);
 }
 
-void set_tune(int tune) {
+void set_tune(int tune)
+{
 	unsigned int new_tune_duration = tune_durations[tune];
+
 	/* don't interrupt currently playing non-repeating tune by repeating */
 	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
 		/* allow interrupting current non-repeating tune by the same tune */
 		if (tune != tune_current || new_tune_duration != 0) {
 			ioctl(buzzer, TONE_SET_ALARM, tune);
 		}
+
 		tune_current = tune;
+
 		if (new_tune_duration != 0) {
 			tune_end = hrt_absolute_time() + new_tune_duration;
+
 		} else {
 			tune_end = 0;
 		}
@@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer)
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_GREEN);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
 	if (use_buzzer) {
 		set_tune(TONE_NOTIFY_POSITIVE_TUNE);
 	}
@@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer)
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_WHITE);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
 	if (use_buzzer) {
 		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
 	}
@@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer)
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
 	if (use_buzzer) {
 		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
 	}
@@ -244,22 +252,25 @@ int led_off(int led)
 void rgbled_set_color(rgbled_color_t color)
 {
 
-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+	}
 }
 
 void rgbled_set_mode(rgbled_mode_t mode)
 {
 
-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+	}
 }
 
 void rgbled_set_pattern(rgbled_pattern_t *pattern)
 {
 
-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+	}
 }
 
 float battery_remaining_estimate_voltage(float voltage, float discharged)
@@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
 		ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
+
 	} else {
 		/* else use voltage */
 		ret = remaining_voltage;
diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 8a946543d..ee0d08391 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
 	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	// MANUAL to SEATBELT.
+	// MANUAL to ALTCTRL.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = true;
-	new_main_state = MAIN_STATE_SEATBELT;
-	ut_assert("tranisition: manual to seatbelt", 
+	new_main_state = MAIN_STATE_ALTCTL;
+	ut_assert("tranisition: manual to altctrl",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+	ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);
 
-	// MANUAL to SEATBELT, invalid local altitude.
+	// MANUAL to ALTCTRL, invalid local altitude.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = false;
-	new_main_state = MAIN_STATE_SEATBELT;
+	new_main_state = MAIN_STATE_ALTCTL;
 	ut_assert("no transition: invalid local altitude",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
 	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
 
-	// MANUAL to EASY.
+	// MANUAL to POSCTRL.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = true;
-	new_main_state = MAIN_STATE_EASY;
-	ut_assert("transition: manual to easy",
+	new_main_state = MAIN_STATE_POSCTL;
+	ut_assert("transition: manual to posctrl",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+	ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);
 
-	// MANUAL to EASY, invalid local position.
+	// MANUAL to POSCTRL, invalid local position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = false;
-	new_main_state = MAIN_STATE_EASY;
+	new_main_state = MAIN_STATE_POSCTL;
 	ut_assert("no transition: invalid position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
 	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 30cd0d48d..cbc2844c1 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
 				gyro_scale.z_offset += gyro_report.z;
 				calibration_counter++;
 
-				if (calibration_counter % (calibration_count / 20) == 0)
+				if (calibration_counter % (calibration_count / 20) == 0) {
 					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+				}
 
 			} else {
 				poll_errcount++;
@@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
 	/* apply new offsets */
 	fd = open(GYRO_DEVICE_PATH, 0);
 
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
 		warn("WARNING: failed to apply new offsets for gyro");
+	}
 
 	close(fd);
 
@@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	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; }
 
-	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();
@@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
 			//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; }
 
-			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; }
 
-			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;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 4ebf266f4..9296db6ed 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -124,6 +124,7 @@ int do_mag_calibration(int mavlink_fd)
 			res = ERROR;
 			return res;
 		}
+
 	} else {
 		/* exit */
 		return ERROR;
@@ -163,8 +164,9 @@ int do_mag_calibration(int mavlink_fd)
 
 				calibration_counter++;
 
-				if (calibration_counter % (calibration_maxcount / 20) == 0)
+				if (calibration_counter % (calibration_maxcount / 20) == 0) {
 					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
+				}
 
 			} else {
 				poll_errcount++;
@@ -198,14 +200,17 @@ int do_mag_calibration(int mavlink_fd)
 		}
 	}
 
-	if (x != NULL)
+	if (x != NULL) {
 		free(x);
+	}
 
-	if (y != NULL)
+	if (y != NULL) {
 		free(y);
+	}
 
-	if (z != NULL)
+	if (z != NULL) {
 		free(z);
+	}
 
 	if (res == OK) {
 		/* apply calibration and set parameters */
@@ -234,23 +239,29 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (res == OK) {
 			/* set parameters */
-			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
+			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
+			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
+			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
+			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
 				res = ERROR;
+			}
 
-			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
+			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
 				res = ERROR;
+			}
 
 			if (res != OK) {
 				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index 2144d3460..a83c81850 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -12,8 +12,8 @@
 
 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_ALTCTL,
+	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
 };
 
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 41f3ca0aa..0776894fb 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
 	/* set parameters */
-	float p = sp.roll;
+	float p = sp.y;
 	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
+	p = sp.x;
 	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
+	p = sp.r;
 	param_set(param_find("TRIM_YAW"), &p);
 
 	/* store to permanent storage */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f09d586c7..abcf72717 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -75,38 +75,38 @@ static bool failsafe_state_changed = true;
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
-    //                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
-    { /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
-    { /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
-    { /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
-    { /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
-    { /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
-    { /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
-    { /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+	//                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
+	{ /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
+	{ /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
+	{ /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
+	{ /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
+	{ /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
+	{ /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
+	{ /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
 };
 
 // You can index into the array with an arming_state_t in order to get it's textual representation
-static const char* state_names[ARMING_STATE_MAX] = {
-    "ARMING_STATE_INIT",
-    "ARMING_STATE_STANDBY",
-    "ARMING_STATE_ARMED",
-    "ARMING_STATE_ARMED_ERROR",
-    "ARMING_STATE_STANDBY_ERROR",
-    "ARMING_STATE_REBOOT",
-    "ARMING_STATE_IN_AIR_RESTORE",
+static const char *state_names[ARMING_STATE_MAX] = {
+	"ARMING_STATE_INIT",
+	"ARMING_STATE_STANDBY",
+	"ARMING_STATE_ARMED",
+	"ARMING_STATE_ARMED_ERROR",
+	"ARMING_STATE_STANDBY_ERROR",
+	"ARMING_STATE_REBOOT",
+	"ARMING_STATE_IN_AIR_RESTORE",
 };
 
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
-                        const struct safety_s   *safety,            /// current safety settings
-                        arming_state_t          new_arming_state,   /// arming state requested
-                        struct actuator_armed_s *armed,             /// current armed status
-                        const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
+			const struct safety_s   *safety,            /// current safety settings
+			arming_state_t          new_arming_state,   /// arming state requested
+			struct actuator_armed_s *armed,             /// current armed status
+			const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
 {
-    // Double check that our static arrays are still valid
-    ASSERT(ARMING_STATE_INIT == 0);
-    ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
-    
+	// Double check that our static arrays are still valid
+	ASSERT(ARMING_STATE_INIT == 0);
+	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+
 	/*
 	 * Perform an atomic state update
 	 */
@@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	/* 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 {
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
+
 		} else {
 			armed->lockdown = false;
 		}
-        
-        // Check that we have a valid state transition
-        bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
-        if (valid_transition) {
-            // We have a good transition. Now perform any secondary validation.
-            if (new_arming_state == ARMING_STATE_ARMED) {
-                // Fail transition if we need safety switch press
-                //      Allow if coming from in air restore
-                //      Allow if HIL_STATE_ON
-                if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
-                    if (mavlink_fd) {
-                        mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
-                    }
-                    valid_transition = false;
-                }
-            } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
-                new_arming_state = ARMING_STATE_STANDBY_ERROR;
-            }
-        }
-        
-        // HIL can always go to standby
-        if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
-            valid_transition = true;
-        }
-        
-        /* Sensors need to be initialized for STANDBY state */
-        if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
-            valid_transition = false;
-        }
-
-        // Finish up the state transition
-        if (valid_transition) {
-            armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
-            armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
-            ret = TRANSITION_CHANGED;
-            status->arming_state = new_arming_state;
-            arming_state_changed = true;
-        }
-    }
-    
+
+		// Check that we have a valid state transition
+		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
+
+		if (valid_transition) {
+			// We have a good transition. Now perform any secondary validation.
+			if (new_arming_state == ARMING_STATE_ARMED) {
+				// Fail transition if we need safety switch press
+				//      Allow if coming from in air restore
+				//      Allow if HIL_STATE_ON
+				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
+					if (mavlink_fd) {
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
+					}
+
+					valid_transition = false;
+				}
+
+			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
+				new_arming_state = ARMING_STATE_STANDBY_ERROR;
+			}
+		}
+
+		// HIL can always go to standby
+		if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+			valid_transition = true;
+		}
+
+		/* Sensors need to be initialized for STANDBY state */
+		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			valid_transition = false;
+		}
+
+		// Finish up the state transition
+		if (valid_transition) {
+			armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
+			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+			ret = TRANSITION_CHANGED;
+			status->arming_state = new_arming_state;
+			arming_state_changed = true;
+		}
+	}
+
 	/* end of atomic state update */
 	irqrestore(flags);
 
-    if (ret == TRANSITION_DENIED) {
-        static const char* errMsg = "Invalid arming transition from %s to %s";
-        if (mavlink_fd) {
-            mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-        }
-        warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-    }
+	if (ret == TRANSITION_DENIED) {
+		static const char *errMsg = "Invalid arming transition from %s to %s";
+
+		if (mavlink_fd) {
+			mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+		}
+
+		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+	}
 
 	return ret;
 }
@@ -215,7 +222,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		ret = TRANSITION_CHANGED;
 		break;
 
-	case MAIN_STATE_SEATBELT:
+	case MAIN_STATE_ALTCTL:
 
 		/* need at minimum altitude estimate */
 		if (!status->is_rotary_wing ||
@@ -226,7 +233,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 
 		break;
 
-	case MAIN_STATE_EASY:
+	case MAIN_STATE_POSCTL:
 
 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
@@ -320,6 +327,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 				/* list directory */
 				DIR		*d;
 				d = opendir("/dev");
+
 				if (d) {
 
 					struct dirent	*direntry;
@@ -331,26 +339,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 						if (!strncmp("tty", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip mtd devices */
 						if (!strncmp("mtd", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip ram devices */
 						if (!strncmp("ram", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip MMC devices */
 						if (!strncmp("mmc", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip mavlink */
 						if (!strcmp("mavlink", direntry->d_name)) {
 							continue;
 						}
+
 						/* skip console */
 						if (!strcmp("console", direntry->d_name)) {
 							continue;
 						}
+
 						/* skip null */
 						if (!strcmp("null", direntry->d_name)) {
 							continue;
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
deleted file mode 100644
index 2aeca3a98..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ /dev/null
@@ -1,169 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@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 fixedwing_att_control_rate.c
- * Implementation of a fixed wing attitude controller.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.h>
-
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/systemlib.h>
-
-#include "fixedwing_att_control_att.h"
-
-
-struct fw_att_control_params {
-	float roll_p;
-	float rollrate_lim;
-	float pitch_p;
-	float pitchrate_lim;
-	float yawrate_lim;
-	float pitch_roll_compensation_p;
-};
-
-struct fw_pos_control_param_handles {
-	param_t roll_p;
-	param_t rollrate_lim;
-	param_t pitch_p;
-	param_t pitchrate_lim;
-	param_t yawrate_lim;
-	param_t pitch_roll_compensation_p;
-};
-
-
-
-/* Internal Prototypes */
-static int parameters_init(struct fw_pos_control_param_handles *h);
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p);
-
-static int parameters_init(struct fw_pos_control_param_handles *h)
-{
-	/* PID parameters */
-	h->roll_p 		=	param_find("FW_ROLL_P");
-	h->rollrate_lim =	param_find("FW_ROLLR_LIM");
-	h->pitch_p 		=	param_find("FW_PITCH_P");
-	h->pitchrate_lim =	param_find("FW_PITCHR_LIM");
-	h->yawrate_lim =	param_find("FW_YAWR_LIM");
-	h->pitch_roll_compensation_p = param_find("FW_PITCH_RCOMP");
-
-	return OK;
-}
-
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_att_control_params *p)
-{
-	param_get(h->roll_p, &(p->roll_p));
-	param_get(h->rollrate_lim, &(p->rollrate_lim));
-	param_get(h->pitch_p, &(p->pitch_p));
-	param_get(h->pitchrate_lim, &(p->pitchrate_lim));
-	param_get(h->yawrate_lim, &(p->yawrate_lim));
-	param_get(h->pitch_roll_compensation_p, &(p->pitch_roll_compensation_p));
-
-	return OK;
-}
-
-int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				   const struct vehicle_attitude_s *att,
-				   const float speed_body[],
-				   struct vehicle_rates_setpoint_s *rates_sp)
-{
-	static int counter = 0;
-	static bool initialized = false;
-
-	static struct fw_att_control_params p;
-	static struct fw_pos_control_param_handles h;
-
-	static PID_t roll_controller;
-	static PID_t pitch_controller;
-
-
-	if (!initialized) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (counter % 100 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim);
-		pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim);
-	}
-
-	/* Roll (P) */
-	rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0);
-
-
-	/* Pitch (P) */
-
-	/* compensate feedforward for loss of lift due to non-horizontal angle of wing */
-	float pitch_sp_rollcompensation = p.pitch_roll_compensation_p * fabsf(sinf(att_sp->roll_body));
-	/* set pitch plus feedforward roll compensation */
-	rates_sp->pitch = pid_calculate(&pitch_controller,
-					att_sp->pitch_body + pitch_sp_rollcompensation,
-					att->pitch, 0, 0);
-
-	/* Yaw (from coordinated turn constraint or lateral force) */
-	rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll))
-			/ (speed_body[0] * cosf(att->roll) * cosf(att->pitch) + speed_body[2] * sinf(att->pitch));
-
-//	printf("rates_sp->yaw %.4f \n", (double)rates_sp->yaw);
-
-	counter++;
-
-	return 0;
-}
-
-
-
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h b/src/modules/fixedwing_att_control/fixedwing_att_control_att.h
deleted file mode 100644
index 600e35b89..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@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 Fixed Wing Attitude Control */
-
-#ifndef FIXEDWING_ATT_CONTROL_ATT_H_
-#define FIXEDWING_ATT_CONTROL_ATT_H_
-
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_global_position.h>
-
-int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-				   const struct vehicle_attitude_s *att,
-				   const float speed_body[],
-				   struct vehicle_rates_setpoint_s *rates_sp);
-
-#endif /* FIXEDWING_ATT_CONTROL_ATT_H_ */
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
deleted file mode 100644
index b6b4546c2..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ /dev/null
@@ -1,367 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	@author Thomas Gubler <thomasgubler@student.ethz.ch>
- *   			@author Doug Weibel <douglas.weibel@colorado.edu>
- *
- * 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 fixedwing_att_control.c
- * Implementation of a fixed wing attitude controller.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.h>
-#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>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/debug_key_value.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-
-#include "fixedwing_att_control_rate.h"
-#include "fixedwing_att_control_att.h"
-
-/* Prototypes */
-/**
- * Deamon management function.
- */
-__EXPORT int fixedwing_att_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int fixedwing_att_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/* Variables */
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-/* Main Thread */
-int fixedwing_att_control_thread_main(int argc, char *argv[])
-{
-	/* read arguments */
-	bool verbose = false;
-
-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-			verbose = true;
-		}
-	}
-
-	/* welcome user */
-	printf("[fixedwing att control] started\n");
-
-	/* declare and safely initialize all structs */
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	struct vehicle_global_position_s global_pos;
-	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));
-
-	/* output structs */
-	struct actuator_controls_s actuators;
-	memset(&actuators, 0, sizeof(actuators));
-
-
-	/* publish actuator controls */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
-		actuators.control[i] = 0.0f;
-	}
-
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
-	orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
-
-	/* subscribe */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	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 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};
-	float speed_body[3] = {0.0f, 0.0f, 0.0f};
-	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
-
-	while (!thread_should_exit) {
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		poll(&fds, 1, 500);
-
-		/* Check if there is a new position measurement or  attitude setpoint */
-		bool pos_updated;
-		orb_check(global_pos_sub, &pos_updated);
-		bool att_sp_updated;
-		orb_check(att_sp_sub, &att_sp_updated);
-
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-
-		if (att_sp_updated)
-			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp);
-
-		if (pos_updated) {
-			orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-
-			if (att.R_valid) {
-				speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
-				speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
-				speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
-
-			} else {
-				speed_body[0] = 0;
-				speed_body[1] = 0;
-				speed_body[2] = 0;
-
-				printf("FW ATT CONTROL: Did not get a valid R\n");
-			}
-		}
-
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
-		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;
-
-		/* 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) {
-
-					/* put plane into loiter */
-					att_sp.roll_body = 0.3f;
-					att_sp.pitch_body = 0.0f;
-
-					/* limit throttle to 60 % of last value if sane */
-					if (isfinite(manual_sp.throttle) &&
-					    (manual_sp.throttle >= 0.0f) &&
-					    (manual_sp.throttle <= 1.0f)) {
-						att_sp.thrust = 0.6f * manual_sp.throttle;
-
-					} else {
-						att_sp.thrust = 0.0f;
-					}
-
-					att_sp.yaw_body = 0;
-
-					// XXX disable yaw control, loiter
-
-				} else {
-
-					att_sp.roll_body = manual_sp.roll;
-					att_sp.pitch_body = manual_sp.pitch;
-					att_sp.yaw_body = 0;
-					att_sp.thrust = manual_sp.throttle;
-				}
-
-				att_sp.timestamp = hrt_absolute_time();
-
-				/* pass through flaps */
-				if (isfinite(manual_sp.flaps)) {
-					actuators.control[4] = manual_sp.flaps;
-
-				} else {
-					actuators.control[4] = 0.0f;
-				}
-
-			} else {
-				/* directly pass through values */
-				actuators.control[0] = manual_sp.roll;
-				/* positive pitch means negative actuator -> pull up */
-				actuators.control[1] = manual_sp.pitch;
-				actuators.control[2] = manual_sp.yaw;
-				actuators.control[3] = manual_sp.throttle;
-
-				if (isfinite(manual_sp.flaps)) {
-					actuators.control[4] = manual_sp.flaps;
-
-				} else {
-					actuators.control[4] = 0.0f;
-				}
-			}
-		}
-		
-		/* 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);
-
-		/* sanity check and publish actuator outputs */
-		if (isfinite(actuators.control[0]) &&
-		    isfinite(actuators.control[1]) &&
-		    isfinite(actuators.control[2]) &&
-		    isfinite(actuators.control[3])) {
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		}
-	}
-
-	printf("[fixedwing_att_control] exiting, stopping all motors.\n");
-	thread_running = false;
-
-	/* kill all outputs */
-	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
-		actuators.control[i] = 0.0f;
-
-	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-
-
-
-	close(att_sub);
-	close(actuator_pub);
-	close(rates_pub);
-
-	fflush(stdout);
-	exit(0);
-
-	return 0;
-
-}
-
-/* Startup Functions */
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: fixedwing_att_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int fixedwing_att_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("fixedwing_att_control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("fixedwing_att_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 20,
-					 2048,
-					 fixedwing_att_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tfixedwing_att_control is running\n");
-
-		} else {
-			printf("\tfixedwing_att_control not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-
-
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
deleted file mode 100644
index cdab39edc..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ /dev/null
@@ -1,211 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@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 fixedwing_att_control_rate.c
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *
- * Implementation of a fixed wing attitude controller.
- *
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.h>
-
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/systemlib.h>
-
-#include "fixedwing_att_control_rate.h"
-
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-// Roll control parameters
-PARAM_DEFINE_FLOAT(FW_ROLLR_P, 0.9f);
-PARAM_DEFINE_FLOAT(FW_ROLLR_I, 0.2f);
-PARAM_DEFINE_FLOAT(FW_ROLLR_AWU, 0.9f);
-PARAM_DEFINE_FLOAT(FW_ROLLR_LIM, 0.7f);   // Roll rate limit in radians/sec, applies to the roll controller
-PARAM_DEFINE_FLOAT(FW_ROLL_P, 4.0f);
-PARAM_DEFINE_FLOAT(FW_PITCH_RCOMP, 0.1f);
-
-//Pitch control parameters
-PARAM_DEFINE_FLOAT(FW_PITCHR_P, 0.8f);
-PARAM_DEFINE_FLOAT(FW_PITCHR_I, 0.2f);
-PARAM_DEFINE_FLOAT(FW_PITCHR_AWU, 0.8f);
-PARAM_DEFINE_FLOAT(FW_PITCHR_LIM, 0.35f);   // Pitch rate limit in radians/sec, applies to the pitch controller
-PARAM_DEFINE_FLOAT(FW_PITCH_P, 8.0f);
-
-//Yaw control parameters					//XXX TODO this is copy paste, asign correct values
-PARAM_DEFINE_FLOAT(FW_YAWR_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_YAWR_I, 0.0f);
-PARAM_DEFINE_FLOAT(FW_YAWR_AWU, 0.0f);
-PARAM_DEFINE_FLOAT(FW_YAWR_LIM, 0.35f);   // Yaw rate limit in radians/sec
-
-/* feedforward compensation */
-PARAM_DEFINE_FLOAT(FW_PITCH_THR_P, 0.1f);	/**< throttle to pitch coupling feedforward */
-
-struct fw_rate_control_params {
-	float rollrate_p;
-	float rollrate_i;
-	float rollrate_awu;
-	float pitchrate_p;
-	float pitchrate_i;
-	float pitchrate_awu;
-	float yawrate_p;
-	float yawrate_i;
-	float yawrate_awu;
-	float pitch_thr_ff;
-};
-
-struct fw_rate_control_param_handles {
-	param_t rollrate_p;
-	param_t rollrate_i;
-	param_t rollrate_awu;
-	param_t pitchrate_p;
-	param_t pitchrate_i;
-	param_t pitchrate_awu;
-	param_t yawrate_p;
-	param_t yawrate_i;
-	param_t yawrate_awu;
-	param_t pitch_thr_ff;
-};
-
-
-
-/* Internal Prototypes */
-static int parameters_init(struct fw_rate_control_param_handles *h);
-static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p);
-
-static int parameters_init(struct fw_rate_control_param_handles *h)
-{
-	/* PID parameters */
-	h->rollrate_p 	 =	param_find("FW_ROLLR_P");   //TODO define rate params for fixed wing
-	h->rollrate_i 	 =	param_find("FW_ROLLR_I");
-	h->rollrate_awu  =	param_find("FW_ROLLR_AWU");
-
-	h->pitchrate_p 	 =	param_find("FW_PITCHR_P");
-	h->pitchrate_i 	 =	param_find("FW_PITCHR_I");
-	h->pitchrate_awu =	param_find("FW_PITCHR_AWU");
-
-	h->yawrate_p 	 =	param_find("FW_YAWR_P");
-	h->yawrate_i 	 =	param_find("FW_YAWR_I");
-	h->yawrate_awu   =	param_find("FW_YAWR_AWU");
-	h->pitch_thr_ff  =	param_find("FW_PITCH_THR_P");
-
-	return OK;
-}
-
-static int parameters_update(const struct fw_rate_control_param_handles *h, struct fw_rate_control_params *p)
-{
-	param_get(h->rollrate_p, &(p->rollrate_p));
-	param_get(h->rollrate_i, &(p->rollrate_i));
-	param_get(h->rollrate_awu, &(p->rollrate_awu));
-	param_get(h->pitchrate_p, &(p->pitchrate_p));
-	param_get(h->pitchrate_i, &(p->pitchrate_i));
-	param_get(h->pitchrate_awu, &(p->pitchrate_awu));
-	param_get(h->yawrate_p, &(p->yawrate_p));
-	param_get(h->yawrate_i, &(p->yawrate_i));
-	param_get(h->yawrate_awu, &(p->yawrate_awu));
-	param_get(h->pitch_thr_ff, &(p->pitch_thr_ff));
-
-	return OK;
-}
-
-int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-				const float rates[],
-				struct actuator_controls_s *actuators)
-{
-	static int counter = 0;
-	static bool initialized = false;
-
-	static struct fw_rate_control_params p;
-	static struct fw_rate_control_param_handles h;
-
-	static PID_t roll_rate_controller;
-	static PID_t pitch_rate_controller;
-	static PID_t yaw_rate_controller;
-
-	static uint64_t last_run = 0;
-	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	last_run = hrt_absolute_time();
-
-	if (!initialized) {
-		parameters_init(&h);
-		parameters_update(&h, &p);
-		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
-		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		initialized = true;
-	}
-
-	/* load new parameters with lower rate */
-	if (counter % 100 == 0) {
-		/* update parameters from storage */
-		parameters_update(&h, &p);
-		pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1);
-		pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1);
-		pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1);
-	}
-
-
-	/* roll rate (PI) */
-	actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
-	/* pitch rate (PI) */
-	actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
-	/* yaw rate (PI) */
-	actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
-
-	counter++;
-
-	return 0;
-}
-
-
-
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
deleted file mode 100644
index 500e3e197..000000000
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@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 Fixed Wing Attitude Rate Control */
-
-#ifndef FIXEDWING_ATT_CONTROL_RATE_H_
-#define FIXEDWING_ATT_CONTROL_RATE_H_
-
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-
-int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
-				const float rates[],
-				struct actuator_controls_s *actuators);
-
-#endif /* FIXEDWING_ATT_CONTROL_RATE_H_ */
diff --git a/src/modules/fixedwing_att_control/module.mk b/src/modules/fixedwing_att_control/module.mk
deleted file mode 100644
index fd1a8724a..000000000
--- a/src/modules/fixedwing_att_control/module.mk
+++ /dev/null
@@ -1,42 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Fixedwing Attitude Control application
-#
-
-MODULE_COMMAND	= fixedwing_att_control
-
-SRCS		= fixedwing_att_control_main.c \
-		  fixedwing_att_control_att.c \
-		  fixedwing_att_control_rate.c
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index cfae07275..bbb39f20f 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[CH_RDR] = _manual.yaw;
 		_actuators.control[CH_THR] = _manual.throttle;
 
-	} else if (_status.main_state == MAIN_STATE_SEATBELT ||
-		_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
+	} else if (_status.main_state == MAIN_STATE_ALTCTL ||
+		_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {
 
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
deleted file mode 100644
index 888dd0942..000000000
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ /dev/null
@@ -1,479 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	@author Thomas Gubler <thomasgubler@student.ethz.ch>
- *   			@author Doug Weibel <douglas.weibel@colorado.edu>
- *
- * 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 fixedwing_pos_control.c
- * Implementation of a fixed wing attitude controller.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <drivers/drv_hrt.h>
-#include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/parameter_update.h>
-#include <systemlib/param/param.h>
-#include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/systemlib.h>
-
-/*
- * Controller parameters, accessible via MAVLink
- *
- */
-PARAM_DEFINE_FLOAT(FW_HEAD_P, 0.1f);
-PARAM_DEFINE_FLOAT(FW_HEADR_I, 0.1f);
-PARAM_DEFINE_FLOAT(FW_HEADR_LIM, 1.5f); //TODO: think about reasonable value
-PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f);		// Radians per meter off track
-PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f);
-PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f);	// Roll angle limit in radians
-PARAM_DEFINE_FLOAT(FW_HEADR_P, 0.1f);
-PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	/**< Pitch angle limit in radians per second */
-
-struct fw_pos_control_params {
-	float heading_p;
-	float headingr_p;
-	float headingr_i;
-	float headingr_lim;
-	float xtrack_p;
-	float altitude_p;
-	float roll_lim;
-	float pitch_lim;
-};
-
-struct fw_pos_control_param_handles {
-	param_t heading_p;
-	param_t headingr_p;
-	param_t headingr_i;
-	param_t headingr_lim;
-	param_t xtrack_p;
-	param_t altitude_p;
-	param_t roll_lim;
-	param_t pitch_lim;
-};
-
-
-struct planned_path_segments_s {
-	bool segment_type;
-	double start_lat;			// Start of line or center of arc
-	double start_lon;
-	double end_lat;
-	double end_lon;
-	float radius;				// Radius of arc
-	float arc_start_bearing;		// Bearing from center to start of arc
-	float arc_sweep;				// Angle (radians) swept out by arc around center.
-	// Positive for clockwise, negative for counter-clockwise
-};
-
-
-/* Prototypes */
-/* Internal Prototypes */
-static int parameters_init(struct fw_pos_control_param_handles *h);
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p);
-
-/**
- * Deamon management function.
- */
-__EXPORT int fixedwing_pos_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of deamon.
- */
-int fixedwing_pos_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-/* Variables */
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-
-/**
- * Parameter management
- */
-static int parameters_init(struct fw_pos_control_param_handles *h)
-{
-	/* PID parameters */
-	h->heading_p 		=	param_find("FW_HEAD_P");
-	h->headingr_p 		=	param_find("FW_HEADR_P");
-	h->headingr_i 		=	param_find("FW_HEADR_I");
-	h->headingr_lim 	=	param_find("FW_HEADR_LIM");
-	h->xtrack_p 		=	param_find("FW_XTRACK_P");
-	h->altitude_p 		=	param_find("FW_ALT_P");
-	h->roll_lim 		=	param_find("FW_ROLL_LIM");
-	h->pitch_lim 		=	param_find("FW_PITCH_LIM");
-
-	return OK;
-}
-
-static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p)
-{
-	param_get(h->heading_p, &(p->heading_p));
-	param_get(h->headingr_p, &(p->headingr_p));
-	param_get(h->headingr_i, &(p->headingr_i));
-	param_get(h->headingr_lim, &(p->headingr_lim));
-	param_get(h->xtrack_p, &(p->xtrack_p));
-	param_get(h->altitude_p, &(p->altitude_p));
-	param_get(h->roll_lim, &(p->roll_lim));
-	param_get(h->pitch_lim, &(p->pitch_lim));
-
-	return OK;
-}
-
-
-/* Main Thread */
-int fixedwing_pos_control_thread_main(int argc, char *argv[])
-{
-	/* read arguments */
-	bool verbose = false;
-
-	for (int i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
-			verbose = true;
-		}
-	}
-
-	/* welcome user */
-	printf("[fixedwing pos control] started\n");
-
-	/* declare and safely initialize all structs */
-	struct vehicle_global_position_s global_pos;
-	memset(&global_pos, 0, sizeof(global_pos));
-	struct vehicle_global_position_s start_pos;		// Temporary variable, replace with
-	memset(&start_pos, 0, sizeof(start_pos));		// previous waypoint when available
-	struct vehicle_global_position_setpoint_s global_setpoint;
-	memset(&global_setpoint, 0, sizeof(global_setpoint));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct crosstrack_error_s xtrack_err;
-	memset(&xtrack_err, 0, sizeof(xtrack_err));
-	struct parameter_update_s param_update;
-	memset(&param_update, 0, sizeof(param_update));
-
-	/* output structs */
-	struct vehicle_attitude_setpoint_s attitude_setpoint;
-	memset(&attitude_setpoint, 0, sizeof(attitude_setpoint));
-
-	/* publish attitude setpoint */
-	attitude_setpoint.roll_body = 0.0f;
-	attitude_setpoint.pitch_body = 0.0f;
-	attitude_setpoint.yaw_body = 0.0f;
-	attitude_setpoint.thrust = 0.0f;
-	orb_advert_t attitude_setpoint_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &attitude_setpoint);
-
-	/* subscribe */
-	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int param_sub = orb_subscribe(ORB_ID(parameter_update));
-
-	/* Setup of loop */
-	struct pollfd fds[2] = {
-		{ .fd = param_sub, .events = POLLIN },
-		{ .fd = att_sub, .events = POLLIN }
-	};
-	bool global_sp_updated_set_once = false;
-
-	float psi_track = 0.0f;
-
-	int counter = 0;
-
-	struct fw_pos_control_params p;
-	struct fw_pos_control_param_handles h;
-
-	PID_t heading_controller;
-	PID_t heading_rate_controller;
-	PID_t offtrack_controller;
-	PID_t altitude_controller;
-
-	parameters_init(&h);
-	parameters_update(&h, &p);
-	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
-	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
-	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
-	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
-
-	/* error and performance monitoring */
-	perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
-	perf_counter_t fw_err_perf = perf_alloc(PC_COUNT, "fixedwing_pos_control_err");
-
-	while (!thread_should_exit) {
-		/* wait for a sensor update, check for exit condition every 500 ms */
-		int ret = poll(fds, 2, 500);
-
-		if (ret < 0) {
-			/* poll error, count it in perf */
-			perf_count(fw_err_perf);
-
-		} else if (ret == 0) {
-			/* no return value, ignore */
-		} else {
-
-			/* only update parameters if they changed */
-			if (fds[0].revents & POLLIN) {
-				/* read from param to clear updated flag */
-				struct parameter_update_s update;
-				orb_copy(ORB_ID(parameter_update), param_sub, &update);
-
-				/* update parameters from storage */
-				parameters_update(&h, &p);
-				pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit
-				pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim);
-				pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim);
-				pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value
-			}
-
-			/* only run controller if attitude changed */
-			if (fds[1].revents & POLLIN) {
-
-
-				static uint64_t last_run = 0;
-				const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-				last_run = hrt_absolute_time();
-
-				/* check if there is a new position or setpoint */
-				bool pos_updated;
-				orb_check(global_pos_sub, &pos_updated);
-				bool global_sp_updated;
-				orb_check(global_setpoint_sub, &global_sp_updated);
-
-				/* load local copies */
-				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-
-				if (pos_updated) {
-					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-				}
-
-				if (global_sp_updated) {
-					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
-					start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint)
-					global_sp_updated_set_once = true;
-					psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon,
-							(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-
-					printf("next wp direction: %0.4f\n", (double)psi_track);
-				}
-
-				/* Simple Horizontal Control */
-				if (global_sp_updated_set_once) {
-					//				if (counter % 100 == 0)
-					//					printf("lat_sp %d, ln_sp %d, lat: %d, lon: %d\n", global_setpoint.lat, global_setpoint.lon, global_pos.lat, global_pos.lon);
-
-					/* calculate crosstrack error */
-					// Only the case of a straight line track following handled so far
-					int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
-										(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
-										(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
-
-					// XXX what is xtrack_err.past_end?
-					if (distance_res == OK /*&& !xtrack_err.past_end*/) {
-
-						float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance
-
-						float psi_c = psi_track + delta_psi_c;
-						float psi_e = psi_c - att.yaw;
-
-						/* wrap difference back onto -pi..pi range */
-						psi_e = _wrap_pi(psi_e);
-
-						if (verbose) {
-							printf("xtrack_err.distance %.4f ", (double)xtrack_err.distance);
-							printf("delta_psi_c %.4f ", (double)delta_psi_c);
-							printf("psi_c %.4f ", (double)psi_c);
-							printf("att.yaw %.4f ", (double)att.yaw);
-							printf("psi_e %.4f ", (double)psi_e);
-						}
-
-						/* calculate roll setpoint, do this artificially around zero */
-						float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f);
-						float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following
-						float psi_rate_c = delta_psi_rate_c + psi_rate_track;
-
-						/* limit turn rate */
-						if (psi_rate_c > p.headingr_lim) {
-							psi_rate_c = p.headingr_lim;
-
-						} else if (psi_rate_c < -p.headingr_lim) {
-							psi_rate_c = -p.headingr_lim;
-						}
-
-						float psi_rate_e = psi_rate_c - att.yawspeed;
-
-						// XXX sanity check: Assume 10 m/s stall speed and no stall condition
-						float ground_speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-
-						if (ground_speed < 10.0f) {
-							ground_speed = 10.0f;
-						}
-
-						float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g
-
-						attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT);
-
-						if (verbose) {
-							printf("psi_rate_c %.4f ", (double)psi_rate_c);
-							printf("psi_rate_e_scaled %.4f ", (double)psi_rate_e_scaled);
-							printf("rollbody %.4f\n", (double)attitude_setpoint.roll_body);
-						}
-
-						if (verbose && counter % 100 == 0)
-							printf("xtrack_err.distance: %0.4f, delta_psi_c: %0.4f\n", xtrack_err.distance, delta_psi_c);
-
-					} else {
-						if (verbose && counter % 100 == 0)
-							printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
-					}
-
-					/* Very simple Altitude Control */
-					if (pos_updated) {
-
-						//TODO: take care of relative vs. ab. altitude
-						attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f);
-
-					}
-
-					// XXX need speed control
-					attitude_setpoint.thrust = 0.7f;
-
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), attitude_setpoint_pub, &attitude_setpoint);
-
-					/* measure in what intervals the controller runs */
-					perf_count(fw_interval_perf);
-
-					counter++;
-
-				} else {
-					// XXX no setpoint, decent default needed (loiter?)
-				}
-			}
-		}
-	}
-
-	printf("[fixedwing_pos_control] exiting.\n");
-	thread_running = false;
-
-
-	close(attitude_setpoint_pub);
-
-	fflush(stdout);
-	exit(0);
-
-	return 0;
-
-}
-
-/* Startup Functions */
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	fprintf(stderr, "usage: fixedwing_pos_control {start|stop|status}\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_create().
- */
-int fixedwing_pos_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("fixedwing_pos_control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("fixedwing_pos_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 20,
-					 2048,
-					 fixedwing_pos_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			printf("\tfixedwing_pos_control is running\n");
-
-		} else {
-			printf("\tfixedwing_pos_control not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
diff --git a/src/modules/fixedwing_pos_control/module.mk b/src/modules/fixedwing_pos_control/module.mk
deleted file mode 100644
index b976377e9..000000000
--- a/src/modules/fixedwing_pos_control/module.mk
+++ /dev/null
@@ -1,40 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Fixedwing PositionControl application
-#
-
-MODULE_COMMAND	= fixedwing_pos_control
-
-SRCS		= fixedwing_pos_control_main.c
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 5276b1c13..1c411fa06 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -273,7 +273,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };
 
 namespace att_control
@@ -706,20 +706,21 @@ FixedwingAttitudeControl::task_main()
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
-					 * and a typical remote can only do 45 degrees, the mapping is
-					 * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
+					 * and a typical remote can only do around 45 degrees, the mapping is
+					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
 					 *
 					 * With this mapping the stick angle is a 1:1 representation of
-					 * the commanded attitude. If more than 45 degrees are desired,
-					 * a scaling parameter can be applied to the remote.
+					 * the commanded attitude.
 					 *
 					 * The trim gets subtracted here from the manual setpoint to get
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
-					pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
-					throttle_sp = _manual.throttle;
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+						+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+						+ _parameters.pitchsp_offset_rad;
+					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
 					/*
@@ -825,10 +826,10 @@ FixedwingAttitudeControl::task_main()
 
 			} else {
 				/* manual/direct control */
-				_actuators.control[0] = _manual.roll;
-				_actuators.control[1] = -_manual.pitch;
-				_actuators.control[2] = _manual.yaw;
-				_actuators.control[3] = _manual.throttle;
+				_actuators.control[0] = _manual.y;
+				_actuators.control[1] = -_manual.x;
+				_actuators.control[2] = _manual.r;
+				_actuators.control[3] = _manual.z;
 				_actuators.control[4] = _manual.flaps;
 			}
 
diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
index f076c94fd..fb8abe95a 100644
--- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -237,7 +237,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };
 
 namespace estimator
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 5cdab10a1..20f3fefcd 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -89,6 +89,7 @@
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
+#include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
 
@@ -135,6 +136,7 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
+	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
@@ -148,13 +150,14 @@ private:
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
+	struct range_finder_report 			_range_finder;			/**< range finder report */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
 
 	/** manual control states */
-	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
+	float		_altctrl_hold_heading;		/**< heading the system should hold in altctrl mode */
 	double		_loiter_hold_lat;
 	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
@@ -182,7 +185,7 @@ private:
 	/* Landingslope object */
 	Landingslope landingslope;
 
-	float flare_curve_alt_last;
+	float flare_curve_alt_rel_last;
 	/* heading hold */
 	float target_bearing;
 
@@ -242,6 +245,7 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
+		float range_finder_rel_alt;
 
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -286,6 +290,7 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
+		param_t range_finder_rel_alt;
 
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -312,6 +317,12 @@ private:
 	bool		vehicle_airspeed_poll();
 
 	/**
+	 * Check for range finder updates.
+	 */
+	bool		range_finder_poll();
+
+
+	/**
 	 * Check for position updates.
 	 */
 	void		vehicle_attitude_poll();
@@ -332,6 +343,11 @@ private:
 	void navigation_capabilities_publish();
 
 	/**
+	 * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+	 */
+	float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+
+	/**
 	 * Control position.
 	 */
 	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
@@ -348,7 +364,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 
 	/*
 	 * Reset takeoff state
@@ -368,6 +384,7 @@ private:
 			float pitch_min_rad, float pitch_max_rad,
 			float throttle_min, float throttle_max, float throttle_cruise,
 			bool climbout_mode, float climbout_pitch_min_rad,
+			float altitude,
 			const math::Vector<3> &ground_speed,
 			fwPosctrl::mTecs::tecs_mode mode = fwPosctrl::mTecs::TECS_MODE_NORMAL);
 
@@ -399,6 +416,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_range_finder_sub(-1),
 
 /* publications */
 	_attitude_sp_pub(-1),
@@ -418,7 +436,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	launch_detected(false),
 	last_manual(false),
 	usePreTakeoffThrust(false),
-	flare_curve_alt_last(0.0f),
+	flare_curve_alt_rel_last(0.0f),
 	launchDetector(),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
@@ -434,7 +452,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_pos_sp_triplet(),
 	_sensor_combined(),
 	_mTecs(),
-	_was_pos_control_mode(false)
+	_was_pos_control_mode(false),
+	_range_finder()
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -459,6 +478,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
@@ -548,6 +568,8 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
 
+	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -646,6 +668,20 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }
 
+bool
+FixedwingPositionControl::range_finder_poll()
+{
+	/* check if there is a range finder measurement */
+	bool range_finder_updated;
+	orb_check(_range_finder_sub, &range_finder_updated);
+
+	if (range_finder_updated) {
+		orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
+	}
+
+	return range_finder_updated;
+}
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -774,6 +810,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }
 
+float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+{
+	float rel_alt_estimated = current_alt - land_setpoint_alt;
+
+	/* only use range finder if:
+	 * parameter (range_finder_use_relative_alt) > 0
+	 * the measurement is valid
+	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
+	 */
+	if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
+		return rel_alt_estimated;
+	}
+
+	return range_finder.distance;
+
+}
+
 bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
@@ -852,7 +905,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						false, math::radians(_parameters.pitch_limit_min), ground_speed);
+						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) {
 
@@ -865,7 +918,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-						false, math::radians(_parameters.pitch_limit_min), ground_speed);
+						false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 
@@ -924,12 +977,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
-			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+			float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);
 
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
-			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
+			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
+
+			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
 
-			if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 
 				/* land with minimal speed */
 
@@ -939,7 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;
 
-				 if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
+				 if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -948,19 +1003,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 				 }
 
-				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
+				float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 
 				/* avoid climbout */
-				if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
+				if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
 				{
-					flare_curve_alt = pos_sp_triplet.current.alt;
+					flare_curve_alt_rel = 0.0f; // stay on ground
 					land_stayonground = true;
 				}
 
-				tecs_update_pitch_throttle(flare_curve_alt, calculate_target_airspeed(airspeed_land), eas2tas,
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+						calculate_target_airspeed(airspeed_land), eas2tas,
 						flare_pitch_angle_rad, math::radians(15.0f),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad, ground_speed, land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
+						false, flare_pitch_angle_rad,
+						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
+						land_motor_lim ? fwPosctrl::mTecs::TECS_MODE_LAND_THROTTLELIM : fwPosctrl::mTecs::TECS_MODE_LAND);
 
 				if (!land_noreturn_vertical) {
 					mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring");
@@ -968,7 +1026,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
-				flare_curve_alt_last = flare_curve_alt;
+				flare_curve_alt_rel_last = flare_curve_alt_rel;
 			} else {
 
 				 /* intersect glide slope:
@@ -976,22 +1034,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				  * if current position is higher or within 10m of slope follow the glide slope
 				  * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
 				  * */
-				float altitude_desired = _global_pos.alt;
-				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
+				float altitude_desired_rel = relative_alt;
+				if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
 					/* stay on slope */
-					altitude_desired = landing_slope_alt_desired;
+					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
 						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
 						land_onslope = true;
 					}
 				} else {
 					/* continue horizontally */
-					altitude_desired =  math::max(_global_pos.alt, L_altitude);
+					altitude_desired_rel =  math::max(relative_alt, L_altitude_rel);
 				}
 
-				tecs_update_pitch_throttle(altitude_desired, calculate_target_airspeed(airspeed_approach), eas2tas,
+				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 						_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+						_pos_sp_triplet.current.alt + relative_alt,
 						false, math::radians(_parameters.pitch_limit_min), ground_speed);
 			}
 
@@ -1030,10 +1089,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				if (altitude_error > 15.0f) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
-					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), eas2tas,
-							math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-							_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), ground_speed, fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
+					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
+							calculate_target_airspeed(1.3f * _parameters.airspeed_min),
+							eas2tas,
+							math::radians(_parameters.pitch_limit_min),
+							math::radians(_parameters.pitch_limit_max),
+							_parameters.throttle_min, _parameters.throttle_max,
+							_parameters.throttle_cruise,
+							true,
+							math::max(math::radians(pos_sp_triplet.current.pitch_min),
+							math::radians(10.0f)),
+							_global_pos.alt,
+							ground_speed,
+							fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
 
 					/* limit roll motion to ensure enough lift */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f));
@@ -1042,7 +1110,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
 								math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 								_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								false, math::radians(_parameters.pitch_limit_min), ground_speed);
+								false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 				}
 
 			} else {
@@ -1074,23 +1142,23 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else if (0/* easy mode enabled */) {
+	} else if (0/* posctrl mode enabled */) {
 
 		_was_pos_control_mode = false;
 
-		/** EASY FLIGHT **/
+		/** POSCTRL FLIGHT **/
 
-		if (0/* switched from another mode to easy */) {
-			_seatbelt_hold_heading = _att.yaw;
-		}
+	if (0/* switched from another mode to posctrl */) {
+		_altctrl_hold_heading = _att.yaw;
+	}
 
-		if (0/* easy on and manual control yaw non-zero */) {
-			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
-		}
+	if (0/* posctrl on and manual control yaw non-zero */) {
+		_altctrl_hold_heading = _att.yaw + _manual.r;
+	}
 
-		//XXX not used
+	//XXX not used
 
-		/* climb out control */
+	/* climb out control */
 //		bool climb_out = false;
 //
 //		/* user wants to climb out */
@@ -1098,44 +1166,45 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 //			climb_out = true;
 //		}
 
-		/* if in seatbelt mode, set airspeed based on manual control */
+	/* if in altctrl mode, set airspeed based on manual control */
 
-		// XXX check if ground speed undershoot should be applied here
-		float seatbelt_airspeed = _parameters.airspeed_min +
-					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.throttle;
+	// XXX check if ground speed undershoot should be applied here
+	float altctrl_airspeed = _parameters.airspeed_min +
+					(_parameters.airspeed_max - _parameters.airspeed_min) *
+					_manual.z;
 
-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d);
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
 		_att_sp.roll_body = _l1_control.nav_roll();
 		_att_sp.yaw_body = _l1_control.nav_bearing();
-		tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
+
+		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
 					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					false, math::radians(_parameters.pitch_limit_min), ground_speed);
+					false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
 
-	} else if (0/* seatbelt mode enabled */) {
+	} else if (0/* altctrl mode enabled */) {
 
 		_was_pos_control_mode = false;
 
-		/** SEATBELT FLIGHT **/
+		/** ALTCTRL FLIGHT **/
 
-		if (0/* switched from another mode to seatbelt */) {
-			_seatbelt_hold_heading = _att.yaw;
+		if (0/* switched from another mode to altctrl */) {
+			_altctrl_hold_heading = _att.yaw;
 		}
 
-		if (0/* seatbelt on and manual control yaw non-zero */) {
-			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		if (0/* altctrl on and manual control yaw non-zero */) {
+			_altctrl_hold_heading = _att.yaw + _manual.r;
 		}
 
-		/* if in seatbelt mode, set airspeed based on manual control */
+		/* if in altctrl mode, set airspeed based on manual control */
 
 		// XXX check if ground speed undershoot should be applied here
-		float seatbelt_airspeed = _parameters.airspeed_min +
+		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.throttle;
+					  _manual.z;
 
 		/* user switched off throttle */
-		if (_manual.throttle < 0.1f) {
+		if (_manual.z < 0.1f) {
 			throttle_max = 0.0f;
 			/* switch to pure pitch based altitude control, give up speed */
 			_tecs.set_speed_weight(0.0f);
@@ -1145,17 +1214,18 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		bool climb_out = false;
 
 		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+		if (_manual.x > 0.3f && _manual.z > 0.8f) {
 			climb_out = true;
 		}
 
-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed_2d);
-		_att_sp.roll_body =	_manual.roll;
-		_att_sp.yaw_body =	_manual.yaw;
-		tecs_update_pitch_throttle(_global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, eas2tas,
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
+		_att_sp.roll_body =	_manual.y;
+		_att_sp.yaw_body =	_manual.r;
+		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
 					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
 					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					climb_out, math::radians(_parameters.pitch_limit_min), ground_speed);
+					climb_out, math::radians(_parameters.pitch_limit_min),
+					_global_pos.alt, ground_speed);
 
 	} else {
 
@@ -1210,6 +1280,7 @@ FixedwingPositionControl::task_main()
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -1289,6 +1360,7 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
+			range_finder_poll();
 			// vehicle_baro_poll();
 
 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
@@ -1357,6 +1429,7 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		float pitch_min_rad, float pitch_max_rad,
 		float throttle_min, float throttle_max, float throttle_cruise,
 		bool climbout_mode, float climbout_pitch_min_rad,
+		float altitude,
 		const math::Vector<3> &ground_speed,
 		fwPosctrl::mTecs::tecs_mode mode)
 {
@@ -1366,9 +1439,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 		if (ground_speed_length > FLT_EPSILON) {
 			flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
 		}
-		_mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
+		_mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode);
 	} else {
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
 					    climbout_mode, climbout_pitch_min_rad,
 					    throttle_min, throttle_max, throttle_cruise,
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 37f06dbe5..f258f77da 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
  * @group L1 Control
  */
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
+
+/**
+ * Relative altitude threshold for range finder measurements for use during landing
+ *
+ * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
+ * set to < 0 to disable
+ * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp
index e5f7023ae..8ce465fe8 100644
--- a/src/modules/fw_pos_control_l1/landingslope.cpp
+++ b/src/modules/fw_pos_control_l1/landingslope.cpp
@@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues()
 	_horizontal_slope_displacement = (_flare_length - _d1);
 }
 
-float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
+float  Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
 {
-	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad);
 }
 
-float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
 	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
-		return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
+		return getLandingSlopeRelativeAltitude(wp_landing_distance);
+	else
+		return 0.0f;
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude)
+{
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+{
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude);
 	else
 		return wp_altitude;
 }
 
-float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
 	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
-		return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
 	else
-		return wp_landing_altitude;
+		return 0.0f;
+}
+
+float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+{
+
+	return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 }
 
diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h
index 76d65a55f..b54fd501c 100644
--- a/src/modules/fw_pos_control_l1/landingslope.h
+++ b/src/modules/fw_pos_control_l1/landingslope.h
@@ -63,11 +63,26 @@ public:
 	Landingslope() {}
 	~Landingslope() {}
 
+
+	/**
+	 *
+	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeRelativeAltitude(float wp_landing_distance);
+
+	/**
+	 *
+	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
+
+
 	/**
 	 *
 	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 */
-	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
+	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude);
 
 	/**
 	 *
@@ -78,11 +93,20 @@ public:
 
 	/**
 	 *
+	 * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative
+	}
+
+	/**
+	 *
 	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 */
 	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
 	{
-		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
+		return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude;
 	}
 
 	/**
@@ -95,8 +119,9 @@ public:
 
 	}
 
+	float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
 
-	float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
+	float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
 
 	void update(float landing_slope_angle_rad,
 			float flare_relative_alt,
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 7ecca0d65..dd88b0949 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -727,9 +727,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
 	if (param == PARAM_INVALID) { return 1; }
 
 	/* buffers for param transmission */
-	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+	char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
 	float val_buf;
-	static mavlink_message_t tx_msg;
+	mavlink_message_t tx_msg;
 
 	/* query parameter type */
 	param_type_t type = param_type(param);
@@ -1535,6 +1535,8 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string)
 {
 	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
 	mavlink_statustext_t statustext;
+	statustext.severity = MAV_SEVERITY_INFO;
+
 	int i = 0;
 
 	while (i < len - 1) {
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 678ce1645..79dd88657 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
 
-		} else if (status->main_state == MAIN_STATE_SEATBELT) {
+		} else if (status->main_state == MAIN_STATE_ALTCTL) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;
 
-		} else if (status->main_state == MAIN_STATE_EASY) {
+		} else if (status->main_state == MAIN_STATE_POSCTL) {
 			*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;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;
 
 		} else if (status->main_state == MAIN_STATE_AUTO) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -819,11 +819,11 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		bool updated = status_sub->update(t);
-		updated |= pos_sp_triplet_sub->update(t);
-		updated |= act_sub->update(t);
+		bool updated = act_sub->update(t);
+		(void)pos_sp_triplet_sub->update(t);
+		(void)status_sub->update(t);
 
-		if (updated) {
+		if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
 			/* translate the current syste state to mavlink state and mode */
 			uint8_t mavlink_state;
 			uint8_t mavlink_base_mode;
@@ -1138,10 +1138,10 @@ protected:
 		if (manual_sub->update(t)) {
 			mavlink_msg_manual_control_send(_channel,
 							mavlink_system.sysid,
-							manual->roll * 1000,
-							manual->pitch * 1000,
-							manual->yaw * 1000,
-							manual->throttle * 1000,
+							manual->x * 1000,
+							manual->y * 1000,
+							manual->z * 1000,
+							manual->r * 1000,
 							0);
 		}
 	}
@@ -1339,22 +1339,23 @@ protected:
 
 	void send(const hrt_abstime t)
 	{
-		(void)range_sub->update(t);
+		if (range_sub->update(t)) {
 
-		uint8_t type;
+			uint8_t type;
 
-		switch (range->type) {
-			case RANGE_FINDER_TYPE_LASER:
-			type = MAV_DISTANCE_SENSOR_LASER;
-			break;
-		}
+			switch (range->type) {
+				case RANGE_FINDER_TYPE_LASER:
+				type = MAV_DISTANCE_SENSOR_LASER;
+				break;
+			}
 
-		uint8_t id = 0;
-		uint8_t orientation = 0;
-		uint8_t covariance = 20;
+			uint8_t id = 0;
+			uint8_t orientation = 0;
+			uint8_t covariance = 20;
 
-		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
-			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+			mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
+				range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+		}
 	}
 };
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7c93c1c00..b03a68c07 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		}
 
 	}
-	
+
     /* If we've received a valid message, mark the flag indicating so.
 	   This is used in the '-w' command-line flag. */
 	_mavlink->set_has_received_messages(true);
@@ -217,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 			_mavlink->_task_should_exit = true;
 
 		} else {
+
+			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
+				warnx("ignoring CMD spoofed with same SYS/COMP ID");
+				return;
+			}
+
 			struct vehicle_command_s vcmd;
 			memset(&vcmd, 0, sizeof(vcmd));
 
@@ -432,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	memset(&manual, 0, sizeof(manual));
 
 	manual.timestamp = hrt_absolute_time();
-	manual.pitch = man.x / 1000.0f;
-	manual.roll = man.y / 1000.0f;
-	manual.yaw = man.r / 1000.0f;
-	manual.throttle = man.z / 1000.0f;
+	manual.x = man.x / 1000.0f;
+	manual.y = man.y / 1000.0f;
+	manual.r = man.r / 1000.0f;
+	manual.z = man.z / 1000.0f;
 
 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 36d95bf06..a69153bf0 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -71,7 +71,7 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
-#include <mathlib/mathlib.h>
+#include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 
 /**
@@ -156,6 +156,7 @@ private:
 		param_t yaw_rate_i;
 		param_t yaw_rate_d;
 		param_t yaw_ff;
+		param_t yaw_rate_max;
 
 		param_t man_roll_max;
 		param_t man_pitch_max;
@@ -168,6 +169,7 @@ private:
 		math::Vector<3> rate_i;				/**< I gain for angular rate error */
 		math::Vector<3> rate_d;				/**< D gain for angular rate error */
 		float yaw_ff;						/**< yaw control feed-forward */
+		float yaw_rate_max;					/**< max yaw rate */
 
 		float man_roll_max;
 		float man_pitch_max;
@@ -225,9 +227,9 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);
 
 	/**
-	 * Main sensor collection task.
+	 * Main attitude control task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };
 
 namespace mc_att_control
@@ -276,6 +278,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_p.zero();
 	_params.rate_i.zero();
 	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -298,7 +305,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
 	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
 	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-
+	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
 	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
 	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
 	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
@@ -367,15 +374,16 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_d(2) = v;
 
 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);
 
 	/* manual control scale */
 	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
 	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
 	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
-
-	_params.man_roll_max *= M_PI / 180.0;
-	_params.man_pitch_max *= M_PI / 180.0;
-	_params.man_yaw_max *= M_PI / 180.0;
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);
 
 	return OK;
 }
@@ -478,7 +486,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 		if (!_v_control_mode.flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.throttle;
+			_v_att_sp.thrust = _manual_control_sp.z;
 			publish_att_sp = true;
 		}
 
@@ -496,7 +504,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max;
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
@@ -512,8 +520,8 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 		if (!_v_control_mode.flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max;
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
@@ -626,6 +634,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	/* calculate angular rates setpoint */
 	_rates_sp = _params.att_p.emult(e_R);
 
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
+
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 }
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index e52c39368..19134c7b4 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -175,6 +175,18 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
 
 /**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
+
+/**
  * Max manual roll
  *
  * @unit deg
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 7c625a0c5..6e611d4ab 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -226,7 +226,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };
 
 namespace pos_control
@@ -617,7 +617,7 @@ MulticopterPositionControl::task_main()
 					reset_alt_sp();
 
 					/* move altitude setpoint with throttle stick */
-					sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
 				}
 
 				if (_control_mode.flag_control_position_enabled) {
@@ -625,8 +625,8 @@ MulticopterPositionControl::task_main()
 					reset_pos_sp();
 
 					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = _manual.pitch;
-					sp_move_rate(1) = _manual.roll;
+					sp_move_rate(0) = _manual.x;
+					sp_move_rate(1) = _manual.y;
 				}
 
 				/* limit setpoint move rate */
@@ -782,7 +782,7 @@ MulticopterPositionControl::task_main()
 							float i = _params.thr_min;
 
 							if (reset_int_z_manual) {
-								i = _manual.throttle;
+								i = _manual.z;
 
 								if (i < _params.thr_min) {
 									i = _params.thr_min;
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index 104df4e59..dacdd46f0 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
 /**
  * Maximum vertical velocity
  *
- * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
  *
  * @unit m/s
  * @min 0.0
@@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
 /**
  * Vertical velocity feed forward
  *
- * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
  *
  * @min 0.0
  * @max 1.0
@@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
 /**
  * Maximum horizontal velocity
  *
- * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
  *
  * @unit m/s
  * @min 0.0
@@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 /**
  * Horizontal velocity feed forward
  *
- * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
  *
  * @min 0.0
  * @max 1.0
@@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
 /**
  * Maximum tilt angle in air
  *
- * Limits maximum tilt in AUTO and EASY modes during flight.
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
  *
  * @unit deg
  * @min 0.0
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index eaafa217d..646ab6ab9 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -100,8 +100,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
 	if (geofence.valid()) {
 		for (size_t i = 0; i < nMissionItems; i++) {
-			static struct mission_item_s missionitem;
-			const ssize_t len = sizeof(struct mission_item_s);
+			struct mission_item_s missionitem;
+			const ssize_t len = sizeof(missionitem);
 
 			if (dm_read(dm_current, i, &missionitem, len) != len) {
 				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -125,8 +125,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 
 
 	for (size_t i = 0; i < nMissionItems; i++) {
-		static struct mission_item_s missionitem;
-		const ssize_t len = sizeof(struct mission_item_s);
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return false;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 12fd35a0a..1a45e1345 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1291,7 +1291,7 @@ Navigator::set_rtl_item()
 			_mission_item.yaw = NAN;
 			_mission_item.loiter_radius = _parameters.loiter_radius;
 			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.acceptance_radius = _parameters.acceptance_radius;
 			_mission_item.time_inside = 0.0f;
 			_mission_item.pitch_min = 0.0f;
@@ -1351,7 +1351,7 @@ Navigator::set_rtl_item()
 			_mission_item.yaw = NAN;
 			_mission_item.loiter_radius = _parameters.loiter_radius;
 			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 			_mission_item.acceptance_radius = _parameters.acceptance_radius;
 			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
 			_mission_item.pitch_min = 0.0f;
diff --git a/src/modules/position_estimator/module.mk b/src/modules/position_estimator/module.mk
deleted file mode 100644
index f64095d9d..000000000
--- a/src/modules/position_estimator/module.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build the position estimator
-#
-
-MODULE_COMMAND		= position_estimator
-
-# XXX this should be converted to a deamon, its a pretty bad example app
-MODULE_PRIORITY	 	= SCHED_PRIORITY_DEFAULT
-MODULE_STACKSIZE	= 4096
-
-SRCS		 	= position_estimator_main.c
diff --git a/src/modules/position_estimator/position_estimator_main.c b/src/modules/position_estimator/position_estimator_main.c
deleted file mode 100644
index e84945299..000000000
--- a/src/modules/position_estimator/position_estimator_main.c
+++ /dev/null
@@ -1,423 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *			 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 position_estimator_main.c
- * Model-identification based position estimator for multirotors
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <float.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/vehicle_status.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <poll.h>
-
-#define N_STATES 6
-#define ERROR_COVARIANCE_INIT 3
-#define R_EARTH 6371000.0
-
-#define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
-#define REPROJECTION_COUNTER_LIMIT 125
-
-__EXPORT int position_estimator_main(int argc, char *argv[]);
-
-static uint16_t position_estimator_counter_position_information;
-
-/* 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;
-
-/**
- * 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°)
- */
-static 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;
-
-}
-
-/**
- * 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°)
- */
-static 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);
-}
-
-/**
- * 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°)
- */
-static 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;
-
-}
-
-/****************************************************************************
- * main
- ****************************************************************************/
-
-int position_estimator_main(int argc, char *argv[])
-{
-
-	/* welcome user */
-	printf("[multirotor position_estimator] started\n");
-
-	/* initialize values */
-	static float u[2] = {0, 0};
-	static float z[3] = {0, 0, 0};
-	static float xapo[N_STATES] = {0, 0, 0, 0, 0, 0};
-	static float Papo[N_STATES * N_STATES] = {ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0,
-			ERROR_COVARIANCE_INIT, 0, 0, 0, 0, 0
-						 };
-
-	static float xapo1[N_STATES];
-	static float Papo1[36];
-
-	static float gps_covariance[3] = {0.0f, 0.0f, 0.0f};
-
-	static uint16_t counter = 0;
-	position_estimator_counter_position_information = 0;
-
-	uint8_t predict_only = 1;
-
-	bool gps_valid = false;
-
-	bool new_initialization = true;
-
-	static double lat_current = 0.0d;//[°]] --> 47.0
-	static double lon_current = 0.0d; //[°]] -->8.5
-	float alt_current = 0.0f;
-
-
-	//TODO: handle flight without gps but with estimator
-
-	/* subscribe to vehicle status, attitude, gps */
-	struct vehicle_gps_position_s gps;
-	gps.fix_type = 0;
-	struct vehicle_status_s vstatus;
-	struct vehicle_attitude_s att;
-
-	int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* subscribe to attitude at 100 Hz */
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-
-	/* wait until gps signal turns valid, only then can we initialize the projection */
-	while (gps.fix_type < 3) {
-		struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
-
-		/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
-		 * this choice is critical, since the vehicle status might not
-		 * actually change, if this app is started after GPS lock was
-		 * aquired.
-		 */
-		if (poll(fds, 1, 5000)) {
-			/* Wait for the GPS update to propagate (we have some time) */
-			usleep(5000);
-			/* Read wether the vehicle status changed */
-			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-			gps_valid = (gps.fix_type > 2);
-		}
-	}
-
-	/* get gps value for first initialization */
-	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-	lat_current = ((double)(gps.lat)) * 1e-7;
-	lon_current = ((double)(gps.lon)) * 1e-7;
-	alt_current = gps.alt * 1e-3;
-
-	/* initialize coordinates */
-	map_projection_init(lat_current, lon_current);
-
-	/* publish global position messages only after first GPS message */
-	struct vehicle_local_position_s local_pos = {
-		.x = 0,
-		.y = 0,
-		.z = 0
-	};
-	orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
-
-	printf("[multirotor position estimator] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
-
-	while (1) {
-
-		/*This runs at the rate of the sensors, if we have also a new gps update this is used in the position_estimator function */
-		struct pollfd fds[1] = { {.fd = vehicle_attitude_sub, .events = POLLIN} };
-
-		if (poll(fds, 1, 5000) <= 0) {
-			/* error / timeout */
-		} else {
-
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-			/* got attitude, updating pos as well */
-			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
-
-			/*copy attitude */
-			u[0] = att.roll;
-			u[1] = att.pitch;
-
-			/* initialize map projection with the last estimate (not at full rate) */
-			if (gps.fix_type > 2) {
-				/* Project gps lat lon (Geographic coordinate system) to plane*/
-				map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1]));
-
-				local_pos.x = z[0];
-				local_pos.y = z[1];
-				/* negative offset from initialization altitude */
-				local_pos.z = alt_current - (gps.alt) * 1e-3;
-
-
-				orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
-			}
-
-
-			// 	gps_covariance[0] = gps.eph; //TODO: needs scaling
-			// 	gps_covariance[1] = gps.eph;
-			// 	gps_covariance[2] = gps.epv;
-
-			// } else {
-			// 	/* we can not use the gps signal (it is of low quality) */
-			// 	predict_only = 1;
-			// }
-
-			// //		predict_only = 0;																//TODO: only for testing, removeme, XXX
-			// //		z[0] = sinf(((float)counter)/180.0f*3.14159265f);								//TODO: only for testing, removeme, XXX
-			// //		usleep(100000);																	//TODO: only for testing, removeme, XXX
-
-
-			// /*Get new estimation (this is calculated in the plane) */
-			// //TODO: if new_initialization == true: use 0,0,0, else use xapo
-			// if (true == new_initialization) {																								//TODO,XXX: uncomment!
-			// 	xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
-			// 	xapo[2] = 0;
-			// 	xapo[4] = 0;
-			// 	position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
-
-			// } else {
-			// 	position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
-			// }
-
-
-
-			// /* Copy values from xapo1 to xapo */
-			// int i;
-
-			// for (i = 0; i < N_STATES; i++) {
-			// 	xapo[i] = xapo1[i];
-			// }
-
-			// if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
-			// 	/* Reproject from plane to geographic coordinate system */
-			// 	//		map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
-			// 	map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
-			// 	//		//DEBUG
-			// 	//			if(counter%500 == 0)
-			// 	//			{
-			// 	//				printf("phi_1: %.10f\n", phi_1);
-			// 	//				printf("lambda_0: %.10f\n", lambda_0);
-			// 	//				printf("lat_estimated: %.10f\n", lat_current);
-			// 	//				printf("lon_estimated: %.10f\n", lon_current);
-			// 	//				printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
-			// 	//				fflush(stdout);
-			// 	//
-			// 	//			}
-
-			// 	//			if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
-			// 	//			{
-			// 	/* send out */
-
-			// 	global_pos.lat = lat_current;
-			// 	global_pos.lon = lon_current;
-			// 	global_pos.alt = xapo1[4];
-			// 	global_pos.vx = xapo1[1];
-			// 	global_pos.vy = xapo1[3];
-			// 	global_pos.vz = xapo1[5];
-
-				/* publish current estimate */
-				// orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
-				//			}
-				//			else
-				//			{
-				//				printf("[position estimator] ERROR: nan values, lat_current=%.4f, lon_current=%.4f, z[0]=%.4f z[1]=%.4f\n", lat_current, lon_current, z[0], z[1]);
-				//				fflush(stdout);
-				//			}
-
-			// }
-
-			counter++;
-		}
-
-	}
-
-	return 0;
-}
-
-
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
deleted file mode 100755
index 977565b8e..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.c
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * kalman_dlqe1.c
- *
- * Code generation for function 'kalman_dlqe1'
- *
- * C source code generated on: Wed Feb 13 20:34:32 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe1.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3],
-                  const real32_T x_aposteriori_k[3], real32_T z, real32_T
-                  x_aposteriori[3])
-{
-	printf("[dlqe input]: x_aposteriori_k %12.8f\t %12.8f\t %12.8f\t z:%12.8f\n", (double)(x_aposteriori_k[0]), (double)(x_aposteriori_k[1]), (double)(x_aposteriori_k[2]), (double)z);
-	printf("[dlqe input]: C[0]: %12.8f\tC[1] %12.8f\tC[2] %12.8f\n", (double)(C[0]), (double)(C[1]), (double)(C[2]));
-  real32_T y;
-  int32_T i0;
-  real32_T b_y[3];
-  int32_T i1;
-  real32_T f0;
-  y = 0.0F;
-  for (i0 = 0; i0 < 3; i0++) {
-    b_y[i0] = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      b_y[i0] += C[i1] * A[i1 + 3 * i0];
-    }
-
-    y += b_y[i0] * x_aposteriori_k[i0];
-  }
-
-  y = z - y;
-  for (i0 = 0; i0 < 3; i0++) {
-    f0 = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
-    }
-
-    x_aposteriori[i0] = f0 + K[i0] * y;
-  }
-  //printf("[dlqe output]: x_aposteriori %12.8f\t %12.8f\t %12.8f\n", (double)(x_aposteriori[0]), (double)(x_aposteriori[1]), (double)(x_aposteriori[2]));
-}
-
-/* End of code generation (kalman_dlqe1.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
deleted file mode 100755
index 2f5330563..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * kalman_dlqe1.h
- *
- * Code generation for function 'kalman_dlqe1'
- *
- * C source code generated on: Wed Feb 13 20:34:32 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_H__
-#define __KALMAN_DLQE1_H__
-/* Include files */
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "kalman_dlqe1_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe1(const real32_T A[9], const real32_T C[3], const real32_T K[3], const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
-#endif
-/* End of code generation (kalman_dlqe1.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
deleted file mode 100755
index 6627f76e9..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe1_initialize.c
- *
- * Code generation for function 'kalman_dlqe1_initialize'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe1.h"
-#include "kalman_dlqe1_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe1_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (kalman_dlqe1_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
deleted file mode 100755
index a77eb5712..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_initialize.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * kalman_dlqe1_initialize.h
- *
- * Code generation for function 'kalman_dlqe1_initialize'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_INITIALIZE_H__
-#define __KALMAN_DLQE1_INITIALIZE_H__
-/* Include files */
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "kalman_dlqe1_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe1_initialize(void);
-#endif
-/* End of code generation (kalman_dlqe1_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
deleted file mode 100755
index a65536f79..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe1_terminate.c
- *
- * Code generation for function 'kalman_dlqe1_terminate'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe1.h"
-#include "kalman_dlqe1_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe1_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (kalman_dlqe1_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
deleted file mode 100755
index 100c7f76c..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_terminate.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * kalman_dlqe1_terminate.h
- *
- * Code generation for function 'kalman_dlqe1_terminate'
- *
- * C source code generated on: Wed Feb 13 20:34:32 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_TERMINATE_H__
-#define __KALMAN_DLQE1_TERMINATE_H__
-/* Include files */
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "kalman_dlqe1_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe1_terminate(void);
-#endif
-/* End of code generation (kalman_dlqe1_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
deleted file mode 100755
index d4b2c2d61..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe1_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * kalman_dlqe1_types.h
- *
- * Code generation for function 'kalman_dlqe1'
- *
- * C source code generated on: Wed Feb 13 20:34:31 2013
- *
- */
-
-#ifndef __KALMAN_DLQE1_TYPES_H__
-#define __KALMAN_DLQE1_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (kalman_dlqe1_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
deleted file mode 100755
index 11b999064..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.c
+++ /dev/null
@@ -1,119 +0,0 @@
-/*
- * kalman_dlqe2.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe2.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1);
-
-/* Function Definitions */
-static real32_T rt_powf_snf(real32_T u0, real32_T u1)
-{
-  real32_T y;
-  real32_T f1;
-  real32_T f2;
-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
-    y = ((real32_T)rtNaN);
-  } else {
-    f1 = (real32_T)fabs(u0);
-    f2 = (real32_T)fabs(u1);
-    if (rtIsInfF(u1)) {
-      if (f1 == 1.0F) {
-        y = ((real32_T)rtNaN);
-      } else if (f1 > 1.0F) {
-        if (u1 > 0.0F) {
-          y = ((real32_T)rtInf);
-        } else {
-          y = 0.0F;
-        }
-      } else if (u1 > 0.0F) {
-        y = 0.0F;
-      } else {
-        y = ((real32_T)rtInf);
-      }
-    } else if (f2 == 0.0F) {
-      y = 1.0F;
-    } else if (f2 == 1.0F) {
-      if (u1 > 0.0F) {
-        y = u0;
-      } else {
-        y = 1.0F / u0;
-      }
-    } else if (u1 == 2.0F) {
-      y = u0 * u0;
-    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
-      y = (real32_T)sqrt(u0);
-    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
-      y = ((real32_T)rtNaN);
-    } else {
-      y = (real32_T)pow(u0, u1);
-    }
-  }
-
-  return y;
-}
-
-void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const
-                  real32_T x_aposteriori_k[3], real32_T z, real32_T
-                  x_aposteriori[3])
-{
-	//printf("[dqle2] dt: %12.8f\tvk1 %12.8f\tk2: %12.8f\tk3: %12.8f\n", (double)(dt), (double)(k1), (double)(k2), (double)(k3));
-	//printf("[dqle2] dt: %8.4f\n", (double)(dt));//, (double)(k1), (double)(k2), (double)(k3));
-  real32_T A[9];
-  real32_T y;
-  int32_T i0;
-  static const int8_T iv0[3] = { 0, 0, 1 };
-
-  real32_T b_k1[3];
-  int32_T i1;
-  static const int8_T iv1[3] = { 1, 0, 0 };
-
-  real32_T f0;
-  A[0] = 1.0F;
-  A[3] = dt;
-  A[6] = 0.5F * rt_powf_snf(dt, 2.0F);
-  A[1] = 0.0F;
-  A[4] = 1.0F;
-  A[7] = dt;
-  y = 0.0F;
-  for (i0 = 0; i0 < 3; i0++) {
-    A[2 + 3 * i0] = (real32_T)iv0[i0];
-    b_k1[i0] = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      b_k1[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];
-    }
-
-    y += b_k1[i0] * x_aposteriori_k[i0];
-  }
-
-  y = z - y;
-  b_k1[0] = k1;
-  b_k1[1] = k2;
-  b_k1[2] = k3;
-  for (i0 = 0; i0 < 3; i0++) {
-    f0 = 0.0F;
-    for (i1 = 0; i1 < 3; i1++) {
-      f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];
-    }
-
-    x_aposteriori[i0] = f0 + b_k1[i0] * y;
-  }
-}
-
-/* End of code generation (kalman_dlqe2.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
deleted file mode 100755
index 30170ae22..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * kalman_dlqe2.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_H__
-#define __KALMAN_DLQE2_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe2_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe2(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T x_aposteriori[3]);
-#endif
-/* End of code generation (kalman_dlqe2.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
deleted file mode 100755
index de5a1d8aa..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe2_initialize.c
- *
- * Code generation for function 'kalman_dlqe2_initialize'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe2.h"
-#include "kalman_dlqe2_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe2_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (kalman_dlqe2_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
deleted file mode 100755
index 3d507ff31..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_initialize.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * kalman_dlqe2_initialize.h
- *
- * Code generation for function 'kalman_dlqe2_initialize'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_INITIALIZE_H__
-#define __KALMAN_DLQE2_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe2_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe2_initialize(void);
-#endif
-/* End of code generation (kalman_dlqe2_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
deleted file mode 100755
index 0757c878c..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * kalman_dlqe2_terminate.c
- *
- * Code generation for function 'kalman_dlqe2_terminate'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "kalman_dlqe2.h"
-#include "kalman_dlqe2_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void kalman_dlqe2_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (kalman_dlqe2_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
deleted file mode 100755
index 23995020b..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_terminate.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * kalman_dlqe2_terminate.h
- *
- * Code generation for function 'kalman_dlqe2_terminate'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_TERMINATE_H__
-#define __KALMAN_DLQE2_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "kalman_dlqe2_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void kalman_dlqe2_terminate(void);
-#endif
-/* End of code generation (kalman_dlqe2_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
deleted file mode 100755
index f7a04d908..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe2_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * kalman_dlqe2_types.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:28 2013
- *
- */
-
-#ifndef __KALMAN_DLQE2_TYPES_H__
-#define __KALMAN_DLQE2_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (kalman_dlqe2_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
deleted file mode 100755
index 9efe2ea7a..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.c
+++ /dev/null
@@ -1,137 +0,0 @@
-/*

- * kalman_dlqe3.c

- *

- * Code generation for function 'kalman_dlqe3'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "randn.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-static real32_T rt_powf_snf(real32_T u0, real32_T u1);

-

-/* Function Definitions */

-static real32_T rt_powf_snf(real32_T u0, real32_T u1)

-{

-  real32_T y;

-  real32_T f1;

-  real32_T f2;

-  if (rtIsNaNF(u0) || rtIsNaNF(u1)) {

-    y = ((real32_T)rtNaN);

-  } else {

-    f1 = (real32_T)fabs(u0);

-    f2 = (real32_T)fabs(u1);

-    if (rtIsInfF(u1)) {

-      if (f1 == 1.0F) {

-        y = ((real32_T)rtNaN);

-      } else if (f1 > 1.0F) {

-        if (u1 > 0.0F) {

-          y = ((real32_T)rtInf);

-        } else {

-          y = 0.0F;

-        }

-      } else if (u1 > 0.0F) {

-        y = 0.0F;

-      } else {

-        y = ((real32_T)rtInf);

-      }

-    } else if (f2 == 0.0F) {

-      y = 1.0F;

-    } else if (f2 == 1.0F) {

-      if (u1 > 0.0F) {

-        y = u0;

-      } else {

-        y = 1.0F / u0;

-      }

-    } else if (u1 == 2.0F) {

-      y = u0 * u0;

-    } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {

-      y = (real32_T)sqrt(u0);

-    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {

-      y = ((real32_T)rtNaN);

-    } else {

-      y = (real32_T)pow(u0, u1);

-    }

-  }

-

-  return y;

-}

-

-void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const

-                  real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate,

-                  real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3])

-{

-  real32_T A[9];

-  int32_T i0;

-  static const int8_T iv0[3] = { 0, 0, 1 };

-

-  real_T b;

-  real32_T y;

-  real32_T b_y[3];

-  int32_T i1;

-  static const int8_T iv1[3] = { 1, 0, 0 };

-

-  real32_T b_k1[3];

-  real32_T f0;

-  A[0] = 1.0F;

-  A[3] = dt;

-  A[6] = 0.5F * rt_powf_snf(dt, 2.0F);

-  A[1] = 0.0F;

-  A[4] = 1.0F;

-  A[7] = dt;

-  for (i0 = 0; i0 < 3; i0++) {

-    A[2 + 3 * i0] = (real32_T)iv0[i0];

-  }

-

-  if (addNoise == 1.0F) {

-    b = randn();

-    z += sigma * (real32_T)b;

-  }

-

-  if (posUpdate != 0.0F) {

-    y = 0.0F;

-    for (i0 = 0; i0 < 3; i0++) {

-      b_y[i0] = 0.0F;

-      for (i1 = 0; i1 < 3; i1++) {

-        b_y[i0] += (real32_T)iv1[i1] * A[i1 + 3 * i0];

-      }

-

-      y += b_y[i0] * x_aposteriori_k[i0];

-    }

-

-    y = z - y;

-    b_k1[0] = k1;

-    b_k1[1] = k2;

-    b_k1[2] = k3;

-    for (i0 = 0; i0 < 3; i0++) {

-      f0 = 0.0F;

-      for (i1 = 0; i1 < 3; i1++) {

-        f0 += A[i0 + 3 * i1] * x_aposteriori_k[i1];

-      }

-

-      x_aposteriori[i0] = f0 + b_k1[i0] * y;

-    }

-  } else {

-    for (i0 = 0; i0 < 3; i0++) {

-      x_aposteriori[i0] = 0.0F;

-      for (i1 = 0; i1 < 3; i1++) {

-        x_aposteriori[i0] += A[i0 + 3 * i1] * x_aposteriori_k[i1];

-      }

-    }

-  }

-}

-

-/* End of code generation (kalman_dlqe3.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
deleted file mode 100755
index 9bbffbbb3..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * kalman_dlqe3.h

- *

- * Code generation for function 'kalman_dlqe3'

- *

- * C source code generated on: Tue Feb 19 15:26:32 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_H__

-#define __KALMAN_DLQE3_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void kalman_dlqe3(real32_T dt, real32_T k1, real32_T k2, real32_T k3, const real32_T x_aposteriori_k[3], real32_T z, real32_T posUpdate, real32_T addNoise, real32_T sigma, real32_T x_aposteriori[3]);

-#endif

-/* End of code generation (kalman_dlqe3.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
deleted file mode 100755
index 8f2275c13..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.c
+++ /dev/null
@@ -1,32 +0,0 @@
-/*

- * kalman_dlqe3_data.c

- *

- * Code generation for function 'kalman_dlqe3_data'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "kalman_dlqe3_data.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-uint32_T method;

-uint32_T state[2];

-uint32_T b_method;

-uint32_T b_state;

-uint32_T c_state[2];

-boolean_T state_not_empty;

-

-/* Function Declarations */

-

-/* Function Definitions */

-/* End of code generation (kalman_dlqe3_data.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
deleted file mode 100755
index 952eb7b89..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_data.h
+++ /dev/null
@@ -1,38 +0,0 @@
-/*

- * kalman_dlqe3_data.h

- *

- * Code generation for function 'kalman_dlqe3_data'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_DATA_H__

-#define __KALMAN_DLQE3_DATA_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-extern uint32_T method;

-extern uint32_T state[2];

-extern uint32_T b_method;

-extern uint32_T b_state;

-extern uint32_T c_state[2];

-extern boolean_T state_not_empty;

-

-/* Variable Definitions */

-

-/* Function Declarations */

-#endif

-/* End of code generation (kalman_dlqe3_data.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
deleted file mode 100755
index b87d604c4..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.c
+++ /dev/null
@@ -1,47 +0,0 @@
-/*

- * kalman_dlqe3_initialize.c

- *

- * Code generation for function 'kalman_dlqe3_initialize'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "kalman_dlqe3_initialize.h"

-#include "kalman_dlqe3_data.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void kalman_dlqe3_initialize(void)

-{

-  int32_T i;

-  static const uint32_T uv0[2] = { 362436069U, 0U };

-

-  rt_InitInfAndNaN(8U);

-  state_not_empty = FALSE;

-  b_state = 1144108930U;

-  b_method = 7U;

-  method = 0U;

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

-    c_state[i] = 362436069U + 158852560U * (uint32_T)i;

-    state[i] = uv0[i];

-  }

-

-  if (state[1] == 0U) {

-    state[1] = 521288629U;

-  }

-}

-

-/* End of code generation (kalman_dlqe3_initialize.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
deleted file mode 100755
index 9dee90f9e..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_initialize.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * kalman_dlqe3_initialize.h

- *

- * Code generation for function 'kalman_dlqe3_initialize'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_INITIALIZE_H__

-#define __KALMAN_DLQE3_INITIALIZE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void kalman_dlqe3_initialize(void);

-#endif

-/* End of code generation (kalman_dlqe3_initialize.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
deleted file mode 100755
index b00858205..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*

- * kalman_dlqe3_terminate.c

- *

- * Code generation for function 'kalman_dlqe3_terminate'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "kalman_dlqe3_terminate.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-

-/* Function Definitions */

-void kalman_dlqe3_terminate(void)

-{

-  /* (no terminate code required) */

-}

-

-/* End of code generation (kalman_dlqe3_terminate.c) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
deleted file mode 100755
index 69cc85c76..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_terminate.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * kalman_dlqe3_terminate.h

- *

- * Code generation for function 'kalman_dlqe3_terminate'

- *

- * C source code generated on: Tue Feb 19 15:26:31 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_TERMINATE_H__

-#define __KALMAN_DLQE3_TERMINATE_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern void kalman_dlqe3_terminate(void);

-#endif

-/* End of code generation (kalman_dlqe3_terminate.h) */

diff --git a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h b/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
deleted file mode 100755
index f36ea4557..000000000
--- a/src/modules/position_estimator_mc/codegen/kalman_dlqe3_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*

- * kalman_dlqe3_types.h

- *

- * Code generation for function 'kalman_dlqe3'

- *

- * C source code generated on: Tue Feb 19 15:26:30 2013

- *

- */

-

-#ifndef __KALMAN_DLQE3_TYPES_H__

-#define __KALMAN_DLQE3_TYPES_H__

-

-/* Type Definitions */

-

-#endif

-/* End of code generation (kalman_dlqe3_types.h) */

diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
deleted file mode 100755
index 5139848bc..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.c
+++ /dev/null
@@ -1,136 +0,0 @@
-/*
- * positionKalmanFilter1D.c
- *
- * Code generation for function 'positionKalmanFilter1D'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const
-  real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T
-  P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T
-  Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3],
-  real32_T P_aposteriori[9])
-{
-  int32_T i0;
-  real32_T f0;
-  int32_T k;
-  real32_T b_A[9];
-  int32_T i1;
-  real32_T P_apriori[9];
-  real32_T y;
-  real32_T K[3];
-  real32_T S;
-  int8_T I[9];
-
-  /* prediction */
-  for (i0 = 0; i0 < 3; i0++) {
-    f0 = 0.0F;
-    for (k = 0; k < 3; k++) {
-      f0 += A[i0 + 3 * k] * x_aposteriori_k[k];
-    }
-
-    x_aposteriori[i0] = f0 + B[i0] * u;
-  }
-
-  for (i0 = 0; i0 < 3; i0++) {
-    for (k = 0; k < 3; k++) {
-      b_A[i0 + 3 * k] = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        b_A[i0 + 3 * k] += A[i0 + 3 * i1] * P_aposteriori_k[i1 + 3 * k];
-      }
-    }
-  }
-
-  for (i0 = 0; i0 < 3; i0++) {
-    for (k = 0; k < 3; k++) {
-      f0 = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        f0 += b_A[i0 + 3 * i1] * A[k + 3 * i1];
-      }
-
-      P_apriori[i0 + 3 * k] = f0 + Q[i0 + 3 * k];
-    }
-  }
-
-  if ((real32_T)fabs(u) < thresh) {
-    x_aposteriori[1] *= decay;
-  }
-
-  /* update */
-  if (gps_update == 1) {
-    y = 0.0F;
-    for (k = 0; k < 3; k++) {
-      y += C[k] * x_aposteriori[k];
-      K[k] = 0.0F;
-      for (i0 = 0; i0 < 3; i0++) {
-        K[k] += C[i0] * P_apriori[i0 + 3 * k];
-      }
-    }
-
-    y = z - y;
-    S = 0.0F;
-    for (k = 0; k < 3; k++) {
-      S += K[k] * C[k];
-    }
-
-    S += R;
-    for (i0 = 0; i0 < 3; i0++) {
-      f0 = 0.0F;
-      for (k = 0; k < 3; k++) {
-        f0 += P_apriori[i0 + 3 * k] * C[k];
-      }
-
-      K[i0] = f0 / S;
-    }
-
-    for (i0 = 0; i0 < 3; i0++) {
-      x_aposteriori[i0] += K[i0] * y;
-    }
-
-    for (i0 = 0; i0 < 9; i0++) {
-      I[i0] = 0;
-    }
-
-    for (k = 0; k < 3; k++) {
-      I[k + 3 * k] = 1;
-    }
-
-    for (i0 = 0; i0 < 3; i0++) {
-      for (k = 0; k < 3; k++) {
-        b_A[k + 3 * i0] = (real32_T)I[k + 3 * i0] - K[k] * C[i0];
-      }
-    }
-
-    for (i0 = 0; i0 < 3; i0++) {
-      for (k = 0; k < 3; k++) {
-        P_aposteriori[i0 + 3 * k] = 0.0F;
-        for (i1 = 0; i1 < 3; i1++) {
-          P_aposteriori[i0 + 3 * k] += b_A[i0 + 3 * i1] * P_apriori[i1 + 3 * k];
-        }
-      }
-    }
-  } else {
-    for (i0 = 0; i0 < 9; i0++) {
-      P_aposteriori[i0] = P_apriori[i0];
-    }
-  }
-}
-
-/* End of code generation (positionKalmanFilter1D.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
deleted file mode 100755
index 205c8eb4e..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D.h
- *
- * Code generation for function 'positionKalmanFilter1D'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_H__
-#define __POSITIONKALMANFILTER1D_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D(const real32_T A[9], const real32_T B[3], const real32_T C[3], const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real32_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
-#endif
-/* End of code generation (positionKalmanFilter1D.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
deleted file mode 100755
index 4c535618a..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.c
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * positionKalmanFilter1D_dT.c
- *
- * Code generation for function 'positionKalmanFilter1D_dT'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D_dT.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3],
-  const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update,
-  const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T
-  x_aposteriori[3], real32_T P_aposteriori[9])
-{
-  real32_T A[9];
-  int32_T i;
-  static const int8_T iv0[3] = { 0, 0, 1 };
-
-  real32_T K[3];
-  real32_T f0;
-  int32_T i0;
-  real32_T b_A[9];
-  int32_T i1;
-  real32_T P_apriori[9];
-  static const int8_T iv1[3] = { 1, 0, 0 };
-
-  real32_T fv0[3];
-  real32_T y;
-  static const int8_T iv2[3] = { 1, 0, 0 };
-
-  real32_T S;
-  int8_T I[9];
-
-  /* dynamics */
-  A[0] = 1.0F;
-  A[3] = dT;
-  A[6] = -0.5F * dT * dT;
-  A[1] = 0.0F;
-  A[4] = 1.0F;
-  A[7] = -dT;
-  for (i = 0; i < 3; i++) {
-    A[2 + 3 * i] = (real32_T)iv0[i];
-  }
-
-  /* prediction */
-  K[0] = 0.5F * dT * dT;
-  K[1] = dT;
-  K[2] = 0.0F;
-  for (i = 0; i < 3; i++) {
-    f0 = 0.0F;
-    for (i0 = 0; i0 < 3; i0++) {
-      f0 += A[i + 3 * i0] * x_aposteriori_k[i0];
-    }
-
-    x_aposteriori[i] = f0 + K[i] * u;
-  }
-
-  for (i = 0; i < 3; i++) {
-    for (i0 = 0; i0 < 3; i0++) {
-      b_A[i + 3 * i0] = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        b_A[i + 3 * i0] += A[i + 3 * i1] * P_aposteriori_k[i1 + 3 * i0];
-      }
-    }
-  }
-
-  for (i = 0; i < 3; i++) {
-    for (i0 = 0; i0 < 3; i0++) {
-      f0 = 0.0F;
-      for (i1 = 0; i1 < 3; i1++) {
-        f0 += b_A[i + 3 * i1] * A[i0 + 3 * i1];
-      }
-
-      P_apriori[i + 3 * i0] = f0 + Q[i + 3 * i0];
-    }
-  }
-
-  if ((real32_T)fabs(u) < thresh) {
-    x_aposteriori[1] *= decay;
-  }
-
-  /* update */
-  if (gps_update == 1) {
-    f0 = 0.0F;
-    for (i = 0; i < 3; i++) {
-      f0 += (real32_T)iv1[i] * x_aposteriori[i];
-      fv0[i] = 0.0F;
-      for (i0 = 0; i0 < 3; i0++) {
-        fv0[i] += (real32_T)iv1[i0] * P_apriori[i0 + 3 * i];
-      }
-    }
-
-    y = z - f0;
-    f0 = 0.0F;
-    for (i = 0; i < 3; i++) {
-      f0 += fv0[i] * (real32_T)iv2[i];
-    }
-
-    S = f0 + (real32_T)R;
-    for (i = 0; i < 3; i++) {
-      f0 = 0.0F;
-      for (i0 = 0; i0 < 3; i0++) {
-        f0 += P_apriori[i + 3 * i0] * (real32_T)iv2[i0];
-      }
-
-      K[i] = f0 / S;
-    }
-
-    for (i = 0; i < 3; i++) {
-      x_aposteriori[i] += K[i] * y;
-    }
-
-    for (i = 0; i < 9; i++) {
-      I[i] = 0;
-    }
-
-    for (i = 0; i < 3; i++) {
-      I[i + 3 * i] = 1;
-    }
-
-    for (i = 0; i < 3; i++) {
-      for (i0 = 0; i0 < 3; i0++) {
-        A[i0 + 3 * i] = (real32_T)I[i0 + 3 * i] - K[i0] * (real32_T)iv1[i];
-      }
-    }
-
-    for (i = 0; i < 3; i++) {
-      for (i0 = 0; i0 < 3; i0++) {
-        P_aposteriori[i + 3 * i0] = 0.0F;
-        for (i1 = 0; i1 < 3; i1++) {
-          P_aposteriori[i + 3 * i0] += A[i + 3 * i1] * P_apriori[i1 + 3 * i0];
-        }
-      }
-    }
-  } else {
-    for (i = 0; i < 9; i++) {
-      P_aposteriori[i] = P_apriori[i];
-    }
-  }
-}
-
-/* End of code generation (positionKalmanFilter1D_dT.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
deleted file mode 100755
index 94cbe2ce6..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_H__
-#define __POSITIONKALMANFILTER1D_DT_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_dT_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_dT(real32_T dT, const real32_T x_aposteriori_k[3], const real32_T P_aposteriori_k[9], real32_T u, real32_T z, uint8_T gps_update, const real32_T Q[9], real_T R, real32_T thresh, real32_T decay, real32_T x_aposteriori[3], real32_T P_aposteriori[9]);
-#endif
-/* End of code generation (positionKalmanFilter1D_dT.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
deleted file mode 100755
index aa89f8a9d..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_initialize.c
- *
- * Code generation for function 'positionKalmanFilter1D_dT_initialize'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D_dT.h"
-#include "positionKalmanFilter1D_dT_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_dT_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (positionKalmanFilter1D_dT_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
deleted file mode 100755
index 8d358a9a3..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_initialize.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_initialize.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT_initialize'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
-#define __POSITIONKALMANFILTER1D_DT_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_dT_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_dT_initialize(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_dT_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
deleted file mode 100755
index 20ed2edbb..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_terminate.c
- *
- * Code generation for function 'positionKalmanFilter1D_dT_terminate'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D_dT.h"
-#include "positionKalmanFilter1D_dT_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_dT_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (positionKalmanFilter1D_dT_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
deleted file mode 100755
index 5eb5807a0..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_terminate.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_terminate.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT_terminate'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
-#define __POSITIONKALMANFILTER1D_DT_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_dT_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_dT_terminate(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_dT_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
deleted file mode 100755
index 43e5f016c..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_dT_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * positionKalmanFilter1D_dT_types.h
- *
- * Code generation for function 'positionKalmanFilter1D_dT'
- *
- * C source code generated on: Fri Nov 30 17:37:33 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_DT_TYPES_H__
-#define __POSITIONKALMANFILTER1D_DT_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (positionKalmanFilter1D_dT_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
deleted file mode 100755
index 5bd87c390..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_initialize.c
- *
- * Code generation for function 'positionKalmanFilter1D_initialize'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D.h"
-#include "positionKalmanFilter1D_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (positionKalmanFilter1D_initialize.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
deleted file mode 100755
index 44bce472f..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_initialize.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_initialize.h
- *
- * Code generation for function 'positionKalmanFilter1D_initialize'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_INITIALIZE_H__
-#define __POSITIONKALMANFILTER1D_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_initialize(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_initialize.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
deleted file mode 100755
index 41e11936f..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.c
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_terminate.c
- *
- * Code generation for function 'positionKalmanFilter1D_terminate'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "positionKalmanFilter1D.h"
-#include "positionKalmanFilter1D_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void positionKalmanFilter1D_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (positionKalmanFilter1D_terminate.c) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
deleted file mode 100755
index e84ea01bc..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_terminate.h
+++ /dev/null
@@ -1,31 +0,0 @@
-/*
- * positionKalmanFilter1D_terminate.h
- *
- * Code generation for function 'positionKalmanFilter1D_terminate'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_TERMINATE_H__
-#define __POSITIONKALMANFILTER1D_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-
-#include "rtwtypes.h"
-#include "positionKalmanFilter1D_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void positionKalmanFilter1D_terminate(void);
-#endif
-/* End of code generation (positionKalmanFilter1D_terminate.h) */
diff --git a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h b/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
deleted file mode 100755
index 4b473f56f..000000000
--- a/src/modules/position_estimator_mc/codegen/positionKalmanFilter1D_types.h
+++ /dev/null
@@ -1,16 +0,0 @@
-/*
- * positionKalmanFilter1D_types.h
- *
- * Code generation for function 'positionKalmanFilter1D'
- *
- * C source code generated on: Fri Nov 30 14:26:11 2012
- *
- */
-
-#ifndef __POSITIONKALMANFILTER1D_TYPES_H__
-#define __POSITIONKALMANFILTER1D_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (positionKalmanFilter1D_types.h) */
diff --git a/src/modules/position_estimator_mc/codegen/randn.c b/src/modules/position_estimator_mc/codegen/randn.c
deleted file mode 100755
index 51aef7b76..000000000
--- a/src/modules/position_estimator_mc/codegen/randn.c
+++ /dev/null
@@ -1,524 +0,0 @@
-/*

- * randn.c

- *

- * Code generation for function 'randn'

- *

- * C source code generated on: Tue Feb 19 15:26:32 2013

- *

- */

-

-/* Include files */

-#include "rt_nonfinite.h"

-#include "kalman_dlqe3.h"

-#include "randn.h"

-#include "kalman_dlqe3_data.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-static uint32_T d_state[625];

-

-/* Function Declarations */

-static real_T b_genrandu(uint32_T mt[625]);

-static real_T eml_rand_mt19937ar(uint32_T e_state[625]);

-static real_T eml_rand_shr3cong(uint32_T e_state[2]);

-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2]);

-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r);

-static void twister_state_vector(uint32_T mt[625], real_T seed);

-

-/* Function Definitions */

-static real_T b_genrandu(uint32_T mt[625])

-{

-  real_T r;

-  int32_T exitg1;

-  uint32_T u[2];

-  boolean_T isvalid;

-  int32_T k;

-  boolean_T exitg2;

-

-  /* <LEGAL>   This is a uniform (0,1) pseudorandom number generator based on: */

-  /* <LEGAL> */

-  /* <LEGAL>    A C-program for MT19937, with initialization improved 2002/1/26. */

-  /* <LEGAL>    Coded by Takuji Nishimura and Makoto Matsumoto. */

-  /* <LEGAL> */

-  /* <LEGAL>    Copyright (C) 1997 - 2002, Makoto Matsumoto and Takuji Nishimura, */

-  /* <LEGAL>    All rights reserved. */

-  /* <LEGAL> */

-  /* <LEGAL>    Redistribution and use in source and binary forms, with or without */

-  /* <LEGAL>    modification, are permitted provided that the following conditions */

-  /* <LEGAL>    are met: */

-  /* <LEGAL> */

-  /* <LEGAL>      1. Redistributions of source code must retain the above copyright */

-  /* <LEGAL>         notice, this list of conditions and the following disclaimer. */

-  /* <LEGAL> */

-  /* <LEGAL>      2. Redistributions in binary form must reproduce the above copyright */

-  /* <LEGAL>         notice, this list of conditions and the following disclaimer in the */

-  /* <LEGAL>         documentation and/or other materials provided with the distribution. */

-  /* <LEGAL> */

-  /* <LEGAL>      3. The names of its contributors may not be used to endorse or promote */

-  /* <LEGAL>         products derived from this software without specific prior written */

-  /* <LEGAL>         permission. */

-  /* <LEGAL> */

-  /* <LEGAL>    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS */

-  /* <LEGAL>    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT */

-  /* <LEGAL>    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR */

-  /* <LEGAL>    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR */

-  /* <LEGAL>    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, */

-  /* <LEGAL>    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, */

-  /* <LEGAL>    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR */

-  /* <LEGAL>    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF */

-  /* <LEGAL>    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING */

-  /* <LEGAL>    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS */

-  /* <LEGAL>    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */

-  do {

-    exitg1 = 0;

-    genrand_uint32_vector(mt, u);

-    r = 1.1102230246251565E-16 * ((real_T)(u[0] >> 5U) * 6.7108864E+7 + (real_T)

-                                  (u[1] >> 6U));

-    if (r == 0.0) {

-      if ((mt[624] >= 1U) && (mt[624] < 625U)) {

-        isvalid = TRUE;

-      } else {

-        isvalid = FALSE;

-      }

-

-      if (isvalid) {

-        isvalid = FALSE;

-        k = 1;

-        exitg2 = FALSE;

-        while ((exitg2 == FALSE) && (k < 625)) {

-          if (mt[k - 1] == 0U) {

-            k++;

-          } else {

-            isvalid = TRUE;

-            exitg2 = TRUE;

-          }

-        }

-      }

-

-      if (!isvalid) {

-        twister_state_vector(mt, 5489.0);

-      }

-    } else {

-      exitg1 = 1;

-    }

-  } while (exitg1 == 0);

-

-  return r;

-}

-

-static real_T eml_rand_mt19937ar(uint32_T e_state[625])

-{

-  real_T r;

-  int32_T exitg1;

-  uint32_T u32[2];

-  int32_T i;

-  static const real_T dv1[257] = { 0.0, 0.215241895984875, 0.286174591792068,

-    0.335737519214422, 0.375121332878378, 0.408389134611989, 0.43751840220787,

-    0.46363433679088, 0.487443966139235, 0.50942332960209, 0.529909720661557,

-    0.549151702327164, 0.567338257053817, 0.584616766106378, 0.601104617755991,

-    0.61689699000775, 0.63207223638606, 0.646695714894993, 0.660822574244419,

-    0.674499822837293, 0.687767892795788, 0.700661841106814, 0.713212285190975,

-    0.725446140909999, 0.737387211434295, 0.749056662017815, 0.760473406430107,

-    0.771654424224568, 0.782615023307232, 0.793369058840623, 0.80392911698997,

-    0.814306670135215, 0.824512208752291, 0.834555354086381, 0.844444954909153,

-    0.854189171008163, 0.863795545553308, 0.87327106808886, 0.882622229585165,

-    0.891855070732941, 0.900975224461221, 0.909987953496718, 0.91889818364959,

-    0.927710533401999, 0.936429340286575, 0.945058684468165, 0.953602409881086,

-    0.96206414322304, 0.970447311064224, 0.978755155294224, 0.986990747099062,

-    0.99515699963509, 1.00325667954467, 1.01129241744, 1.01926671746548,

-    1.02718196603564, 1.03504043983344, 1.04284431314415, 1.05059566459093,

-    1.05829648333067, 1.06594867476212, 1.07355406579244, 1.0811144097034,

-    1.08863139065398, 1.09610662785202, 1.10354167942464, 1.11093804601357,

-    1.11829717411934, 1.12562045921553, 1.13290924865253, 1.14016484436815,

-    1.14738850542085, 1.15458145035993, 1.16174485944561, 1.16887987673083,

-    1.17598761201545, 1.18306914268269, 1.19012551542669, 1.19715774787944,

-    1.20416683014438, 1.2111537262437, 1.21811937548548, 1.22506469375653,

-    1.23199057474614, 1.23889789110569, 1.24578749554863, 1.2526602218949,

-    1.25951688606371, 1.26635828701823, 1.27318520766536, 1.27999841571382,

-    1.28679866449324, 1.29358669373695, 1.30036323033084, 1.30712898903073,

-    1.31388467315022, 1.32063097522106, 1.32736857762793, 1.33409815321936,

-    1.3408203658964, 1.34753587118059, 1.35424531676263, 1.36094934303328,

-    1.36764858359748, 1.37434366577317, 1.38103521107586, 1.38772383568998,

-    1.39441015092814, 1.40109476367925, 1.4077782768464, 1.41446128977547,

-    1.42114439867531, 1.42782819703026, 1.43451327600589, 1.44120022484872,

-    1.44788963128058, 1.45458208188841, 1.46127816251028, 1.46797845861808,

-    1.47468355569786, 1.48139403962819, 1.48811049705745, 1.49483351578049,

-    1.50156368511546, 1.50830159628131, 1.51504784277671, 1.521803020761,

-    1.52856772943771, 1.53534257144151, 1.542128153229, 1.54892508547417,

-    1.55573398346918, 1.56255546753104, 1.56939016341512, 1.57623870273591,

-    1.58310172339603, 1.58997987002419, 1.59687379442279, 1.60378415602609,

-    1.61071162236983, 1.61765686957301, 1.62462058283303, 1.63160345693487,

-    1.63860619677555, 1.64562951790478, 1.65267414708306, 1.65974082285818,

-    1.66683029616166, 1.67394333092612, 1.68108070472517, 1.68824320943719,

-    1.69543165193456, 1.70264685479992, 1.7098896570713, 1.71716091501782,

-    1.72446150294804, 1.73179231405296, 1.73915426128591, 1.74654827828172,

-    1.75397532031767, 1.76143636531891, 1.76893241491127, 1.77646449552452,

-    1.78403365954944, 1.79164098655216, 1.79928758454972, 1.80697459135082,

-    1.81470317596628, 1.82247454009388, 1.83028991968276, 1.83815058658281,

-    1.84605785028518, 1.8540130597602, 1.86201760539967, 1.87007292107127,

-    1.878180486293, 1.88634182853678, 1.8945585256707, 1.90283220855043,

-    1.91116456377125, 1.91955733659319, 1.92801233405266, 1.93653142827569,

-    1.94511656000868, 1.95376974238465, 1.96249306494436, 1.97128869793366,

-    1.98015889690048, 1.98910600761744, 1.99813247135842, 2.00724083056053,

-    2.0164337349062, 2.02571394786385, 2.03508435372962, 2.04454796521753,

-    2.05410793165065, 2.06376754781173, 2.07353026351874, 2.0833996939983,

-    2.09337963113879, 2.10347405571488, 2.11368715068665, 2.12402331568952,

-    2.13448718284602, 2.14508363404789, 2.15581781987674, 2.16669518035431,

-    2.17772146774029, 2.18890277162636, 2.20024554661128, 2.21175664288416,

-    2.22344334009251, 2.23531338492992, 2.24737503294739, 2.25963709517379,

-    2.27210899022838, 2.28480080272449, 2.29772334890286, 2.31088825060137,

-    2.32430801887113, 2.33799614879653, 2.35196722737914, 2.36623705671729,

-    2.38082279517208, 2.39574311978193, 2.41101841390112, 2.42667098493715,

-    2.44272531820036, 2.4592083743347, 2.47614993967052, 2.49358304127105,

-    2.51154444162669, 2.53007523215985, 2.54922155032478, 2.56903545268184,

-    2.58957598670829, 2.61091051848882, 2.63311639363158, 2.65628303757674,

-    2.68051464328574, 2.70593365612306, 2.73268535904401, 2.76094400527999,

-    2.79092117400193, 2.82287739682644, 2.85713873087322, 2.89412105361341,

-    2.93436686720889, 2.97860327988184, 3.02783779176959, 3.08352613200214,

-    3.147889289518, 3.2245750520478, 3.32024473383983, 3.44927829856143,

-    3.65415288536101, 3.91075795952492 };

-

-  real_T u;

-  static const real_T dv2[257] = { 1.0, 0.977101701267673, 0.959879091800108,

-    0.9451989534423, 0.932060075959231, 0.919991505039348, 0.908726440052131,

-    0.898095921898344, 0.887984660755834, 0.878309655808918, 0.869008688036857,

-    0.860033621196332, 0.851346258458678, 0.842915653112205, 0.834716292986884,

-    0.826726833946222, 0.818929191603703, 0.811307874312656, 0.803849483170964,

-    0.796542330422959, 0.789376143566025, 0.782341832654803, 0.775431304981187,

-    0.768637315798486, 0.761953346836795, 0.755373506507096, 0.748892447219157,

-    0.742505296340151, 0.736207598126863, 0.729995264561476, 0.72386453346863,

-    0.717811932630722, 0.711834248878248, 0.705928501332754, 0.700091918136512,

-    0.694321916126117, 0.688616083004672, 0.682972161644995, 0.677388036218774,

-    0.671861719897082, 0.66639134390875, 0.660975147776663, 0.655611470579697,

-    0.650298743110817, 0.645035480820822, 0.639820277453057, 0.634651799287624,

-    0.629528779924837, 0.624450015547027, 0.619414360605834, 0.614420723888914,

-    0.609468064925773, 0.604555390697468, 0.599681752619125, 0.594846243767987,

-    0.590047996332826, 0.585286179263371, 0.580559996100791, 0.575868682972354,

-    0.571211506735253, 0.566587763256165, 0.561996775814525, 0.557437893618766,

-    0.552910490425833, 0.548413963255266, 0.543947731190026, 0.539511234256952,

-    0.535103932380458, 0.530725304403662, 0.526374847171684, 0.522052074672322,

-    0.517756517229756, 0.513487720747327, 0.509245245995748, 0.505028667943468,

-    0.500837575126149, 0.49667156905249, 0.492530263643869, 0.488413284705458,

-    0.484320269426683, 0.480250865909047, 0.476204732719506, 0.47218153846773,

-    0.468180961405694, 0.464202689048174, 0.460246417812843, 0.456311852678716,

-    0.452398706861849, 0.448506701507203, 0.444635565395739, 0.440785034665804,

-    0.436954852547985, 0.433144769112652, 0.429354541029442, 0.425583931338022,

-    0.421832709229496, 0.418100649837848, 0.414387534040891, 0.410693148270188,

-    0.407017284329473, 0.403359739221114, 0.399720314980197, 0.396098818515832,

-    0.392495061459315, 0.388908860018789, 0.385340034840077, 0.381788410873393,

-    0.378253817245619, 0.374736087137891, 0.371235057668239, 0.367750569779032,

-    0.364282468129004, 0.360830600989648, 0.357394820145781, 0.353974980800077,

-    0.350570941481406, 0.347182563956794, 0.343809713146851, 0.340452257044522,

-    0.337110066637006, 0.333783015830718, 0.330470981379163, 0.327173842813601,

-    0.323891482376391, 0.320623784956905, 0.317370638029914, 0.314131931596337,

-    0.310907558126286, 0.307697412504292, 0.30450139197665, 0.301319396100803,

-    0.298151326696685, 0.294997087799962, 0.291856585617095, 0.288729728482183,

-    0.285616426815502, 0.282516593083708, 0.279430141761638, 0.276356989295668,

-    0.273297054068577, 0.270250256365875, 0.267216518343561, 0.264195763997261,

-    0.261187919132721, 0.258192911337619, 0.255210669954662, 0.252241126055942,

-    0.249284212418529, 0.246339863501264, 0.24340801542275, 0.240488605940501,

-    0.237581574431238, 0.23468686187233, 0.231804410824339, 0.228934165414681,

-    0.226076071322381, 0.223230075763918, 0.220396127480152, 0.217574176724331,

-    0.214764175251174, 0.211966076307031, 0.209179834621125, 0.206405406397881,

-    0.203642749310335, 0.200891822494657, 0.198152586545776, 0.195425003514135,

-    0.192709036903589, 0.190004651670465, 0.187311814223801, 0.1846304924268,

-    0.181960655599523, 0.179302274522848, 0.176655321443735, 0.174019770081839,

-    0.171395595637506, 0.168782774801212, 0.166181285764482, 0.163591108232366,

-    0.161012223437511, 0.158444614155925, 0.15588826472448, 0.153343161060263,

-    0.150809290681846, 0.148286642732575, 0.145775208005994, 0.143274978973514,

-    0.140785949814445, 0.138308116448551, 0.135841476571254, 0.133386029691669,

-    0.130941777173644, 0.12850872228, 0.126086870220186, 0.123676228201597,

-    0.12127680548479, 0.11888861344291, 0.116511665625611, 0.114145977827839,

-    0.111791568163838, 0.109448457146812, 0.107116667774684, 0.104796225622487,

-    0.102487158941935, 0.10018949876881, 0.0979032790388625, 0.095628536713009,

-    0.093365311912691, 0.0911136480663738, 0.0888735920682759,

-    0.0866451944505581, 0.0844285095703535, 0.082223595813203,

-    0.0800305158146631, 0.0778493367020961, 0.0756801303589272,

-    0.0735229737139814, 0.0713779490588905, 0.0692451443970068,

-    0.0671246538277886, 0.065016577971243, 0.0629210244377582, 0.06083810834954,

-    0.0587679529209339, 0.0567106901062031, 0.0546664613248891,

-    0.0526354182767924, 0.0506177238609479, 0.0486135532158687,

-    0.0466230949019305, 0.0446465522512946, 0.0426841449164746,

-    0.0407361106559411, 0.0388027074045262, 0.0368842156885674,

-    0.0349809414617162, 0.0330932194585786, 0.0312214171919203,

-    0.0293659397581334, 0.0275272356696031, 0.0257058040085489,

-    0.0239022033057959, 0.0221170627073089, 0.0203510962300445,

-    0.0186051212757247, 0.0168800831525432, 0.0151770883079353,

-    0.0134974506017399, 0.0118427578579079, 0.0102149714397015,

-    0.00861658276939875, 0.00705087547137324, 0.00552240329925101,

-    0.00403797259336304, 0.00260907274610216, 0.0012602859304986,

-    0.000477467764609386 };

-

-  real_T x;

-  do {

-    exitg1 = 0;

-    genrand_uint32_vector(e_state, u32);

-    i = (int32_T)((u32[1] >> 24U) + 1U);

-    r = (((real_T)(u32[0] >> 3U) * 1.6777216E+7 + (real_T)((int32_T)u32[1] &

-           16777215)) * 2.2204460492503131E-16 - 1.0) * dv1[i];

-    if (fabs(r) <= dv1[i - 1]) {

-      exitg1 = 1;

-    } else if (i < 256) {

-      u = b_genrandu(e_state);

-      if (dv2[i] + u * (dv2[i - 1] - dv2[i]) < exp(-0.5 * r * r)) {

-        exitg1 = 1;

-      }

-    } else {

-      do {

-        u = b_genrandu(e_state);

-        x = log(u) * 0.273661237329758;

-        u = b_genrandu(e_state);

-      } while (!(-2.0 * log(u) > x * x));

-

-      if (r < 0.0) {

-        r = x - 3.65415288536101;

-      } else {

-        r = 3.65415288536101 - x;

-      }

-

-      exitg1 = 1;

-    }

-  } while (exitg1 == 0);

-

-  return r;

-}

-

-static real_T eml_rand_shr3cong(uint32_T e_state[2])

-{

-  real_T r;

-  uint32_T icng;

-  uint32_T jsr;

-  uint32_T ui;

-  int32_T j;

-  static const real_T dv0[65] = { 0.340945, 0.4573146, 0.5397793, 0.6062427,

-    0.6631691, 0.7136975, 0.7596125, 0.8020356, 0.8417227, 0.8792102, 0.9148948,

-    0.9490791, 0.9820005, 1.0138492, 1.044781, 1.0749254, 1.1043917, 1.1332738,

-    1.161653, 1.189601, 1.2171815, 1.2444516, 1.2714635, 1.298265, 1.3249008,

-    1.3514125, 1.3778399, 1.4042211, 1.4305929, 1.4569915, 1.4834527, 1.5100122,

-    1.5367061, 1.5635712, 1.5906454, 1.617968, 1.6455802, 1.6735255, 1.7018503,

-    1.7306045, 1.7598422, 1.7896223, 1.8200099, 1.851077, 1.8829044, 1.9155831,

-    1.9492166, 1.9839239, 2.0198431, 2.0571356, 2.095993, 2.136645, 2.1793713,

-    2.2245175, 2.2725186, 2.3239338, 2.3795008, 2.4402218, 2.5075117, 2.5834658,

-    2.6713916, 2.7769942, 2.7769942, 2.7769942, 2.7769942 };

-

-  real_T x;

-  real_T y;

-  real_T s;

-  icng = 69069U * e_state[0] + 1234567U;

-  jsr = e_state[1] ^ e_state[1] << 13U;

-  jsr ^= jsr >> 17U;

-  jsr ^= jsr << 5U;

-  ui = icng + jsr;

-  j = (int32_T)(ui & 63U);

-  r = (real_T)(int32_T)ui * 4.6566128730773926E-10 * dv0[j + 1];

-  if (fabs(r) <= dv0[j]) {

-  } else {

-    x = (fabs(r) - dv0[j]) / (dv0[j + 1] - dv0[j]);

-    icng = 69069U * icng + 1234567U;

-    jsr ^= jsr << 13U;

-    jsr ^= jsr >> 17U;

-    jsr ^= jsr << 5U;

-    y = (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10;

-    s = x + (0.5 + y);

-    if (s > 1.301198) {

-      if (r < 0.0) {

-        r = 0.4878992 * x - 0.4878992;

-      } else {

-        r = 0.4878992 - 0.4878992 * x;

-      }

-    } else if (s <= 0.9689279) {

-    } else {

-      s = 0.4878992 * x;

-      x = 0.4878992 - 0.4878992 * x;

-      if (0.5 + y > 12.67706 - 12.37586 * exp(-0.5 * (0.4878992 - s) * x)) {

-        if (r < 0.0) {

-          r = -(0.4878992 - s);

-        } else {

-          r = 0.4878992 - s;

-        }

-      } else if (exp(-0.5 * dv0[j + 1] * dv0[j + 1]) + (0.5 + y) * 0.01958303 /

-                 dv0[j + 1] <= exp(-0.5 * r * r)) {

-      } else {

-        do {

-          icng = 69069U * icng + 1234567U;

-          jsr ^= jsr << 13U;

-          jsr ^= jsr >> 17U;

-          jsr ^= jsr << 5U;

-          x = log(0.5 + (real_T)(int32_T)(icng + jsr) * 2.328306436538696E-10) /

-            2.776994;

-          icng = 69069U * icng + 1234567U;

-          jsr ^= jsr << 13U;

-          jsr ^= jsr >> 17U;

-          jsr ^= jsr << 5U;

-        } while (!(-2.0 * log(0.5 + (real_T)(int32_T)(icng + jsr) *

-                              2.328306436538696E-10) > x * x));

-

-        if (r < 0.0) {

-          r = x - 2.776994;

-        } else {

-          r = 2.776994 - x;

-        }

-      }

-    }

-  }

-

-  e_state[0] = icng;

-  e_state[1] = jsr;

-  return r;

-}

-

-static void genrand_uint32_vector(uint32_T mt[625], uint32_T u[2])

-{

-  int32_T i;

-  uint32_T mti;

-  int32_T kk;

-  uint32_T y;

-  uint32_T b_y;

-  uint32_T c_y;

-  uint32_T d_y;

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

-    u[i] = 0U;

-  }

-

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

-    mti = mt[624] + 1U;

-    if (mti >= 625U) {

-      for (kk = 0; kk < 227; kk++) {

-        y = (mt[kk] & 2147483648U) | (mt[1 + kk] & 2147483647U);

-        if ((int32_T)(y & 1U) == 0) {

-          b_y = y >> 1U;

-        } else {

-          b_y = y >> 1U ^ 2567483615U;

-        }

-

-        mt[kk] = mt[397 + kk] ^ b_y;

-      }

-

-      for (kk = 0; kk < 396; kk++) {

-        y = (mt[227 + kk] & 2147483648U) | (mt[228 + kk] & 2147483647U);

-        if ((int32_T)(y & 1U) == 0) {

-          c_y = y >> 1U;

-        } else {

-          c_y = y >> 1U ^ 2567483615U;

-        }

-

-        mt[227 + kk] = mt[kk] ^ c_y;

-      }

-

-      y = (mt[623] & 2147483648U) | (mt[0] & 2147483647U);

-      if ((int32_T)(y & 1U) == 0) {

-        d_y = y >> 1U;

-      } else {

-        d_y = y >> 1U ^ 2567483615U;

-      }

-

-      mt[623] = mt[396] ^ d_y;

-      mti = 1U;

-    }

-

-    y = mt[(int32_T)mti - 1];

-    mt[624] = mti;

-    y ^= y >> 11U;

-    y ^= y << 7U & 2636928640U;

-    y ^= y << 15U & 4022730752U;

-    y ^= y >> 18U;

-    u[i] = y;

-  }

-}

-

-static void genrandu(uint32_T s, uint32_T *e_state, real_T *r)

-{

-  int32_T hi;

-  uint32_T test1;

-  uint32_T test2;

-  hi = (int32_T)(s / 127773U);

-  test1 = 16807U * (s - (uint32_T)hi * 127773U);

-  test2 = 2836U * (uint32_T)hi;

-  if (test1 < test2) {

-    *e_state = (test1 - test2) + 2147483647U;

-  } else {

-    *e_state = test1 - test2;

-  }

-

-  *r = (real_T)*e_state * 4.6566128752457969E-10;

-}

-

-static void twister_state_vector(uint32_T mt[625], real_T seed)

-{

-  uint32_T r;

-  int32_T mti;

-  if (seed < 4.294967296E+9) {

-    if (seed >= 0.0) {

-      r = (uint32_T)seed;

-    } else {

-      r = 0U;

-    }

-  } else if (seed >= 4.294967296E+9) {

-    r = MAX_uint32_T;

-  } else {

-    r = 0U;

-  }

-

-  mt[0] = r;

-  for (mti = 0; mti < 623; mti++) {

-    r = (r ^ r >> 30U) * 1812433253U + (uint32_T)(1 + mti);

-    mt[1 + mti] = r;

-  }

-

-  mt[624] = 624U;

-}

-

-real_T randn(void)

-{

-  real_T r;

-  uint32_T e_state;

-  real_T t;

-  real_T b_r;

-  uint32_T f_state;

-  if (method == 0U) {

-    if (b_method == 4U) {

-      do {

-        genrandu(b_state, &e_state, &r);

-        genrandu(e_state, &b_state, &t);

-        b_r = 2.0 * r - 1.0;

-        t = 2.0 * t - 1.0;

-        t = t * t + b_r * b_r;

-      } while (!(t <= 1.0));

-

-      r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);

-    } else if (b_method == 5U) {

-      r = eml_rand_shr3cong(c_state);

-    } else {

-      if (!state_not_empty) {

-        memset(&d_state[0], 0, 625U * sizeof(uint32_T));

-        twister_state_vector(d_state, 5489.0);

-        state_not_empty = TRUE;

-      }

-

-      r = eml_rand_mt19937ar(d_state);

-    }

-  } else if (method == 4U) {

-    e_state = state[0];

-    do {

-      genrandu(e_state, &f_state, &r);

-      genrandu(f_state, &e_state, &t);

-      b_r = 2.0 * r - 1.0;

-      t = 2.0 * t - 1.0;

-      t = t * t + b_r * b_r;

-    } while (!(t <= 1.0));

-

-    state[0] = e_state;

-    r = (2.0 * r - 1.0) * sqrt(-2.0 * log(t) / t);

-  } else {

-    r = eml_rand_shr3cong(state);

-  }

-

-  return r;

-}

-

-/* End of code generation (randn.c) */

diff --git a/src/modules/position_estimator_mc/codegen/randn.h b/src/modules/position_estimator_mc/codegen/randn.h
deleted file mode 100755
index 8a2aa9277..000000000
--- a/src/modules/position_estimator_mc/codegen/randn.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*

- * randn.h

- *

- * Code generation for function 'randn'

- *

- * C source code generated on: Tue Feb 19 15:26:32 2013

- *

- */

-

-#ifndef __RANDN_H__

-#define __RANDN_H__

-/* Include files */

-#include <math.h>

-#include <stddef.h>

-#include <stdlib.h>

-#include <string.h>

-#include "rt_nonfinite.h"

-

-#include "rtwtypes.h"

-#include "kalman_dlqe3_types.h"

-

-/* Type Definitions */

-

-/* Named Constants */

-

-/* Variable Declarations */

-

-/* Variable Definitions */

-

-/* Function Declarations */

-extern real_T randn(void);

-#endif

-/* End of code generation (randn.h) */

diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.c b/src/modules/position_estimator_mc/codegen/rtGetInf.c
deleted file mode 100755
index c6fa7884e..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetInf.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * rtGetInf.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-/*
- * Abstract:
- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf
- */
-#include "rtGetInf.h"
-#define NumBitsPerChar	8U
-
-/* Function: rtGetInf ==================================================
- * Abstract:
- * Initialize rtInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetInf(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T inf = 0.0;
-  if (bitsPerReal == 32U) {
-    inf = rtGetInfF();
-  } else {
-    uint16_T one = 1U;
-    enum {
-      LittleEndian,
-      BigEndian
-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-    switch (machByteOrder) {
-     case LittleEndian:
-      {
-        union {
-          LittleEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        inf = tmpVal.fltVal;
-        break;
-      }
-
-     case BigEndian:
-      {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        inf = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return inf;
-}
-
-/* Function: rtGetInfF ==================================================
- * Abstract:
- * Initialize rtInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetInfF(void)
-{
-  IEEESingle infF;
-  infF.wordL.wordLuint = 0x7F800000U;
-  return infF.wordL.wordLreal;
-}
-
-/* Function: rtGetMinusInf ==================================================
- * Abstract:
- * Initialize rtMinusInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetMinusInf(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T minf = 0.0;
-  if (bitsPerReal == 32U) {
-    minf = rtGetMinusInfF();
-  } else {
-    uint16_T one = 1U;
-    enum {
-      LittleEndian,
-      BigEndian
-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-    switch (machByteOrder) {
-     case LittleEndian:
-      {
-        union {
-          LittleEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0xFFF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        minf = tmpVal.fltVal;
-        break;
-      }
-
-     case BigEndian:
-      {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0xFFF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        minf = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return minf;
-}
-
-/* Function: rtGetMinusInfF ==================================================
- * Abstract:
- * Initialize rtMinusInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetMinusInfF(void)
-{
-  IEEESingle minfF;
-  minfF.wordL.wordLuint = 0xFF800000U;
-  return minfF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetInf.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetInf.h b/src/modules/position_estimator_mc/codegen/rtGetInf.h
deleted file mode 100755
index e7b2a2d1c..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetInf.h
+++ /dev/null
@@ -1,23 +0,0 @@
-/*
- * rtGetInf.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RTGETINF_H__
-#define __RTGETINF_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetInf(void);
-extern real32_T rtGetInfF(void);
-extern real_T rtGetMinusInf(void);
-extern real32_T rtGetMinusInfF(void);
-
-#endif
-/* End of code generation (rtGetInf.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.c b/src/modules/position_estimator_mc/codegen/rtGetNaN.c
deleted file mode 100755
index 552770149..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetNaN.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * rtGetNaN.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-/*
- * Abstract:
- *       MATLAB for code generation function to initialize non-finite, NaN
- */
-#include "rtGetNaN.h"
-#define NumBitsPerChar	8U
-
-/* Function: rtGetNaN ==================================================
- * Abstract:
- * Initialize rtNaN needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetNaN(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T nan = 0.0;
-  if (bitsPerReal == 32U) {
-    nan = rtGetNaNF();
-  } else {
-  uint16_T one = 1U;
-  enum {
-    LittleEndian,
-    BigEndian
-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-  switch (machByteOrder) {
-   case LittleEndian:
-    {
-      union {
-        LittleEndianIEEEDouble bitVal;
-        real_T fltVal;
-      } tmpVal;
-
-      tmpVal.bitVal.words.wordH = 0xFFF80000U;
-      tmpVal.bitVal.words.wordL = 0x00000000U;
-      nan = tmpVal.fltVal;
-      break;
-    }
-
-   case BigEndian:
-    {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
-        nan = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return nan;
-}
-
-/* Function: rtGetNaNF ==================================================
- * Abstract:
- * Initialize rtNaNF needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetNaNF(void)
-{
-  IEEESingle nanF = { { 0 } };
-  uint16_T one = 1U;
-  enum {
-    LittleEndian,
-    BigEndian
-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-  switch (machByteOrder) {
-   case LittleEndian:
-    {
-      nanF.wordL.wordLuint = 0xFFC00000U;
-      break;
-    }
-
-   case BigEndian:
-    {
-      nanF.wordL.wordLuint = 0x7FFFFFFFU;
-      break;
-    }
-  }
-
-  return nanF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetNaN.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rtGetNaN.h b/src/modules/position_estimator_mc/codegen/rtGetNaN.h
deleted file mode 100755
index 5acdd9790..000000000
--- a/src/modules/position_estimator_mc/codegen/rtGetNaN.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * rtGetNaN.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RTGETNAN_H__
-#define __RTGETNAN_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetNaN(void);
-extern real32_T rtGetNaNF(void);
-
-#endif
-/* End of code generation (rtGetNaN.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c b/src/modules/position_estimator_mc/codegen/rt_nonfinite.c
deleted file mode 100755
index de121c4a0..000000000
--- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.c
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * rt_nonfinite.c
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-/*
- * Abstract:
- *      MATLAB for code generation function to initialize non-finites,
- *      (Inf, NaN and -Inf).
- */
-#include "rt_nonfinite.h"
-#include "rtGetNaN.h"
-#include "rtGetInf.h"
-
-real_T rtInf;
-real_T rtMinusInf;
-real_T rtNaN;
-real32_T rtInfF;
-real32_T rtMinusInfF;
-real32_T rtNaNF;
-
-/* Function: rt_InitInfAndNaN ==================================================
- * Abstract:
- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
- * generated code. NaN is initialized as non-signaling. Assumes IEEE.
- */
-void rt_InitInfAndNaN(size_t realSize)
-{
-  (void) (realSize);
-  rtNaN = rtGetNaN();
-  rtNaNF = rtGetNaNF();
-  rtInf = rtGetInf();
-  rtInfF = rtGetInfF();
-  rtMinusInf = rtGetMinusInf();
-  rtMinusInfF = rtGetMinusInfF();
-}
-
-/* Function: rtIsInf ==================================================
- * Abstract:
- * Test if value is infinite
- */
-boolean_T rtIsInf(real_T value)
-{
-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
-}
-
-/* Function: rtIsInfF =================================================
- * Abstract:
- * Test if single-precision value is infinite
- */
-boolean_T rtIsInfF(real32_T value)
-{
-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
-}
-
-/* Function: rtIsNaN ==================================================
- * Abstract:
- * Test if value is not a number
- */
-boolean_T rtIsNaN(real_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan(value)? TRUE:FALSE;
-#else
-  return (value!=value)? 1U:0U;
-#endif
-}
-
-/* Function: rtIsNaNF =================================================
- * Abstract:
- * Test if single-precision value is not a number
- */
-boolean_T rtIsNaNF(real32_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan((real_T)value)? true:false;
-#else
-  return (value!=value)? 1U:0U;
-#endif
-}
-
-
-/* End of code generation (rt_nonfinite.c) */
diff --git a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h b/src/modules/position_estimator_mc/codegen/rt_nonfinite.h
deleted file mode 100755
index 3bbcfd087..000000000
--- a/src/modules/position_estimator_mc/codegen/rt_nonfinite.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
- * rt_nonfinite.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RT_NONFINITE_H__
-#define __RT_NONFINITE_H__
-
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-#include <float.h>
-#endif
-#include <stddef.h>
-#include "rtwtypes.h"
-
-extern real_T rtInf;
-extern real_T rtMinusInf;
-extern real_T rtNaN;
-extern real32_T rtInfF;
-extern real32_T rtMinusInfF;
-extern real32_T rtNaNF;
-extern void rt_InitInfAndNaN(size_t realSize);
-extern boolean_T rtIsInf(real_T value);
-extern boolean_T rtIsInfF(real32_T value);
-extern boolean_T rtIsNaN(real_T value);
-extern boolean_T rtIsNaNF(real32_T value);
-
-typedef struct {
-  struct {
-    uint32_T wordH;
-    uint32_T wordL;
-  } words;
-} BigEndianIEEEDouble;
-
-typedef struct {
-  struct {
-    uint32_T wordL;
-    uint32_T wordH;
-  } words;
-} LittleEndianIEEEDouble;
-
-typedef struct {
-  union {
-    real32_T wordLreal;
-    uint32_T wordLuint;
-  } wordL;
-} IEEESingle;
-
-#endif
-/* End of code generation (rt_nonfinite.h) */
diff --git a/src/modules/position_estimator_mc/codegen/rtwtypes.h b/src/modules/position_estimator_mc/codegen/rtwtypes.h
deleted file mode 100755
index 8916e8572..000000000
--- a/src/modules/position_estimator_mc/codegen/rtwtypes.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- * rtwtypes.h
- *
- * Code generation for function 'kalman_dlqe2'
- *
- * C source code generated on: Thu Feb 14 12:52:29 2013
- *
- */
-
-#ifndef __RTWTYPES_H__
-#define __RTWTYPES_H__
-#ifndef TRUE
-# define TRUE (1U)
-#endif
-#ifndef FALSE
-# define FALSE (0U)
-#endif
-#ifndef __TMWTYPES__
-#define __TMWTYPES__
-
-#include <limits.h>
-
-/*=======================================================================* 
- * Target hardware information
- *   Device type: Generic->MATLAB Host Computer
- *   Number of bits:     char:   8    short:   16    int:  32
- *                       long:  32      native word size:  32
- *   Byte ordering: LittleEndian
- *   Signed integer division rounds to: Undefined
- *   Shift right on a signed integer as arithmetic shift: off
- *=======================================================================*/
-
-/*=======================================================================* 
- * Fixed width word size data types:                                     * 
- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
- *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
- *=======================================================================*/
-
-typedef signed char int8_T;
-typedef unsigned char uint8_T;
-typedef short int16_T;
-typedef unsigned short uint16_T;
-typedef int int32_T;
-typedef unsigned int uint32_T;
-typedef float real32_T;
-typedef double real64_T;
-
-/*===========================================================================* 
- * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
- *                           ulong_T, char_T and byte_T.                     * 
- *===========================================================================*/
-
-typedef double real_T;
-typedef double time_T;
-typedef unsigned char boolean_T;
-typedef int int_T;
-typedef unsigned int uint_T;
-typedef unsigned long ulong_T;
-typedef char char_T;
-typedef char_T byte_T;
-
-/*===========================================================================* 
- * Complex number type definitions                                           * 
- *===========================================================================*/
-#define CREAL_T	
-   typedef struct {  
-     real32_T re;  
-     real32_T im;  
-   } creal32_T;  
-
-   typedef struct {  
-     real64_T re;  
-     real64_T im;  
-   } creal64_T;  
-
-   typedef struct {  
-     real_T re;  
-     real_T im;  
-   } creal_T;  
-
-   typedef struct {  
-     int8_T re;  
-     int8_T im;  
-   } cint8_T;  
-
-   typedef struct {  
-     uint8_T re;  
-     uint8_T im;  
-   } cuint8_T;  
-
-   typedef struct {  
-     int16_T re;  
-     int16_T im;  
-   } cint16_T;  
-
-   typedef struct {  
-     uint16_T re;  
-     uint16_T im;  
-   } cuint16_T;  
-
-   typedef struct {  
-     int32_T re;  
-     int32_T im;  
-   } cint32_T;  
-
-   typedef struct {  
-     uint32_T re;  
-     uint32_T im;  
-   } cuint32_T;  
-
-
-/*=======================================================================* 
- * Min and Max:                                                          * 
- *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
- *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
- *=======================================================================*/
-
-#define MAX_int8_T  	((int8_T)(127))
-#define MIN_int8_T  	((int8_T)(-128))
-#define MAX_uint8_T 	((uint8_T)(255))
-#define MIN_uint8_T 	((uint8_T)(0))
-#define MAX_int16_T 	((int16_T)(32767))
-#define MIN_int16_T 	((int16_T)(-32768))
-#define MAX_uint16_T	((uint16_T)(65535))
-#define MIN_uint16_T	((uint16_T)(0))
-#define MAX_int32_T 	((int32_T)(2147483647))
-#define MIN_int32_T 	((int32_T)(-2147483647-1))
-#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
-#define MIN_uint32_T	((uint32_T)(0))
-
-/* Logical type definitions */
-#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
-#  ifndef false
-#   define false (0U)
-#  endif
-#  ifndef true
-#   define true (1U)
-#  endif
-#endif
-
-/*
- * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
- * for signed integer values.
- */
-#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
-#error "This code must be compiled using a 2's complement representation for signed integer values"
-#endif
-
-/*
- * Maximum length of a MATLAB identifier (function/variable)
- * including the null-termination character. Referenced by
- * rt_logging.c and rt_matrx.c.
- */
-#define TMW_NAME_LENGTH_MAX	64
-
-#endif
-#endif
-/* End of code generation (rtwtypes.h) */
diff --git a/src/modules/position_estimator_mc/kalman_dlqe1.m b/src/modules/position_estimator_mc/kalman_dlqe1.m
deleted file mode 100755
index ff939d029..000000000
--- a/src/modules/position_estimator_mc/kalman_dlqe1.m
+++ /dev/null
@@ -1,3 +0,0 @@
-function [x_aposteriori] = kalman_dlqe1(A,C,K,x_aposteriori_k,z)
-    x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);      
-end
\ No newline at end of file
diff --git a/src/modules/position_estimator_mc/kalman_dlqe2.m b/src/modules/position_estimator_mc/kalman_dlqe2.m
deleted file mode 100755
index 2a50164ef..000000000
--- a/src/modules/position_estimator_mc/kalman_dlqe2.m
+++ /dev/null
@@ -1,9 +0,0 @@
-function [x_aposteriori] = kalman_dlqe2(dt,k1,k2,k3,x_aposteriori_k,z)
-    st = 1/2*dt^2;
-    A = [1,dt,st;
-        0,1,dt;
-        0,0,1];
-    C=[1,0,0];
-    K = [k1;k2;k3];
-    x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);      
-end
\ No newline at end of file
diff --git a/src/modules/position_estimator_mc/kalman_dlqe3.m b/src/modules/position_estimator_mc/kalman_dlqe3.m
deleted file mode 100755
index 4c6421b7f..000000000
--- a/src/modules/position_estimator_mc/kalman_dlqe3.m
+++ /dev/null
@@ -1,17 +0,0 @@
-function [x_aposteriori] = kalman_dlqe3(dt,k1,k2,k3,x_aposteriori_k,z,posUpdate,addNoise,sigma)

-    st = 1/2*dt^2;

-    A = [1,dt,st;

-        0,1,dt;

-        0,0,1];

-    C=[1,0,0];

-    K = [k1;k2;k3];

-    if addNoise==1

-        noise = sigma*randn(1,1);

-        z = z + noise;

-    end

-    if(posUpdate)

-        x_aposteriori=A*x_aposteriori_k+K*(z-C*A*x_aposteriori_k);

-    else

-        x_aposteriori=A*x_aposteriori_k;

-    end

-end
\ No newline at end of file
diff --git a/src/modules/position_estimator_mc/module.mk b/src/modules/position_estimator_mc/module.mk
deleted file mode 100644
index 40b135ea4..000000000
--- a/src/modules/position_estimator_mc/module.mk
+++ /dev/null
@@ -1,60 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# Makefile to build the position estimator
-#
-
-MODULE_COMMAND	 = position_estimator_mc
-
-SRCS		 = position_estimator_mc_main.c \
-			   position_estimator_mc_params.c \
-			   codegen/positionKalmanFilter1D_initialize.c \
-			   codegen/positionKalmanFilter1D_terminate.c \
-			   codegen/positionKalmanFilter1D.c \
-			   codegen/rt_nonfinite.c \
-			   codegen/rtGetInf.c \
-			   codegen/rtGetNaN.c \
-			   codegen/positionKalmanFilter1D_dT_initialize.c \
-			   codegen/positionKalmanFilter1D_dT_terminate.c \
-			   codegen/kalman_dlqe1.c \
-			   codegen/kalman_dlqe1_initialize.c \
-			   codegen/kalman_dlqe1_terminate.c \
-			   codegen/kalman_dlqe2.c \
-			   codegen/kalman_dlqe2_initialize.c \
-			   codegen/kalman_dlqe2_terminate.c \
-			   codegen/kalman_dlqe3.c \
-			   codegen/kalman_dlqe3_initialize.c \
-			   codegen/kalman_dlqe3_terminate.c \
-			   codegen/kalman_dlqe3_data.c \
-			   codegen/randn.c
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D.m b/src/modules/position_estimator_mc/positionKalmanFilter1D.m
deleted file mode 100755
index 144ff8c7c..000000000
--- a/src/modules/position_estimator_mc/positionKalmanFilter1D.m
+++ /dev/null
@@ -1,19 +0,0 @@
-function [x_aposteriori,P_aposteriori]=positionKalmanFilter1D(A,B,C,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
-%prediction
-    x_apriori=A*x_aposteriori_k+B*u;
-    P_apriori=A*P_aposteriori_k*A'+Q;
-    if abs(u)<thresh
-        x_apriori(2)=decay*x_apriori(2);
-    end
-    %update
-    if gps_update==1
-        y=z-C*x_apriori;
-        S=C*P_apriori*C'+R;
-        K=(P_apriori*C')/S;
-        x_aposteriori=x_apriori+K*y;
-        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
-    else
-        x_aposteriori=x_apriori;
-        P_aposteriori=P_apriori;
-    end
-end
diff --git a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m b/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m
deleted file mode 100755
index f94cce1fb..000000000
--- a/src/modules/position_estimator_mc/positionKalmanFilter1D_dT.m
+++ /dev/null
@@ -1,26 +0,0 @@
-function  [x_aposteriori,P_aposteriori]=positionKalmanFilter1D_dT(dT,x_aposteriori_k,P_aposteriori_k,u,z,gps_update,Q,R,thresh,decay)
-    %dynamics
-    A = [1 dT -0.5*dT*dT;
-         0 1 -dT;
-         0 0 1];
-    B = [0.5*dT*dT; dT; 0];
-    C = [1 0 0];
-    %prediction
-    x_apriori=A*x_aposteriori_k+B*u;
-    P_apriori=A*P_aposteriori_k*A'+Q;
-    if abs(u)<thresh
-        x_apriori(2)=decay*x_apriori(2);
-    end
-    %update
-    if gps_update==1
-        y=z-C*x_apriori;
-        S=C*P_apriori*C'+R;
-        K=(P_apriori*C')/S;
-        x_aposteriori=x_apriori+K*y;
-        P_aposteriori=(eye(size(P_apriori))-K*C)*P_apriori;
-    else
-        x_aposteriori=x_apriori;
-        P_aposteriori=P_apriori;
-    end
-end
-
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
deleted file mode 100755
index 363961819..000000000
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ /dev/null
@@ -1,515 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: 	Damian Aregger	<daregger@student.ethz.ch>
- *   			Tobias Naegeli <naegelit@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 position_estimator_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/sensor_combined.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls_effective.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_vicon_position.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <uORB/topics/vehicle_gps_position.h>
-#include <mavlink/mavlink_log.h>
-#include <poll.h>
-#include <systemlib/geo/geo.h>
-#include <systemlib/err.h>
-#include <systemlib/systemlib.h>
-
-#include <drivers/drv_hrt.h>
-
-#include "position_estimator_mc_params.h"
-//#include <uORB/topics/debug_key_value.h>
-#include "codegen/kalman_dlqe2.h"
-#include "codegen/kalman_dlqe2_initialize.h"
-#include "codegen/kalman_dlqe3.h"
-#include "codegen/kalman_dlqe3_initialize.h"
-
-static bool thread_should_exit = false;	/**< Deamon exit flag */
-static bool thread_running = false;	/**< Deamon status flag */
-static int position_estimator_mc_task;	/**< Handle of deamon task / thread */
-
-__EXPORT int position_estimator_mc_main(int argc, char *argv[]);
-
-int position_estimator_mc_thread_main(int argc, char *argv[]);
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		warnx("%s\n", reason);
-	warnx("usage: position_estimator_mc {start|stop|status}");
-	exit(1);
-}
-
-/**
- * The position_estimator_mc_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_mc_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("position_estimator_mc already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		position_estimator_mc_task = task_spawn_cmd("position_estimator_mc",
-					 SCHED_RR,
-					 SCHED_PRIORITY_MAX - 5,
-					 4096,
-					 position_estimator_mc_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("position_estimator_mc is running");
-		} else {
-			warnx("position_estimator_mc not started");
-		}
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-/****************************************************************************
- * main
- ****************************************************************************/
-int position_estimator_mc_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	warnx("[position_estimator_mc] started");
-	int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[position_estimator_mc] started");
-
-	/* initialize values */
-	float z[3] = {0, 0, 0}; /* output variables from tangent plane mapping */
-	// float rotMatrix[4] = {1.0f,  0.0f, 0.0f,  1.0f};
-	float x_x_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
-	float x_y_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
-	float x_z_aposteriori_k[3] = {1.0f, 0.0f, 0.0f};
-	float x_x_aposteriori[3] = {0.0f, 0.0f, 0.0f};
-	float x_y_aposteriori[3] = {1.0f, 0.0f, 0.0f};
-	float x_z_aposteriori[3] = {1.0f, 0.0f, 0.0f};
-
-	// XXX this is terribly wrong and should actual dT instead
-	const float dT_const_50 = 1.0f/50.0f;
-
-	float addNoise = 0.0f;
-	float sigma = 0.0f;
-	//computed from dlqe in matlab
-	const float K_vicon_50Hz[3] = {0.5297f, 0.9873f, 0.9201f};
-	// XXX implement baro filter
-	const float K_baro[3] = {0.0248f, 0.0377f, 0.0287f};
-	float K[3] = {0.0f, 0.0f, 0.0f};
-	int baro_loop_cnt = 0;
-	int baro_loop_end = 70; /* measurement for 1 second */
-	float p0_Pa = 0.0f; /* to determin while start up */
-	float rho0 = 1.293f; /* standard pressure */
-	const float const_earth_gravity = 9.81f;
-
-	float posX = 0.0f;
-	float posY = 0.0f;
-	float posZ = 0.0f;
-
-	double lat_current;
-	double lon_current;
-	float alt_current;
-
-	float gps_origin_altitude = 0.0f;
-
-	/* Initialize filter */
-	kalman_dlqe2_initialize();
-	kalman_dlqe3_initialize();
-
-	/* declare and safely initialize all structs */
-	struct sensor_combined_s sensor;
-	memset(&sensor, 0, sizeof(sensor));
-	struct vehicle_attitude_s att;
-	memset(&att, 0, sizeof(att));
-	struct vehicle_status_s vehicle_status;
-	memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */
-	struct vehicle_vicon_position_s vicon_pos;
-	memset(&vicon_pos, 0, sizeof(vicon_pos));
-	struct actuator_controls_effective_s act_eff;
-	memset(&act_eff, 0, sizeof(act_eff));
-	struct vehicle_gps_position_s gps;
-	memset(&gps, 0, sizeof(gps));
-	struct vehicle_local_position_s local_pos_est;
-	memset(&local_pos_est, 0, sizeof(local_pos_est));
-	struct vehicle_global_position_s global_pos_est;
-	memset(&global_pos_est, 0, sizeof(global_pos_est));
-
-	/* subscribe */
-	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int sub_params = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	int actuator_eff_sub = orb_subscribe(ORB_ID(actuator_controls_effective_0));
-	int vehicle_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-
-	/* advertise */
-	orb_advert_t local_pos_est_pub = 0;
-	orb_advert_t global_pos_est_pub = 0;
-
-	struct position_estimator_mc_params pos1D_params;
-	struct position_estimator_mc_param_handles pos1D_param_handles;
-	/* initialize parameter handles */
-	parameters_init(&pos1D_param_handles);
-
-	bool flag_use_gps = false;
-	bool flag_use_baro = false;
-	bool flag_baro_initialized = false; /* in any case disable baroINITdone */
-	/* FIRST PARAMETER READ at START UP*/
-	struct parameter_update_s update;
-	orb_copy(ORB_ID(parameter_update), sub_params, &update); /* read from param to clear updated flag */
-	/* FIRST PARAMETER UPDATE */
-	parameters_update(&pos1D_param_handles, &pos1D_params);
-	flag_use_baro = pos1D_params.baro;
-	sigma = pos1D_params.sigma;
-	addNoise = pos1D_params.addNoise;
-	/* END FIRST PARAMETER UPDATE */
-
-	/* try to grab a vicon message - if it fails, go for GPS. */
-
-	/* make sure the next orb_check() can't return true unless we get a timely update */
-	orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
-	/* allow 200 ms for vicon to come in */
-	usleep(200000);
-	/* check if we got vicon */
-	bool update_check;
-	orb_check(vicon_pos_sub, &update_check);
-	/* if no update was available, use GPS */
-	flag_use_gps = !update_check;
-
-	if (flag_use_gps) {
-		mavlink_log_info(mavlink_fd, "[pos_est_mc] GPS locked");
-		/* wait until gps signal turns valid, only then can we initialize the projection */
-
-		// 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.
-		 */
-
-		while (!(gps.fix_type == 3
-			&& (gps.eph_m < hdop_threshold_m)
-			&& (gps.epv_m < vdop_threshold_m)
-			&& (hrt_absolute_time() - gps.timestamp_position < 2000000))) {
-
-			struct pollfd fds1[2] = {
-					{ .fd = vehicle_gps_sub, .events = POLLIN },
-					{ .fd = sub_params,   .events = POLLIN },
-			};
-
-			/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
-			 * this choice is critical, since the vehicle status might not
-			 * actually change, if this app is started after GPS lock was
-			 * aquired.
-			 */
-			if (poll(fds1, 2, 5000)) {
-				if (fds1[0].revents & POLLIN){
-					/* Read gps position */
-					orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-				}
-				if (fds1[1].revents & POLLIN){
-					/* Read out parameters to check for an update there, e.g. useGPS variable */
-					/* read from param to clear updated flag */
-					struct parameter_update_s updated;
-					orb_copy(ORB_ID(parameter_update), sub_params, &updated);
-					/* update parameters */
-					parameters_update(&pos1D_param_handles, &pos1D_params);
-				}
-			}
-			static int printcounter = 0;
-			if (printcounter == 100) {
-				printcounter = 0;
-				warnx("[pos_est_mc] wait for GPS fix");
-			}
-			printcounter++;
-		}
-
-		/* get gps value for first initialization */
-		orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-		lat_current = ((double)(gps.lat)) * 1e-7d;
-		lon_current = ((double)(gps.lon)) * 1e-7d;
-		alt_current = gps.alt * 1e-3f;
-		gps_origin_altitude = alt_current;
-		/* initialize coordinates */
-		map_projection_init(lat_current, lon_current);
-		/* publish global position messages only after first GPS message */
-		printf("[pos_est_mc] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
-
-	} else {
-		mavlink_log_info(mavlink_fd, "[pos_est_mc] I'm NOT using GPS - I use VICON");
-		/* onboard calculated position estimations */
-	}
-	thread_running = true;
-
-	struct pollfd fds2[3] = {
-		{ .fd = vehicle_gps_sub,   .events = POLLIN },
-		{ .fd = vicon_pos_sub,   .events = POLLIN },
-		{ .fd = sub_params,   .events = POLLIN },
-	};
-
-	bool vicon_updated = false;
-	bool gps_updated = false;
-
-	/**< main_loop */
-	while (!thread_should_exit) {
-		int ret = poll(fds2, 3, 20);  //wait maximal this 20 ms = 50 Hz minimum rate
-		if (ret < 0) {
-			/* poll error */
-		} else {
-			if (fds2[2].revents & POLLIN){
-				/* new parameter */
-				/* read from param to clear updated flag */
-				struct parameter_update_s updated;
-				orb_copy(ORB_ID(parameter_update), sub_params, &updated);
-				/* update parameters */
-				parameters_update(&pos1D_param_handles, &pos1D_params);
-				flag_use_baro = pos1D_params.baro;
-				sigma = pos1D_params.sigma;
-				addNoise = pos1D_params.addNoise;
-			}
-			vicon_updated = false; /* default is no vicon_updated */
-			if (fds2[1].revents & POLLIN) {
-				/* new vicon position */
-				orb_copy(ORB_ID(vehicle_vicon_position), vicon_pos_sub, &vicon_pos);
-				posX = vicon_pos.x;
-				posY = vicon_pos.y;
-				posZ = vicon_pos.z;
-				vicon_updated = true; /* set flag for vicon update */
-			} /* end of poll call for vicon updates */
-			gps_updated = false;
-			if (fds2[0].revents & POLLIN) {
-				/* new GPS value */
-				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-				/* Project gps lat lon (Geographic coordinate system) to plane*/
-				map_projection_project(((double)(gps.lat)) * 1e-7d, ((double)(gps.lon)) * 1e-7d, &(z[0]), &(z[1]));
-				posX = z[0];
-				posY = z[1];
-				posZ = (float)(gps.alt * 1e-3f);
-				gps_updated = true;
-			}
-
-			/* Main estimator loop */
-			orb_copy(ORB_ID(actuator_controls_effective_0), actuator_eff_sub, &act_eff);
-			orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
-			orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensor);
-			// barometric pressure estimation at start up
-			if (!flag_baro_initialized){
-				// mean calculation over several measurements
-				if (baro_loop_cnt<baro_loop_end) {
-					p0_Pa += (sensor.baro_pres_mbar*100);
-					baro_loop_cnt++;
-				} else {
-					p0_Pa /= (float)(baro_loop_cnt);
-					flag_baro_initialized = true;
-					char *baro_m_start = "barometer initialized with p0 = ";
-					char p0_char[15];
-					sprintf(p0_char, "%8.2f", (double)(p0_Pa/100));
-					char *baro_m_end = " mbar";
-					char str[80];
-					strcpy(str,baro_m_start);
-					strcat(str,p0_char);
-					strcat(str,baro_m_end);
-					mavlink_log_info(mavlink_fd, str);
-				}
-			}
-			if (flag_use_gps) {
-				/* initialize map projection with the last estimate (not at full rate) */
-				if (gps.fix_type > 2) {
-					/* x-y-position/velocity estimation in earth frame = gps frame */
-					kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,gps_updated,0.0f,0.0f,x_x_aposteriori);
-					memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
-					kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,gps_updated,0.0f,0.0f,x_y_aposteriori);
-					memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
-					/* z-position/velocity estimation in earth frame = vicon frame */
-					float z_est = 0.0f;
-					if (flag_baro_initialized && flag_use_baro) {
-						z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100))/(rho0*const_earth_gravity);
-						K[0] = K_vicon_50Hz[0];
-						K[1] = K_vicon_50Hz[1];
-						K[2] = K_vicon_50Hz[2];
-						gps_updated = 1.0f; /* always enable the update, cause baro update = 200 Hz */
-					} else {
-						z_est = posZ;
-						K[0] = K_vicon_50Hz[0];
-						K[1] = K_vicon_50Hz[1];
-						K[2] = K_vicon_50Hz[2];
-					}
-
-					kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,gps_updated,0.0f,0.0f,x_z_aposteriori);
-					memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
-					local_pos_est.x = x_x_aposteriori_k[0];
-					local_pos_est.vx = x_x_aposteriori_k[1];
-					local_pos_est.y = x_y_aposteriori_k[0];
-					local_pos_est.vy = x_y_aposteriori_k[1];
-					local_pos_est.z = x_z_aposteriori_k[0];
-					local_pos_est.vz = x_z_aposteriori_k[1];
-					local_pos_est.timestamp = hrt_absolute_time();
-					if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))) {
-						/* publish local position estimate */
-						if (local_pos_est_pub > 0) {
-							orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
-						} else {
-							local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
-						}
-						/* publish on GPS updates */
-						if (gps_updated) {
-
-							double lat, lon;
-							float alt = z_est + gps_origin_altitude;
-
-							map_projection_reproject(local_pos_est.x, local_pos_est.y, &lat, &lon);
-
-							global_pos_est.lat = lat;
-							global_pos_est.lon = lon;
-							global_pos_est.alt = alt;
-
-							if (global_pos_est_pub > 0) {
-								orb_publish(ORB_ID(vehicle_global_position), global_pos_est_pub, &global_pos_est);
-							} else {
-								global_pos_est_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos_est);
-							}
-						}
-					}
-				}
-			} else {
-				/* x-y-position/velocity estimation in earth frame = vicon frame */
-				kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_x_aposteriori_k,posX,vicon_updated,addNoise,sigma,x_x_aposteriori);
-				memcpy(x_x_aposteriori_k, x_x_aposteriori, sizeof(x_x_aposteriori));
-				kalman_dlqe3(dT_const_50,K_vicon_50Hz[0],K_vicon_50Hz[1],K_vicon_50Hz[2],x_y_aposteriori_k,posY,vicon_updated,addNoise,sigma,x_y_aposteriori);
-				memcpy(x_y_aposteriori_k, x_y_aposteriori, sizeof(x_y_aposteriori));
-				/* z-position/velocity estimation in earth frame = vicon frame */
-				float z_est = 0.0f;
-				float local_sigma = 0.0f;
-				if (flag_baro_initialized && flag_use_baro) {
-					z_est = -p0_Pa*logf(p0_Pa/(sensor.baro_pres_mbar*100.0f))/(rho0*const_earth_gravity);
-					K[0] = K_vicon_50Hz[0];
-					K[1] = K_vicon_50Hz[1];
-					K[2] = K_vicon_50Hz[2];
-					vicon_updated = 1; /* always enable the update, cause baro update = 200 Hz */
-					local_sigma = 0.0f; /* don't add noise on barometer in any case */
-				} else {
-					z_est = posZ;
-					K[0] = K_vicon_50Hz[0];
-					K[1] = K_vicon_50Hz[1];
-					K[2] = K_vicon_50Hz[2];
-					local_sigma = sigma;
-				}
-				kalman_dlqe3(dT_const_50,K[0],K[1],K[2],x_z_aposteriori_k,z_est,vicon_updated,addNoise,local_sigma,x_z_aposteriori);
-				memcpy(x_z_aposteriori_k, x_z_aposteriori, sizeof(x_z_aposteriori));
-				local_pos_est.x = x_x_aposteriori_k[0];
-				local_pos_est.vx = x_x_aposteriori_k[1];
-				local_pos_est.y = x_y_aposteriori_k[0];
-				local_pos_est.vy = x_y_aposteriori_k[1];
-				local_pos_est.z = x_z_aposteriori_k[0];
-				local_pos_est.vz = x_z_aposteriori_k[1];
-				local_pos_est.timestamp = hrt_absolute_time();
-				if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
-					if(local_pos_est_pub > 0)
-						orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
-					else
-						local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
-					//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
-					//mavlink_log_info(mavlink_fd, buf);
-				}
-			}
-		} /* end of poll return value check */
-	}
-
-	printf("[pos_est_mc] exiting.\n");
-	mavlink_log_info(mavlink_fd, "[pos_est_mc] exiting");
-	thread_running = false;
-	return 0;
-}
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.c b/src/modules/position_estimator_mc/position_estimator_mc_params.c
deleted file mode 100755
index 72e5bc73b..000000000
--- a/src/modules/position_estimator_mc/position_estimator_mc_params.c
+++ /dev/null
@@ -1,68 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	Damian Aregger <daregger@student.ethz.ch>
- *   			Tobias Naegeli <naegelit@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 position_estimator_mc_params.c
- * 
- * Parameters for position_estimator_mc
- */
-
-#include "position_estimator_mc_params.h"
-
-/* Kalman Filter covariances */
-/* gps measurement noise standard deviation */
-PARAM_DEFINE_FLOAT(POS_EST_ADDN, 1.0f);
-PARAM_DEFINE_FLOAT(POS_EST_SIGMA, 0.0f);
-PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f);
-PARAM_DEFINE_INT32(POS_EST_BARO, 0.0f);
-
-int parameters_init(struct position_estimator_mc_param_handles *h)
-{
-	h->addNoise	= param_find("POS_EST_ADDN");
-	h->sigma = param_find("POS_EST_SIGMA");
-	h->r = param_find("POS_EST_R");
-	h->baro_param_handle = param_find("POS_EST_BARO");
-	return OK;
-}
-
-int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p)
-{
-	param_get(h->addNoise, &(p->addNoise));
-	param_get(h->sigma, &(p->sigma));
-	param_get(h->r, &(p->R));
-	param_get(h->baro_param_handle, &(p->baro));
-	return OK;
-}
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_params.h b/src/modules/position_estimator_mc/position_estimator_mc_params.h
deleted file mode 100755
index c4c95f7b4..000000000
--- a/src/modules/position_estimator_mc/position_estimator_mc_params.h
+++ /dev/null
@@ -1,69 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: 	Damian Aregger <daregger@student.ethz.ch>
- *   			Tobias Naegeli <naegelit@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 position_estimator_mc_params.h
- * 
- * Parameters for Position Estimator
- */
-
-#include <systemlib/param/param.h>
-
-struct position_estimator_mc_params {
-	float addNoise;
-	float sigma;
-	float R;
-	int baro; /* consider barometer */
-};
-
-struct position_estimator_mc_param_handles {
-	param_t addNoise;
-	param_t sigma;
-	param_t r;
-	param_t baro_param_handle;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct position_estimator_mc_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct position_estimator_mc_param_handles *h, struct position_estimator_mc_params *p);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 9558198f3..ebf4f3e8e 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -254,10 +254,25 @@ mixer_tick(void)
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
 
+		/* set S.BUS1 or S.BUS2 outputs */
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) {
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
+		} else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) {
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+		}
+
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
 		/* set the disarmed servo outputs. */
 		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
+
+		/* set S.BUS1 or S.BUS2 outputs */
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)
+			sbus1_output(r_page_servos, PX4IO_SERVO_COUNT);
+
+		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT)
+			sbus2_output(r_page_servos, PX4IO_SERVO_COUNT);
 	}
 }
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 4db948484..ca175bfbc 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -219,6 +219,8 @@ extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
 extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels);
+extern bool	sbus1_output(uint16_t *values, uint16_t num_values);
+extern bool	sbus2_output(uint16_t *values, uint16_t num_values);
 
 /** global debug level for isr_debug() */
 extern volatile uint8_t debug_level;
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 9e5d7e7e2..fd7c6081f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -463,9 +463,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			#ifdef ENABLE_SBUS_OUT
 			ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT));
 
-			/* disable the conflicting options */
-			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
-				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI);
+			/* disable the conflicting options with SBUS 1 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS2_OUT);
+			}
+
+			/* disable the conflicting options with SBUS 2 */
+			if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) {
+				value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI |
+					PX4IO_P_SETUP_FEATURES_ADC_RSSI |
+					PX4IO_P_SETUP_FEATURES_SBUS1_OUT);
 			}
 			#endif
 
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index f6ec542eb..0e7dc621c 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -93,7 +93,7 @@ int
 sbus_init(const char *device)
 {
 	if (sbus_fd < 0)
-		sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
+		sbus_fd = open(device, O_RDWR | O_NONBLOCK);
 
 	if (sbus_fd >= 0) {
 		struct termios t;
@@ -113,11 +113,22 @@ sbus_init(const char *device)
 	} else {
 		debug("S.Bus: open failed");
 	}
-
 	return sbus_fd;
 }
 
 bool
+sbus1_output(uint16_t *values, uint16_t num_values)
+{
+	write(sbus_fd, 'A', 1);
+}
+
+bool
+sbus2_output(uint16_t *values, uint16_t num_values)
+{
+	write(sbus_fd, 'B', 1);
+}
+
+bool
 sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels)
 {
 	ssize_t		ret;
diff --git a/src/modules/sdlog/module.mk b/src/modules/sdlog/module.mk
deleted file mode 100644
index 89da2d827..000000000
--- a/src/modules/sdlog/module.mk
+++ /dev/null
@@ -1,43 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-#
-# sdlog Application
-#
-
-MODULE_COMMAND	 = sdlog
-# The main thread only buffers to RAM, needs a high priority
-MODULE_PRIORITY	 = "SCHED_PRIORITY_MAX-30"
-
-SRCS		 = sdlog.c \
-		   sdlog_ringbuffer.c
diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c
deleted file mode 100644
index c22523bf2..000000000
--- a/src/modules/sdlog/sdlog.c
+++ /dev/null
@@ -1,840 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file sdlog.c
- * @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Simple SD logger for flight data. Buffers new sensor values and
- * does the heavy SD I/O in a low-priority worker thread.
- */
-
-#include <nuttx/config.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <sys/prctl.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <poll.h>
-#include <stdlib.h>
-#include <string.h>
-#include <ctype.h>
-#include <systemlib/err.h>
-#include <unistd.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_controls_effective.h>
-#include <uORB/topics/vehicle_command.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/vehicle_vicon_position.h>
-#include <uORB/topics/optical_flow.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/airspeed.h>
-
-#include <systemlib/systemlib.h>
-
-#include <mavlink/mavlink_log.h>
-
-#include "sdlog_ringbuffer.h"
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log folders */
-
-static const char *mountpoint = "/fs/microsd";
-static const char *mfile_in = "/etc/logging/logconv.m";
-int sysvector_file = -1;
-int mavlink_fd = -1;
-struct sdlog_logbuffer lb;
-
-/* mutex / condition to synchronize threads */
-pthread_mutex_t sysvector_mutex;
-pthread_cond_t sysvector_cond;
-
-/**
- * System state vector log buffer writing
- */
-static void *sdlog_sysvector_write_thread(void *arg);
-
-/**
- * Create the thread to write the system vector
- */
-pthread_t sysvector_write_start(struct sdlog_logbuffer *logbuf);
-
-/**
- * SD log management function.
- */
-__EXPORT int sdlog_main(int argc, char *argv[]);
-
-/**
- * Mainloop of sd log deamon.
- */
-int sdlog_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static int file_exist(const char *filename);
-
-static int file_copy(const char *file_old, const char *file_new);
-
-static void handle_command(struct vehicle_command_s *cmd);
-
-/**
- * Print the current status.
- */
-static void print_sdlog_status(void);
-
-/**
- * Create folder for current logging session.
- */
-static int create_logfolder(char *folder_path);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-
-	errx(1, "usage: sdlog {start|stop|status} [-s <number of skipped lines>]\n\n");
-}
-
-// XXX turn this into a C++ class
-unsigned sensor_combined_bytes = 0;
-unsigned actuator_outputs_bytes = 0;
-unsigned actuator_controls_bytes = 0;
-unsigned sysvector_bytes = 0;
-unsigned blackbox_file_bytes = 0;
-uint64_t starttime = 0;
-
-/* logging on or off, default to true */
-bool logging_enabled = true;
-
-/**
- * The sd log deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- *
- * The actual stack size should be set in the call
- * to task_spawn_cmd().
- */
-int sdlog_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start")) {
-
-		if (thread_running) {
-			printf("sdlog already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("sdlog",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_DEFAULT - 30,
-					 4096,
-					 sdlog_thread_main,
-					 (const char **)argv);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-		if (!thread_running) {
-			printf("\tsdlog is not started\n");
-		}
-
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			print_sdlog_status();
-
-		} else {
-			printf("\tsdlog not started\n");
-		}
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-int create_logfolder(char *folder_path)
-{
-	/* make folder on sdcard */
-	uint16_t foldernumber = 1; // start with folder 0001
-	int mkdir_ret;
-
-	/* look for the next folder that does not exist */
-	while (foldernumber < MAX_NO_LOGFOLDER) {
-		/* set up file path: e.g. /mnt/sdcard/sensorfile0001.txt */
-		sprintf(folder_path, "%s/session%04u", mountpoint, foldernumber);
-		mkdir_ret = mkdir(folder_path, S_IRWXU | S_IRWXG | S_IRWXO);
-		/* the result is -1 if the folder exists */
-
-		if (mkdir_ret == 0) {
-			/* folder does not exist, success */
-
-			/* now copy the Matlab/Octave file */
-			char mfile_out[100];
-			sprintf(mfile_out, "%s/session%04u/run_to_plot_data.m", mountpoint, foldernumber);
-			int ret = file_copy(mfile_in, mfile_out);
-
-			if (!ret) {
-				warnx("copied m file to %s", mfile_out);
-
-			} else {
-				warnx("failed copying m file from %s to\n %s", mfile_in, mfile_out);
-			}
-
-			break;
-
-		} else if (mkdir_ret == -1) {
-			/* folder exists already */
-			foldernumber++;
-			continue;
-
-		} else {
-			warn("failed creating new folder");
-			return -1;
-		}
-	}
-
-	if (foldernumber >= MAX_NO_LOGFOLDER) {
-		/* we should not end up here, either we have more than MAX_NO_LOGFOLDER on the SD card, or another problem */
-		warn("all %d possible folders exist already", MAX_NO_LOGFOLDER);
-		return -1;
-	}
-
-	return 0;
-}
-
-
-static void *
-sdlog_sysvector_write_thread(void *arg)
-{
-	/* set name */
-	prctl(PR_SET_NAME, "sdlog microSD I/O", 0);
-
-	struct sdlog_logbuffer *logbuf = (struct sdlog_logbuffer *)arg;
-
-	int poll_count = 0;
-	struct sdlog_sysvector sysvect;
-	memset(&sysvect, 0, sizeof(sysvect));
-
-	while (!thread_should_exit) {
-		
-		/* make sure threads are synchronized */
-		pthread_mutex_lock(&sysvector_mutex);
-
-		/* only wait if no data is available to process */
-		if (sdlog_logbuffer_is_empty(logbuf)) {
-			/* blocking wait for new data at this line */
-			pthread_cond_wait(&sysvector_cond, &sysvector_mutex);
-		}
-
-		/* only quickly load data, do heavy I/O a few lines down */
-		int ret = sdlog_logbuffer_read(logbuf, &sysvect);
-		/* continue */
-		pthread_mutex_unlock(&sysvector_mutex);
-
-		if (ret == OK) {
-			sysvector_bytes += write(sysvector_file, (const char *)&sysvect, sizeof(sysvect));
-		}
-
-		if (poll_count % 100 == 0) {
-			fsync(sysvector_file);
-		}
-
-		poll_count++;
-	}
-
-	fsync(sysvector_file);
-
-	return OK;
-}
-
-pthread_t
-sysvector_write_start(struct sdlog_logbuffer *logbuf)
-{
-	pthread_attr_t receiveloop_attr;
-	pthread_attr_init(&receiveloop_attr);
-
-	struct sched_param param;
-	/* low priority, as this is expensive disk I/O */
-	param.sched_priority = SCHED_PRIORITY_DEFAULT - 40;
-	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
-
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-
-	pthread_t thread;
-	pthread_create(&thread, &receiveloop_attr, sdlog_sysvector_write_thread, logbuf);
-	return thread;
-
-	// XXX we have to destroy the attr at some point
-}
-
-
-int sdlog_thread_main(int argc, char *argv[])
-{
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-	if (mavlink_fd < 0) {
-		warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
-	}
-
-	/* log every n'th value (skip three per default) */
-	int skip_value = 3;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-	int ch;
-
-	while ((ch = getopt(argc, argv, "s:r")) != EOF) {
-		switch (ch) {
-		case 's':
-			{
-			/* log only every n'th (gyro clocked) value */
-			unsigned s = strtoul(optarg, NULL, 10);
-
-			if (s < 1 || s > 250) {
-				errx(1, "Wrong skip value of %d, out of range (1..250)\n", s);
-			} else {
-				skip_value = s;
-			}
-			}
-			break;
-
-		case 'r':
-			/* log only on request, disable logging per default */
-			logging_enabled = false;
-			break;
-
-		case '?':
-			if (optopt == 'c') {
-				warnx("Option -%c requires an argument.\n", optopt);
-			} else if (isprint(optopt)) {
-				warnx("Unknown option `-%c'.\n", optopt);
-			} else {
-				warnx("Unknown option character `\\x%x'.\n", optopt);
-			}
-
-		default:
-			usage("unrecognized flag");
-			errx(1, "exiting.");
-		}
-	}
-
-	if (file_exist(mountpoint) != OK) {
-		errx(1, "logging mount point %s not present, exiting.", mountpoint);
-	}
-
-	char folder_path[64];
-
-	if (create_logfolder(folder_path))
-		errx(1, "unable to create logging folder, exiting.");
-
-	FILE *gpsfile;
-	FILE *blackbox_file;
-
-	/* string to hold the path to the sensorfile */
-	char path_buf[64] = "";
-
-	/* only print logging path, important to find log file later */
-	warnx("logging to directory %s\n", folder_path);
-
-	/* set up file path: e.g. /mnt/sdcard/session0001/actuator_controls0.bin */
-	sprintf(path_buf, "%s/%s.bin", folder_path, "sysvector");
-
-	if (0 == (sysvector_file = open(path_buf, O_CREAT | O_WRONLY | O_DSYNC))) {
-		errx(1, "opening %s failed.\n", path_buf);
-	}
-
-	/* set up file path: e.g. /mnt/sdcard/session0001/gps.txt */
-	sprintf(path_buf, "%s/%s.txt", folder_path, "gps");
-
-	if (NULL == (gpsfile = fopen(path_buf, "w"))) {
-		errx(1, "opening %s failed.\n", path_buf);
-	}
-
-	int gpsfile_no = fileno(gpsfile);
-
-	/* set up file path: e.g. /mnt/sdcard/session0001/blackbox.txt */
-	sprintf(path_buf, "%s/%s.txt", folder_path, "blackbox");
-
-	if (NULL == (blackbox_file = fopen(path_buf, "w"))) {
-		errx(1, "opening %s failed.\n", path_buf);
-	}
-
-	// XXX for fsync() calls
-	int blackbox_file_no = fileno(blackbox_file);
-
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 25;
-	/* Sanity check variable and index */
-	ssize_t fdsc_count = 0;
-	/* file descriptors to wait for */
-	struct pollfd fds[fdsc];
-
-
-	struct {
-		struct sensor_combined_s raw;
-		struct vehicle_attitude_s att;
-		struct vehicle_attitude_setpoint_s att_sp;
-		struct actuator_outputs_s act_outputs;
-		struct actuator_controls_s act_controls;
-		struct actuator_controls_effective_s act_controls_effective;
-		struct vehicle_command_s cmd;
-		struct vehicle_local_position_s local_pos;
-		struct vehicle_global_position_s global_pos;
-		struct vehicle_gps_position_s gps_pos;
-		struct vehicle_vicon_position_s vicon_pos;
-		struct optical_flow_s flow;
-		struct battery_status_s batt;
-		struct differential_pressure_s diff_pres;
-		struct airspeed_s airspeed;
-	} buf;
-	memset(&buf, 0, sizeof(buf));
-
-	struct {
-		int cmd_sub;
-		int sensor_sub;
-		int att_sub;
-		int spa_sub;
-		int act_0_sub;
-		int controls_0_sub;
-		int controls_effective_0_sub;
-		int local_pos_sub;
-		int global_pos_sub;
-		int gps_pos_sub;
-		int vicon_pos_sub;
-		int flow_sub;
-		int batt_sub;
-		int diff_pres_sub;
-		int airspeed_sub;
-	} subs;
-
-	/* --- MANAGEMENT - LOGGING COMMAND --- */
-	/* subscribe to ORB for vehicle command */
-	subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	fds[fdsc_count].fd = subs.cmd_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GPS POSITION --- */
-	/* subscribe to ORB for global position */
-	subs.gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	fds[fdsc_count].fd = subs.gps_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- SENSORS RAW VALUE --- */
-	/* subscribe to ORB for sensors raw */
-	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	fds[fdsc_count].fd = subs.sensor_sub;
-	/* do not rate limit, instead use skip counter (aliasing on rate limit) */
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE VALUE --- */
-	/* subscribe to ORB for attitude */
-	subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	fds[fdsc_count].fd = subs.att_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ATTITUDE SETPOINT VALUE --- */
-	/* subscribe to ORB for attitude setpoint */
-	/* struct already allocated */
-	subs.spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	fds[fdsc_count].fd = subs.spa_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/** --- ACTUATOR OUTPUTS --- */
-	subs.act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0));
-	fds[fdsc_count].fd = subs.act_0_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR CONTROL VALUE --- */
-	/* subscribe to ORB for actuator control */
-	subs.controls_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	fds[fdsc_count].fd = subs.controls_0_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- ACTUATOR CONTROL EFFECTIVE VALUE --- */
-	/* subscribe to ORB for actuator control */
-	subs.controls_effective_0_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
-	fds[fdsc_count].fd = subs.controls_effective_0_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- LOCAL POSITION --- */
-	/* subscribe to ORB for local position */
-	subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	fds[fdsc_count].fd = subs.local_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- GLOBAL POSITION --- */
-	/* subscribe to ORB for global position */
-	subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	fds[fdsc_count].fd = subs.global_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- VICON POSITION --- */
-	/* subscribe to ORB for vicon position */
-	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
-	fds[fdsc_count].fd = subs.vicon_pos_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- FLOW measurements --- */
-	/* subscribe to ORB for flow measurements */
-	subs.flow_sub = orb_subscribe(ORB_ID(optical_flow));
-	fds[fdsc_count].fd = subs.flow_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- BATTERY STATUS --- */
-	/* subscribe to ORB for flow measurements */
-	subs.batt_sub = orb_subscribe(ORB_ID(battery_status));
-	fds[fdsc_count].fd = subs.batt_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- DIFFERENTIAL PRESSURE --- */
-	/* subscribe to ORB for flow measurements */
-	subs.diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	fds[fdsc_count].fd = subs.diff_pres_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
-	/* --- AIRSPEED --- */
-	/* subscribe to ORB for airspeed */
-	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
-	fds[fdsc_count].fd = subs.airspeed_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.
-	 */
-	if (fdsc_count > fdsc) {
-		warn("WARNING: Not enough space for poll fds allocated. Check %s:%d.\n", __FILE__, __LINE__);
-		fdsc_count = fdsc;
-	}
-
-	/*
-	 * set up poll to block for new data,
-	 * wait for a maximum of 1000 ms (1 second)
-	 */
-	// const int timeout = 1000;
-
-	thread_running = true;
-
-	/* initialize log buffer with a size of 10 */
-	sdlog_logbuffer_init(&lb, 10);
-
-	/* initialize thread synchronization */
-	pthread_mutex_init(&sysvector_mutex, NULL);
-  	pthread_cond_init(&sysvector_cond, NULL);
-
-	/* start logbuffer emptying thread */
-	pthread_t sysvector_pthread = sysvector_write_start(&lb);
-
-	starttime = hrt_absolute_time();
-
-	/* track skipping */
-	int skip_count = 0;
-
-	while (!thread_should_exit) {
-
-		/* only poll for commands, gps and sensor_combined */
-		int poll_ret = poll(fds, 3, 1000);
-
-		/* handle the poll result */
-		if (poll_ret == 0) {
-			/* XXX this means none of our providers is giving us data - might be an error? */
-		} else if (poll_ret < 0) {
-			/* XXX this is seriously bad - should be an emergency */
-		} else {
-
-			int ifds = 0;
-
-			/* --- VEHICLE COMMAND VALUE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				/* copy command into local buffer */
-				orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd);
-
-				/* always log to blackbox, even when logging disabled */
-				blackbox_file_bytes += fprintf(blackbox_file, "[%10.4f\tVCMD] CMD #%d [%f\t%f\t%f\t%f\t%f\t%f\t%f]\n", hrt_absolute_time()/1000000.0d,
-					buf.cmd.command, (double)buf.cmd.param1, (double)buf.cmd.param2, (double)buf.cmd.param3, (double)buf.cmd.param4,
-					(double)buf.cmd.param5, (double)buf.cmd.param6, (double)buf.cmd.param7);
-
-				handle_command(&buf.cmd);
-			}
-
-			/* --- VEHICLE GPS VALUE --- */
-			if (fds[ifds++].revents & POLLIN) {
-				/* copy gps position into local buffer */
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
-
-				/* if logging disabled, continue */
-				if (logging_enabled) {
-
-				/* write KML line */
-				}
-			}
-
-			/* --- SENSORS RAW VALUE --- */
-			if (fds[ifds++].revents & POLLIN) {
-
-				// /* copy sensors raw data into local buffer */
-				// orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-				// /* write out */
-				// sensor_combined_bytes += write(sensorfile, (const char*)&(buf.raw), sizeof(buf.raw));
-
-				/* always copy sensors raw data into local buffer, since poll flags won't clear else */
-				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.raw);
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs.controls_0_sub, &buf.act_controls);
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, subs.controls_effective_0_sub, &buf.act_controls_effective);
-				orb_copy(ORB_ID(actuator_outputs_0), subs.act_0_sub, &buf.act_outputs);
-				orb_copy(ORB_ID(vehicle_attitude_setpoint), subs.spa_sub, &buf.att_sp);
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
-				orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
-				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-				orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
-				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
-				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
-				orb_copy(ORB_ID(differential_pressure), subs.diff_pres_sub, &buf.diff_pres);
-				orb_copy(ORB_ID(airspeed), subs.airspeed_sub, &buf.airspeed);
-				orb_copy(ORB_ID(battery_status), subs.batt_sub, &buf.batt);
-
-				/* if skipping is on or logging is disabled, ignore */
-				if (skip_count < skip_value || !logging_enabled) {
-					skip_count++;
-					/* do not log data */
-					continue;
-				} else {
-					/* log data, reset */
-					skip_count = 0;
-				}
-
-				struct sdlog_sysvector sysvect = {
-					.timestamp = buf.raw.timestamp,
-					.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
-					.accel = {buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2]},
-					.mag = {buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1], buf.raw.magnetometer_ga[2]},
-					.baro = buf.raw.baro_pres_mbar,
-					.baro_alt = buf.raw.baro_alt_meter,
-					.baro_temp = buf.raw.baro_temp_celcius,
-					.control = {buf.act_controls.control[0], buf.act_controls.control[1], buf.act_controls.control[2], buf.act_controls.control[3]},
-					.actuators = {
-						buf.act_outputs.output[0], buf.act_outputs.output[1], buf.act_outputs.output[2], buf.act_outputs.output[3],
-						buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]
-					},
-					.vbat = buf.batt.voltage_v,
-					.bat_current = buf.batt.current_a,
-					.bat_discharged = buf.batt.discharged_mah,
-					.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
-					.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
-					.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
-					.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
-					.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]},
-					.vicon = {buf.vicon_pos.x, buf.vicon_pos.y, buf.vicon_pos.z, buf.vicon_pos.roll, buf.vicon_pos.pitch, buf.vicon_pos.yaw},
-					.control_effective = {buf.act_controls_effective.control_effective[0], buf.act_controls_effective.control_effective[1], buf.act_controls_effective.control_effective[2], buf.act_controls_effective.control_effective[3]},
-					.flow = {buf.flow.flow_raw_x, buf.flow.flow_raw_y, buf.flow.flow_comp_x_m, buf.flow.flow_comp_y_m, buf.flow.ground_distance_m, buf.flow.quality},
-					.diff_pressure = buf.diff_pres.differential_pressure_pa,
-					.ind_airspeed = buf.airspeed.indicated_airspeed_m_s,
-					.true_airspeed = buf.airspeed.true_airspeed_m_s
-				};
-
-				/* put into buffer for later IO */
-				pthread_mutex_lock(&sysvector_mutex);
-				sdlog_logbuffer_write(&lb, &sysvect);
-				/* signal the other thread new data, but not yet unlock */
-				if ((unsigned)lb.count > (lb.size / 2)) {
-					/* only request write if several packets can be written at once */
-					pthread_cond_signal(&sysvector_cond);
-				}
-				/* unlock, now the writer thread may run */
-				pthread_mutex_unlock(&sysvector_mutex);
-			}
-
-		}
-
-	}
-
-	print_sdlog_status();
-
-	/* wake up write thread one last time */
-	pthread_mutex_lock(&sysvector_mutex);
-	pthread_cond_signal(&sysvector_cond);
-	/* unlock, now the writer thread may return */
-	pthread_mutex_unlock(&sysvector_mutex);
-
-	/* wait for write thread to return */
-	(void)pthread_join(sysvector_pthread, NULL);
-
-  	pthread_mutex_destroy(&sysvector_mutex);
-  	pthread_cond_destroy(&sysvector_cond);
-
-	warnx("exiting.\n\n");
-
-	/* finish KML file */
-	// XXX
-	fclose(gpsfile);
-	fclose(blackbox_file);
-
-	thread_running = false;
-
-	return 0;
-}
-
-void print_sdlog_status()
-{
-	unsigned bytes = sysvector_bytes + sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;
-	float mebibytes = bytes / 1024.0f / 1024.0f;
-	float seconds = ((float)(hrt_absolute_time() - starttime)) / 1000000.0f;
-
-	warnx("wrote %4.2f MiB (average %5.3f MiB/s).\n", (double)mebibytes, (double)(mebibytes / seconds));
-}
-
-/**
- * @return 0 if file exists
- */
-int file_exist(const char *filename)
-{
-	struct stat buffer;
-	return stat(filename, &buffer);
-}
-
-int file_copy(const char *file_old, const char *file_new)
-{
-	FILE *source, *target;
-	source = fopen(file_old, "r");
-	int ret = 0;
-
-	if (source == NULL) {
-		warnx("failed opening input file to copy");
-		return 1;
-	}
-
-	target = fopen(file_new, "w");
-
-	if (target == NULL) {
-		fclose(source);
-		warnx("failed to open output file to copy");
-		return 1;
-	}
-
-	char buf[128];
-	int nread;
-
-	while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) {
-		int ret = fwrite(buf, 1, nread, target);
-
-		if (ret <= 0) {
-			warnx("error writing file");
-			ret = 1;
-			break;
-		}
-	}
-
-	fsync(fileno(target));
-
-	fclose(source);
-	fclose(target);
-
-	return ret;
-}
-
-void handle_command(struct vehicle_command_s *cmd)
-{
-	/* result of the command */
-	uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
-
-	/* request to set different system mode */
-	switch (cmd->command) {
-
-	case VEHICLE_CMD_PREFLIGHT_STORAGE:
-
-		if (((int)(cmd->param3)) == 1)	{
-
-			/* enable logging */
-			mavlink_log_info(mavlink_fd, "[log] file:");
-			mavlink_log_info(mavlink_fd, "logdir");
-			logging_enabled = true;
-		}
-		if (((int)(cmd->param3)) == 0)	{
-
-			/* disable logging */
-			mavlink_log_info(mavlink_fd, "[log] stopped.");
-			logging_enabled = false;
-		}
-		break;
-
-	default:
-		/* silently ignore */
-		break;
-	}
-
-}
diff --git a/src/modules/sdlog/sdlog_ringbuffer.c b/src/modules/sdlog/sdlog_ringbuffer.c
deleted file mode 100644
index d7c8a4759..000000000
--- a/src/modules/sdlog/sdlog_ringbuffer.c
+++ /dev/null
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file sdlog_log.c
- * MAVLink text logging.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#include <string.h>
-#include <stdlib.h>
-
-#include "sdlog_ringbuffer.h"
-
-void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size)
-{
-	lb->size  = size;
-	lb->start = 0;
-	lb->count = 0;
-	lb->elems = (struct sdlog_sysvector *)calloc(lb->size, sizeof(struct sdlog_sysvector));
-}
-
-int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb)
-{
-	return lb->count == (int)lb->size;
-}
-
-int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb)
-{
-	return lb->count == 0;
-}
-
-
-// XXX make these functions thread-safe
-void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem)
-{
-	int end = (lb->start + lb->count) % lb->size;
-	memcpy(&(lb->elems[end]), elem, sizeof(struct sdlog_sysvector));
-
-	if (sdlog_logbuffer_is_full(lb)) {
-		lb->start = (lb->start + 1) % lb->size; /* full, overwrite */
-
-	} else {
-		++lb->count;
-	}
-}
-
-int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem)
-{
-	if (!sdlog_logbuffer_is_empty(lb)) {
-		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct sdlog_sysvector));
-		lb->start = (lb->start + 1) % lb->size;
-		--lb->count;
-		return 0;
-
-	} else {
-		return 1;
-	}
-}
diff --git a/src/modules/sdlog/sdlog_ringbuffer.h b/src/modules/sdlog/sdlog_ringbuffer.h
deleted file mode 100644
index b65916459..000000000
--- a/src/modules/sdlog/sdlog_ringbuffer.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file sdlog_ringbuffer.h
- * microSD logging
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#ifndef SDLOG_RINGBUFFER_H_
-#define SDLOG_RINGBUFFER_H_
-
-#pragma pack(push, 1)
-struct sdlog_sysvector {
-	uint64_t timestamp;		/**<  time [us] */
-	float gyro[3];			/**< [rad/s] */
-	float accel[3];			/**< [m/s^2] */
-	float mag[3];			/**< [gauss] */
-	float baro;			/**< pressure [millibar] */
-	float baro_alt;			/**< altitude above MSL [meter] */
-	float baro_temp;		/**< [degree celcius] */
-	float control[4];		/**< roll, pitch, yaw [-1..1], thrust [0..1] */
-	float actuators[8];		/**< motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512) */
-	float vbat;			/**< battery voltage in [volt] */
-	float bat_current;		/**< battery discharge current */
-	float bat_discharged;		/**< discharged energy in mAh */
-	float adc[4];			/**< ADC ports [volt] */
-	float local_position[3];	/**< tangent plane mapping into x,y,z [m] */
-	int32_t gps_raw_position[3];	/**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
-	float attitude[3];		/**< roll, pitch, yaw [rad] */
-	float rotMatrix[9];		/**< unitvectors */
-	float vicon[6];			/**< Vicon ground truth x, y, z and roll, pitch, yaw */
-	float control_effective[4]; 	/**< roll, pitch, yaw [-1..1], thrust [0..1] */
-	float flow[6];			/**< flow raw x, y, flow metric x, y, flow ground dist, flow quality */
-	float diff_pressure;		/**< differential pressure */
-	float ind_airspeed;		/**< indicated airspeed */
-	float true_airspeed;		/**< true airspeed */
-};
-#pragma pack(pop)
-
-struct sdlog_logbuffer {
-	unsigned int start;
-	// unsigned int end;
-	unsigned int size;
-	int count;
-	struct sdlog_sysvector *elems;
-};
-
-void sdlog_logbuffer_init(struct sdlog_logbuffer *lb, int size);
-
-int sdlog_logbuffer_is_full(struct sdlog_logbuffer *lb);
-
-int sdlog_logbuffer_is_empty(struct sdlog_logbuffer *lb);
-
-void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvector *elem);
-
-int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem);
-
-#endif
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
index 96a443c6e..91230a37c 100644
--- a/src/modules/segway/BlockSegwayController.cpp
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -35,8 +35,8 @@ void BlockSegwayController::update() {
 
 	// handle autopilot modes
 	if (_status.main_state == MAIN_STATE_AUTO ||
-	    _status.main_state == MAIN_STATE_SEATBELT ||
-	    _status.main_state == MAIN_STATE_EASY) {
+	    _status.main_state == MAIN_STATE_ALTCTL ||
+	    _status.main_state == MAIN_STATE_POSCTL) {
 		_actuators.control[0] = spdCmd;
 		_actuators.control[1] = spdCmd;
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index bc49f5c85..07be3560a 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -536,6 +536,20 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
 
 /**
+ * Failsafe channel mapping.
+ *
+ * The RC mapping index indicates which channel is used for failsafe
+ * If 0, whichever channel is mapped to throttle is used
+ * otherwise the value indicates the specific rc channel to use
+ *
+ * @min 0
+ * @max 18
+ *
+ *
+ */
+PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0);  //Default to throttle function
+
+/**
  * Throttle control channel mapping.
  *
  * The channel index (starting from 1 for channel 1) indicates
@@ -585,13 +599,13 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 
 /**
- * Assist switch channel mapping.
+ * Posctl switch channel mapping.
  *
  * @min 0
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
 
 /**
  * Loiter switch channel mapping.
@@ -602,8 +616,6 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 
-//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
-
 /**
  * Flaps channel mapping.
  *
@@ -655,3 +667,83 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
  * @group Radio Calibration
  */
 PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
+
+/**
+ * Threshold for selecting assist mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
+
+/**
+ * Threshold for selecting auto mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
+
+/**
+ * Threshold for selecting posctl mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_POSCTL_TH, 0.5f);
+
+/**
+ * Threshold for selecting return to launch mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
+
+/**
+ * Threshold for selecting loiter mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index e260aae45..18bf97f8d 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -175,7 +175,8 @@ private:
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);
+	switch_pos_t	get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	switch_pos_t	get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
 
 	/**
 	 * Gather and publish RC input data.
@@ -219,8 +220,8 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
-	math::Matrix<3,3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix<3,3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix<3, 3>	_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 */
 
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
@@ -250,14 +251,13 @@ private:
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
+		int rc_map_failsafe;
 
 		int rc_map_mode_sw;
 		int rc_map_return_sw;
-		int rc_map_assisted_sw;
+		int rc_map_posctl_sw;
 		int rc_map_loiter_sw;
 
-//		int rc_map_offboard_ctrl_mode_sw;
-
 		int rc_map_flaps;
 
 		int rc_map_aux1;
@@ -266,7 +266,17 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;
 
-		int32_t rc_fs_thr;
+		int32_t rc_fails_thr;
+		float rc_assist_th;
+		float rc_auto_th;
+		float rc_posctl_th;
+		float rc_return_th;
+		float rc_loiter_th;
+		bool rc_assist_inv;
+		bool rc_auto_inv;
+		bool rc_posctl_inv;
+		bool rc_return_inv;
+		bool rc_loiter_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -293,14 +303,13 @@ private:
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
+		param_t rc_map_failsafe;
 
 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
-		param_t rc_map_assisted_sw;
+		param_t rc_map_posctl_sw;
 		param_t rc_map_loiter_sw;
 
-//		param_t rc_map_offboard_ctrl_mode_sw;
-
 		param_t rc_map_flaps;
 
 		param_t rc_map_aux1;
@@ -309,7 +318,12 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;
 
-		param_t rc_fs_thr;
+		param_t rc_fails_thr;
+		param_t rc_assist_th;
+		param_t rc_auto_th;
+		param_t rc_posctl_th;
+		param_t rc_return_th;
+		param_t rc_loiter_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -416,7 +430,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };
 
 namespace sensors
@@ -499,6 +513,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
 	_parameter_handles.rc_map_yaw 	= param_find("RC_MAP_YAW");
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
+	_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
 
 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
 	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
@@ -507,19 +522,22 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
 
 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_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");
 	_parameter_handles.rc_map_aux3 = param_find("RC_MAP_AUX3");
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
-	/* RC failsafe */
-	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
+	/* RC thresholds */
+	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
+	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
+	_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
+	_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
+	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
+	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -620,8 +638,9 @@ Sensors::parameters_update()
 	}
 
 	/* handle wrong values */
-	if (!rc_valid)
+	if (!rc_valid) {
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
+	}
 
 	const char *paramerr = "FAIL PARM LOAD";
 
@@ -642,6 +661,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -650,7 +673,7 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
-	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+	if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
 		warnx("%s", paramerr);
 	}
 
@@ -662,16 +685,27 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
-//	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_fs_thr, &(_parameters.rc_fs_thr));
+	param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
+	param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
+	_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
+	_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
+	param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
+	_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
+	_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
+	param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
+	_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
+	_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
+	param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
+	_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
+	_parameters.rc_return_th = fabs(_parameters.rc_return_th);
+	param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
+	_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
+	_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -681,13 +715,11 @@ Sensors::parameters_update()
 
 	_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[POSCTL] = _parameters.rc_map_posctl_sw - 1;
 	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 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;
 	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
@@ -800,12 +832,14 @@ Sensors::gyro_init()
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 		/* set the gyro internal sampling rate up to at least 1000Hz */
-		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) {
 			ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		}
 
 		/* set the driver to poll at 1000Hz */
-		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) {
 			ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		}
 
 #else
 
@@ -860,12 +894,15 @@ Sensors::mag_init()
 
 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);
 
-	if (ret < 0)
+	if (ret < 0) {
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
-	else if (ret == 1)
+
+	} else if (ret == 1) {
 		_mag_is_external = true;
-	else
+
+	} else {
 		_mag_is_external = false;
+	}
 
 	close(fd);
 }
@@ -965,10 +1002,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
-		if (_mag_is_external)
+		if (_mag_is_external) {
 			vect = _external_mag_rotation * vect;
-		else
+
+		} else {
 			vect = _board_rotation * vect;
+		}
 
 		raw.magnetometer_ga[0] = vect(0);
 		raw.magnetometer_ga[1] = vect(1);
@@ -1086,8 +1125,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.gyro_scale[2],
 		};
 
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) {
 			warn("WARNING: failed to set scale / offsets for gyro");
+		}
 
 		close(fd);
 
@@ -1101,8 +1141,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.accel_scale[2],
 		};
 
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) {
 			warn("WARNING: failed to set scale / offsets for accel");
+		}
 
 		close(fd);
 
@@ -1116,8 +1157,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.mag_scale[2],
 		};
 
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) {
 			warn("WARNING: failed to set scale / offsets for mag");
+		}
 
 		close(fd);
 
@@ -1131,8 +1173,10 @@ Sensors::parameter_update_poll(bool forced)
 				1.0f,
 			};
 
-			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
+			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+			}
+
 			close(fd);
 		}
 
@@ -1150,10 +1194,12 @@ void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
 	/* only read if publishing */
-	if (!_publishing)
+	if (!_publishing) {
 		return;
+	}
 
 	hrt_abstime t = hrt_absolute_time();
+
 	/* rate limit to 100 Hz */
 	if (t - _last_adc >= 10000) {
 		/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
@@ -1178,6 +1224,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 					if (voltage > BATT_V_IGNORE_THRESHOLD) {
 						_battery_status.voltage_v = voltage;
+
 						/* one-time initialization of low-pass value to avoid long init delays */
 						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
 							_battery_status.voltage_filtered_v = voltage;
@@ -1196,19 +1243,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					/* handle current only if voltage is valid */
 					if (_battery_status.voltage_v > 0.0f) {
 						float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
+
 						/* check measured current value */
 						if (current >= 0.0f) {
 							_battery_status.timestamp = t;
 							_battery_status.current_a = current;
+
 							if (_battery_current_timestamp != 0) {
 								/* initialize discharged value */
-								if (_battery_status.discharged_mah < 0.0f)
+								if (_battery_status.discharged_mah < 0.0f) {
 									_battery_status.discharged_mah = 0.0f;
+								}
+
 								_battery_discharged += current * (t - _battery_current_timestamp);
 								_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
 							}
 						}
 					}
+
 					_battery_current_timestamp = t;
 
 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1241,7 +1293,9 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					}
 				}
 			}
+
 			_last_adc = t;
+
 			if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
 				/* announce the battery status if needed, just publish else */
 				if (_battery_pub > 0) {
@@ -1260,6 +1314,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
 {
 	if (_rc.function[func] >= 0) {
 		float value = _rc.chan[_rc.function[func]].scaled;
+
 		if (value < min_value) {
 			return min_value;
 
@@ -1269,24 +1324,44 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
 		} else {
 			return value;
 		}
+
 	} else {
 		return 0.0f;
 	}
 }
 
 switch_pos_t
-Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
+Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
-		float value = _rc.chan[_rc.function[func]].scaled;
-		if (value > STICK_ON_OFF_LIMIT) {
+		float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
+
+		if (on_inv ? value < on_th : value > on_th) {
 			return SWITCH_POS_ON;
 
-		} else if (value < -STICK_ON_OFF_LIMIT) {
+		} else if (mid_inv ? value < mid_th : value > mid_th) {
+			return SWITCH_POS_MIDDLE;
+
+		} else {
 			return SWITCH_POS_OFF;
+		}
+
+	} else {
+		return SWITCH_POS_NONE;
+	}
+}
+
+switch_pos_t
+Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+{
+	if (_rc.function[func] >= 0) {
+		float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
+
+		if (on_inv ? value < on_th : value > on_th) {
+			return SWITCH_POS_ON;
 
 		} else {
-			return SWITCH_POS_MIDDLE;
+			return SWITCH_POS_OFF;
 		}
 
 	} else {
@@ -1318,13 +1393,18 @@ Sensors::rc_poll()
 			/* signal looks good */
 			signal_lost = false;
 
-			/* check throttle failsafe */
-			int8_t thr_ch = _rc.function[THROTTLE];
-			if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
-				/* throttle failsafe configured */
-				if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
-					(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
-					/* throttle failsafe triggered, signal is lost by receiver */
+			/* check failsafe */
+			int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle
+
+			if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping
+				fs_ch = _parameters.rc_map_failsafe - 1;
+			}
+
+			if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) {
+				/* failsafe configured */
+				if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) ||
+				    (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
+					/* failsafe triggered, signal is lost by receiver */
 					signal_lost = true;
 				}
 			}
@@ -1332,8 +1412,9 @@ Sensors::rc_poll()
 
 		unsigned channel_limit = rc_input.channel_count;
 
-		if (channel_limit > _rc_max_chan_count)
+		if (channel_limit > _rc_max_chan_count) {
 			channel_limit = _rc_max_chan_count;
+		}
 
 		/* read out and scale values from raw message even if signal is invalid */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1341,11 +1422,13 @@ Sensors::rc_poll()
 			/*
 			 * 1) Constrain to min/max values, as later processing depends on bounds.
 			 */
-			if (rc_input.values[i] < _parameters.min[i])
+			if (rc_input.values[i] < _parameters.min[i]) {
 				rc_input.values[i] = _parameters.min[i];
+			}
 
-			if (rc_input.values[i] > _parameters.max[i])
+			if (rc_input.values[i] > _parameters.max[i]) {
 				rc_input.values[i] = _parameters.max[i];
+			}
 
 			/*
 			 * 2) Scale around the mid point differently for lower and upper range.
@@ -1377,8 +1460,9 @@ Sensors::rc_poll()
 			_rc.chan[i].scaled *= _parameters.rev[i];
 
 			/* handle any parameter-induced blowups */
-			if (!isfinite(_rc.chan[i].scaled))
+			if (!isfinite(_rc.chan[i].scaled)) {
 				_rc.chan[i].scaled = 0.0f;
+			}
 		}
 
 		_rc.chan_count = rc_input.channel_count;
@@ -1402,10 +1486,10 @@ Sensors::rc_poll()
 			manual.timestamp = rc_input.timestamp_last_signal;
 
 			/* limit controls */
-			manual.roll = get_rc_value(ROLL, -1.0, 1.0);
-			manual.pitch = get_rc_value(PITCH, -1.0, 1.0);
-			manual.yaw = get_rc_value(YAW, -1.0, 1.0);
-			manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0);
+			manual.y = get_rc_value(ROLL, -1.0, 1.0);
+			manual.x = get_rc_value(PITCH, -1.0, 1.0);
+			manual.r = get_rc_value(YAW, -1.0, 1.0);
+			manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
 			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
 			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
 			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
@@ -1414,10 +1498,10 @@ Sensors::rc_poll()
 			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
 
 			/* mode switches */
-			manual.mode_switch = get_rc_switch_position(MODE);
-			manual.assisted_switch = get_rc_switch_position(ASSISTED);
-			manual.loiter_switch = get_rc_switch_position(LOITER);
-			manual.return_switch = get_rc_switch_position(RETURN);
+			manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
@@ -1433,10 +1517,10 @@ Sensors::rc_poll()
 
 			actuator_group_3.timestamp = rc_input.timestamp_last_signal;
 
-			actuator_group_3.control[0] = manual.roll;
-			actuator_group_3.control[1] = manual.pitch;
-			actuator_group_3.control[2] = manual.yaw;
-			actuator_group_3.control[3] = manual.throttle;
+			actuator_group_3.control[0] = manual.y;
+			actuator_group_3.control[1] = manual.x;
+			actuator_group_3.control[2] = manual.r;
+			actuator_group_3.control[3] = manual.z;
 			actuator_group_3.control[4] = manual.flaps;
 			actuator_group_3.control[5] = manual.aux1;
 			actuator_group_3.control[6] = manual.aux2;
@@ -1560,8 +1644,9 @@ Sensors::task_main()
 		diff_pres_poll(raw);
 
 		/* Inform other processes that new data is available to copy */
-		if (_publishing)
+		if (_publishing) {
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
+		}
 
 		/* Look for new r/c input data */
 		rc_poll();
@@ -1598,18 +1683,21 @@ Sensors::start()
 
 int sensors_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: sensors {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (sensors::g_sensors != nullptr)
+		if (sensors::g_sensors != nullptr) {
 			errx(0, "already running");
+		}
 
 		sensors::g_sensors = new Sensors;
 
-		if (sensors::g_sensors == nullptr)
+		if (sensors::g_sensors == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
@@ -1621,8 +1709,9 @@ int sensors_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (sensors::g_sensors == nullptr)
+		if (sensors::g_sensors == nullptr) {
 			errx(1, "not running");
+		}
 
 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
@@ -1641,4 +1730,3 @@ int sensors_main(int argc, char *argv[])
 	warnx("unrecognized command");
 	return 1;
 }
-
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 2d773fd25..7a499ca72 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -521,73 +521,15 @@ param_save_default(void)
 		return ERROR;
 	}
 
-	if (res == OK) {
-		res = param_export(fd, false);
+	res = param_export(fd, false);
 
-	        if (res != OK) {
-	            warnx("failed to write parameters to file: %s", filename);
-	        }
+	if (res != OK) {
+		warnx("failed to write parameters to file: %s", filename);
 	}
 
 	close(fd);
 
 	return res;
-
-#if 0
-	const char *filename_tmp = malloc(strlen(filename) + 5);
-	sprintf(filename_tmp, "%s.tmp", filename);
-
-	/* delete temp file if exist */
-	res = unlink(filename_tmp);
-
-	if (res != OK && errno == ENOENT)
-		res = OK;
-
-	if (res != OK)
-		warn("failed to delete temp file: %s", filename_tmp);
-
-	if (res == OK) {
-		/* write parameters to temp file */
-		fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
-
-		if (fd < 0) {
-			warn("failed to open temp file: %s", filename_tmp);
-			res = ERROR;
-		}
-
-		if (res == OK) {
-			res = param_export(fd, false);
-
-			if (res != OK)
-				warnx("failed to write parameters to file: %s", filename_tmp);
-		}
-
-		close(fd);
-	}
-
-	if (res == OK) {
-		/* delete parameters file */
-		res = unlink(filename);
-
-		if (res != OK && errno == ENOENT)
-			res = OK;
-
-		if (res != OK)
-			warn("failed to delete parameters file: %s", filename);
-	}
-
-	if (res == OK) {
-		/* rename temp file to parameters */
-		res = rename(filename_tmp, filename);
-
-		if (res != OK)
-			warn("failed to rename %s to %s", filename_tmp, filename);
-	}
-
-	free(filename_tmp);
-
-	return res;
-#endif
 }
 
 /**
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index 21e15ec56..c0c1a5cb4 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -140,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) {
 		/* sanity checks pass, enable channel */
 		if (count) {
 			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
-			warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 		}
 
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index a23d89cd2..19a29635b 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -64,22 +64,39 @@ struct manual_control_setpoint_s {
 	/**
 	 * Any of the channels may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
+	 * The variable names follow the definition of the
+	 * MANUAL_CONTROL mavlink message.
+	 * The default range is from -1 to 1 (mavlink message -1000 to 1000)
+	 * The range for the z variable is defined from 0 to 1. (The z field of
+	 * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
 	 */
-	float roll;			 	/**< ailerons roll / roll rate input, -1..1 */
-	float pitch;			/**< elevator / pitch / pitch rate, -1..1 */
-	float yaw;				/**< rudder / yaw rate / yaw, -1..1 */
-	float throttle;			/**< throttle / collective thrust / altitude, 0..1 */
+	float x;			/**< stick position in x direction -1..1
+					  in general corresponds to forward/back motion or pitch of vehicle,
+					  in general a positive value means forward or negative pitch and
+					  a negative value means backward or positive pitch */
+	float y;			/**< stick position in y direction -1..1
+					  in general corresponds to right/left motion or roll of vehicle,
+					  in general a positive value means right or positive roll and
+					  a negative value means left or negative roll */
+	float z;			/**< throttle stick position 0..1
+					  in general corresponds to up/down motion or thrust of vehicle,
+					  in general the value corresponds to the demanded throttle by the user,
+					  if the input is used for setting the setpoint of a vertical position
+					  controller any value > 0.5 means up and any value < 0.5 means down */
+	float r;			/**< yaw stick/twist positon, -1..1
+					  in general corresponds to the righthand rotation around the vertical
+					  (downwards) axis of the vehicle */
 	float flaps;			/**< flap position */
-	float aux1;				/**< default function: camera yaw / azimuth */
-	float aux2;				/**< default function: camera pitch / tilt */
-	float aux3;				/**< default function: camera trigger */
-	float aux4;				/**< default function: camera roll */
-	float aux5;				/**< default function: payload drop */
+	float aux1;			/**< default function: camera yaw / azimuth */
+	float aux2;			/**< default function: camera pitch / tilt */
+	float aux3;			/**< default function: camera trigger */
+	float aux4;			/**< default function: camera roll */
+	float aux5;			/**< default function: payload drop */
 
-	switch_pos_t mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
-	switch_pos_t return_switch;			/**< land 2 position switch (mandatory): land, no effect */
-	switch_pos_t assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
-	switch_pos_t loiter_switch;		/**< mission 2 position switch (optional): mission, loiter */
+	switch_pos_t mode_switch;			/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
 }; /**< manual control inputs */
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index c168b2fac..b60402452 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -64,7 +64,7 @@ enum RC_CHANNELS_FUNCTION {
 	YAW      = 3,
 	MODE = 4,
 	RETURN = 5,
-	ASSISTED = 6,
+	POSCTL = 6,
 	LOITER = 7,
 	OFFBOARD_MODE = 8,
 	FLAPS   = 9,
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 435230432..f56355246 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -63,8 +63,8 @@
 /* main state machine */
 typedef enum {
 	MAIN_STATE_MANUAL = 0,
-	MAIN_STATE_SEATBELT,
-	MAIN_STATE_EASY,
+	MAIN_STATE_ALTCTL,
+	MAIN_STATE_POSCTL,
 	MAIN_STATE_AUTO,
 	MAIN_STATE_MAX
 } main_state_t;
diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c
index 0cbba0a37..984d19bd9 100644
--- a/src/systemcmds/param/param.c
+++ b/src/systemcmds/param/param.c
@@ -63,6 +63,7 @@ static void	do_show(const char* search_string);
 static void	do_show_print(void *arg, param_t param);
 static void	do_set(const char* name, const char* val);
 static void	do_compare(const char* name, const char* vals[], unsigned comparisons);
+static void	do_reset();
 
 int
 param_main(int argc, char *argv[])
@@ -130,6 +131,10 @@ param_main(int argc, char *argv[])
 				errx(1, "not enough arguments.\nTry 'param compare PARAM_NAME 3'");
 			}
 		}
+
+		if (!strcmp(argv[1], "reset")) {
+			do_reset();
+		}
 	}
 	
 	errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
@@ -402,3 +407,16 @@ do_compare(const char* name, const char* vals[], unsigned comparisons)
 
 	exit(ret);
 }
+
+static void
+do_reset()
+{
+	param_reset_all();
+
+	if (param_save_default()) {
+		warnx("Param export failed.");
+		exit(1);
+	} else {
+		exit(0);
+	}
+}