Streamlined ar drone interface, removed a lot of old cruft, preparing for generic multirotor control

26 files changed, 2350 insertions, 340 deletions

diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index f8c17fe2d..a9b04f592 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -42,6 +42,7 @@
 #include <string.h>
 #include <stdbool.h>
 #include <unistd.h>
+#include <math.h>
 #include <fcntl.h>
 #include <errno.h>
 #include <debug.h>
@@ -53,66 +54,46 @@
 #include "attitude_control.h"
 #include "rate_control.h"
 #include "ardrone_motor_control.h"
-#include "position_control.h"
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/ardrone_control.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/ardrone_motors_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 
-#include "ardrone_control_helper.h"
-
 __EXPORT int ardrone_control_main(int argc, char *argv[]);
 
-/****************************************************************************
- * Internal Definitions
- ****************************************************************************/
-
-
-enum {
-	CONTROL_MODE_RATES = 0,
-	CONTROL_MODE_ATTITUDE = 1,
-} control_mode;
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-/*File descriptors */
-int ardrone_write;
-int gpios;
-
-bool position_control_thread_started;
-
-/****************************************************************************
- * pthread loops
- ****************************************************************************/
-static void *position_control_loop(void *arg)
-{
-	struct vehicle_status_s *state = (struct vehicle_status_s *)arg;
-	// Set thread name
-	prctl(PR_SET_NAME, "ardrone pos ctrl", getpid());
-
-	while (1) {
-		if (state->state_machine == SYSTEM_STATE_AUTO) {
-//			control_position();   //FIXME TODO XXX
-			/* temporary 50 Hz execution */
-			usleep(20000);
-
-		} else {
-			position_control_thread_started = false;
-			break;
-		}
-	}
-
-	return NULL;
-}
-
-/****************************************************************************
- * main
- ****************************************************************************/
+// static void turn_xy_plane(const float_vect3 *vector, float yaw,
+// 		   float_vect3 *result);
+// static void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
+// 			float_vect3 *result);
+
+// static void turn_xy_plane(const float_vect3 *vector, float yaw,
+// 		   float_vect3 *result)
+// {
+// 	//turn clockwise
+// 	static uint16_t counter;
+
+// 	result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y);
+// 	result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y);
+// 	result->z = vector->z; //leave direction normal to xy-plane untouched
+
+// 	counter++;
+// }
+
+// static void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
+// 			float_vect3 *result)
+// {
+// 	turn_xy_plane(vector, yaw, result);
+// //	result->x = vector->x;
+// //	result->y = vector->y;
+// //	result->z = vector->z;
+// 	//	result->x = cos(yaw) * vector->x + sin(yaw) * vector->y;
+// 	//	result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y;
+// 	//	result->z = vector->z; //leave direction normal to xy-plane untouched
+// }
 
 int ardrone_control_main(int argc, char *argv[])
 {
@@ -121,21 +102,26 @@ int ardrone_control_main(int argc, char *argv[])
 
 	/* default values for arguments */
 	char *ardrone_uart_name = "/dev/ttyS1";
-	control_mode = CONTROL_MODE_RATES;
+
+	/* File descriptors */
+	int ardrone_write;
+	int gpios;
+
+	enum {
+		CONTROL_MODE_RATES = 0,
+		CONTROL_MODE_ATTITUDE = 1,
+	} control_mode = CONTROL_MODE_ATTITUDE;
 
 	char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
 
 	/* read commandline arguments */
-	int i;
-
-	for (i = 1; i < argc; i++) {
+	for (int i = 1; i < argc; i++) {
 		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
 			if (argc > i + 1) {
 				ardrone_uart_name = argv[i + 1];
-
 			} else {
 				printf(commandline_usage);
-				return 0;
+				return ERROR;
 			}
 
 		} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
@@ -148,12 +134,12 @@ int ardrone_control_main(int argc, char *argv[])
 
 				} else {
 					printf(commandline_usage);
-					return 0;
+					return ERROR;
 				}
 
 			} else {
 				printf(commandline_usage);
-				return 0;
+				return ERROR;
 			}
 		}
 	}
@@ -161,70 +147,70 @@ int ardrone_control_main(int argc, char *argv[])
 	/* open uarts */
 	printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name);
 	ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY);
+	if (ardrone_write < 0) {
+		fprintf(stderr, "[ardrone_control] Failed opening AR.Drone UART, exiting.\n");
+		exit(ERROR);
+	}
 
 	/* initialize motors */
-	ar_init_motors(ardrone_write, &gpios);
-	int counter = 0;
+	if (OK != ar_init_motors(ardrone_write, &gpios)) {
+		close(ardrone_write);
+		fprintf(stderr, "[ardrone_control] Failed initializing AR.Drone motors, exiting.\n");
+		exit(ERROR);
+	}
 
 	/* Led animation */
+	int counter = 0;
 	int led_counter = 0;
 
-	/* pthread for position control */
-	pthread_t position_control_thread;
-	position_control_thread_started = false;
-
-	/* structures */
+	/* declare and safely initialize all structs */
 	struct vehicle_status_s state;
+	memset(&state, 0, sizeof(state));
 	struct vehicle_attitude_s att;
-	struct ardrone_control_s ar_control;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
 	struct manual_control_setpoint_s manual;
+	memset(&manual, 0, sizeof(manual));
 	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
 	struct ardrone_motors_setpoint_s setpoint;
+	memset(&setpoint, 0, sizeof(setpoint));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
-	/* publish AR.Drone motor control state */
-	int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control);
-
 	while (1) {
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of manual setpoint */
-		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
-		/* get a local copy of attitude */
-		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
-
-		if (state.state_machine == SYSTEM_STATE_AUTO) {
-			if (false == position_control_thread_started) {
-				pthread_attr_t position_control_thread_attr;
-				pthread_attr_init(&position_control_thread_attr);
-				pthread_attr_setstacksize(&position_control_thread_attr, 2048);
-				pthread_create(&position_control_thread, &position_control_thread_attr, position_control_loop, &state);
-				position_control_thread_started = true;
-			}
-
-			control_attitude(0, 0, 0, 0, &att, &state, ardrone_pub, &ar_control);
-
-			//No check for remote sticks to disarm in auto mode, land/disarm with ground station
-
-		} else if (state.state_machine == SYSTEM_STATE_MANUAL) {
-			if (control_mode == CONTROL_MODE_RATES) {
-				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
-				orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
-				control_rates(&raw, &setpoint);
-
-			} else if (control_mode == CONTROL_MODE_ATTITUDE) {
-				control_attitude(manual.roll, manual.pitch, manual.yaw,
-					manual.throttle, &att, &state, ardrone_pub, &ar_control);
-			}
-
-		} else {
-
+		if (control_mode == CONTROL_MODE_RATES) {
+			orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+			orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
+			control_rates(ardrone_write, &raw, &setpoint);
+
+		} else if (control_mode == CONTROL_MODE_ATTITUDE) {
+
+			// XXX Add failsafe logic for RC loss situations
+			/* hardcore, last-resort safety checking */
+			//if (status->rc_signal_lost) {
+
+			/* get a local copy of the vehicle state */
+			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+			/* get a local copy of manual setpoint */
+			orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+			/* get a local copy of attitude */
+			orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+			/* get a local copy of attitude setpoint */
+			orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+
+			att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
+			att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
+			att_sp.yaw_body = -manual.yaw * M_PI_F;
+
+			control_attitude(ardrone_write, &att_sp, &att, &state);
 		}
 
 		if (counter % 30 == 0) {
@@ -274,8 +260,8 @@ int ardrone_control_main(int argc, char *argv[])
 	close(ardrone_write);
 	ar_multiplexing_deinit(gpios);
 
-	printf("[ardrone_control] ending now...\r\n");
+	printf("[ardrone_control] ending now...\n");
 	fflush(stdout);
-	return 0;
+	return OK;
 }
 
diff --git a/apps/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c
index f13427eea..414ec671f 100644
--- a/apps/ardrone_control/ardrone_motor_control.c
+++ b/apps/ardrone_control/ardrone_motor_control.c
@@ -182,7 +182,7 @@ int ar_select_motor(int fd, uint8_t motor)
 	return ret;
 }
 
-void ar_init_motors(int ardrone_uart, int *gpios_pin)
+int ar_init_motors(int ardrone_uart, int *gpios_pin)
 {
 	/* Initialize multiplexing */
 	*gpios_pin = ar_multiplexing_init();
@@ -255,9 +255,10 @@ void ar_init_motors(int ardrone_uart, int *gpios_pin)
 	errcounter += ar_select_motor(*gpios_pin, 0);
 
 	if (errcounter != 0) {
-		printf("AR: init sequence incomplete, failed %d times", -errcounter);
+		fprintf(stderr, "[ar motors] init sequence incomplete, failed %d times", -errcounter);
 		fflush(stdout);
 	}
+	return errcounter;
 }
 
 /*
diff --git a/apps/ardrone_control/ardrone_motor_control.h b/apps/ardrone_control/ardrone_motor_control.h
index bf41e3f3b..8b8aa777b 100644
--- a/apps/ardrone_control/ardrone_motor_control.h
+++ b/apps/ardrone_control/ardrone_motor_control.h
@@ -42,9 +42,9 @@
 
 
 /**
- * @brief Generate the 8-byte motor set packet
+ * Generate the 5-byte motor set packet.
  *
- * @return the number of bytes (8)
+ * @return the number of bytes (5)
  */
 void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
 
@@ -55,7 +55,10 @@ void ar_enable_broadcast(int fd);
 int ar_multiplexing_init(void);
 int ar_multiplexing_deinit(int fd);
 
-void ar_init_motors(int ardrone_uart, int *gpios_uart);
+/**
+ * Initialize the motors.
+ */
+int ar_init_motors(int ardrone_uart, int *gpios_pin);
 
 void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
 
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index d9942fc73..db82a206e 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -47,50 +47,19 @@
 #include <stdio.h>
 #include <stdint.h>
 #include <stdbool.h>
-#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
 #include "ardrone_motor_control.h"
 #include <float.h>
 #include <math.h>
 #include "pid.h"
 #include <arch/board/up_hrt.h>
 
-extern int ardrone_write;
-extern int gpios;
+#define CONTROL_PID_ATTITUDE_INTERVAL	5e-3f
+#define MAX_MOTOR_COUNT 16
 
-#define CONTROL_PID_ATTITUDE_INTERVAL	5e-3
-
-void turn_xy_plane(const float_vect3 *vector, float yaw,
-		   float_vect3 *result);
-void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
-			float_vect3 *result);
-
-void turn_xy_plane(const float_vect3 *vector, float yaw,
-		   float_vect3 *result)
-{
-	//turn clockwise
-	static uint16_t counter;
-
-	result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y);
-	result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y);
-	result->z = vector->z; //leave direction normal to xy-plane untouched
-
-	counter++;
-}
-
-void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
-			float_vect3 *result)
+void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status)
 {
-	turn_xy_plane(vector, yaw, result);
-//	result->x = vector->x;
-//	result->y = vector->y;
-//	result->z = vector->z;
-	//	result->x = cos(yaw) * vector->x + sin(yaw) * vector->y;
-	//	result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y;
-	//	result->z = vector->z; //leave direction normal to xy-plane untouched
-}
+	const unsigned int motor_count = 4;
 
-void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
-{
 	static int motor_skip_counter = 0;
 
 	static PID_t yaw_pos_controller;
@@ -102,13 +71,12 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 	const float max_thrust = 1.0f;			/**< 100% max thrust */
 	const float scaling = 512.0f;			/**< 100% thrust equals a value of 512 */
 
-	const float min_gas = min_thrust * scaling;
-	const float max_gas = max_thrust * scaling;
+	const float min_gas = min_thrust * scaling;	/**< value range sent to motors, minimum */
+	const float max_gas = max_thrust * scaling;	/**< value range sent to motors, maximum */
 
-	static uint16_t motor_pwm[4] = {0, 0, 0, 0};
-	static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
-//	static float remote_control_weight_z = 1;
-//	static float position_control_weight_z = 0;
+	/* initialize all fields to zero */
+	static uint16_t motor_pwm[MAX_MOTOR_COUNT];
+	static float motor_calc[MAX_MOTOR_COUNT];
 
 	static float pid_yawpos_lim;
 	static float pid_yawspeed_lim;
@@ -116,13 +84,6 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 
 	static bool initialized;
 
-	static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller;
-
-	static hrt_abstime now_time;
-	static hrt_abstime last_time;
-
-	static commander_state_machine_t current_state;
-
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
 
@@ -134,36 +95,30 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 			 PID_MODE_DERIVATIV_CALC, 154);
 
 		pid_init(&yaw_speed_controller,
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
 			 PID_MODE_DERIVATIV_CALC, 155);
 
 		pid_init(&nick_controller,
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
 			 PID_MODE_DERIVATIV_SET, 156);
 
 		pid_init(&roll_controller,
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
-			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
 			 PID_MODE_DERIVATIV_SET, 157);
 
 		pid_yawpos_lim = 	global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
 		pid_yawspeed_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
 		pid_att_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
 
-		//TODO: true initialization? get gps while on ground?
-		attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f;
-		attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f;
-		attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f;
-
-		last_time = 0;
 		initialized = true;
 	}
 
@@ -176,144 +131,87 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
 
 		pid_set_parameters(&yaw_speed_controller,
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
 
 		pid_set_parameters(&nick_controller,
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
 
 		pid_set_parameters(&roll_controller,
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
 
 		pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
 		pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
 		pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
 	}
 
-	current_state = status->state_machine;
-	float_vect3 attitude_setpoint_bodyframe = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
-
-	if (current_state == SYSTEM_STATE_AUTO) {
-
-		attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0];
-		attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1];
-		attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2];
-
-		float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z;
-
-		// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
-		if (yaw_e > M_PI_F) {
-			yaw_e -= 2.0f * M_PI_F;
-		}
-
-		if (yaw_e < -M_PI_F) {
-			yaw_e += 2.0f * M_PI_F;
-		}
-
-		attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0.0f, yaw_e, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL);
-
-
-		/* limit control output */
-		if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) {
-			attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim;
-			yaw_pos_controller.saturated = 1;
-		}
-
-		if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) {
-			attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim;
-			yaw_pos_controller.saturated = 1;
-		}
-
-		//transform attitude setpoint from position controller from navi to body frame on xy_plane
-		float_vect3 attitude_setpoint_bodyframe_from_positioncontroller;
-		navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw
-		//now everything is in body frame
-
-
-		//TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?)
-		attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x;
-		attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y;
-		attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
-
-	} else if (current_state == SYSTEM_STATE_MANUAL) {
-		attitude_setpoint_bodyframe.x = -roll * M_PI_F / 8.0f;
-		attitude_setpoint_bodyframe.y = -pitch * M_PI_F / 8.0f;
-		attitude_setpoint_bodyframe.z = -yaw * M_PI_F;
-	}
-
-	/* add an attitude offset which needs to be estimated somewhere */
-	attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET];
-	attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET];
-
 	/*Calculate Controllers*/
 	//control Nick
-	float nick_control = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	float nick_control = pid_calculate(&nick_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET],
+					att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
 	//control Roll
-	float roll_control = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET],
+					att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
 	//control Yaw Speed
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
 
-	//compensation to keep force in z-direction
-	float zcompensation;
+	/*
+	 * compensate the vertical loss of thrust
+	 * when thrust plane has an angle.
+	 * start with a factor of 1.0 (no change)
+	 */
+	float zcompensation = 1.0f;
 
-	if (fabsf(att->roll) > 0.5f) {
-		zcompensation = 1.13949393f;
+	if (fabsf(att->roll) > 1.0f) {
+		zcompensation *= 1.85081571768f;
 
 	} else {
-		zcompensation = 1.0f / cosf(att->roll);
+		zcompensation *= 1.0f / cosf(att->roll);
 	}
 
-	if (fabsf(att->pitch) > 0.5f) {
-		zcompensation *= 1.13949393f;
+	if (fabsf(att->pitch) > 1.0f) {
+		zcompensation *= 1.85081571768f;
 
 	} else {
 		zcompensation *= 1.0f / cosf(att->pitch);
 	}
 
-	// use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT]
-	// to compute thrust for Z position control
-	//
-	//	float motor_thrust = min_gas +
-	//			( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation )
-	//		   + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation );
-	//calculate the basic thrust
-
-
-
-	float motor_thrust = 0;
+	float motor_thrust;
 
 	// FLYING MODES
-	if (current_state == SYSTEM_STATE_MANUAL) {
-		motor_thrust = thrust;
+	if (status->state_machine == SYSTEM_STATE_MANUAL) {
+		motor_thrust = att_sp->thrust;
 
-	} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
-		motor_thrust = thrust;	//TODO
+	} else if (status->state_machine == SYSTEM_STATE_GROUND_READY ||
+		status->state_machine == SYSTEM_STATE_STABILIZED ||
+		status->state_machine == SYSTEM_STATE_AUTO ||
+		status->state_machine == SYSTEM_STATE_MISSION_ABORT) {
+		motor_thrust = att_sp->thrust;	//TODO
 
-	} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
-		motor_thrust = thrust;	//TODO
+	} else if (status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
+		motor_thrust = att_sp->thrust;	//TODO
 
-	} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
-		motor_thrust = 0;	//immediately cut off thrust!
+	} else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
+		/* immediately cut off motors */
+		motor_thrust = 0.0f;
 
 	} else {
-		motor_thrust = 0;	// Motor thrust must be zero in any other mode!
+		/* limit motor throttle to zero for an unknown mode */
+		motor_thrust = 0.0f;
 	}
 
-	if (status->rc_signal_lost) motor_thrust = 0;
-
-	// Convertion to motor-step units
+	/* compensate thrust vector for roll / pitch contributions */
 	motor_thrust *= zcompensation;
 
-	//limit control output
-	//yawspeed
+	/* limit yaw rate output */
 	if (yaw_rate_control > pid_yawspeed_lim) {
 		yaw_rate_control = pid_yawspeed_lim;
 		yaw_speed_controller.saturated = 1;
@@ -345,25 +243,14 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 		roll_controller.saturated = 1;
 	}
 
-	/* Emit controller values */
-	ar_control->setpoint_thrust_cast = motor_thrust;
-	ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x;
-	ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y;
-	ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z;
-	ar_control->attitude_control_output[0] = roll_control;
-	ar_control->attitude_control_output[1] = nick_control;
-	ar_control->attitude_control_output[2] = yaw_rate_control;
-	ar_control->zcompensation = zcompensation;
-	orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control);
-
-	float output_band = 0.f;
+	float output_band = 0.0f;
 	float band_factor = 0.75f;
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
 	float yaw_factor = 1.0f;
 
 	if (motor_thrust <= min_thrust) {
 		motor_thrust = min_thrust;
-		output_band = 0.f;
+		output_band = 0.0f;
 
 	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
 		output_band = band_factor * (motor_thrust - min_thrust);
@@ -409,9 +296,7 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 		motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor);
 	}
 
-	uint8_t i;
-
-	for (i = 0; i < 4; i++) {
+	for (int i = 0; i < 4; i++) {
 		//check for limits
 		if (motor_calc[i] < motor_thrust - output_band) {
 			motor_calc[i] = motor_thrust - output_band;
@@ -444,20 +329,14 @@ void control_attitude(float roll, float pitch, float yaw, float thrust, const st
 	motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
 	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
 
-	//SEND MOTOR COMMANDS
+	/* send motors via UART */
 	if (motor_skip_counter % 5 == 0) {
-		uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
-		ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-		write(ardrone_write, motorSpeedBuf, 5);
+		if (motor_skip_counter % 25) printf("mot: %1.3f-%i\n", motor_thrust, motor_pwm[0]);
+		uint8_t buf[5] = {1, 2, 3, 4, 5};
+		ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
+		write(ardrone_write, buf, sizeof(buf));
 	}
 
 	motor_skip_counter++;
 
-//		now_time = hrt_absolute_time() / 1000000;
-//		if(now_time - last_time > 0)
-//		{
-//			printf("Counter: %ld\n",control_counter);
-//			last_time = now_time;
-//			control_counter = 0;
-//		}
 }
diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h
index a93a511ba..a7b01268b 100644
--- a/apps/ardrone_control/attitude_control.h
+++ b/apps/ardrone_control/attitude_control.h
@@ -42,11 +42,11 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/ardrone_control.h>
 #include <uORB/topics/vehicle_status.h>
 
-void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att,
-		      const struct vehicle_status_s *status, int ardrone_pub,
-		      struct ardrone_control_s *ar_control);
+void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+		const struct vehicle_status_s *status);
 
 #endif /* ATTITUDE_CONTROL_H_ */
diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c
index 4abba6255..d2bedadf2 100644
--- a/apps/ardrone_control/rate_control.c
+++ b/apps/ardrone_control/rate_control.c
@@ -54,7 +54,7 @@ typedef struct  {
 } quad_motors_setpoint_t;
 
 
-void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints)
+void control_rates(int ardrone_write, struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints)
 {
 	static quad_motors_setpoint_t actuators_desired;
 	//static quad_motors_setpoint_t quad_motors_setpoint_desired;
diff --git a/apps/ardrone_control/rate_control.h b/apps/ardrone_control/rate_control.h
index a69745d9b..bd3191c1c 100644
--- a/apps/ardrone_control/rate_control.h
+++ b/apps/ardrone_control/rate_control.h
@@ -33,8 +33,9 @@
  *
  ****************************************************************************/
 
-/*
- * @file Definition of attitude rate control
+/**
+ * @file rate_control.h
+ * Definition of attitude rate control
  */
 #ifndef RATE_CONTROL_H_
 #define RATE_CONTROL_H_
@@ -43,6 +44,6 @@
 #include <uORB/topics/ardrone_motors_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 
-void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints);
+void control_rates(int ardrone_write, struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints);
 
 #endif /* RATE_CONTROL_H_ */
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 3663503d1..68bb36b2d 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1092,8 +1092,10 @@ int commander_main(int argc, char *argv[])
 				last_print_time = hrt_absolute_time();
 			}
 			/* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */
-			current_status.rc_signal_lost = true;
+			current_status.rc_signal_cutting_off = true;
 			current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp;
+			/* if the RC signal is gone for a full second, consider it lost */
+			if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true;
 		}
 
 		/* End mode switch */
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 68cf7ead9..981520e7c 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -71,6 +71,7 @@
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
+ #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include "waypoints.h"
 #include "mavlink_log.h"
 
@@ -446,6 +447,7 @@ static void *uorb_receiveloop(void *arg)
 		struct ardrone_control_s ar_control;
 		struct vehicle_local_position_setpoint_s local_sp;
 		struct vehicle_global_position_setpoint_s global_sp;
+		struct vehicle_attitude_setpoint_s att_sp;
 	} buf;
 
 	/* --- SENSORS RAW VALUE --- */
@@ -472,13 +474,13 @@ static void *uorb_receiveloop(void *arg)
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- ARDRONE CONTROL --- */
-	/* subscribe to ORB for AR.Drone controller outputs */
-	int ar_sub = orb_subscribe(ORB_ID(ardrone_control));
-	orb_set_interval(ar_sub, 200);		/* 5Hz updates */
-	fds[fdsc_count].fd = ar_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
+	// /* --- ARDRONE CONTROL --- */
+	// /* subscribe to ORB for AR.Drone controller outputs */
+	// int ar_sub = orb_subscribe(ORB_ID(ardrone_control));
+	// orb_set_interval(ar_sub, 200);		/* 5Hz updates */
+	// fds[fdsc_count].fd = ar_sub;
+	// fds[fdsc_count].events = POLLIN;
+	// fdsc_count++;
 
 	/* --- SYSTEM STATE --- */
 	/* struct already globally allocated */
@@ -537,6 +539,15 @@ static void *uorb_receiveloop(void *arg)
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- ATTITUDE SETPOINT VALUE --- */
+	/* subscribe to ORB for attitude setpoint */
+	/* struct already allocated */
+	int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
+	orb_set_interval(spa_sub, 2000);	/* 0.5 Hz updates */
+	fds[fdsc_count].fd = spa_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	unsigned int sensors_raw_counter = 0;
 	unsigned int attitude_counter = 0;
 	unsigned int gps_counter = 0;
@@ -610,25 +621,25 @@ static void *uorb_receiveloop(void *arg)
 				gps_counter++;
 			}
 
-			/* --- ARDRONE CONTROL OUTPUTS --- */
-			if (fds[3].revents & POLLIN) {
-				/* copy ardrone control data into local buffer */
-				orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
-				uint64_t timestamp    = buf.ar_control.timestamp;
-				float setpoint_roll   = buf.ar_control.setpoint_attitude[0];
-				float setpoint_pitch  = buf.ar_control.setpoint_attitude[1];
-				float setpoint_yaw    = buf.ar_control.setpoint_attitude[2];
-				float setpoint_thrust = buf.ar_control.setpoint_thrust_cast;
-
-				float control_roll = buf.ar_control.attitude_control_output[0];
-				float control_pitch = buf.ar_control.attitude_control_output[1];
-				float control_yaw = buf.ar_control.attitude_control_output[2];
-
-				mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch);
-				mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw);
-			}
+			// /* --- ARDRONE CONTROL OUTPUTS --- */
+			// if (fds[3].revents & POLLIN) {
+			// 	/* copy ardrone control data into local buffer */
+			// 	orb_copy(ORB_ID(ardrone_control), ar_sub, &buf.ar_control);
+			// 	uint64_t timestamp    = buf.ar_control.timestamp;
+			// 	float setpoint_roll   = buf.ar_control.setpoint_attitude[0];
+			// 	float setpoint_pitch  = buf.ar_control.setpoint_attitude[1];
+			// 	float setpoint_yaw    = buf.ar_control.setpoint_attitude[2];
+			// 	float setpoint_thrust = buf.ar_control.setpoint_thrust_cast;
+
+			// 	float control_roll = buf.ar_control.attitude_control_output[0];
+			// 	float control_pitch = buf.ar_control.attitude_control_output[1];
+			// 	float control_yaw = buf.ar_control.attitude_control_output[2];
+
+			// 	mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, timestamp / 1000, setpoint_roll, setpoint_pitch, setpoint_yaw, setpoint_thrust);
+			// 	mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.roll", control_roll);
+			// 	mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.pitch", control_pitch);
+			// 	mavlink_msg_named_value_float_send(MAVLINK_COMM_0, timestamp / 1000, "cl.yaw", control_yaw);
+			// }
 
 			/* --- SYSTEM STATUS --- */
 			if (fds[4].revents & POLLIN) {
@@ -759,6 +770,13 @@ static void *uorb_receiveloop(void *arg)
 				orb_copy(ORB_ID(vehicle_local_position_setpoint), spl_sub, &buf.local_sp);
 				mavlink_msg_local_position_setpoint_send(MAVLINK_COMM_0, MAV_FRAME_LOCAL_NED, buf.local_sp.x, buf.local_sp.y, buf.local_sp.z, buf.local_sp.yaw);
 			}
+
+			/* --- VEHICLE ATTITUDE SETPOINT --- */
+			if (fds[11].revents & POLLIN) {
+				/* copy local position data into local buffer */
+				orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp);
+				mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust);
+			}
 		}
 	}
 
diff --git a/apps/multirotor_position_control/Makefile b/apps/multirotor_position_control/Makefile
new file mode 100644
index 000000000..23e2d49fb
--- /dev/null
+++ b/apps/multirotor_position_control/Makefile
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build uORB
+#
+
+APPNAME		 = ardrone_control
+PRIORITY	 = SCHED_PRIORITY_MAX - 15
+STACKSIZE	 = 2048
+
+# explicit list of sources - not everything is built currently
+CSRCS		 = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/multirotor_position_control/ardrone_control.c b/apps/multirotor_position_control/ardrone_control.c
new file mode 100644
index 000000000..89983724e
--- /dev/null
+++ b/apps/multirotor_position_control/ardrone_control.c
@@ -0,0 +1,290 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-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 Implementation of AR.Drone 1.0 / 2.0 control interface
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <sys/prctl.h>
+#include <arch/board/up_hrt.h>
+#include "ardrone_control.h"
+#include "attitude_control.h"
+#include "rate_control.h"
+#include "ardrone_motor_control.h"
+#include "position_control.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/ardrone_control.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/sensor_combined.h>
+
+#include "ardrone_control_helper.h"
+
+__EXPORT int ardrone_control_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Internal Definitions
+ ****************************************************************************/
+
+
+enum {
+	CONTROL_MODE_RATES = 0,
+	CONTROL_MODE_ATTITUDE = 1,
+} control_mode;
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/*File descriptors */
+int ardrone_write;
+int gpios;
+
+bool position_control_thread_started;
+
+/****************************************************************************
+ * pthread loops
+ ****************************************************************************/
+static void *position_control_loop(void *arg)
+{
+	struct vehicle_status_s *state = (struct vehicle_status_s *)arg;
+	// Set thread name
+	prctl(PR_SET_NAME, "ardrone pos ctrl", getpid());
+
+	while (1) {
+		if (state->state_machine == SYSTEM_STATE_AUTO) {
+//			control_position();   //FIXME TODO XXX
+			/* temporary 50 Hz execution */
+			usleep(20000);
+
+		} else {
+			position_control_thread_started = false;
+			break;
+		}
+	}
+
+	return NULL;
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+
+int ardrone_control_main(int argc, char *argv[])
+{
+	/* welcome user */
+	printf("[ardrone_control] Control started, taking over motors\n");
+
+	/* default values for arguments */
+	char *ardrone_uart_name = "/dev/ttyS1";
+	control_mode = CONTROL_MODE_ATTITUDE;
+
+	char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
+
+	/* read commandline arguments */
+	int i;
+
+	for (i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
+			if (argc > i + 1) {
+				ardrone_uart_name = argv[i + 1];
+
+			} else {
+				printf(commandline_usage);
+				return 0;
+			}
+
+		} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
+			if (argc > i + 1) {
+				if (strcmp(argv[i + 1], "rates") == 0) {
+					control_mode = CONTROL_MODE_RATES;
+
+				} else if (strcmp(argv[i + 1], "attitude") == 0) {
+					control_mode = CONTROL_MODE_ATTITUDE;
+
+				} else {
+					printf(commandline_usage);
+					return 0;
+				}
+
+			} else {
+				printf(commandline_usage);
+				return 0;
+			}
+		}
+	}
+
+	/* open uarts */
+	printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name);
+	ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY);
+	if (ardrone_write < 0) {
+		printf("[ardrone_control] Failed opening AR.Drone UART, exiting.\n");
+		exit(ERROR);
+	}
+
+	/* initialize motors */
+	ar_init_motors(ardrone_write, &gpios);
+	int counter = 0;
+
+	/* Led animation */
+	int led_counter = 0;
+
+	/* pthread for position control */
+	pthread_t position_control_thread;
+	position_control_thread_started = false;
+
+	/* structures */
+	struct vehicle_status_s state;
+	struct vehicle_attitude_s att;
+	struct ardrone_control_s ar_control;
+	struct manual_control_setpoint_s manual;
+	struct sensor_combined_s raw;
+	struct ardrone_motors_setpoint_s setpoint;
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
+
+	/* publish AR.Drone motor control state */
+	int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control);
+
+	while (1) {
+		/* get a local copy of the vehicle state */
+		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+		/* get a local copy of manual setpoint */
+		orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual);
+		/* get a local copy of attitude */
+		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+
+		if (state.state_machine == SYSTEM_STATE_AUTO) {
+			if (false == position_control_thread_started) {
+				pthread_attr_t position_control_thread_attr;
+				pthread_attr_init(&position_control_thread_attr);
+				pthread_attr_setstacksize(&position_control_thread_attr, 2048);
+				pthread_create(&position_control_thread, &position_control_thread_attr, position_control_loop, &state);
+				position_control_thread_started = true;
+			}
+
+			control_attitude(0, 0, 0, 0, &att, &state, ardrone_pub, &ar_control);
+
+			//No check for remote sticks to disarm in auto mode, land/disarm with ground station
+
+		} else if (state.state_machine == SYSTEM_STATE_MANUAL) {
+			if (control_mode == CONTROL_MODE_RATES) {
+				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+				orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
+				control_rates(&raw, &setpoint);
+
+			} else if (control_mode == CONTROL_MODE_ATTITUDE) {
+
+				// XXX Add failsafe logic for RC loss situations
+				/* hardcore, last-resort safety checking */
+				//if (status->rc_signal_lost) {
+
+				control_attitude(manual.roll, manual.pitch, manual.yaw,
+					manual.throttle, &att, &state, ardrone_pub, &ar_control);
+			}
+
+		} else {
+
+		}
+
+		if (counter % 30 == 0) {
+			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
+
+			if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
+
+			if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
+
+			if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
+
+			if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
+
+			if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
+
+			if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
+
+			if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
+
+			if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
+
+			led_counter++;
+
+			if (led_counter == 12) led_counter = 0;
+		}
+
+		/* run at approximately 200 Hz */
+		usleep(5000);
+
+		// This is a hardcore debug code piece to validate
+		// the motor interface
+		// uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
+		// ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20);
+		// write(ardrone_write, motorSpeedBuf, 5);
+		// usleep(15000);
+
+		counter++;
+	}
+
+	/* close uarts */
+	close(ardrone_write);
+	ar_multiplexing_deinit(gpios);
+
+	printf("[ardrone_control] ending now...\r\n");
+	fflush(stdout);
+	return 0;
+}
+
diff --git a/apps/multirotor_position_control/ardrone_control.h b/apps/multirotor_position_control/ardrone_control.h
new file mode 100644
index 000000000..7f9567f86
--- /dev/null
+++ b/apps/multirotor_position_control/ardrone_control.h
@@ -0,0 +1,12 @@
+/*
+ * ardrone_control.h
+ *
+ *  Created on: Mar 23, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef ARDRONE_CONTROL_H_
+#define ARDRONE_CONTROL_H_
+
+
+#endif /* ARDRONE_CONTROL_H_ */
diff --git a/apps/multirotor_position_control/ardrone_control_helper.c b/apps/multirotor_position_control/ardrone_control_helper.c
new file mode 100644
index 000000000..c073119e0
--- /dev/null
+++ b/apps/multirotor_position_control/ardrone_control_helper.c
@@ -0,0 +1,60 @@
+#include "ardrone_control_helper.h"
+#include <unistd.h>
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+// int read_sensors_raw(sensors_raw_t *sensors_raw)
+// {
+// 	static int ret;
+// 	ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full);
+
+// 	if (ret == 0) {
+// 		memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw));
+// //		printf("Timestamp %d\n", &global_data_sensors_raw->timestamp);
+
+// 	} else {
+// 		printf("Controller timeout, no new sensor values available\n");
+// 	}
+
+// 	global_data_unlock(&global_data_sensors_raw->access_conf_rate_full);
+// 	return ret;
+// }
+
+// int read_attitude(global_data_attitude_t *attitude)
+// {
+
+// 	static int ret;
+// 	ret = global_data_wait(&global_data_attitude->access_conf);
+
+// 	if (ret == 0) {
+// 		memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll));
+// 		memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch));
+// 		memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw));
+// 		memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed));
+// 		memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed));
+// 		memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed));
+
+// 	} else {
+// 		printf("Controller timeout, no new attitude values available\n");
+// 	}
+
+// 	global_data_unlock(&global_data_attitude->access_conf);
+
+
+
+// 	return ret;
+// }
+
+// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint)
+// {
+
+// 	if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock?
+// 		rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw;
+// 		rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne;
+// 		rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se;
+// 		rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw;
+
+// 		global_data_unlock(&global_data_quad_motors_setpoint->access_conf);
+// 	}
+// }
diff --git a/apps/multirotor_position_control/ardrone_control_helper.h b/apps/multirotor_position_control/ardrone_control_helper.h
new file mode 100644
index 000000000..22eebe986
--- /dev/null
+++ b/apps/multirotor_position_control/ardrone_control_helper.h
@@ -0,0 +1,21 @@
+/*
+ * ardrone_control_helper.h
+ *
+ *  Created on: May 15, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef ARDRONE_CONTROL_HELPER_H_
+#define ARDRONE_CONTROL_HELPER_H_
+
+#include <stdint.h>
+
+// typedef struct {
+// 	int16_t	gyro_raw[3]; // l3gd20
+// } sensors_raw_t;
+
+// /* Copy quad_motors_setpoint values from global memory to private variables */ //TODO: change this once the new mavlink message for rates is available
+// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint);
+
+
+#endif /* ARDRONE_CONTROL_HELPER_H_ */
diff --git a/apps/multirotor_position_control/ardrone_motor_control.c b/apps/multirotor_position_control/ardrone_motor_control.c
new file mode 100644
index 000000000..f13427eea
--- /dev/null
+++ b/apps/multirotor_position_control/ardrone_motor_control.c
@@ -0,0 +1,281 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-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 ardrone_motor_control.c
+ * Implementation of AR.Drone 1.0 / 2.0 motor control interface
+ */
+
+
+
+#include "ardrone_motor_control.h"
+
+static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
+static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 };
+
+typedef union {
+	uint16_t motor_value;
+	uint8_t bytes[2];
+} motor_union_t;
+
+/**
+ * @brief Generate the 8-byte motor set packet
+ *
+ * @return the number of bytes (8)
+ */
+void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
+{
+	motor_buf[0] = 0x20;
+	motor_buf[1] = 0x00;
+	motor_buf[2] = 0x00;
+	motor_buf[3] = 0x00;
+	motor_buf[4] = 0x00;
+	/*
+	 * {0x20, 0x00, 0x00, 0x00, 0x00};
+	 * 0x20 is start sign / motor command
+	 */
+	motor_union_t curr_motor;
+	uint16_t nineBitMask = 0x1FF;
+
+	/* Set motor 1 */
+	curr_motor.motor_value = (motor1 & nineBitMask) << 4;
+	motor_buf[0] |= curr_motor.bytes[1];
+	motor_buf[1] |= curr_motor.bytes[0];
+
+	/* Set motor 2 */
+	curr_motor.motor_value = (motor2 & nineBitMask) << 3;
+	motor_buf[1] |= curr_motor.bytes[1];
+	motor_buf[2] |= curr_motor.bytes[0];
+
+	/* Set motor 3 */
+	curr_motor.motor_value = (motor3 & nineBitMask) << 2;
+	motor_buf[2] |= curr_motor.bytes[1];
+	motor_buf[3] |= curr_motor.bytes[0];
+
+	/* Set motor 4 */
+	curr_motor.motor_value = (motor4 & nineBitMask) << 1;
+	motor_buf[3] |= curr_motor.bytes[1];
+	motor_buf[4] |= curr_motor.bytes[0];
+}
+
+void ar_enable_broadcast(int fd)
+{
+	ar_select_motor(fd, 0);
+}
+
+int ar_multiplexing_init()
+{
+	int		fd;
+	
+	fd = open(GPIO_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		printf("GPIO: open fail\n");
+		return fd;
+	}
+
+	/* deactivate all outputs */
+	int ret = 0;
+	ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+	if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
+		printf("GPIO: output set fail\n");
+		close(fd);
+		return -1;
+	}
+
+	if (ret < 0) {
+		printf("GPIO: clearing pins fail\n");
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+int ar_multiplexing_deinit(int fd)
+{
+	if (fd < 0) {
+		printf("GPIO: no valid descriptor\n");
+		return fd;
+	}
+
+	int ret = 0;
+
+	/* deselect motor 1-4 */
+	ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+	if (ret != 0) {
+		printf("GPIO: clear failed %d times\n", ret);
+	}
+
+	if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) {
+		printf("GPIO: input set fail\n");
+		return -1;
+	}
+
+	close(fd);
+
+	return ret;
+}
+
+int ar_select_motor(int fd, uint8_t motor)
+{
+	int ret = 0;
+	unsigned long gpioset;
+	/*
+	 *  Four GPIOS:
+	 *		GPIO_EXT1
+	 *		GPIO_EXT2
+	 *		GPIO_UART2_CTS
+	 *		GPIO_UART2_RTS
+	 */
+
+	/* select motor 0 to enable broadcast */
+	if (motor == 0) {
+		/* select motor 1-4 */
+		ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
+
+	} else {
+		/* deselect all */
+		ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+		/* select reqested motor */	
+		ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
+
+		/* deselect all others */
+		// gpioset = motor_gpios ^ motor_gpio[motor - 1];
+		// ret += ioctl(fd, GPIO_SET, gpioset);
+	}
+
+	return ret;
+}
+
+void ar_init_motors(int ardrone_uart, int *gpios_pin)
+{
+	/* Initialize multiplexing */
+	*gpios_pin = ar_multiplexing_init();
+
+	/* Write ARDrone commands on UART2 */
+	uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
+	uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
+
+	/* initialize all motors
+	 * - select one motor at a time
+	 * - configure motor
+	 */
+	int i;
+	int errcounter = 0;
+
+	for (i = 1; i < 5; ++i) {
+		/* Initialize motors 1-4 */
+		initbuf[3] = i;
+		errcounter += ar_select_motor(*gpios_pin, i);
+
+		write(ardrone_uart, initbuf + 0, 1);
+
+		/* sleep 400 ms */
+		usleep(200000);
+		usleep(200000);
+
+		write(ardrone_uart, initbuf + 1, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		write(ardrone_uart, initbuf + 2, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		write(ardrone_uart, initbuf + 3, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		write(ardrone_uart, initbuf + 4, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		/* enable multicast */
+		write(ardrone_uart, multicastbuf + 0, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 1, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 2, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 3, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 4, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 5, 1);
+		/* wait 5 ms */
+		usleep(50000);
+	}
+
+	/* start the multicast part */
+	errcounter += ar_select_motor(*gpios_pin, 0);
+
+	if (errcounter != 0) {
+		printf("AR: init sequence incomplete, failed %d times", -errcounter);
+		fflush(stdout);
+	}
+}
+
+/*
+ * Sets the leds on the motor controllers, 1 turns led on, 0 off.
+ */
+void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
+{
+	/*
+	 * 2 bytes are sent. The first 3 bits describe the command: 011 means led control
+	 * the following 4 bits are the red leds for motor 4, 3, 2, 1
+	 * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1
+	 * the last bit is unknown.
+	 *
+	 * The packet is therefore:
+	 * 011 rrrr 0000 gggg 0
+	 */
+	uint8_t leds[2];
+	leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
+	leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
+	write(ardrone_uart, leds, 2);
+}
diff --git a/apps/multirotor_position_control/ardrone_motor_control.h b/apps/multirotor_position_control/ardrone_motor_control.h
new file mode 100644
index 000000000..bf41e3f3b
--- /dev/null
+++ b/apps/multirotor_position_control/ardrone_motor_control.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ * px4/ardrone_offboard_control.h
+ *
+ *   Copyright (C) 2012 PX4 Autopilot Project. 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 NuttX 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.
+ *
+ ****************************************************************************/
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <poll.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <drivers/drv_gpio.h>
+
+
+/**
+ * @brief Generate the 8-byte motor set packet
+ *
+ * @return the number of bytes (8)
+ */
+void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
+
+int ar_select_motor(int fd, uint8_t motor);
+
+void ar_enable_broadcast(int fd);
+
+int ar_multiplexing_init(void);
+int ar_multiplexing_deinit(int fd);
+
+void ar_init_motors(int ardrone_uart, int *gpios_uart);
+
+void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
+
diff --git a/apps/multirotor_position_control/attitude_control.c b/apps/multirotor_position_control/attitude_control.c
new file mode 100644
index 000000000..8bcd33ede
--- /dev/null
+++ b/apps/multirotor_position_control/attitude_control.c
@@ -0,0 +1,459 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@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 Implementation of attitude controller
+ */
+
+#include "attitude_control.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
+#include "ardrone_motor_control.h"
+#include <float.h>
+#include <math.h>
+#include "pid.h"
+#include <arch/board/up_hrt.h>
+
+extern int ardrone_write;
+extern int gpios;
+
+#define CONTROL_PID_ATTITUDE_INTERVAL	5e-3f
+
+void turn_xy_plane(const float_vect3 *vector, float yaw,
+		   float_vect3 *result);
+void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
+			float_vect3 *result);
+
+void turn_xy_plane(const float_vect3 *vector, float yaw,
+		   float_vect3 *result)
+{
+	//turn clockwise
+	static uint16_t counter;
+
+	result->x = (cosf(yaw) * vector->x + sinf(yaw) * vector->y);
+	result->y = (-sinf(yaw) * vector->x + cosf(yaw) * vector->y);
+	result->z = vector->z; //leave direction normal to xy-plane untouched
+
+	counter++;
+}
+
+void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
+			float_vect3 *result)
+{
+	turn_xy_plane(vector, yaw, result);
+//	result->x = vector->x;
+//	result->y = vector->y;
+//	result->z = vector->z;
+	//	result->x = cos(yaw) * vector->x + sin(yaw) * vector->y;
+	//	result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y;
+	//	result->z = vector->z; //leave direction normal to xy-plane untouched
+}
+
+void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
+{
+	static int motor_skip_counter = 0;
+
+	static PID_t yaw_pos_controller;
+	static PID_t yaw_speed_controller;
+	static PID_t nick_controller;
+	static PID_t roll_controller;
+
+	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
+	const float max_thrust = 1.0f;			/**< 100% max thrust */
+	const float scaling = 512.0f;			/**< 100% thrust equals a value of 512 */
+
+	const float min_gas = min_thrust * scaling;
+	const float max_gas = max_thrust * scaling;
+
+	static uint16_t motor_pwm[4] = {0, 0, 0, 0};
+	static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+//	static float remote_control_weight_z = 1;
+//	static float position_control_weight_z = 0;
+
+	static float pid_yawpos_lim;
+	static float pid_yawspeed_lim;
+	static float pid_att_lim;
+
+	static bool initialized;
+
+	static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller;
+	static commander_state_machine_t current_state;
+
+	/* initialize the pid controllers when the function is called for the first time */
+	if (initialized == false) {
+
+		pid_init(&yaw_pos_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU],
+			 PID_MODE_DERIVATIV_CALC, 154);
+
+		pid_init(&yaw_speed_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
+			 PID_MODE_DERIVATIV_CALC, 155);
+
+		pid_init(&nick_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
+			 PID_MODE_DERIVATIV_SET, 156);
+
+		pid_init(&roll_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
+			 PID_MODE_DERIVATIV_SET, 157);
+
+		pid_yawpos_lim = 	global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
+		pid_yawspeed_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+
+		//TODO: true initialization? get gps while on ground?
+		attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f;
+		attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f;
+		attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f;
+
+		initialized = true;
+	}
+
+	/* load new parameters with lower rate */
+	if (motor_skip_counter % 50 == 0) {
+		pid_set_parameters(&yaw_pos_controller,
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
+
+		pid_set_parameters(&yaw_speed_controller,
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
+
+		pid_set_parameters(&nick_controller,
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
+
+		pid_set_parameters(&roll_controller,
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
+
+		pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
+		pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+	}
+
+	current_state = status->state_machine;
+	float_vect3 attitude_setpoint_bodyframe = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
+
+	if (current_state == SYSTEM_STATE_AUTO) {
+
+		attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0];
+		attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1];
+		attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2];
+
+		float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z;
+
+		// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
+		if (yaw_e > M_PI_F) {
+			yaw_e -= 2.0f * M_PI_F;
+		}
+
+		if (yaw_e < -M_PI_F) {
+			yaw_e += 2.0f * M_PI_F;
+		}
+
+		attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0.0f, yaw_e, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL);
+
+
+		/* limit control output */
+		if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) {
+			attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim;
+			yaw_pos_controller.saturated = 1;
+		}
+
+		if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) {
+			attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim;
+			yaw_pos_controller.saturated = 1;
+		}
+
+		//transform attitude setpoint from position controller from navi to body frame on xy_plane
+		float_vect3 attitude_setpoint_bodyframe_from_positioncontroller;
+		navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw
+		//now everything is in body frame
+
+
+		//TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?)
+		attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x;
+		attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y;
+		attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
+
+	} else if (current_state == SYSTEM_STATE_MANUAL) {
+		attitude_setpoint_bodyframe.x = -roll * M_PI_F / 8.0f;
+		attitude_setpoint_bodyframe.y = -pitch * M_PI_F / 8.0f;
+		attitude_setpoint_bodyframe.z = -yaw * M_PI_F;
+	}
+
+	/* add an attitude offset which needs to be estimated somewhere */
+	attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET];
+	attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET];
+
+	/*Calculate Controllers*/
+	//control Nick
+	float nick_control = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	//control Roll
+	float roll_control = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	//control Yaw Speed
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
+
+	/*
+	 * compensate the vertical loss of thrust
+	 * when thrust plane has an angle.
+	 * start with a factor of 1.0 (no change)
+	 */
+	float zcompensation = 1.0f;
+
+	if (fabsf(att->roll) > 1.0f) {
+		zcompensation *= 1.85081571768f;
+
+	} else {
+		zcompensation *= 1.0f / cosf(att->roll);
+	}
+
+	if (fabsf(att->pitch) > 1.0f) {
+		zcompensation *= 1.85081571768f;
+
+	} else {
+		zcompensation *= 1.0f / cosf(att->pitch);
+	}
+
+	// use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT]
+	// to compute thrust for Z position control
+	//
+	//	float motor_thrust = min_gas +
+	//			( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation )
+	//		   + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation );
+	//calculate the basic thrust
+
+
+
+	float motor_thrust = 0;
+
+	// FLYING MODES
+	if (current_state == SYSTEM_STATE_MANUAL) {
+		motor_thrust = thrust;
+
+	} else if (current_state == SYSTEM_STATE_GROUND_READY ||
+		current_state == SYSTEM_STATE_STABILIZED ||
+		current_state == SYSTEM_STATE_AUTO ||
+		current_state == SYSTEM_STATE_MISSION_ABORT) {
+		motor_thrust = thrust;	//TODO
+
+	} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
+		motor_thrust = thrust;	//TODO
+
+	} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
+		/* immediately cut off motors */
+		motor_thrust = 0;
+
+	} else {
+		/* limit motor throttle to zero for an unknown mode */
+		motor_thrust = 0;
+	}
+
+	printf("t1:%1.3f ");
+
+	/* compensate thrust vector for roll / pitch contributions */
+	motor_thrust *= zcompensation;
+
+	printf("t2:%1.3f\n");
+
+	/* limit yaw rate output */
+	if (yaw_rate_control > pid_yawspeed_lim) {
+		yaw_rate_control = pid_yawspeed_lim;
+		yaw_speed_controller.saturated = 1;
+	}
+
+	if (yaw_rate_control < -pid_yawspeed_lim) {
+		yaw_rate_control = -pid_yawspeed_lim;
+		yaw_speed_controller.saturated = 1;
+	}
+
+	if (nick_control > pid_att_lim) {
+		nick_control = pid_att_lim;
+		nick_controller.saturated = 1;
+	}
+
+	if (nick_control < -pid_att_lim) {
+		nick_control = -pid_att_lim;
+		nick_controller.saturated = 1;
+	}
+
+
+	if (roll_control > pid_att_lim) {
+		roll_control = pid_att_lim;
+		roll_controller.saturated = 1;
+	}
+
+	if (roll_control < -pid_att_lim) {
+		roll_control = -pid_att_lim;
+		roll_controller.saturated = 1;
+	}
+
+	/* Emit controller values */
+	ar_control->setpoint_thrust_cast = motor_thrust;
+	ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x;
+	ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y;
+	ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z;
+	ar_control->attitude_control_output[0] = roll_control;
+	ar_control->attitude_control_output[1] = nick_control;
+	ar_control->attitude_control_output[2] = yaw_rate_control;
+	ar_control->zcompensation = zcompensation;
+	orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control);
+
+	float output_band = 0.f;
+	float band_factor = 0.75f;
+	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
+	float yaw_factor = 1.0f;
+
+	if (motor_thrust <= min_thrust) {
+		motor_thrust = min_thrust;
+		output_band = 0.f;
+
+	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
+		output_band = band_factor * (motor_thrust - min_thrust);
+
+	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
+		output_band = band_factor * startpoint_full_control;
+
+	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
+		output_band = band_factor * (max_thrust - motor_thrust);
+	}
+
+	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
+
+	// FRONT (MOTOR 1)
+	motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control);
+
+	// RIGHT (MOTOR 2)
+	motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control);
+
+	// BACK (MOTOR 3)
+	motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control);
+
+	// LEFT (MOTOR 4)
+	motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control);
+
+	// if we are not in the output band
+	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
+	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
+	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
+	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
+
+		yaw_factor = 0.5f;
+		// FRONT (MOTOR 1)
+		motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor);
+
+		// RIGHT (MOTOR 2)
+		motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control * yaw_factor);
+
+		// BACK (MOTOR 3)
+		motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control * yaw_factor);
+
+		// LEFT (MOTOR 4)
+		motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor);
+	}
+
+	for (int i = 0; i < 4; i++) {
+		//check for limits
+		if (motor_calc[i] < motor_thrust - output_band) {
+			motor_calc[i] = motor_thrust - output_band;
+		}
+
+		if (motor_calc[i] > motor_thrust + output_band) {
+			motor_calc[i] = motor_thrust + output_band;
+		}
+	}
+
+	/* set the motor values */
+
+	/* scale up from 0..1 to 10..512) */
+	motor_pwm[0] = (uint16_t) motor_calc[0] * ((float)max_gas - min_gas) + min_gas;
+	motor_pwm[1] = (uint16_t) motor_calc[1] * ((float)max_gas - min_gas) + min_gas;
+	motor_pwm[2] = (uint16_t) motor_calc[2] * ((float)max_gas - min_gas) + min_gas;
+	motor_pwm[3] = (uint16_t) motor_calc[3] * ((float)max_gas - min_gas) + min_gas;
+
+	/* Keep motors spinning while armed and prevent overflows */
+
+	/* Failsafe logic - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
+	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
+	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
+	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
+
+	/* Failsafe logic - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
+	motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
+	motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
+	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
+
+	/* send motors via UART */
+	if (motor_skip_counter % 5 == 0) {
+		uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
+		ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
+		write(ardrone_write, motorSpeedBuf, 5);
+	}
+
+	motor_skip_counter++;
+
+}
diff --git a/apps/multirotor_position_control/attitude_control.h b/apps/multirotor_position_control/attitude_control.h
new file mode 100644
index 000000000..a93a511ba
--- /dev/null
+++ b/apps/multirotor_position_control/attitude_control.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file attitude control for quadrotors */
+
+#ifndef ATTITUDE_CONTROL_H_
+#define ATTITUDE_CONTROL_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/ardrone_control.h>
+#include <uORB/topics/vehicle_status.h>
+
+void control_attitude(float roll, float pitch, float yaw, float thrust, const struct vehicle_attitude_s *att,
+		      const struct vehicle_status_s *status, int ardrone_pub,
+		      struct ardrone_control_s *ar_control);
+
+#endif /* ATTITUDE_CONTROL_H_ */
diff --git a/apps/multirotor_position_control/pid.c b/apps/multirotor_position_control/pid.c
new file mode 100644
index 000000000..5ce05670e
--- /dev/null
+++ b/apps/multirotor_position_control/pid.c
@@ -0,0 +1,109 @@
+#include "pid.h"
+
+#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
+
+void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
+	      uint8_t mode, uint8_t plot_i)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->intmax = intmax;
+	pid->mode = mode;
+	pid->plot_i = plot_i;
+	pid->count = 0;
+	pid->saturated = 0;
+
+	pid->sp = 0;
+	pid->error_previous = 0;
+	pid->integral = 0;
+}
+void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->intmax = intmax;
+	//	pid->mode = mode;
+
+	//	pid->sp = 0;
+	//	pid->error_previous = 0;
+	//	pid->integral = 0;
+}
+
+//void pid_set(PID_t *pid, float sp)
+//{
+//	pid->sp = sp;
+//	pid->error_previous = 0;
+//	pid->integral = 0;
+//}
+
+/**
+ *
+ * @param pid
+ * @param val
+ * @param dt
+ * @return
+ */
+float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+{
+	/*  error = setpoint - actual_position
+	 integral = integral + (error*dt)
+	 derivative = (error - previous_error)/dt
+	 output = (Kp*error) + (Ki*integral) + (Kd*derivative)
+	 previous_error = error
+	 wait(dt)
+	 goto start
+	 */
+
+	float i, d;
+	pid->sp = sp;
+	float error = pid->sp - val;
+
+	if (pid->saturated && (pid->integral * error > 0)) {
+		//Output is saturated and the integral would get bigger (positive or negative)
+		i = pid->integral;
+
+		//Reset saturation. If we are still saturated this will be set again at output limit check.
+		pid->saturated = 0;
+
+	} else {
+		i = pid->integral + (error * dt);
+	}
+
+	// Anti-Windup. Needed if we don't use the saturation above.
+	if (pid->intmax != 0.0) {
+		if (i > pid->intmax) {
+			pid->integral = pid->intmax;
+
+		} else if (i < -pid->intmax) {
+
+			pid->integral = -pid->intmax;
+
+		} else {
+			pid->integral = i;
+		}
+
+		//Send Controller integrals
+		//		Disabled because of new possibilities with debug_vect.
+		//		Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
+		//		if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
+		//		{
+		//			mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
+		//		}
+	}
+
+	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / dt;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
+		d = -val_dot;
+
+	} else {
+		d = 0;
+	}
+
+	pid->error_previous = error;
+
+	return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+}
diff --git a/apps/multirotor_position_control/pid.h b/apps/multirotor_position_control/pid.h
new file mode 100644
index 000000000..a721c839c
--- /dev/null
+++ b/apps/multirotor_position_control/pid.h
@@ -0,0 +1,40 @@
+/*
+ * pid.h
+ *
+ *  Created on: May 29, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#include <stdint.h>
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
+ * val_dot in pid_calculate() will be ignored */
+#define PID_MODE_DERIVATIV_CALC	0
+/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+#define PID_MODE_DERIVATIV_SET	1
+
+typedef struct {
+	float kp;
+	float ki;
+	float kd;
+	float intmax;
+	float sp;
+	float integral;
+	float error_previous;
+	uint8_t mode;
+	uint8_t plot_i;
+	uint8_t count;
+	uint8_t saturated;
+} PID_t;
+
+void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
+void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
+//void pid_set(PID_t *pid, float sp);
+float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
+
+
+
+#endif /* PID_H_ */
diff --git a/apps/multirotor_position_control/position_control.c b/apps/multirotor_position_control/position_control.c
new file mode 100644
index 000000000..6ba165459
--- /dev/null
+++ b/apps/multirotor_position_control/position_control.c
@@ -0,0 +1,308 @@
+/****************************************************************************
+ * ardrone_control/position_control.c
+ *
+ *   Copyright (C) 2008, 2012 Thomas Gubler, Julian Oes, Lorenz Meier. All rights reserved.
+ *   Author: Based on the pixhawk quadrotor controller
+ *
+ * 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 NuttX 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.
+ *
+ ****************************************************************************/
+
+#include "position_control.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+#include <stdbool.h>
+#include <float.h>
+#include "pid.h"
+
+#ifndef FM_PI
+#define FM_PI 3.1415926535897932384626433832795f
+#endif
+
+
+
+#define CONTROL_PID_POSITION_INTERVAL	0.020
+
+int read_lock_position(global_data_position_t *position)
+{
+	static int ret;
+	ret = global_data_wait(&global_data_position->access_conf);
+
+	if (ret == 0) {
+		memcpy(&position->lat, &global_data_position->lat, sizeof(global_data_position->lat));
+		memcpy(&position->lon, &global_data_position->lon, sizeof(global_data_position->lon));
+		memcpy(&position->alt, &global_data_position->alt, sizeof(global_data_position->alt));
+		memcpy(&position->relative_alt, &global_data_position->relative_alt, sizeof(global_data_position->relative_alt));
+		memcpy(&position->vx, &global_data_position->vx, sizeof(global_data_position->vx));
+		memcpy(&position->vy, &global_data_position->vy, sizeof(global_data_position->vy));
+		memcpy(&position->vz, &global_data_position->vz, sizeof(global_data_position->vz));
+		memcpy(&position->hdg, &global_data_position->hdg, sizeof(global_data_position->hdg));
+
+
+	} else {
+		printf("Controller timeout, no new position values available\n");
+	}
+
+	global_data_unlock(&global_data_position->access_conf);
+	return ret;
+}
+
+float get_distance_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next)
+{
+	double lat_now_rad = lat_now / 180.0 * M_PI;
+	double lon_now_rad = lon_now / 180.0 * M_PI;
+	double lat_next_rad = lat_next / 180.0 * M_PI;
+	double lon_next_rad = lon_next / 180.0 * M_PI;
+
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
+	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+	const double radius_earth = 6371000.0;
+
+	return radius_earth * c;
+}
+
+float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next)
+{
+	double lat_now_rad = lat_now / 180.0 * M_PI;
+	double lon_now_rad = lon_now / 180.0 * M_PI;
+	double lat_next_rad = lat_next / 180.0 * M_PI;
+	double lon_next_rad = lon_next / 180.0 * M_PI;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+
+	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+
+	if (theta < 0) {
+		theta = theta + 2 * FM_PI;
+	}
+
+	return theta;
+}
+
+void control_position(void)
+{
+	static PID_t distance_controller;
+
+	static int read_ret;
+	static global_data_position_t position_estimated;
+
+	static uint16_t counter;
+
+	static bool initialized;
+	static uint16_t pm_counter;
+
+	static float lat_next;
+	static float lon_next;
+
+	static float pitch_current;
+
+	static float thrust_total;
+
+
+	if (initialized == false) {
+
+		global_data_lock(&global_data_parameter_storage->access_conf);
+
+		pid_init(&distance_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
+			 PID_MODE_DERIVATIV_CALC, 150);//150
+
+//		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+//		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+		pm_counter = global_data_parameter_storage->counter;
+
+		global_data_unlock(&global_data_parameter_storage->access_conf);
+
+		thrust_total = 0.0f;
+
+		/* Position initialization */
+		/* Wait for new position estimate */
+		do {
+			read_ret = read_lock_position(&position_estimated);
+		} while (read_ret != 0);
+
+		lat_next = position_estimated.lat;
+		lon_next = position_estimated.lon;
+
+		/* attitude initialization */
+		global_data_lock(&global_data_attitude->access_conf);
+		pitch_current = global_data_attitude->pitch;
+		global_data_unlock(&global_data_attitude->access_conf);
+
+		initialized = true;
+	}
+
+	/* load new parameters with 10Hz */
+	if (counter % 50 == 0) {
+		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
+			/* check whether new parameters are available */
+			if (global_data_parameter_storage->counter > pm_counter) {
+				pid_set_parameters(&distance_controller,
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
+
+//
+//				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+//				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+				pm_counter = global_data_parameter_storage->counter;
+				printf("Position controller changed pid parameters\n");
+			}
+		}
+
+		global_data_unlock(&global_data_parameter_storage->access_conf);
+	}
+
+
+	/* Wait for new position estimate */
+	do {
+		read_ret = read_lock_position(&position_estimated);
+	} while (read_ret != 0);
+
+	/* Get next waypoint */ //TODO: add local copy
+
+	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
+		lat_next = global_data_position_setpoint->x;
+		lon_next = global_data_position_setpoint->y;
+		global_data_unlock(&global_data_position_setpoint->access_conf);
+	}
+
+	/* Get distance to waypoint */
+	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
+//	if(counter % 5 == 0)
+//		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
+
+	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
+	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
+
+	if (counter % 5 == 0)
+		printf("bearing: %.4f\n", bearing);
+
+	/* Calculate speed in direction of bearing (needed for controller) */
+	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
+//	if(counter % 5 == 0)
+//		printf("speed_norm: %.4f\n", speed_norm);
+	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
+
+	/* Control Thrust in bearing direction  */
+	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
+				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
+
+//	if(counter % 5 == 0)
+//		printf("horizontal thrust: %.4f\n", horizontal_thrust);
+
+	/* Get total thrust (from remote for now) */
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
+		global_data_unlock(&global_data_rc_channels->access_conf);
+	}
+
+	const float max_gas = 500.0f;
+	thrust_total *= max_gas / 20000.0f; //TODO: check this
+	thrust_total += max_gas / 2.0f;
+
+
+	if (horizontal_thrust > thrust_total) {
+		horizontal_thrust = thrust_total;
+
+	} else if (horizontal_thrust < -thrust_total) {
+		horizontal_thrust = -thrust_total;
+	}
+
+
+
+	//TODO: maybe we want to add a speed controller later...
+
+	/* Calclulate thrust in east and north direction */
+	float thrust_north = cosf(bearing) * horizontal_thrust;
+	float thrust_east = sinf(bearing) * horizontal_thrust;
+
+	if (counter % 10 == 0) {
+		printf("thrust north: %.4f\n", thrust_north);
+		printf("thrust east: %.4f\n", thrust_east);
+		fflush(stdout);
+	}
+
+	/* Get current attitude */
+	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
+		pitch_current = global_data_attitude->pitch;
+		global_data_unlock(&global_data_attitude->access_conf);
+	}
+
+	/* Get desired pitch & roll */
+	float pitch_desired = 0.0f;
+	float roll_desired = 0.0f;
+
+	if (thrust_total != 0) {
+		float pitch_fraction = -thrust_north / thrust_total;
+		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
+
+		if (roll_fraction < -1) {
+			roll_fraction = -1;
+
+		} else if (roll_fraction > 1) {
+			roll_fraction = 1;
+		}
+
+//		if(counter % 5 == 0)
+//		{
+//			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
+//			fflush(stdout);
+//		}
+
+		pitch_desired = asinf(pitch_fraction);
+		roll_desired = asinf(roll_fraction);
+	}
+
+	/*Broadcast desired angles */
+	global_data_lock(&global_data_ardrone_control->access_conf);
+	global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[0] = roll_desired;
+	global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[1] = pitch_desired;
+	global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[2] = bearing; //TODO: add yaw setpoint
+	global_data_unlock(&global_data_ardrone_control->access_conf);
+	global_data_broadcast(&global_data_ardrone_control->access_conf);
+
+
+	counter++;
+}
diff --git a/apps/multirotor_position_control/position_control.h b/apps/multirotor_position_control/position_control.h
new file mode 100644
index 000000000..dbc0650e6
--- /dev/null
+++ b/apps/multirotor_position_control/position_control.h
@@ -0,0 +1,13 @@
+/*
+ * position_control.h
+ *
+ *  Created on: May 29, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef POSITION_CONTROL_H_
+#define POSITION_CONTROL_H_
+
+void control_position(void);
+
+#endif /* POSITION_CONTROL_H_ */
diff --git a/apps/multirotor_position_control/rate_control.c b/apps/multirotor_position_control/rate_control.c
new file mode 100644
index 000000000..4abba6255
--- /dev/null
+++ b/apps/multirotor_position_control/rate_control.c
@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <nagelit@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 Implementation of attitude rate control
+ */
+
+#include "rate_control.h"
+#include "ardrone_control_helper.h"
+#include "ardrone_motor_control.h"
+#include <arch/board/up_hrt.h>
+
+extern int ardrone_write;
+extern int gpios;
+
+typedef struct  {
+	uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration
+	uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration
+	uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration
+	uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration
+	uint8_t target_system; ///< System ID of the system that should set these motor commands
+} quad_motors_setpoint_t;
+
+
+void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints)
+{
+	static quad_motors_setpoint_t actuators_desired;
+	//static quad_motors_setpoint_t quad_motors_setpoint_desired;
+
+	static int16_t outputBand = 0;
+
+//	static uint16_t control_counter;
+	static hrt_abstime now_time;
+	static hrt_abstime last_time;
+
+	static float setpointXrate;
+	static float setpointYrate;
+	static float setpointZrate;
+
+	static float setpointRateCast[3];
+	static float Kp;
+//	static float Ki;
+	static float setpointThrustCast;
+	static float startpointFullControll;
+	static float maxThrustSetpoints;
+
+	static float gyro_filtered[3];
+	static float gyro_filtered_offset[3];
+	static float gyro_alpha;
+	static float gyro_alpha_offset;
+//	static float errXrate;
+	static float attRatesScaled[3];
+
+	static uint16_t offsetCnt;
+//	static float antiwindup;
+	static int motor_skip_counter;
+
+	static int read_ret;
+
+	static bool initialized;
+
+	if (initialized == false) {
+		initialized = true;
+
+		/* Read sensors for initial values */
+
+		gyro_filtered_offset[0] = 0.00026631611f * (float)raw->gyro_raw[0];
+		gyro_filtered_offset[1] = 0.00026631611f * (float)raw->gyro_raw[1];
+		gyro_filtered_offset[2] = 0.00026631611f * (float)raw->gyro_raw[2];
+
+		gyro_filtered[0] = 0.00026631611f * (float)raw->gyro_raw[0];
+		gyro_filtered[1] = 0.00026631611f * (float)raw->gyro_raw[1];
+		gyro_filtered[2] = 0.00026631611f * (float)raw->gyro_raw[2];
+
+		outputBand = 0;
+		startpointFullControll = 150.0f;
+		maxThrustSetpoints = 511.0f;
+		//Kp=60;
+		//Kp=40.0f;
+		//Kp=45;
+		Kp = 30.0f;
+//		Ki=0.0f;
+//		antiwindup=50.0f;
+	}
+
+	/* Get setpoint */
+
+
+	//Rate Controller
+	setpointRateCast[0] = -((float)setpoints->motor_right_ne - 9999.0f) * 0.01f / 180.0f * 3.141f;
+	setpointRateCast[1] = -((float)setpoints->motor_front_nw - 9999.0f) * 0.01f / 180.0f * 3.141f;
+	setpointRateCast[2] = 0; //-((float)setpoints->motor_back_se-9999.0f)*0.01f;
+	//Ki=actuatorDesired.motorRight_NE*0.001f;
+	setpointThrustCast = setpoints->motor_left_sw;
+
+	attRatesScaled[0] = 0.000317603994f * (float)raw->gyro_raw[0];
+	attRatesScaled[1] = 0.000317603994f * (float)raw->gyro_raw[1];
+	attRatesScaled[2] = 0.000317603994f * (float)raw->gyro_raw[2];
+
+	//filtering of the gyroscope values
+
+	//compute filter coefficient alpha
+
+	//gyro_alpha=0.005/(2.0f*3.1415f*200.0f+0.005f);
+	//gyro_alpha=0.009;
+	gyro_alpha = 0.09f;
+	gyro_alpha_offset = 0.001f;
+	//gyro_alpha=0.001;
+	//offset estimation and filtering
+	offsetCnt++;
+	uint8_t i;
+
+	for (i = 0; i < 3; i++) {
+		if (offsetCnt < 5000) {
+			gyro_filtered_offset[i] = attRatesScaled[i] * gyro_alpha_offset +  gyro_filtered_offset[i] * (1 - gyro_alpha_offset);
+		}
+
+		gyro_filtered[i] = 1.0f * ((attRatesScaled[i] - gyro_filtered_offset[i]) * gyro_alpha + gyro_filtered[i] * (1 - gyro_alpha)) - 0 * setpointRateCast[i];
+	}
+
+	// //START DEBUG
+	// /* write filtered values to global_data_attitude */
+	// global_data_attitude->rollspeed = gyro_filtered[0];
+	// global_data_attitude->pitchspeed = gyro_filtered[1];
+	// global_data_attitude->yawspeed = gyro_filtered[2];
+	// //END DEBUG
+
+	//rate controller
+
+	//X-axis
+	setpointXrate = -Kp * (setpointRateCast[0] - gyro_filtered[0]);
+	//Y-axis
+	setpointYrate = -Kp * (setpointRateCast[1] - gyro_filtered[1]);
+	//Z-axis
+	setpointZrate = -Kp * (setpointRateCast[2] - gyro_filtered[2]);
+
+
+
+
+	//Mixing
+	if (setpointThrustCast <= 0) {
+		setpointThrustCast = 0;
+		outputBand = 0;
+	}
+
+	if ((setpointThrustCast < startpointFullControll) && (setpointThrustCast > 0)) {
+		outputBand = 0.75f * setpointThrustCast;
+	}
+
+	if ((setpointThrustCast >= startpointFullControll) && (setpointThrustCast < maxThrustSetpoints - 0.75f * startpointFullControll)) {
+		outputBand = 0.75f * startpointFullControll;
+	}
+
+	if (setpointThrustCast >= maxThrustSetpoints - 0.75f * startpointFullControll) {
+		setpointThrustCast = 0.75f * startpointFullControll;
+		outputBand = 0.75f * startpointFullControll;
+	}
+
+	actuators_desired.motor_front_nw = setpointThrustCast + (setpointXrate + setpointYrate + setpointZrate);
+	actuators_desired.motor_right_ne = setpointThrustCast + (-setpointXrate + setpointYrate - setpointZrate);
+	actuators_desired.motor_back_se = setpointThrustCast + (-setpointXrate - setpointYrate + setpointZrate);
+	actuators_desired.motor_left_sw = setpointThrustCast + (setpointXrate - setpointYrate - setpointZrate);
+
+
+	if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_front_nw = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_front_nw = setpointThrustCast - outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_right_ne = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_right_ne = setpointThrustCast - outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_back_se = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_back_se = setpointThrustCast - outputBand;
+	}
+
+	if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_left_sw = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_left_sw = setpointThrustCast - outputBand;
+	}
+
+	//printf("%lu,%lu,%lu,%lu\n",actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
+
+	if (motor_skip_counter % 5 == 0) {
+		uint8_t motorSpeedBuf[5];
+		ar_get_motor_packet(motorSpeedBuf, actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
+//		uint8_t* motorSpeedBuf = ar_get_motor_packet(1, 1, 1, 1);
+//			if(motor_skip_counter %50 == 0)
+//			{
+//				if(0==actuators_desired.motor_front_nw || 0 == actuators_desired.motor_right_ne || 0 == actuators_desired.motor_back_se || 0 == actuators_desired.motor_left_sw)
+//					printf("Motors set: %u, %u, %u, %u\n", actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
+//			printf("input: %u\n", setpoints->motor_front_nw);
+//			printf("Roll casted desired: %f, Pitch casted desired: %f, Yaw casted desired: %f\n", setpointRateCast[0], setpointRateCast[1], setpointRateCast[2]);
+//			}
+		write(ardrone_write, motorSpeedBuf, 5);
+//			motor_skip_counter = 0;
+	}
+
+	motor_skip_counter++;
+
+	//START DEBUG
+//	global_data_lock(&global_data_ardrone_control->access_conf);
+//	global_data_ardrone_control->timestamp = hrt_absolute_time();
+//	global_data_ardrone_control->gyro_scaled[0] = attRatesScaled[0];
+//	global_data_ardrone_control->gyro_scaled[1] = attRatesScaled[1];
+//	global_data_ardrone_control->gyro_scaled[2] = attRatesScaled[2];
+//	global_data_ardrone_control->gyro_filtered[0] = gyro_filtered[0];
+//	global_data_ardrone_control->gyro_filtered[1] = gyro_filtered[1];
+//	global_data_ardrone_control->gyro_filtered[2] = gyro_filtered[2];
+//	global_data_ardrone_control->gyro_filtered_offset[0] = gyro_filtered_offset[0];
+//	global_data_ardrone_control->gyro_filtered_offset[1] = gyro_filtered_offset[1];
+//	global_data_ardrone_control->gyro_filtered_offset[2] = gyro_filtered_offset[2];
+//	global_data_ardrone_control->setpoint_rate_cast[0] = setpointRateCast[0];
+//	global_data_ardrone_control->setpoint_rate_cast[1] = setpointRateCast[1];
+//	global_data_ardrone_control->setpoint_rate_cast[2] = setpointRateCast[2];
+//	global_data_ardrone_control->setpoint_thrust_cast = setpointThrustCast;
+//	global_data_ardrone_control->setpoint_rate[0] = setpointXrate;
+//	global_data_ardrone_control->setpoint_rate[1] = setpointYrate;
+//	global_data_ardrone_control->setpoint_rate[2] = setpointZrate;
+//	global_data_ardrone_control->motor_front_nw = actuators_desired.motor_front_nw;
+//	global_data_ardrone_control->motor_right_ne = actuators_desired.motor_right_ne;
+//	global_data_ardrone_control->motor_back_se = actuators_desired.motor_back_se;
+//	global_data_ardrone_control->motor_left_sw = actuators_desired.motor_left_sw;
+//	global_data_unlock(&global_data_ardrone_control->access_conf);
+//	global_data_broadcast(&global_data_ardrone_control->access_conf);
+	//END DEBUG
+
+
+
+//	gettimeofday(&tv, NULL);
+//	now = ((uint32_t)tv.tv_sec) * 1000 + tv.tv_usec/1000;
+//	time_elapsed = now - last_run;
+//	if (time_elapsed*1000 > CONTROL_LOOP_USLEEP)
+//	{
+//		sleep_time = (int32_t)CONTROL_LOOP_USLEEP - ((int32_t)time_elapsed*1000 - (int32_t)CONTROL_LOOP_USLEEP);
+//
+//		if(motor_skip_counter %500 == 0)
+//		{
+//			printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run);
+//		}
+//	}
+//
+//	if (sleep_time <= 0)
+//	{
+//		printf("WARNING: CPU Overload!\n");
+//		printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run);
+//		usleep(CONTROL_LOOP_USLEEP);
+//	}
+//	else
+//	{
+//		usleep(sleep_time);
+//	}
+//	last_run = now;
+//
+//	now_time = hrt_absolute_time();
+//	if(control_counter % 500 == 0)
+//	{
+//		printf("Now: %lu\n",(unsigned long)now_time);
+//		printf("Last: %lu\n",(unsigned long)last_time);
+//		printf("Difference: %lu\n", (unsigned long)(now_time - last_time));
+//		printf("now seconds: %lu\n", (unsigned long)(now_time / 1000000));
+//	}
+//	last_time = now_time;
+//
+//	now_time = hrt_absolute_time() / 1000000;
+//	if(now_time - last_time > 0)
+//	{
+//		printf("Counter: %ld\n",control_counter);
+//		last_time = now_time;
+//		control_counter = 0;
+//	}
+//	control_counter++;
+}
diff --git a/apps/multirotor_position_control/rate_control.h b/apps/multirotor_position_control/rate_control.h
new file mode 100644
index 000000000..a69745d9b
--- /dev/null
+++ b/apps/multirotor_position_control/rate_control.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <nagelit@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 Definition of attitude rate control
+ */
+#ifndef RATE_CONTROL_H_
+#define RATE_CONTROL_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/sensor_combined.h>
+
+void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints);
+
+#endif /* RATE_CONTROL_H_ */
diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h
index 6a1908007..65ba98e9d 100644
--- a/apps/uORB/topics/vehicle_attitude_setpoint.h
+++ b/apps/uORB/topics/vehicle_attitude_setpoint.h
@@ -42,7 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include <uORB.h>
+#include "../uORB.h"
 
 /**
  * @addtogroup topics
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 933b58e8b..9f72c8f78 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -110,7 +110,8 @@ struct vehicle_status_s
 	bool preflight_gyro_calibration;		/**< true if gyro calibration is requested */
 	bool preflight_mag_calibration;			/**< true if mag calibration is requested */
 
-	bool rc_signal_lost;				/**< true if no operator override channel is available */
+	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
+	bool rc_signal_cutting_off;			/**< true if RC reception is weak / cutting off */
 	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
 
 	/* see SYS_STATUS mavlink message for the following */