Add PX4Flow board modules and corresponding ORB msgs.

12 files changed, 2024 insertions, 0 deletions

diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
new file mode 100644
index 000000000..c177c8fd2
--- /dev/null
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -0,0 +1,589 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_position_control.c
+ *
+ * Optical flow position controller
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <math.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/filtered_bottom_flow.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <poll.h>
+
+#include "flow_position_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 flow_position_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of position controller.
+ */
+static int flow_position_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 flow_position_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			printf("flow position control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("flow_position_control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 6,
+					 4096,
+					 flow_position_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("\tflow position control app is running\n");
+		else
+			printf("\tflow position control app not started\n");
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+static int
+flow_position_control_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	thread_running = true;
+	printf("[flow position control] starting\n");
+
+	uint32_t counter = 0;
+	const float time_scale = powf(10.0f,-6.0f);
+
+	/* structures */
+	struct vehicle_status_s vstatus;
+	struct vehicle_attitude_s att;
+	struct manual_control_setpoint_s manual;
+	struct filtered_bottom_flow_s filtered_flow;
+	struct vehicle_local_position_s local_pos;
+
+	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
+	int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+
+	orb_advert_t speed_sp_pub;
+	bool speed_setpoint_adverted = false;
+
+	/* parameters init*/
+	struct flow_position_control_params params;
+	struct flow_position_control_param_handles param_handles;
+	parameters_init(&param_handles);
+	parameters_update(&param_handles, &params);
+
+	/* init flow sum setpoint */
+	float flow_sp_sumx = 0.0f;
+	float flow_sp_sumy = 0.0f;
+
+	/* init yaw setpoint */
+	float yaw_sp = 0.0f;
+
+	/* init height setpoint */
+	float height_sp = params.height_min;
+
+	/* height controller states */
+	bool start_phase = true;
+	bool landing_initialized = false;
+	float landing_thrust_start = 0.0f;
+
+	/* states */
+	float integrated_h_error = 0.0f;
+	float last_local_pos_z = 0.0f;
+	bool update_flow_sp_sumx = false;
+	bool update_flow_sp_sumy = false;
+	uint64_t last_time = 0.0f;
+	float dt = 0.0f; // s
+
+
+	/* register the perf counter */
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_control_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_control_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_control_err");
+
+	static bool sensors_ready = false;
+
+	while (!thread_should_exit)
+	{
+		/* wait for first attitude msg to be sure all data are available */
+		if (sensors_ready)
+		{
+			/* polling */
+			struct pollfd fds[2] = {
+				{ .fd = filtered_bottom_flow_sub, .events = POLLIN }, // positions from estimator
+				{ .fd = parameter_update_sub,   .events = POLLIN }
+
+			};
+
+			/* wait for a position update, check for exit condition every 500 ms */
+			int ret = poll(fds, 2, 500);
+
+			if (ret < 0)
+			{
+				/* poll error, count it in perf */
+				perf_count(mc_err_perf);
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+//				printf("[flow position control] no filtered flow updates\n");
+			}
+			else
+			{
+				/* parameter update available? */
+				if (fds[1].revents & POLLIN)
+				{
+					/* read from param to clear updated flag */
+					struct parameter_update_s update;
+					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
+
+					parameters_update(&param_handles, &params);
+					printf("[flow position control] parameters updated.\n");
+				}
+
+				/* only run controller if position/speed changed */
+				if (fds[0].revents & POLLIN)
+				{
+					perf_begin(mc_loop_perf);
+
+					/* get a local copy of the vehicle state */
+					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					/* get a local copy of manual setpoint */
+					orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual);
+					/* get a local copy of attitude */
+					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+					/* get a local copy of filtered bottom flow */
+					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
+					/* get a local copy of local position */
+					orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos);
+
+					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+					{
+						float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1
+						float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1
+						float manual_yaw = manual.yaw / params.rc_scale_yaw; // -1 to 1
+
+						/* calc dt */
+						if(last_time == 0)
+						{
+							last_time = hrt_absolute_time();
+							continue;
+						}
+						dt = ((float) (hrt_absolute_time() - last_time)) * time_scale;
+						last_time = hrt_absolute_time();
+
+						/* update flow sum setpoint */
+						if (update_flow_sp_sumx)
+						{
+							flow_sp_sumx = filtered_flow.sumx;
+							update_flow_sp_sumx = false;
+						}
+						if (update_flow_sp_sumy)
+						{
+							flow_sp_sumy = filtered_flow.sumy;
+							update_flow_sp_sumy = false;
+						}
+
+						/* calc new bodyframe speed setpoints */
+						float speed_body_x = (flow_sp_sumx - filtered_flow.sumx) * params.pos_p - filtered_flow.vx * params.pos_d;
+						float speed_body_y = (flow_sp_sumy - filtered_flow.sumy) * params.pos_p - filtered_flow.vy * params.pos_d;
+						float speed_limit_height_factor = height_sp; // the settings are for 1 meter
+
+						/* overwrite with rc input if there is any */
+						if(isfinite(manual_pitch) && isfinite(manual_roll))
+						{
+							if(fabsf(manual_pitch) > params.manual_threshold)
+							{
+								speed_body_x = -manual_pitch * params.limit_speed_x * speed_limit_height_factor;
+								update_flow_sp_sumx = true;
+							}
+
+							if(fabsf(manual_roll) > params.manual_threshold)
+							{
+								speed_body_y = manual_roll * params.limit_speed_y * speed_limit_height_factor;
+								update_flow_sp_sumy = true;
+							}
+						}
+
+						/* limit speed setpoints */
+						if((speed_body_x <= params.limit_speed_x * speed_limit_height_factor) &&
+								(speed_body_x >= -params.limit_speed_x * speed_limit_height_factor))
+						{
+							speed_sp.vx = speed_body_x;
+						}
+						else
+						{
+							if(speed_body_x > params.limit_speed_x * speed_limit_height_factor)
+								speed_sp.vx = params.limit_speed_x * speed_limit_height_factor;
+							if(speed_body_x < -params.limit_speed_x * speed_limit_height_factor)
+								speed_sp.vx = -params.limit_speed_x * speed_limit_height_factor;
+						}
+
+						if((speed_body_y <= params.limit_speed_y * speed_limit_height_factor) &&
+								(speed_body_y >= -params.limit_speed_y * speed_limit_height_factor))
+						{
+							speed_sp.vy = speed_body_y;
+						}
+						else
+						{
+							if(speed_body_y > params.limit_speed_y * speed_limit_height_factor)
+								speed_sp.vy = params.limit_speed_y * speed_limit_height_factor;
+							if(speed_body_y < -params.limit_speed_y * speed_limit_height_factor)
+								speed_sp.vy = -params.limit_speed_y * speed_limit_height_factor;
+						}
+
+						/* manual yaw change */
+						if(isfinite(manual_yaw) && isfinite(manual.throttle))
+						{
+							if(fabsf(manual_yaw) > params.manual_threshold && manual.throttle > 0.2f)
+							{
+								yaw_sp += manual_yaw * params.limit_yaw_step;
+
+								/* modulo for rotation -pi +pi */
+								if(yaw_sp < -M_PI_F)
+									yaw_sp = yaw_sp + M_TWOPI_F;
+								else if(yaw_sp > M_PI_F)
+									yaw_sp = yaw_sp - M_TWOPI_F;
+							}
+						}
+
+						/* forward yaw setpoint */
+						speed_sp.yaw_sp = yaw_sp;
+
+
+						/* manual height control
+						 * 0-20%: thrust linear down
+						 * 20%-40%: down
+						 * 40%-60%: stabilize altitude
+						 * 60-100%: up
+						 */
+						float thrust_control = 0.0f;
+
+						if (isfinite(manual.throttle))
+						{
+							if (start_phase)
+							{
+								/* control start thrust with stick input */
+								if (manual.throttle < 0.4f)
+								{
+									/* first 40% for up to feedforward */
+									thrust_control = manual.throttle / 0.4f * params.thrust_feedforward;
+								}
+								else
+								{
+									/* second 60% for up to feedforward + 10% */
+									thrust_control = (manual.throttle - 0.4f) / 0.6f * 0.1f + params.thrust_feedforward;
+								}
+
+								/* exit start phase if setpoint is reached */
+								if (height_sp < -local_pos.z && thrust_control > params.limit_thrust_lower)
+								{
+									start_phase = false;
+									/* switch to stabilize */
+									thrust_control = params.thrust_feedforward;
+								}
+							}
+							else
+							{
+								if (manual.throttle < 0.2f)
+								{
+									/* landing initialization */
+									if (!landing_initialized)
+									{
+										/* consider last thrust control to avoid steps */
+										landing_thrust_start = speed_sp.thrust_sp;
+										landing_initialized = true;
+									}
+
+									/* set current height as setpoint to avoid steps */
+									if (-local_pos.z > params.height_min)
+										height_sp = -local_pos.z;
+									else
+										height_sp = params.height_min;
+
+									/* lower 20% stick range controls thrust down */
+									thrust_control = manual.throttle / 0.2f * landing_thrust_start;
+
+									/* assume ground position here */
+									if (thrust_control < 0.1f)
+									{
+										/* reset integral if on ground */
+										integrated_h_error = 0.0f;
+										/* switch to start phase */
+										start_phase = true;
+										/* reset height setpoint */
+										height_sp = params.height_min;
+									}
+								}
+								else
+								{
+									/* stabilized mode */
+									landing_initialized = false;
+
+									/* calc new thrust with PID */
+									float height_error = (local_pos.z - (-height_sp));
+
+									/* update height setpoint if needed*/
+									if (manual.throttle < 0.4f)
+									{
+										/* down */
+										if (height_sp > params.height_min + params.height_rate &&
+												fabsf(height_error) < params.limit_height_error)
+											height_sp -= params.height_rate * dt;
+									}
+
+									if (manual.throttle > 0.6f)
+									{
+										/* up */
+										if (height_sp < params.height_max &&
+												fabsf(height_error) < params.limit_height_error)
+											height_sp += params.height_rate * dt;
+									}
+
+									/* instead of speed limitation, limit height error (downwards) */
+									if(height_error > params.limit_height_error)
+										height_error = params.limit_height_error;
+									else if(height_error < -params.limit_height_error)
+										height_error = -params.limit_height_error;
+
+									integrated_h_error = integrated_h_error + height_error;
+									float integrated_thrust_addition = integrated_h_error * params.height_i;
+
+									if(integrated_thrust_addition > params.limit_thrust_int)
+										integrated_thrust_addition = params.limit_thrust_int;
+									if(integrated_thrust_addition < -params.limit_thrust_int)
+										integrated_thrust_addition = -params.limit_thrust_int;
+
+									float height_speed = last_local_pos_z - local_pos.z;
+									float thrust_diff = height_error * params.height_p - height_speed * params.height_d;
+
+									thrust_control = params.thrust_feedforward + thrust_diff + integrated_thrust_addition;
+
+									/* add attitude component
+									 * F = Fz / (cos(pitch)*cos(roll)) -> can be found in rotM
+									 */
+//									// TODO problem with attitude
+//									if (att.R_valid && att.R[2][2] > 0)
+//										thrust_control = thrust_control / att.R[2][2];
+
+									/* set thrust lower limit */
+									if(thrust_control < params.limit_thrust_lower)
+										thrust_control = params.limit_thrust_lower;
+								}
+							}
+
+							/* set thrust upper limit */
+							if(thrust_control > params.limit_thrust_upper)
+								thrust_control = params.limit_thrust_upper;
+						}
+						/* store actual height for speed estimation */
+						last_local_pos_z = local_pos.z;
+
+						speed_sp.thrust_sp = thrust_control;
+						speed_sp.timestamp = hrt_absolute_time();
+
+						/* publish new speed setpoint */
+						if(isfinite(speed_sp.vx) && isfinite(speed_sp.vy) && isfinite(speed_sp.yaw_sp) && isfinite(speed_sp.thrust_sp))
+						{
+
+							if(speed_setpoint_adverted)
+							{
+								orb_publish(ORB_ID(vehicle_bodyframe_speed_setpoint), speed_sp_pub, &speed_sp);
+							}
+							else
+							{
+								speed_sp_pub = orb_advertise(ORB_ID(vehicle_bodyframe_speed_setpoint), &speed_sp);
+								speed_setpoint_adverted = true;
+							}
+						}
+						else
+						{
+							warnx("NaN in flow position controller!");
+						}
+					}
+					else
+					{
+						/* in manual or stabilized state just reset speed and flow sum setpoint */
+						speed_sp.vx = 0.0f;
+						speed_sp.vy = 0.0f;
+						flow_sp_sumx = filtered_flow.sumx;
+						flow_sp_sumy = filtered_flow.sumy;
+						if(isfinite(att.yaw))
+						{
+							yaw_sp = att.yaw;
+							speed_sp.yaw_sp = att.yaw;
+						}
+						if(isfinite(manual.throttle))
+							speed_sp.thrust_sp = manual.throttle;
+					}
+
+					/* measure in what intervals the controller runs */
+					perf_count(mc_interval_perf);
+					perf_end(mc_loop_perf);
+				}
+			}
+
+			counter++;
+		}
+		else
+		{
+			/* sensors not ready waiting for first attitude msg */
+
+			/* polling */
+			struct pollfd fds[1] = {
+				{ .fd = vehicle_attitude_sub, .events = POLLIN },
+			};
+
+			/* wait for a flow msg, check for exit condition every 5 s */
+			int ret = poll(fds, 1, 5000);
+
+			if (ret < 0)
+			{
+				/* poll error, count it in perf */
+				perf_count(mc_err_perf);
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+				printf("[flow position control] no attitude received.\n");
+			}
+			else
+			{
+				if (fds[0].revents & POLLIN)
+				{
+					sensors_ready = true;
+					printf("[flow position control] initialized.\n");
+				}
+			}
+		}
+	}
+
+	printf("[flow position control] ending now...\n");
+
+	thread_running = false;
+
+	close(parameter_update_sub);
+	close(vehicle_attitude_sub);
+	close(vehicle_local_position_sub);
+	close(vehicle_status_sub);
+	close(manual_control_setpoint_sub);
+	close(speed_sp_pub);
+
+	perf_print_counter(mc_loop_perf);
+	perf_free(mc_loop_perf);
+
+	fflush(stdout);
+	return 0;
+}
+
diff --git a/src/examples/flow_position_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c
new file mode 100644
index 000000000..4f3598a57
--- /dev/null
+++ b/src/examples/flow_position_control/flow_position_control_params.c
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_position_control_params.c
+ */
+
+#include "flow_position_control_params.h"
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
+PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
+PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
+PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
+PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
+PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
+PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
+PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
+PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight
+PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f);
+PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f);
+PARAM_DEFINE_FLOAT(FPC_L_H_ERR, 0.1f);
+PARAM_DEFINE_FLOAT(FPC_L_TH_I, 0.05f);
+PARAM_DEFINE_FLOAT(FPC_L_TH_U, 0.8f);
+PARAM_DEFINE_FLOAT(FPC_L_TH_L, 0.6f);
+PARAM_DEFINE_FLOAT(FPC_L_YAW_STEP, 0.03f);
+PARAM_DEFINE_FLOAT(FPC_MAN_THR, 0.1f);
+
+
+int parameters_init(struct flow_position_control_param_handles *h)
+{
+	/* PID parameters */
+	h->pos_p	 			=	param_find("FPC_POS_P");
+	h->pos_d 				=	param_find("FPC_POS_D");
+	h->height_p 			=	param_find("FPC_H_P");
+	h->height_i 			=	param_find("FPC_H_I");
+	h->height_d 			=	param_find("FPC_H_D");
+	h->height_rate 			=	param_find("FPC_H_RATE");
+	h->height_min			=	param_find("FPC_H_MIN");
+	h->height_max			=	param_find("FPC_H_MAX");
+	h->thrust_feedforward 	=	param_find("FPC_T_FFWD");
+	h->limit_speed_x 		=	param_find("FPC_L_S_X");
+	h->limit_speed_y 		=	param_find("FPC_L_S_Y");
+	h->limit_height_error	=	param_find("FPC_L_H_ERR");
+	h->limit_thrust_int 	=	param_find("FPC_L_TH_I");
+	h->limit_thrust_upper 	=	param_find("FPC_L_TH_U");
+	h->limit_thrust_lower 	=	param_find("FPC_L_TH_L");
+	h->limit_yaw_step		=	param_find("FPC_L_YAW_STEP");
+	h->manual_threshold 	=	param_find("FPC_MAN_THR");
+	h->rc_scale_pitch		=   param_find("RC_SCALE_PITCH");
+	h->rc_scale_roll		=   param_find("RC_SCALE_ROLL");
+	h->rc_scale_yaw			=   param_find("RC_SCALE_YAW");
+
+	return OK;
+}
+
+int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p)
+{
+	param_get(h->pos_p, &(p->pos_p));
+	param_get(h->pos_d, &(p->pos_d));
+	param_get(h->height_p, &(p->height_p));
+	param_get(h->height_i, &(p->height_i));
+	param_get(h->height_d, &(p->height_d));
+	param_get(h->height_rate, &(p->height_rate));
+	param_get(h->height_min, &(p->height_min));
+	param_get(h->height_max, &(p->height_max));
+	param_get(h->thrust_feedforward, &(p->thrust_feedforward));
+	param_get(h->limit_speed_x, &(p->limit_speed_x));
+	param_get(h->limit_speed_y, &(p->limit_speed_y));
+	param_get(h->limit_height_error, &(p->limit_height_error));
+	param_get(h->limit_thrust_int, &(p->limit_thrust_int));
+	param_get(h->limit_thrust_upper, &(p->limit_thrust_upper));
+	param_get(h->limit_thrust_lower, &(p->limit_thrust_lower));
+	param_get(h->limit_yaw_step, &(p->limit_yaw_step));
+	param_get(h->manual_threshold, &(p->manual_threshold));
+	param_get(h->rc_scale_pitch, &(p->rc_scale_pitch));
+	param_get(h->rc_scale_roll, &(p->rc_scale_roll));
+	param_get(h->rc_scale_yaw, &(p->rc_scale_yaw));
+
+	return OK;
+}
diff --git a/src/examples/flow_position_control/flow_position_control_params.h b/src/examples/flow_position_control/flow_position_control_params.h
new file mode 100644
index 000000000..d0c8fc722
--- /dev/null
+++ b/src/examples/flow_position_control/flow_position_control_params.h
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_position_control_params.h
+ * 
+ * Parameters for position controller
+ */
+
+#include <systemlib/param/param.h>
+
+struct flow_position_control_params {
+	float pos_p;
+	float pos_d;
+	float height_p;
+	float height_i;
+	float height_d;
+	float height_rate;
+	float height_min;
+	float height_max;
+	float thrust_feedforward;
+	float limit_speed_x;
+	float limit_speed_y;
+	float limit_height_error;
+	float limit_thrust_int;
+	float limit_thrust_upper;
+	float limit_thrust_lower;
+	float limit_yaw_step;
+	float manual_threshold;
+	float rc_scale_pitch;
+	float rc_scale_roll;
+	float rc_scale_yaw;
+};
+
+struct flow_position_control_param_handles {
+	param_t pos_p;
+	param_t pos_d;
+	param_t height_p;
+	param_t height_i;
+	param_t height_d;
+	param_t height_rate;
+	param_t height_min;
+	param_t height_max;
+	param_t thrust_feedforward;
+	param_t limit_speed_x;
+	param_t limit_speed_y;
+	param_t limit_height_error;
+	param_t limit_thrust_int;
+	param_t limit_thrust_upper;
+	param_t limit_thrust_lower;
+	param_t limit_yaw_step;
+	param_t manual_threshold;
+	param_t rc_scale_pitch;
+	param_t rc_scale_roll;
+	param_t rc_scale_yaw;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct flow_position_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct flow_position_control_param_handles *h, struct flow_position_control_params *p);
diff --git a/src/examples/flow_position_control/module.mk b/src/examples/flow_position_control/module.mk
new file mode 100644
index 000000000..b10dc490a
--- /dev/null
+++ b/src/examples/flow_position_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build multirotor position control
+#
+
+MODULE_COMMAND	= flow_position_control
+
+SRCS		= flow_position_control_main.c \
+		  flow_position_control_params.c
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
new file mode 100644
index 000000000..c0b16d2e7
--- /dev/null
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -0,0 +1,458 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_position_estimator_main.c
+ *
+ * Optical flow position estimator
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/filtered_bottom_flow.h>
+#include <systemlib/perf_counter.h>
+#include <poll.h>
+
+#include "flow_position_estimator_params.h"
+
+__EXPORT int flow_position_estimator_main(int argc, char *argv[]);
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int daemon_task;				/**< Handle of daemon task / thread */
+
+int flow_position_estimator_thread_main(int argc, char *argv[]);
+static void usage(const char *reason);
+
+static void usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int flow_position_estimator_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			printf("flow position estimator already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		daemon_task = task_spawn("flow_position_estimator",
+					 SCHED_RR,
+					 SCHED_PRIORITY_MAX - 5,
+					 4096,
+					 flow_position_estimator_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("\tflow position estimator is running\n");
+		else
+			printf("\tflow position estimator not started\n");
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+int flow_position_estimator_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	thread_running = true;
+	printf("[flow position estimator] starting\n");
+
+	/* rotation matrix for transformation of optical flow speed vectors */
+	static const int8_t rotM_flow_sensor[3][3] =   {{  0, 1, 0 },
+													{ -1, 0, 0 },
+													{  0, 0, 1 }}; // 90deg rotated
+	const float time_scale = powf(10.0f,-6.0f);
+	static float speed[3] = {0.0f, 0.0f, 0.0f};
+	static float flow_speed[3] = {0.0f, 0.0f, 0.0f};
+	static float global_speed[3] = {0.0f, 0.0f, 0.0f};
+	static uint32_t counter = 0;
+	static uint64_t time_last_flow = 0; // in ms
+	static float dt = 0.0f; // seconds
+	static float sonar_last = 0.0f;
+	static bool sonar_valid = false;
+	static float sonar_lp = 0.0f;
+
+	/* subscribe to vehicle status, attitude, sensors and flow*/
+	struct vehicle_status_s vstatus;
+	memset(&vstatus, 0, sizeof(vstatus));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct optical_flow_s flow;
+	memset(&flow, 0, sizeof(flow));
+
+	/* subscribe to parameter changes */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	/* subscribe to vehicle status */
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* subscribe to attitude */
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+
+	/* subscribe to attitude setpoint */
+	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+
+	/* subscribe to optical flow*/
+	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
+
+	/* init local position and filtered flow struct */
+	struct vehicle_local_position_s local_pos = {
+			.x = 0.0f,
+			.y = 0.0f,
+			.z = 0.0f,
+			.vx = 0.0f,
+			.vy = 0.0f,
+			.vz = 0.0f
+	};
+	struct filtered_bottom_flow_s filtered_flow = {
+			.sumx = 0.0f,
+			.sumy = 0.0f,
+			.vx = 0.0f,
+			.vy = 0.0f
+	};
+
+	/* advert pub messages */
+	orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
+	orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow);
+
+	/* vehicle flying status parameters */
+	bool vehicle_liftoff = false;
+	bool sensors_ready = false;
+
+	/* parameters init*/
+	struct flow_position_estimator_params params;
+	struct flow_position_estimator_param_handles param_handles;
+	parameters_init(&param_handles);
+	parameters_update(&param_handles, &params);
+
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err");
+
+	while (!thread_should_exit)
+	{
+		if (sensors_ready)
+		{
+			/*This runs at the rate of the sensors */
+			struct pollfd fds[2] = {
+					{ .fd = optical_flow_sub, .events = POLLIN },
+					{ .fd = parameter_update_sub,   .events = POLLIN }
+			};
+
+			/* wait for a sensor update, check for exit condition every 500 ms */
+			int ret = poll(fds, 2, 500);
+
+			if (ret < 0)
+			{
+				/* poll error, count it in perf */
+				perf_count(mc_err_perf);
+
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+//				printf("[flow position estimator] no bottom flow.\n");
+			}
+			else
+			{
+
+				/* parameter update available? */
+				if (fds[1].revents & POLLIN)
+				{
+					/* read from param to clear updated flag */
+					struct parameter_update_s update;
+					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
+
+					parameters_update(&param_handles, &params);
+					printf("[flow position estimator] parameters updated.\n");
+				}
+
+				/* only if flow data changed */
+				if (fds[0].revents & POLLIN)
+				{
+					perf_begin(mc_loop_perf);
+
+					orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
+					/* got flow, updating attitude and status as well */
+					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
+					orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
+					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+
+					/* vehicle state estimation */
+					float sonar_new = flow.ground_distance_m;
+
+					/* set liftoff boolean
+					 * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m)
+					 * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
+					 * -> minimum sonar value 0.3m
+					 */
+					if (!vehicle_liftoff)
+					{
+						if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
+							vehicle_liftoff = true;
+					}
+					else
+					{
+						if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
+							vehicle_liftoff = false;
+					}
+
+					/* calc dt between flow timestamps */
+					/* ignore first flow msg */
+					if(time_last_flow == 0)
+					{
+						time_last_flow = flow.timestamp;
+						continue;
+					}
+					dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
+					time_last_flow = flow.timestamp;
+
+					/* only make position update if vehicle is lift off or DEBUG is activated*/
+					if (vehicle_liftoff || params.debug)
+					{
+						/* copy flow */
+						flow_speed[0] = flow.flow_comp_x_m;
+						flow_speed[1] = flow.flow_comp_y_m;
+						flow_speed[2] = 0.0f;
+
+						/* convert to bodyframe velocity */
+						for(uint8_t i = 0; i < 3; i++)
+						{
+							float sum = 0.0f;
+							for(uint8_t j = 0; j < 3; j++)
+								sum = sum + flow_speed[j] * rotM_flow_sensor[j][i];
+
+							speed[i] = sum;
+						}
+
+						/* update filtered flow */
+						filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt;
+						filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt;
+						filtered_flow.vx = speed[0];
+						filtered_flow.vy = speed[1];
+
+						// TODO add yaw rotation correction (with distance to vehicle zero)
+
+						/* convert to globalframe velocity
+						 * -> local position is currently not used for position control
+						 */
+						for(uint8_t i = 0; i < 3; i++)
+						{
+							float sum = 0.0f;
+							for(uint8_t j = 0; j < 3; j++)
+								sum = sum + speed[j] * att.R[i][j];
+
+							global_speed[i] = sum;
+						}
+
+						local_pos.x = local_pos.x + global_speed[0] * dt;
+						local_pos.y = local_pos.y + global_speed[1] * dt;
+						local_pos.vx = global_speed[0];
+						local_pos.vy = global_speed[1];
+					}
+					else
+					{
+						/* set speed to zero and let position as it is */
+						filtered_flow.vx = 0;
+						filtered_flow.vy = 0;
+						local_pos.vx = 0;
+						local_pos.vy = 0;
+					}
+
+					/* filtering ground distance */
+					if (!vehicle_liftoff || !vstatus.flag_system_armed)
+					{
+						/* not possible to fly */
+						sonar_valid = false;
+						local_pos.z = 0.0f;
+					}
+					else
+					{
+						sonar_valid = true;
+					}
+
+					if (sonar_valid || params.debug)
+					{
+						/* simple lowpass sonar filtering */
+						/* if new value or with sonar update frequency */
+						if (sonar_new != sonar_last || counter % 10 == 0)
+						{
+							sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
+							sonar_last = sonar_new;
+						}
+
+						float height_diff = sonar_new - sonar_lp;
+
+						/* if over 1/2m spike follow lowpass */
+						if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold)
+						{
+							local_pos.z = -sonar_lp;
+						}
+						else
+						{
+							local_pos.z = -sonar_new;
+						}
+					}
+
+					filtered_flow.timestamp = hrt_absolute_time();
+					local_pos.timestamp = hrt_absolute_time();
+
+					/* publish local position */
+					if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
+							&& isfinite(local_pos.vx) && isfinite(local_pos.vy))
+					{
+						orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
+					}
+
+					/* publish filtered flow */
+					if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy))
+					{
+						orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow);
+					}
+
+					/* measure in what intervals the position estimator runs */
+					perf_count(mc_interval_perf);
+					perf_end(mc_loop_perf);
+
+				}
+			}
+
+		}
+		else
+		{
+			/* sensors not ready waiting for first attitude msg */
+
+			/* polling */
+			struct pollfd fds[1] = {
+				{ .fd = vehicle_attitude_sub, .events = POLLIN },
+			};
+
+			/* wait for a attitude message, check for exit condition every 5 s */
+			int ret = poll(fds, 1, 5000);
+
+			if (ret < 0)
+			{
+				/* poll error, count it in perf */
+				perf_count(mc_err_perf);
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+				printf("[flow position estimator] no attitude received.\n");
+			}
+			else
+			{
+				if (fds[0].revents & POLLIN){
+					sensors_ready = true;
+					printf("[flow position estimator] initialized.\n");
+				}
+			}
+		}
+
+		counter++;
+	}
+
+	printf("[flow position estimator] exiting.\n");
+	thread_running = false;
+
+	close(vehicle_attitude_setpoint_sub);
+	close(vehicle_attitude_sub);
+	close(vehicle_status_sub);
+	close(parameter_update_sub);
+	close(optical_flow_sub);
+
+	perf_print_counter(mc_loop_perf);
+	perf_free(mc_loop_perf);
+
+	fflush(stdout);
+	return 0;
+}
+
+
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.c b/src/examples/flow_position_estimator/flow_position_estimator_params.c
new file mode 100644
index 000000000..ec3c3352d
--- /dev/null
+++ b/src/examples/flow_position_estimator/flow_position_estimator_params.c
@@ -0,0 +1,72 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_position_estimator_params.c
+ * 
+ * Parameters for position estimator
+ */
+
+#include "flow_position_estimator_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(FPE_LO_THRUST, 0.4f);
+PARAM_DEFINE_FLOAT(FPE_SONAR_LP_U, 0.5f);
+PARAM_DEFINE_FLOAT(FPE_SONAR_LP_L, 0.2f);
+PARAM_DEFINE_INT32(FPE_DEBUG, 0);
+
+
+int parameters_init(struct flow_position_estimator_param_handles *h)
+{
+	/* PID parameters */
+	h->minimum_liftoff_thrust	=	param_find("FPE_LO_THRUST");
+	h->sonar_upper_lp_threshold	=	param_find("FPE_SONAR_LP_U");
+	h->sonar_lower_lp_threshold	=	param_find("FPE_SONAR_LP_L");
+	h->debug					=	param_find("FPE_DEBUG");
+
+	return OK;
+}
+
+int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p)
+{
+	param_get(h->minimum_liftoff_thrust, &(p->minimum_liftoff_thrust));
+	param_get(h->sonar_upper_lp_threshold, &(p->sonar_upper_lp_threshold));
+	param_get(h->sonar_lower_lp_threshold, &(p->sonar_lower_lp_threshold));
+	param_get(h->debug, &(p->debug));
+
+	return OK;
+}
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_params.h b/src/examples/flow_position_estimator/flow_position_estimator_params.h
new file mode 100644
index 000000000..f9a9bb303
--- /dev/null
+++ b/src/examples/flow_position_estimator/flow_position_estimator_params.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_position_estimator_params.h
+ * 
+ * Parameters for position estimator
+ */
+
+#include <systemlib/param/param.h>
+
+struct flow_position_estimator_params {
+	float minimum_liftoff_thrust;
+	float sonar_upper_lp_threshold;
+	float sonar_lower_lp_threshold;
+	int debug;
+};
+
+struct flow_position_estimator_param_handles {
+	param_t minimum_liftoff_thrust;
+	param_t sonar_upper_lp_threshold;
+	param_t sonar_lower_lp_threshold;
+	param_t debug;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct flow_position_estimator_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct flow_position_estimator_param_handles *h, struct flow_position_estimator_params *p);
diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk
new file mode 100644
index 000000000..88c9ceb93
--- /dev/null
+++ b/src/examples/flow_position_estimator/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build position estimator
+#
+
+MODULE_COMMAND		= flow_position_estimator
+
+SRCS		= flow_position_estimator_main.c \
+		  flow_position_estimator_params.c
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
new file mode 100644
index 000000000..9648728c8
--- /dev/null
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -0,0 +1,361 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_speed_control.c
+ *
+ * Optical flow speed controller
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <math.h>
+#include <sys/prctl.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
+#include <uORB/topics/filtered_bottom_flow.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+#include <poll.h>
+
+#include "flow_speed_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 flow_speed_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of position controller.
+ */
+static int flow_speed_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 flow_speed_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start"))
+	{
+		if (thread_running)
+		{
+			printf("flow speed control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("flow_speed_control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 6,
+					 4096,
+					 flow_speed_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("\tflow speed control app is running\n");
+		else
+			printf("\tflow speed control app not started\n");
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+static int
+flow_speed_control_thread_main(int argc, char *argv[])
+{
+	/* welcome user */
+	thread_running = true;
+	printf("[flow speed control] starting\n");
+
+	uint32_t counter = 0;
+
+	/* structures */
+	struct vehicle_status_s vstatus;
+	struct filtered_bottom_flow_s filtered_flow;
+	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
+
+	struct vehicle_attitude_setpoint_s att_sp;
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
+	int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
+
+	orb_advert_t att_sp_pub;
+	bool attitude_setpoint_adverted = false;
+
+	/* parameters init*/
+	struct flow_speed_control_params params;
+	struct flow_speed_control_param_handles param_handles;
+	parameters_init(&param_handles);
+	parameters_update(&param_handles, &params);
+
+	/* register the perf counter */
+	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime");
+	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval");
+	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
+
+	static bool sensors_ready = false;
+
+	while (!thread_should_exit)
+	{
+		/* wait for first attitude msg to be sure all data are available */
+		if (sensors_ready)
+		{
+			/* polling */
+			struct pollfd fds[2] = {
+				{ .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller
+				{ .fd = parameter_update_sub,   .events = POLLIN }
+			};
+
+			/* wait for a position update, check for exit condition every 5000 ms */
+			int ret = poll(fds, 2, 500);
+
+			if (ret < 0)
+			{
+				/* poll error, count it in perf */
+				perf_count(mc_err_perf);
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+//				printf("[flow speed control] no bodyframe speed setpoints updates\n");
+			}
+			else
+			{
+				/* parameter update available? */
+				if (fds[1].revents & POLLIN)
+				{
+					/* read from param to clear updated flag */
+					struct parameter_update_s update;
+					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
+
+					parameters_update(&param_handles, &params);
+					printf("[flow speed control] parameters updated.\n");
+				}
+
+				/* only run controller if position/speed changed */
+				if (fds[0].revents & POLLIN)
+				{
+					perf_begin(mc_loop_perf);
+
+					/* get a local copy of the vehicle state */
+					orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+					/* get a local copy of filtered bottom flow */
+					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
+					/* get a local copy of bodyframe speed setpoint */
+					orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
+
+					if (vstatus.state_machine == SYSTEM_STATE_AUTO)
+					{
+						/* calc new roll/pitch */
+						float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
+						float roll_body  =  (speed_sp.vy - filtered_flow.vy) * params.speed_p;
+
+						/* limit roll and pitch corrections */
+						if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
+						{
+							att_sp.pitch_body = pitch_body;
+						}
+						else
+						{
+							if(pitch_body > params.limit_pitch)
+								att_sp.pitch_body = params.limit_pitch;
+							if(pitch_body < -params.limit_pitch)
+								att_sp.pitch_body = -params.limit_pitch;
+						}
+
+						if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll))
+						{
+							att_sp.roll_body = roll_body;
+						}
+						else
+						{
+							if(roll_body > params.limit_roll)
+								att_sp.roll_body = params.limit_roll;
+							if(roll_body < -params.limit_roll)
+								att_sp.roll_body = -params.limit_roll;
+						}
+
+						/* set yaw setpoint forward*/
+						att_sp.yaw_body = speed_sp.yaw_sp;
+
+						/* add trim from parameters */
+						att_sp.roll_body = att_sp.roll_body + params.trim_roll;
+						att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch;
+
+						att_sp.thrust = speed_sp.thrust_sp;
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* publish new attitude setpoint */
+						if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust))
+						{
+							if (attitude_setpoint_adverted)
+							{
+								orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+							}
+							else
+							{
+								att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+								attitude_setpoint_adverted = true;
+							}
+						}
+						else
+						{
+							warnx("NaN in flow speed controller!");
+						}
+					}
+					else
+					{
+						/* reset attitude setpoint */
+						att_sp.roll_body = 0.0f;
+						att_sp.pitch_body = 0.0f;
+						att_sp.thrust = 0.0f;
+						att_sp.yaw_body = 0.0f;
+					}
+
+					/* measure in what intervals the controller runs */
+					perf_count(mc_interval_perf);
+					perf_end(mc_loop_perf);
+				}
+			}
+
+			counter++;
+		}
+		else
+		{
+			/* sensors not ready waiting for first attitude msg */
+
+			/* polling */
+			struct pollfd fds[1] = {
+				{ .fd = vehicle_attitude_sub, .events = POLLIN },
+			};
+
+			/* wait for a flow msg, check for exit condition every 5 s */
+			int ret = poll(fds, 1, 5000);
+
+			if (ret < 0)
+			{
+				/* poll error, count it in perf */
+				perf_count(mc_err_perf);
+			}
+			else if (ret == 0)
+			{
+				/* no return value, ignore */
+				printf("[flow speed control] no attitude received.\n");
+			}
+			else
+			{
+				if (fds[0].revents & POLLIN)
+				{
+					sensors_ready = true;
+					printf("[flow speed control] initialized.\n");
+				}
+			}
+		}
+	}
+
+	printf("[flow speed control] ending now...\n");
+
+	thread_running = false;
+
+	close(parameter_update_sub);
+	close(vehicle_attitude_sub);
+	close(vehicle_bodyframe_speed_setpoint_sub);
+	close(filtered_bottom_flow_sub);
+	close(vehicle_status_sub);
+	close(att_sp_pub);
+
+	perf_print_counter(mc_loop_perf);
+	perf_free(mc_loop_perf);
+
+	fflush(stdout);
+	return 0;
+}
diff --git a/src/examples/flow_speed_control/flow_speed_control_params.c b/src/examples/flow_speed_control/flow_speed_control_params.c
new file mode 100644
index 000000000..8dfe54173
--- /dev/null
+++ b/src/examples/flow_speed_control/flow_speed_control_params.c
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_speed_control_params.c
+ * 
+ */
+
+#include "flow_speed_control_params.h"
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f);
+PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f);
+PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f);
+
+int parameters_init(struct flow_speed_control_param_handles *h)
+{
+	/* PID parameters */
+	h->speed_p	 			=	param_find("FSC_S_P");
+	h->limit_pitch 			=	param_find("FSC_L_PITCH");
+	h->limit_roll 			=	param_find("FSC_L_ROLL");
+	h->trim_roll 			=	param_find("TRIM_ROLL");
+	h->trim_pitch 			=	param_find("TRIM_PITCH");
+
+
+	return OK;
+}
+
+int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p)
+{
+	param_get(h->speed_p, &(p->speed_p));
+	param_get(h->limit_pitch, &(p->limit_pitch));
+	param_get(h->limit_roll, &(p->limit_roll));
+	param_get(h->trim_roll, &(p->trim_roll));
+	param_get(h->trim_pitch, &(p->trim_pitch));
+
+	return OK;
+}
diff --git a/src/examples/flow_speed_control/flow_speed_control_params.h b/src/examples/flow_speed_control/flow_speed_control_params.h
new file mode 100644
index 000000000..eec27a2bf
--- /dev/null
+++ b/src/examples/flow_speed_control/flow_speed_control_params.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Samuel Zihlmann <samuezih@ee.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 flow_speed_control_params.h
+ * 
+ * Parameters for speed controller
+ */
+
+#include <systemlib/param/param.h>
+
+struct flow_speed_control_params {
+	float speed_p;
+	float limit_pitch;
+	float limit_roll;
+	float trim_roll;
+	float trim_pitch;
+};
+
+struct flow_speed_control_param_handles {
+	param_t speed_p;
+	param_t limit_pitch;
+	param_t limit_roll;
+	param_t trim_roll;
+	param_t trim_pitch;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct flow_speed_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p);
diff --git a/src/examples/flow_speed_control/module.mk b/src/examples/flow_speed_control/module.mk
new file mode 100644
index 000000000..5a4182146
--- /dev/null
+++ b/src/examples/flow_speed_control/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Build flow speed control
+#
+
+MODULE_COMMAND	= flow_speed_control
+
+SRCS		= flow_speed_control_main.c \
+		  flow_speed_control_params.c