Added deamon example, reworked / merged multirotor attitude control. Ready for AR.Drone interface changes and integration tests

22 files changed, 399 insertions, 565 deletions

diff --git a/apps/ardrone_control/Makefile b/apps/ardrone_control/Makefile
index a6d471567..35fbe01d7 100644
--- a/apps/ardrone_control/Makefile
+++ b/apps/ardrone_control/Makefile
@@ -40,6 +40,6 @@ PRIORITY	 = SCHED_PRIORITY_MAX - 15
 STACKSIZE	 = 4096
 
 # 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
+CSRCS		 = ardrone_control.c ardrone_motor_control.c rate_control.c attitude_control.c pid.c
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index bc5598f9c..72e8ae14e 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -62,6 +62,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/ardrone_motors_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/actuator_controls.h>
 
 __EXPORT int ardrone_control_main(int argc, char *argv[]);
 
@@ -181,6 +182,8 @@ int ardrone_control_main(int argc, char *argv[])
 	memset(&raw, 0, sizeof(raw));
 	struct ardrone_motors_setpoint_s setpoint;
 	memset(&setpoint, 0, sizeof(setpoint));
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -251,9 +254,8 @@ int ardrone_control_main(int argc, char *argv[])
 
 				float roll_control, pitch_control, yaw_control, thrust_control;
 
-				multirotor_control_attitude(&att_sp, &att, &state, motor_test_mode,
-					&roll_control, &pitch_control, &yaw_control, &thrust_control);
-				ardrone_mixing_and_output(ardrone_write, roll_control, pitch_control, yaw_control, thrust_control, motor_test_mode);
+				multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
+				ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
 
 			} else {
 				/* invalid mode, complain */
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index d31e36e33..77e08a535 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -56,8 +56,9 @@
 
 #define MAX_MOTOR_COUNT 16
 
-void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
-	bool verbose, float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output)
+void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
+	const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
+	struct actuator_controls_s *actuators, bool verbose)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -218,13 +219,18 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		roll_controller.saturated = 1;
 	}
 
-	*roll_output = roll_control;
-	*pitch_output = pitch_control;
-	*yaw_output = yaw_rate_control;
-	*thrust_output = motor_thrust;
+	actuators->control[0] = roll_control;
+	actuators->control[1] = pitch_control;
+	actuators->control[2] = yaw_rate_control;
+	actuators->control[3] = motor_thrust;
 }
 
-void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose) {
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) {
+
+	float roll_control = actuators->control[0];
+	float pitch_control = actuators->control[1];
+	float yaw_control = actuators->control[2];
+	float motor_thrust = actuators->control[3];
 
 	unsigned int motor_skip_counter = 0;
 
diff --git a/apps/ardrone_control/attitude_control.h b/apps/ardrone_control/attitude_control.h
index 56f52dabe..e14d29966 100644
--- a/apps/ardrone_control/attitude_control.h
+++ b/apps/ardrone_control/attitude_control.h
@@ -51,11 +51,11 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/ardrone_control.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-		const struct vehicle_status_s *status, bool verbose,
-		float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output);
+		const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose);
 
-void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose);
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose);
 
 #endif /* ATTITUDE_CONTROL_H_ */
diff --git a/apps/ardrone_control/rate_control.c b/apps/ardrone_control/rate_control.c
index df20cd1ab..6c6b004e4 100644
--- a/apps/ardrone_control/rate_control.c
+++ b/apps/ardrone_control/rate_control.c
@@ -39,7 +39,6 @@
  */
 
 #include "rate_control.h"
-#include "ardrone_control_helper.h"
 #include "ardrone_motor_control.h"
 #include <arch/board/up_hrt.h>
 
diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c
index e17bf478a..91e60a8f9 100644
--- a/apps/examples/px4_deamon_app/px4_deamon_app.c
+++ b/apps/examples/px4_deamon_app/px4_deamon_app.c
@@ -53,20 +53,20 @@ __EXPORT int px4_deamon_app_main(int argc, char *argv[]);
 /**
  * Mainloop of deamon.
  */
-int mavlink_thread_main(int argc, char *argv[]);
+int px4_deamon_thread_main(int argc, char *argv[]);
 
 /**
  * Print the correct usage.
  */
 static void usage(const char *reason);
 
-int mavlink_thread_main(int argc, char *argv[]) {
+int px4_deamon_thread_main(int argc, char *argv[]) {
 
 	printf("[deamon] starting\n");
 
 	while (true) {
 		printf("Hello Deamon!\n");
-		sleep(1);
+		sleep(10);
 		if (thread_should_exit) break;
 	}
 
@@ -106,7 +106,7 @@ int px4_deamon_app_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		mavlink_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		deamon_task = task_create("deamon", SCHED_PRIORITY_DEFAULT, 4096, px4_deamon_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		thread_running = true;
 		exit(0);
 	}
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index b6fdc1a3e..47eab6be8 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -130,6 +130,7 @@ static int pub_hil_global_pos = -1;
 static int ardrone_motors_pub = -1;
 static int cmd_pub = -1;
 static int sensor_sub = -1;
+static int att_sub = -1;
 static int global_pos_sub = -1;
 static int local_pos_sub = -1;
 static int flow_pub = -1;
@@ -412,31 +413,40 @@ void get_mavlink_mode_and_state(const struct vehicle_status_s *c_status, uint8_t
  */
 static void *receiveloop(void *arg)
 {
+	int uart_fd = *((int*)arg);
+
+	const int timeout = 1000;
 	uint8_t ch;
 
 	mavlink_message_t msg;
 
 	prctl(PR_SET_NAME, "mavlink uart rcv", getpid());
 
-	while (1) {
+	while (!thread_should_exit) {
 
 		if (mavlink_exit_requested) break;
 
-		/* blocking read on next byte */
-		int nread = read(uart, &ch, 1);
+		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
 
+		if (poll(fds, 1, timeout) > 0) {
+			/* non-blocking read until buffer is empty */
+			int nread = 0;
 
-		if (nread > 0 && mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
-			/* handle generic messages and commands */
-			handleMessage(&msg);
+			do {
+				nread = read(uart_fd, &ch, 1);
 
-			/* Handle packet with waypoint component */
-			mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
+				if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char
+					/* handle generic messages and commands */
+					handleMessage(&msg);
 
-			/* Handle packet with parameter component */
-			mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
-		}
+					/* Handle packet with waypoint component */
+					mavlink_wpm_message_handler(&msg, &global_pos, &local_pos);
 
+					/* Handle packet with parameter component */
+					mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
+				}
+			} while (nread > 0);
+		}
 	}
 
 	return NULL;
@@ -455,6 +465,10 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval)
 			/* senser sub triggers RAW IMU */
 			orb_set_interval(sensor_sub, min_interval);
 			break;
+		case MAVLINK_MSG_ID_ATTITUDE:
+			/* attitude sub triggers attitude */
+			orb_set_interval(att_sub, 100);
+			break;
 		default:
 			/* not found */
 			ret = ERROR;
@@ -499,15 +513,13 @@ static void *uorb_receiveloop(void *arg)
 	/* --- SENSORS RAW VALUE --- */
 	/* subscribe to ORB for sensors raw */
 	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	orb_set_interval(sensor_sub, 100);	/* 10Hz updates */
 	fds[fdsc_count].fd = sensor_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
 	/* --- ATTITUDE VALUE --- */
 	/* subscribe to ORB for attitude */
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	orb_set_interval(att_sub, 100);		/* 10Hz updates */
+	att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	fds[fdsc_count].fd = att_sub;
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
@@ -611,9 +623,9 @@ static void *uorb_receiveloop(void *arg)
 	 * set up poll to block for new data,
 	 * wait for a maximum of 1000 ms (1 second)
 	 */
-	const int timeout = 5000;
+	const int timeout = 1000;
 
-	while (1) {
+	while (!thread_should_exit) {
 		if (mavlink_exit_requested) break;
 
 		int poll_ret = poll(fds, fdsc_count, timeout);
@@ -1271,7 +1283,7 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	if (uart < 0) {
 		printf("[mavlink] FAILED to open %s, terminating.\n", uart_name);
-		return -1;
+		return ERROR;
 	}
 
 	/* Flush UART */
@@ -1289,7 +1301,7 @@ int mavlink_thread_main(int argc, char *argv[])
 	pthread_attr_t receiveloop_attr;
 	pthread_attr_init(&receiveloop_attr);
 	pthread_attr_setstacksize(&receiveloop_attr, 2048);
-	pthread_create(&receive_thread, &receiveloop_attr, receiveloop, NULL);
+	pthread_create(&receive_thread, &receiveloop_attr, receiveloop, &uart);
 
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
@@ -1305,23 +1317,28 @@ int mavlink_thread_main(int argc, char *argv[])
 
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 921600) {
-		/* 1000 Hz / 1 ms */
-		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1);
+		/* set no limit */
+		/* 500 Hz / 2 ms */
+		//set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2);
 	} else if (baudrate >= 460800) {
 		/* 250 Hz / 4 ms */
 		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 4);
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 4);
 	} else if (baudrate >= 115200) {
 		/* 50 Hz / 20 ms */
 		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 20);
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 20);
 	} else if (baudrate >= 57600) {
 		/* 10 Hz / 100 ms */
 		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100);
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100);
 	} else {
 		/* very low baud rate, limit to 1 Hz / 1000 ms */
 		set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000);
+		set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000);
 	}
 
-	while (1) {
+	while (!thread_should_exit) {
 
 		if (mavlink_exit_requested) break;
 
diff --git a/apps/multirotor_control/Makefile b/apps/multirotor_att_control/Makefile
index 7ae1d472a..03cf33e43 100644
--- a/apps/multirotor_control/Makefile
+++ b/apps/multirotor_att_control/Makefile
@@ -35,11 +35,8 @@
 # Makefile to build uORB
 #
 
-APPNAME		 = multirotor_control
+APPNAME		 = multirotor_att_control
 PRIORITY	 = SCHED_PRIORITY_MAX - 15
 STACKSIZE	 = 2048
 
-# explicit list of sources - not everything is built currently
-CSRCS		 = multirotor_control.c multirotor_attitude_control.c pid.c
-
 include $(APPDIR)/mk/app.mk
diff --git a/apps/multirotor_control/multirotor_control.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 2edc1ba84..baee507b2 100644
--- a/apps/multirotor_control/multirotor_control.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,9 +33,9 @@
  ****************************************************************************/
 
 /*
- * @file multirotor_control.c
+ * @file multirotor_att_control_main.c
  *
- * Implementation of multirotor controllers
+ * Implementation of multirotor attitude control main loop.
  */
 
 #include <nuttx/config.h>
@@ -49,20 +49,25 @@
 #include <debug.h>
 #include <getopt.h>
 #include <time.h>
+#include <math.h>
 #include <poll.h>
 #include <sys/prctl.h>
 #include <arch/board/up_hrt.h>
-#include "multirotor_control.h"
-#include "multirotor_attitude_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 <uORB/topics/actuator_controls.h>
-#include <uORB/topics/rc_channels.h>
 
 #include <systemlib/perf_counter.h>
 
-__EXPORT int multirotor_control_main(int argc, char *argv[]);
+#include "multirotor_attitude_control.h"
+
+__EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
 
 static enum {
@@ -77,17 +82,35 @@ static int mc_task;
 static int
 mc_thread_main(int argc, char *argv[])
 {
+	bool motor_test_mode = false;
+
 	/* structures */
+	/* declare and safely initialize all structs */
 	struct vehicle_status_s state;
+	memset(&state, 0, sizeof(state));
 	struct vehicle_attitude_s att;
-	struct rc_channels_s rc;
+	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));
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));
+
 	struct actuator_controls_s actuators;
 	struct actuator_armed_s armed;
 
 	/* 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 rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	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));
 
 	/* rate-limit the attitude subscription to 200Hz to pace our loop */
 	orb_set_interval(att_sub, 5);
@@ -101,29 +124,55 @@ mc_thread_main(int argc, char *argv[])
 	int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
 	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_control");
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
 
 	/* welcome user */
-	printf("[multirotor_control] starting\n");
+	printf("[multirotor_att_control] starting\n");
 
 	while (!thread_should_exit) {
 
-		/* wait for a sensor update */
-		poll(&fds, 1, -1);
+		/* wait for a sensor update, check for exit condition every 500 ms */
+		poll(&fds, 1, 500);
 
 		perf_begin(mc_loop_perf);
 
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-
-		/* get a local copy of rc inputs */
-		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
-
+		/* 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;
+		if (motor_test_mode) {
+			att_sp.roll_body = 0.0f;
+			att_sp.pitch_body = 0.0f;
+			att_sp.yaw_body = 0.0f;
+			att_sp.thrust = 0.3f;
+		} else {
+			if (state.state_machine == SYSTEM_STATE_MANUAL ||
+				state.state_machine == SYSTEM_STATE_GROUND_READY ||
+				state.state_machine == SYSTEM_STATE_STABILIZED ||
+				state.state_machine == SYSTEM_STATE_AUTO ||
+				state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
+				state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
+				att_sp.thrust = manual.throttle;
+
+			} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
+				/* immediately cut off motors */
+				att_sp.thrust = 0.0f;
+
+			} else {
+				/* limit motor throttle to zero for an unknown mode */
+				att_sp.thrust = 0.0f;
+			}
+			
+		}
 
-		/* run the attitude controller */
-		multirotor_control_attitude(&rc, &att, &state, &actuators);
+		multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode);
+		//ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
 
 		/* publish the result */
 		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
@@ -131,7 +180,7 @@ mc_thread_main(int argc, char *argv[])
 		perf_end(mc_loop_perf);
 	}
 
-	printf("[multirotor_control] stopping\r\n");
+	printf("[multirotor att control] stopping.\n");
 
 	/* kill all outputs */
 	armed.armed = false;
@@ -143,7 +192,7 @@ mc_thread_main(int argc, char *argv[])
 
 	close(att_sub);
 	close(state_sub);
-	close(rc_sub);
+	close(manual_sub);
 	close(actuator_pub);
 	close(armed_pub);
 
@@ -159,16 +208,17 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: multirotor_control [-m <mode>] {start|stop}\n");
+	fprintf(stderr, "usage: multirotor_att_control [-m <mode>] {start|stop}\n");
 	fprintf(stderr, "    <mode> is 'rates' or 'attitude'\n");
 	exit(1);
 }
 
-int multirotor_control_main(int argc, char *argv[])
+int multirotor_att_control_main(int argc, char *argv[])
 {
 	int	ch;
 
 	control_mode = CONTROL_MODE_RATES;
+	unsigned int optioncount = 0;
 
 	while ((ch = getopt(argc, argv, "m:")) != EOF) {
 		switch (ch) {
@@ -180,12 +230,13 @@ int multirotor_control_main(int argc, char *argv[])
 			} else {
 				usage("unrecognized -m value");
 			}
+			optioncount += 2;
 		default:
 			usage("unrecognized option");
 		}
 	}
-	argc -= optind;
-	argv += optind;
+	argc -= optioncount;
+	argv += optioncount;
 
 	if (argc < 1)
 		usage("missing command");
@@ -193,7 +244,7 @@ int multirotor_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		thread_should_exit = false;
-		mc_task = task_create("multirotor_attitude", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
+		mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL);
 		exit(0);
 	}
 
@@ -202,6 +253,6 @@ int multirotor_control_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	usage("unrecognised command");
+	usage("unrecognized command");
 	exit(1);
 }
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
new file mode 100644
index 000000000..367a0c3e1
--- /dev/null
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           @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 multirotor_attitude_control.c
+ * Implementation of attitude controller
+ */
+
+#include "multirotor_attitude_control.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <float.h>
+#include <math.h>
+#include <systemlib/pid/pid.h>
+#include <arch/board/up_hrt.h>
+
+void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
+	const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
+	struct actuator_controls_s *actuators, bool verbose)
+{
+	static uint64_t last_run = 0;
+	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+	last_run = hrt_absolute_time();
+
+	static int motor_skip_counter = 0;
+
+	static PID_t yaw_pos_controller;
+	static PID_t yaw_speed_controller;
+	static PID_t pitch_controller;
+	static PID_t roll_controller;
+
+	static float pid_yawpos_lim;
+	static float pid_yawspeed_lim;
+	static float pid_att_lim;
+
+	static bool initialized = false;
+
+	/* 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(&pitch_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 =	global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim =	global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+
+		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(&pitch_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 = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+	}
+
+	/*Calculate Controllers*/
+	//control Nick
+	float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET],
+					att->pitch, att->pitchspeed, deltaT);
+	//control Roll
+	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, deltaT);
+	//control Yaw Speed
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); 	//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);
+	}
+
+	float motor_thrust = 0.0f;
+
+	// FLYING MODES
+	motor_thrust = att_sp->thrust;
+
+	//printf("mot0: %3.1f\n", motor_thrust);
+
+	/* compensate thrust vector for roll / pitch contributions */
+	motor_thrust *= zcompensation;
+
+	/* 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 (pitch_control > pid_att_lim) {
+		pitch_control = pid_att_lim;
+		pitch_controller.saturated = 1;
+	}
+
+	if (pitch_control < -pid_att_lim) {
+		pitch_control = -pid_att_lim;
+		pitch_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;
+	}
+
+	actuators->control[0] = roll_control;
+	actuators->control[1] = pitch_control;
+	actuators->control[2] = yaw_rate_control;
+	actuators->control[3] = motor_thrust;
+}
diff --git a/apps/multirotor_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index c3aa47041..c3caa7598 100644
--- a/apps/multirotor_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
@@ -1,9 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Laurens Mackay <mackayl@student.ethz.ch>
+ *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+ *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           @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
@@ -34,22 +37,25 @@
  *
  ****************************************************************************/
 
-/**
+/*
  * @file multirotor_attitude_control.h
- *
- * attitude control for multirotors 
+ * Attitude control for multi rotors.
  */
 
-#ifndef ATTITUDE_CONTROL_H_
-#define ATTITUDE_CONTROL_H_
+#ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
+#define MULTIROTOR_ATTITUDE_CONTROL_H_
 
 #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>
 #include <uORB/topics/actuator_controls.h>
 
-void multirotor_control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att,
-				 const struct vehicle_status_s *status, struct actuator_controls_s *actuators);
+void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+		const struct vehicle_status_s *status, struct actuator_controls_s *actuators, bool verbose);
+
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose);
 
-#endif /* ATTITUDE_CONTROL_H_ */
+#endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */
diff --git a/apps/multirotor_control/multirotor_attitude_control.c b/apps/multirotor_control/multirotor_attitude_control.c
deleted file mode 100644
index 5d298ab54..000000000
--- a/apps/multirotor_control/multirotor_attitude_control.c
+++ /dev/null
@@ -1,456 +0,0 @@
-/****************************************************************************
- *
- *   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 multirotor_attitude_control.c
- *
- * Implementation of multirotor attitude controller.
- */
-
-#include "multirotor_attitude_control.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <float.h>
-#include <math.h>
-#include <arch/board/up_hrt.h>
-#include <systemlib/pid/pid.h>
-
-extern int multirotor_write;
-extern int gpios;
-
-#define CONTROL_PID_ATTITUDE_INTERVAL	5e-3
-
-// 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 multirotor_control_attitude(const struct rc_channels_s *rc, 
-				 const struct vehicle_attitude_s *att,
-				 const struct vehicle_status_s *status,
-				 struct actuator_controls_s *actuators)
-{
-	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;
-
-	// XXXM
-	static const float min_gas = 1;
-	static const float max_gas = 512;
-
-
-	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 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) {
-
-		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,
-			 (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],
-			 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],
-			 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],
-			 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;
-	}
-
-	/* load new parameters with lower rate */
-	if (motor_skip_counter % 50 == 0) {
-		pid_set_parameters(&yaw_pos_controller,
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
-				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
-				   (max_gas - min_gas) * 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]);
-
-		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]);
-
-		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]);
-
-		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 = {0.0f, 0.0f, 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) {
-		// 	yaw_e -= 2.0f * M_PI;
-		// }
-
-		// if (yaw_e < -M_PI) {
-		// 	yaw_e += 2.0f * M_PI;
-		// }
-
-		// attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, 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 = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f;
-		attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f;
-		attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265;
-	}
-
-	/* 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 = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
-	//control Roll
-	float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
-	//control Yaw Speed
-	float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
-
-	//compensation to keep force in z-direction
-	float zcompensation;
-
-	if (fabs(att->roll) > 0.5f) {
-		zcompensation = 1.13949393f;
-
-	} else {
-		zcompensation = 1.0f / cosf(att->roll);
-	}
-
-	if (fabs(att->pitch) > 0.5f) {
-		zcompensation *= 1.13949393f;
-
-	} 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 = (float)rc->chan[rc->function[THROTTLE]].scale;
-
-	} 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 = (float)rc->chan[rc->function[THROTTLE]].scale;	//TODO
-
-	} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
-		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;	//TODO
-
-	} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
-		motor_thrust = 0.0f;	//immediately cut off thrust!
-
-	} else {
-		motor_thrust = 0.0f;	// Motor thrust must be zero in any other mode!
-	}
-
-	// Convertion to motor-step units
-	motor_thrust *= zcompensation;
-	motor_thrust *= max_gas / 20000.0f; //TODO: check this
-	motor_thrust += (max_gas - min_gas) / 2.f;
-
-	// XXXM
-	// RC channels: assumed to be -10000 to +10000
-
-
-
-	//limit control output
-	//yawspeed
-	if (yaw > pid_yawspeed_lim) {
-		yaw = pid_yawspeed_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (yaw < -pid_yawspeed_lim) {
-		yaw = -pid_yawspeed_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (nick > pid_att_lim) {
-		nick = pid_att_lim;
-		nick_controller.saturated = 1;
-	}
-
-	if (nick < -pid_att_lim) {
-		nick = -pid_att_lim;
-		nick_controller.saturated = 1;
-	}
-
-
-	if (roll > pid_att_lim) {
-		roll = pid_att_lim;
-		roll_controller.saturated = 1;
-	}
-
-	if (roll < -pid_att_lim) {
-		roll = -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;
-	// ar_control->attitude_control_output[1] = nick;
-	// ar_control->attitude_control_output[2] = yaw;
-	// ar_control->zcompensation = zcompensation;
-	// orb_publish(ORB_ID(multirotor_control), multirotor_pub, ar_control);
-
-	static float output_band = 0.f;
-	static float band_factor = 0.75f;
-	static float startpoint_full_control = 150.0f;		//TODO
-	static float yaw_factor = 1.0f;
-
-
-
-
-
-
-	// MIXING AND THRUST LIMITING START
-
-
-
-	if (motor_thrust <= min_gas) {
-		motor_thrust = min_gas;
-		output_band = 0.f;
-
-	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) {
-		output_band = band_factor * (motor_thrust - min_gas);
-
-	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) {
-		output_band = band_factor * startpoint_full_control;
-
-	} else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) {
-		output_band = band_factor * (max_gas - 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 / 2 + nick / 2 - yaw);
-
-	// RIGHT (MOTOR 2)
-	motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw);
-
-	// BACK (MOTOR 3)
-	motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw);
-
-	// LEFT (MOTOR 4)
-	motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw);
-
-	// 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 / 2 + nick / 2 - yaw * yaw_factor);
-
-		// RIGHT (MOTOR 2)
-		motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor);
-
-		// BACK (MOTOR 3)
-		motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor);
-
-		// LEFT (MOTOR 4)
-		motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor);
-	}
-
-	uint8_t i;
-
-	for (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;
-		}
-	}
-
-	// Write out actual thrust
-
-	// XXXM
-	motor_pwm[0] = (uint16_t) motor_calc[0];
-	motor_pwm[1] = (uint16_t) motor_calc[1];
-	motor_pwm[2] = (uint16_t) motor_calc[2];
-	motor_pwm[3] = (uint16_t) motor_calc[3];
-
-	//SEND MOTOR COMMANDS
-	
-	// XXXM 
-
-	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/multirotor_control/multirotor_control.h b/apps/multirotor_control/multirotor_control.h
deleted file mode 100644
index cd779d369..000000000
--- a/apps/multirotor_control/multirotor_control.h
+++ /dev/null
@@ -1,12 +0,0 @@
-/**
- * @file multirotor_control.h
- *
- *  Created on: Mar 23, 2012
- *      Author: thomasgubler
- */
-
-#ifndef multirotor_CONTROL_H_
-#define multirotor_CONTROL_H_
-
-
-#endif /* multirotor_CONTROL_H_ */
diff --git a/apps/multirotor_position_control/Makefile b/apps/multirotor_pos_control/Makefile
index f95a4b50a..c88c85435 100644
--- a/apps/multirotor_position_control/Makefile
+++ b/apps/multirotor_pos_control/Makefile
@@ -35,7 +35,7 @@
 # Makefile to build multirotor position control
 #
 
-APPNAME		 = multirotor_position_control
+APPNAME		 = multirotor_pos_control
 PRIORITY	 = SCHED_PRIORITY_MAX - 25
 STACKSIZE	 = 2048
 
diff --git a/apps/multirotor_position_control/multirotor_position_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c
index 52930fd83..ff32fb460 100644
--- a/apps/multirotor_position_control/multirotor_position_control.c
+++ b/apps/multirotor_pos_control/multirotor_pos_control.c
@@ -59,7 +59,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
 
-__EXPORT int multirotor_position_control_main(int argc, char *argv[]);
+__EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
 
 static bool thread_should_exit;
 static bool thread_running = false;
@@ -69,7 +69,7 @@ static int
 mpc_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
-	printf("[multirotor pos ctrl] Control started, taking over position control\n");
+	printf("[multirotor pos control] Control started, taking over position control\n");
 
 	/* structures */
 	struct vehicle_status_s state;
@@ -129,7 +129,7 @@ mpc_thread_main(int argc, char *argv[])
 	close(ardrone_write);
 	ar_multiplexing_deinit(gpios);
 
-	printf("[ardrone_control] ending now...\r\n");
+	printf("[multirotor pos control] ending now...\r\n");
 	fflush(stdout);
 	return 0;
 }
diff --git a/apps/multirotor_position_control/position_control.c b/apps/multirotor_pos_control/position_control.c
index fd731a33d..fd731a33d 100644
--- a/apps/multirotor_position_control/position_control.c
+++ b/apps/multirotor_pos_control/position_control.c
diff --git a/apps/multirotor_position_control/position_control.h b/apps/multirotor_pos_control/position_control.h
index 5ba59362e..5ba59362e 100644
--- a/apps/multirotor_position_control/position_control.h
+++ b/apps/multirotor_pos_control/position_control.h
diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c
index 77b7b9c47..6a06a786f 100644
--- a/apps/sensors/sensors.c
+++ b/apps/sensors/sensors.c
@@ -433,7 +433,7 @@ int sensors_thread_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	while (1) {
+	while (!thread_should_exit) {
 		pthread_mutex_lock(&sensors_read_ready_mutex);
 
 		struct timespec time_to_wait = {0, 0};
diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile
index 172eef979..e1a6db598 100644
--- a/apps/systemlib/Makefile
+++ b/apps/systemlib/Makefile
@@ -43,7 +43,8 @@ CSRCS		 = err.c \
 # XXX this really should be a CONFIG_* test
 #
 ifeq ($(TARGET),px4fmu)
-CSRCS		+= systemlib.c
+CSRCS		+= systemlib.c \
+		   pid/pid.c
 endif
 
 include $(APPDIR)/mk/app.mk
diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c
index a24657a41..807373c15 100644
--- a/apps/systemlib/pid/pid.c
+++ b/apps/systemlib/pid/pid.c
@@ -41,7 +41,7 @@
 
 #include "pid.h"
 
-void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	      uint8_t mode, uint8_t plot_i)
 {
 	pid->kp = kp;
@@ -57,7 +57,7 @@ void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->error_previous = 0;
 	pid->integral = 0;
 }
-void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+__EXPORT void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -84,7 +84,7 @@ void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
  * @param dt
  * @return
  */
-float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
 {
 	/*  error = setpoint - actual_position
 	 integral = integral + (error*dt)
diff --git a/apps/systemlib/pid/pid.h b/apps/systemlib/pid/pid.h
index d62843ed4..83bf09b59 100644
--- a/apps/systemlib/pid/pid.h
+++ b/apps/systemlib/pid/pid.h
@@ -64,10 +64,10 @@ typedef struct {
 	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);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
+__EXPORT 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);
+__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
 
 
 
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index 01b67cfef..da119e14f 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -71,7 +71,7 @@ CONFIGURED_APPS += commander
 #CONFIGURED_APPS += sdlog
 CONFIGURED_APPS += sensors
 CONFIGURED_APPS += ardrone_control
-CONFIGURED_APPS += multirotor_control
+CONFIGURED_APPS += multirotor_att_control
 CONFIGURED_APPS += px4/attitude_estimator_bm
 CONFIGURED_APPS += fixedwing_control
 CONFIGURED_APPS += mix_and_link