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

10 files changed, 569 insertions, 236 deletions

diff --git a/apps/examples/px4_deamon_app/px4_deamon_app.c b/apps/examples/px4_deamon_app/px4_deamon_app.c
index 6eb046d45..a5d847777 100644
--- a/apps/examples/px4_deamon_app/px4_deamon_app.c
+++ b/apps/examples/px4_deamon_app/px4_deamon_app.c
@@ -97,7 +97,6 @@ int px4_deamon_app_main(int argc, char *argv[])
 					 4096,
 					 px4_deamon_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
 		exit(0);
 	}
 
@@ -123,6 +122,8 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
 
 	printf("[deamon] starting\n");
 
+	thread_running = true;
+
 	while (!thread_should_exit) {
 		printf("Hello Deamon!\n");
 		sleep(10);
@@ -130,5 +131,7 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
 
 	printf("[deamon] exiting.\n");
 
+	thread_running = false;
+
 	return 0;
 }
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index f36fb009d..c13f462ea 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -68,6 +68,7 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -134,6 +135,8 @@ static struct vehicle_command_s vcmd;
 
 static struct actuator_armed_s armed;
 
+static struct vehicle_vicon_position_s vicon_position;
+
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -191,8 +194,10 @@ static struct mavlink_subscriptions {
 
 static struct mavlink_publications {
 	orb_advert_t offboard_control_sp_pub;
+	orb_advert_t vicon_position_pub;
 } mavlink_pubs = {
-	.offboard_control_sp_pub = -1
+	.offboard_control_sp_pub = -1,
+	.vicon_position_pub = -1
 };
 
 
@@ -1240,9 +1245,26 @@ void handleMessage(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (cmd_pub <= 0) {
 			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+		} else {
+			/* create command */
+			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+		}
+	}
+
+	/* Handle Vicon position estimates */
+	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
+		mavlink_vicon_position_estimate_t pos;
+		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
+
+		vicon_position.x = pos.x;
+		vicon_position.y = pos.y;
+		vicon_position.z = pos.z;
+
+		if (mavlink_pubs.vicon_position_pub <= 0) {
+			mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+		} else {
+			orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position);
 		}
-		/* create command */
-		orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 	}
 
 	/* Handle quadrotor motor setpoints */
diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c
index 3f9c72517..7cf687306 100644
--- a/apps/multirotor_pos_control/multirotor_pos_control.c
+++ b/apps/multirotor_pos_control/multirotor_pos_control.c
@@ -49,24 +49,94 @@
 #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/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <systemlib/systemlib.h>
+
+#include "multirotor_pos_control_params.h"
+
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
 
 __EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
 
-static bool thread_should_exit;
-static bool thread_running = false;
-static int mpc_task;
+/**
+ * Mainloop of position controller.
+ */
+static int multirotor_pos_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_spawn().
+ */
+int multirotor_pos_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("multirotor pos control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("multirotor pos control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 60,
+					 4096,
+					 multirotor_pos_control_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tmultirotor pos control app is running\n");
+		} else {
+			printf("\tmultirotor pos control app not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
 
 static int
-mpc_thread_main(int argc, char *argv[])
+multirotor_pos_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
 	printf("[multirotor pos control] Control started, taking over position control\n");
@@ -76,7 +146,7 @@ mpc_thread_main(int argc, char *argv[])
 	struct vehicle_attitude_s att;
 	//struct vehicle_global_position_setpoint_s global_pos_sp;
 	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_local_position_s local_pos;
+	struct vehicle_vicon_position_s local_pos;
 	struct manual_control_setpoint_s manual;
 	struct vehicle_attitude_setpoint_s att_sp;
 
@@ -84,13 +154,23 @@ mpc_thread_main(int argc, char *argv[])
 	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 local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
 	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 
 	/* publish attitude setpoint */
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
+	thread_running = true;
+
+	int loopcounter = 0;
+
+	struct multirotor_position_control_params p;
+	struct multirotor_position_control_param_handles h;
+	parameters_init(&h);
+	parameters_update(&h, &p);
+
+
 	while (1) {
 		/* get a local copy of the vehicle state */
 		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@@ -99,15 +179,34 @@ mpc_thread_main(int argc, char *argv[])
 		/* get a local copy of attitude */
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
 		/* get a local copy of local position setpoint */
 		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
 
-		if (state.state_machine == SYSTEM_STATE_AUTO) {
-			position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp);
+		if (loopcounter == 500) {
+			parameters_update(&h, &p);
+			loopcounter = 0;
+		}
+
+		// if (state.state_machine == SYSTEM_STATE_AUTO) {
+			
+			// XXX IMPLEMENT POSITION CONTROL HERE
+
+			float dT = 1.0f / 50.0f;
+
+			float x_setpoint = 0.0f;
+
+			/* local pos is the Vicon position */
+
+			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT;
+			att_sp.roll_body = 0.0f;
+			att_sp.yaw_body = 0.0f;
+			att_sp.thrust = 0.4f;
+			att_sp.timestamp = hrt_absolute_time();
+
 			/* publish new attitude setpoint */
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		} else if (state.state_machine == SYSTEM_STATE_STABILIZE) {
+		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
 			/* set setpoint to current position */
 			// XXX select pos reset channel on remote
 			/* reset setpoint to current position  (position hold) */
@@ -117,19 +216,18 @@ mpc_thread_main(int argc, char *argv[])
 			// 	local_pos_sp.z = local_pos.z;
 			// 	local_pos_sp.yaw = att.yaw;
 			// }
-		}
+		// }
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
+		loopcounter++;
 
-		counter++;
 	}
 
-	/* close uarts */
-	close(ardrone_write);
-	ar_multiplexing_deinit(gpios);
+	printf("[multirotor pos control] ending now...\n");
+
+	thread_running = false;
 
-	printf("[multirotor pos control] ending now...\r\n");
 	fflush(stdout);
 	return 0;
 }
diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/apps/multirotor_pos_control/multirotor_pos_control_params.c
new file mode 100644
index 000000000..6b73dc405
--- /dev/null
+++ b/apps/multirotor_pos_control/multirotor_pos_control_params.c
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file multirotor_position_control_params.c
+ * 
+ * Parameters for EKF filter
+ */
+
+#include "multirotor_pos_control_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+
+int parameters_init(struct multirotor_position_control_param_handles *h)
+{
+	/* PID parameters */
+	h->p 	=	param_find("MC_POS_P");
+
+	return OK;
+}
+
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
+{
+	param_get(h->p, &(p->p));
+
+	return OK;
+}
diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/apps/multirotor_pos_control/multirotor_pos_control_params.h
new file mode 100644
index 000000000..274f6c22a
--- /dev/null
+++ b/apps/multirotor_pos_control/multirotor_pos_control_params.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file multirotor_position_control_params.h
+ * 
+ * Parameters for position controller
+ */
+
+#include <systemlib/param/param.h>
+
+struct multirotor_position_control_params {
+	float p;
+	float i;
+	float d;
+};
+
+struct multirotor_position_control_param_handles {
+	param_t p;
+	param_t i;
+	param_t d;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct multirotor_position_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);
diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c
index b98f5bede..9c53a5bf6 100644
--- a/apps/multirotor_pos_control/position_control.c
+++ b/apps/multirotor_pos_control/position_control.c
@@ -1,235 +1,235 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file multirotor_position_control.c
- * Implementation of the position control for a multirotor VTOL
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <stdint.h>
-#include <math.h>
-#include <stdbool.h>
-#include <float.h>
-#include <systemlib/pid/pid.h>
-
-#include "multirotor_position_control.h"
-
-void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
- const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
- const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-{
-	static PID_t distance_controller;
-
-	static int read_ret;
-	static global_data_position_t position_estimated;
-
-	static uint16_t counter;
-
-	static bool initialized;
-	static uint16_t pm_counter;
-
-	static float lat_next;
-	static float lon_next;
-
-	static float pitch_current;
-
-	static float thrust_total;
-
-
-	if (initialized == false) {
-
-		pid_init(&distance_controller,
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-			 PID_MODE_DERIVATIV_CALC, 150);//150
-
-//		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-//		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-		thrust_total = 0.0f;
-
-		/* Position initialization */
-		/* Wait for new position estimate */
-		do {
-			read_ret = read_lock_position(&position_estimated);
-		} while (read_ret != 0);
-
-		lat_next = position_estimated.lat;
-		lon_next = position_estimated.lon;
-
-		/* attitude initialization */
-		global_data_lock(&global_data_attitude->access_conf);
-		pitch_current = global_data_attitude->pitch;
-		global_data_unlock(&global_data_attitude->access_conf);
-
-		initialized = true;
-	}
-
-	/* load new parameters with 10Hz */
-	if (counter % 50 == 0) {
-		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-			/* check whether new parameters are available */
-			if (global_data_parameter_storage->counter > pm_counter) {
-				pid_set_parameters(&distance_controller,
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
-
-//
-//				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-//				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
-
-				pm_counter = global_data_parameter_storage->counter;
-				printf("Position controller changed pid parameters\n");
-			}
-		}
+// /****************************************************************************
+//  *
+//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
+//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+//  *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+//  *
+//  * Redistribution and use in source and binary forms, with or without
+//  * modification, are permitted provided that the following conditions
+//  * are met:
+//  *
+//  * 1. Redistributions of source code must retain the above copyright
+//  *    notice, this list of conditions and the following disclaimer.
+//  * 2. Redistributions in binary form must reproduce the above copyright
+//  *    notice, this list of conditions and the following disclaimer in
+//  *    the documentation and/or other materials provided with the
+//  *    distribution.
+//  * 3. Neither the name PX4 nor the names of its contributors may be
+//  *    used to endorse or promote products derived from this software
+//  *    without specific prior written permission.
+//  *
+//  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+//  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+//  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//  * POSSIBILITY OF SUCH DAMAGE.
+//  *
+//  ****************************************************************************/
+
+// /**
+//  * @file multirotor_position_control.c
+//  * Implementation of the position control for a multirotor VTOL
+//  */
+
+// #include <stdio.h>
+// #include <stdlib.h>
+// #include <stdio.h>
+// #include <stdint.h>
+// #include <math.h>
+// #include <stdbool.h>
+// #include <float.h>
+// #include <systemlib/pid/pid.h>
+
+// #include "multirotor_position_control.h"
+
+// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
+//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
+//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
+// {
+// 	static PID_t distance_controller;
+
+// 	static int read_ret;
+// 	static global_data_position_t position_estimated;
+
+// 	static uint16_t counter;
+
+// 	static bool initialized;
+// 	static uint16_t pm_counter;
+
+// 	static float lat_next;
+// 	static float lon_next;
+
+// 	static float pitch_current;
+
+// 	static float thrust_total;
+
+
+// 	if (initialized == false) {
+
+// 		pid_init(&distance_controller,
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
+// 			 PID_MODE_DERIVATIV_CALC, 150);//150
+
+// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+// 		thrust_total = 0.0f;
+
+// 		/* Position initialization */
+// 		/* Wait for new position estimate */
+// 		do {
+// 			read_ret = read_lock_position(&position_estimated);
+// 		} while (read_ret != 0);
+
+// 		lat_next = position_estimated.lat;
+// 		lon_next = position_estimated.lon;
+
+// 		/* attitude initialization */
+// 		global_data_lock(&global_data_attitude->access_conf);
+// 		pitch_current = global_data_attitude->pitch;
+// 		global_data_unlock(&global_data_attitude->access_conf);
+
+// 		initialized = true;
+// 	}
+
+// 	/* load new parameters with 10Hz */
+// 	if (counter % 50 == 0) {
+// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
+// 			/* check whether new parameters are available */
+// 			if (global_data_parameter_storage->counter > pm_counter) {
+// 				pid_set_parameters(&distance_controller,
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
+
+// //
+// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+// 				pm_counter = global_data_parameter_storage->counter;
+// 				printf("Position controller changed pid parameters\n");
+// 			}
+// 		}
 
-		global_data_unlock(&global_data_parameter_storage->access_conf);
-	}
+// 		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);
+// 	/* Wait for new position estimate */
+// 	do {
+// 		read_ret = read_lock_position(&position_estimated);
+// 	} while (read_ret != 0);
 
-	/* Get next waypoint */ //TODO: add local copy
+// 	/* 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);
-	}
+// 	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 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);
+// 	/* 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);
+// 	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
+// 	/* 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
+// 	/* 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);
+// //	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);
-	}
+// 	/* 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;
+// 	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;
+// 	if (horizontal_thrust > thrust_total) {
+// 		horizontal_thrust = thrust_total;
 
-	} else 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...
+// 	//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;
+// 	/* 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);
-	}
+// 	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 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;
+// 	/* 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 (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;
+// 		if (roll_fraction < -1) {
+// 			roll_fraction = -1;
 
-		} else 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);
-//		}
+// //		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);
-	}
+// 		pitch_desired = asinf(pitch_fraction);
+// 		roll_desired = asinf(roll_fraction);
+// 	}
 
-	att_sp.roll = roll_desired;
-	att_sp.pitch = pitch_desired;
+// 	att_sp.roll = roll_desired;
+// 	att_sp.pitch = pitch_desired;
 
-	counter++;
-}
+// 	counter++;
+// }
diff --git a/apps/multirotor_pos_control/position_control.h b/apps/multirotor_pos_control/position_control.h
index 5ba59362e..2144ebc34 100644
--- a/apps/multirotor_pos_control/position_control.h
+++ b/apps/multirotor_pos_control/position_control.h
@@ -40,11 +40,11 @@
  * Definition of the position control for a multirotor VTOL
  */
 
-#ifndef POSITION_CONTROL_H_
-#define POSITION_CONTROL_H_
+// #ifndef POSITION_CONTROL_H_
+// #define POSITION_CONTROL_H_
 
-void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
- const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
- const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
+//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
+//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
 
-#endif /* POSITION_CONTROL_H_ */
+// #endif /* POSITION_CONTROL_H_ */
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index f211648a9..b5e1d739c 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -77,6 +77,9 @@ ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
 #include "topics/vehicle_local_position.h"
 ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
 
+#include "topics/vehicle_vicon_position.h"
+ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
+
 #include "topics/vehicle_rates_setpoint.h"
 ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
 
diff --git a/apps/uORB/topics/vehicle_vicon_position.h b/apps/uORB/topics/vehicle_vicon_position.h
new file mode 100644
index 000000000..91d933f44
--- /dev/null
+++ b/apps/uORB/topics/vehicle_vicon_position.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_vicon_position.h
+ * Definition of the raw VICON Motion Capture position
+ */
+
+#ifndef TOPIC_VEHICLE_VICON_POSITION_H_
+#define TOPIC_VEHICLE_VICON_POSITION_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Fused local position in NED.
+ */
+struct vehicle_vicon_position_s
+{
+	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
+	bool valid;				/**< true if position satisfies validity criteria of estimator */
+
+	float x;				/**< X positin in meters in NED earth-fixed frame */
+	float y;				/**< X positin in meters in NED earth-fixed frame */
+	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
+	float vx;
+	float vy;
+	float vz;
+
+	// TODO Add covariances here
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_vicon_position);
+
+#endif
diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig
index fab4d6a7e..e76c4cf48 100644
--- a/nuttx/configs/px4fmu/nsh/appconfig
+++ b/nuttx/configs/px4fmu/nsh/appconfig
@@ -74,6 +74,7 @@ CONFIGURED_APPS += sdlog
 CONFIGURED_APPS += sensors
 CONFIGURED_APPS += ardrone_interface
 CONFIGURED_APPS += multirotor_att_control
+CONFIGURED_APPS += multirotor_pos_control
 CONFIGURED_APPS += px4/attitude_estimator_bm
 CONFIGURED_APPS += fixedwing_control
 CONFIGURED_APPS += position_estimator