Fresh import of the PX4 firmware sources.

6 files changed, 1212 insertions, 0 deletions

diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile
new file mode 100644
index 000000000..985708ae5
--- /dev/null
+++ b/apps/fixedwing_control/Makefile
@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# fixedwing_control Application
+#
+
+APPNAME		= fixedwing_control
+PRIORITY	= SCHED_PRIORITY_MAX - 1
+STACKSIZE	= 12288
+
+CSRCS		= fixedwing_control.c 
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c
new file mode 100644
index 000000000..d9d39f14d
--- /dev/null
+++ b/apps/fixedwing_control/fixedwing_control.c
@@ -0,0 +1,938 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file fixedwing_control.c
+ * Implementation of a fixed wing attitude and position controller.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <math.h>
+#include <termios.h>
+#include <time.h>
+#include <arch/board/up_hrt.h>
+#include <arch/board/board.h>
+#include <arch/board/drv_pwm_servo.h>
+#include <nuttx/spi.h>
+#include "../mix_and_link/mix_and_link.h"
+#include "fixedwing_control.h"
+
+__EXPORT int fixedwing_control_main(int argc, char *argv[]);
+
+#define PID_DT 0.01f
+#define PID_SCALER 0.1f
+#define PID_DERIVMODE_CALC 0
+#define HIL_MODE 32
+#define RAD2DEG ((1.0/180.0)*M_PI)
+#define AUTO -1000
+#define MANUAL 3000
+#define SERVO_MIN 1000
+#define SERVO_MAX 2000
+
+pthread_t control_thread;
+pthread_t nav_thread;
+pthread_t servo_thread;
+
+/**
+ * Servo channels function enumerator used for
+ * the servo writing part
+ */
+
+enum SERVO_CHANNELS_FUNCTION {
+
+	AIL_1    = 0,
+	AIL_2    = 1,
+	MOT      = 2,
+	ACT_1    = 3,
+	ACT_2    = 4,
+	ACT_3    = 5,
+	ACT_4    = 6,
+	ACT_5    = 7
+};
+
+/**
+ * The plane_data structure.
+ *
+ * The plane data structure is the local storage of all the flight information of the aircraft
+ */
+typedef struct {
+	double lat;
+	double lon;
+	float alt;
+	float vx;
+	float vy;
+	float vz;
+	float yaw;
+	float hdg;
+	float pitch;
+	float roll;
+	float yawspeed;
+	float pitchspeed;
+	float rollspeed;
+	float rollb;	/* body frame angles */
+	float pitchb;
+	float yawb;
+	float p;
+	float q;
+	float r;	/* body angular rates */
+
+	/* PID parameters*/
+
+	float Kp_att;
+	float Ki_att;
+	float Kd_att;
+	float Kp_pos;
+	float Ki_pos;
+	float Kd_pos;
+	float intmax_att;
+	float intmax_pos;
+
+	/* Next waypoint*/
+
+	float wp_x;
+	float wp_y;
+	float wp_z;
+
+	/* Setpoints */
+
+	float airspeed;
+	float groundspeed;
+	float roll_setpoint;
+	float pitch_setpoint;
+	float throttle_setpoint;
+
+	/* Navigation mode*/
+	int mode;
+
+} plane_data_t;
+
+/**
+ * The control_outputs structure.
+ *
+ * The control outputs structure contains the control outputs
+ * of the aircraft
+ */
+typedef struct {
+	float roll_ailerons;
+	float pitch_elevator;
+	float yaw_rudder;
+	float throttle;
+	// set the aux values to 0 per default
+	float aux1;
+	float aux2;
+	float aux3;
+	float aux4;
+	uint8_t mode;	// HIL_ENABLED: 32
+	uint8_t nav_mode;
+} control_outputs_t;
+
+/**
+ * Generic PID algorithm with PD scaling
+ */
+static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax);
+
+/*
+ * Output calculations
+ */
+
+static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed);
+static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z);
+static void calc_bodyframe_angles(float roll, float pitch, float yaw);
+static float calc_bearing(void);
+static float calc_roll_ail(void);
+static float calc_pitch_elev(void);
+static float calc_yaw_rudder(float hdg);
+static float calc_throttle(void);
+static float calc_gnd_speed(void);
+static void get_parameters(void);
+static float calc_roll_setpoint(void);
+static float calc_pitch_setpoint(void);
+static float calc_throttle_setpoint(void);
+static float calc_wp_distance(void);
+static void set_plane_mode(void);
+
+/*
+ * The control, navigation and servo loop threads
+ */
+
+static void *control_loop(void *arg);
+static void *nav_loop(void *arg);
+static void *servo_loop(void *arg);
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+plane_data_t plane_data;
+control_outputs_t control_outputs;
+float scaler = 1; //M_PI;
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/**
+ *
+ * Calculates the PID control output given an error. Incorporates PD scaling and low-pass filter for the derivative component.
+ *
+ * @param error the input error
+ * @param error_deriv the derivative of the input error
+ * @param dt time constant
+ * @param scaler PD scaler
+ * @param K_p P gain
+ * @param K_i I gain
+ * @param K_d D gain
+ * @param intmax Integration limit
+ *
+ * @return the PID control output
+ */
+
+static float pid(float error, float error_deriv, uint16_t dt, float scale, float K_p, float K_i, float K_d, float intmax)
+{
+	float Kp = K_p;
+	float Ki = K_i;
+	float Kd = K_d;
+	float delta_time = dt;
+	float lerror;
+	float imax = intmax;
+	float integrator;
+	float derivative;
+	float lderiv;
+	int fCut = 20;		/* anything above 20 Hz is considered noise - low pass filter for the derivative */
+	float output = 0;
+
+	output += error * Kp;
+
+	if ((fabs(Kd) > 0) && (dt > 0)) {
+
+		if (PID_DERIVMODE_CALC) {
+			derivative = (error - lerror) / delta_time;
+
+			/*
+			 * discrete low pass filter, cuts out the
+			 * high frequency noise that can drive the controller crazy
+			 */
+			float RC = 1 / (2 * M_PI * fCut);
+			derivative = lderiv +
+				     (delta_time / (RC + delta_time)) * (derivative - lderiv);
+
+			/* update state */
+			lerror 	= error;
+			lderiv  = derivative;
+
+		} else {
+			derivative = error_deriv;
+		}
+
+		/* add in derivative component */
+		output 	+= Kd * derivative;
+	}
+
+	//printf("PID derivative %i\n", (int)(1000*derivative));
+
+	/* scale the P and D components with the PD scaler */
+	output *= scale;
+
+	/* Compute integral component if time has elapsed */
+	if ((fabs(Ki) > 0) && (dt > 0)) {
+		integrator 		+= (error * Ki) * scaler * delta_time;
+
+		if (integrator < -imax) {
+			integrator = -imax;
+
+		} else if (integrator > imax) {
+			integrator = imax;
+		}
+
+		output += integrator;
+	}
+
+	//printf("PID Integrator %i\n", (int)(1000*integrator));
+
+	return output;
+}
+
+/**
+ * Load parameters from global storage.
+ *
+ * Fetches the current parameters from the global parameter storage and writes them
+ * to the plane_data structure
+ */
+
+static void get_parameters()
+{
+	plane_data.Kp_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P];
+	plane_data.Ki_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I];
+	plane_data.Kd_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D];
+	plane_data.Kp_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P];
+	plane_data.Ki_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I];
+	plane_data.Kd_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D];
+	plane_data.intmax_att = global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU];
+	plane_data.intmax_pos = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU];
+	plane_data.airspeed =  global_data_parameter_storage->pm.param_values[PARAM_AIRSPEED];
+	plane_data.wp_x =  global_data_parameter_storage->pm.param_values[PARAM_WPLON];
+	plane_data.wp_y =  global_data_parameter_storage->pm.param_values[PARAM_WPLAT];
+	plane_data.wp_z =  global_data_parameter_storage->pm.param_values[PARAM_WPALT];
+	plane_data.mode = global_data_parameter_storage->pm.param_values[PARAM_FLIGHTMODE];
+}
+
+/**
+ * Calculates the body angular rates.
+ *
+ * Calculates the rates of the plane using inertia matrix and
+ * writes them to the plane_data structure
+ *
+ * @param roll
+ * @param pitch
+ * @param yaw
+ * @param rollspeed
+ * @param pitchspeed
+ * @param yawspeed
+ *
+ */
+static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
+{
+	plane_data.p = rollspeed - sinf(pitch) * yawspeed;
+	plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cos(pitch) * yawspeed;
+	plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cos(pitch) * yawspeed;
+}
+
+/**
+ *
+ * Calculates the attitude angles in the body reference frame.
+ *
+ * Writes them to the plane data structure
+ *
+ * @param roll
+ * @param pitch
+ * @param yaw
+ */
+
+static void calc_bodyframe_angles(float roll, float pitch, float yaw)
+{
+	plane_data.rollb = cosf(yaw) * cosf(pitch) * roll +
+			   (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * pitch
+			   + (-cosf(yaw) * sinf(pitch) * cosf(roll)  + sinf(yaw) * sinf(roll)) * yaw;
+	plane_data.pitchb = -sinf(yaw) * cosf(pitch) * roll +
+			    (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * pitch
+			    + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * yaw;
+	plane_data.yawb = sinf(pitch) * roll - cosf(pitch) * sinf(roll) * pitch + cosf(pitch) * cosf(roll) * yaw;
+}
+
+/**
+ * calc_rotation_matrix
+ *
+ * Calculates the rotation matrix
+ *
+ * @param roll
+ * @param pitch
+ * @param yaw
+ * @param x
+ * @param y
+ * @param z
+ *
+ */
+
+static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, float y, float z)
+{
+	plane_data.rollb = cosf(yaw) * cosf(pitch) * x +
+			   (cosf(yaw) * sinf(pitch) * sinf(roll) + sinf(yaw) * cosf(roll)) * y
+			   + (-cosf(yaw) * sinf(pitch) * cosf(roll)  + sinf(yaw) * sinf(roll)) * z;
+	plane_data.pitchb = -sinf(yaw) * cosf(pitch) * x +
+			    (-sinf(yaw) * sinf(pitch) * sinf(roll) + cosf(yaw) * cosf(roll)) * y
+			    + (sinf(yaw) * sinf(pitch) * cosf(roll) + cosf(yaw) * sinf(roll)) * z;
+	plane_data.yawb = sinf(pitch) * x - cosf(pitch) * sinf(roll) * y + cosf(pitch) * cosf(roll) * z;
+}
+
+/**
+ * calc_bearing
+ *
+ * Calculates the bearing error of the plane compared to the waypoints
+ *
+ * @return bearing Bearing error
+ *
+ */
+
+static float calc_bearing()
+{
+	float bearing = 90 + atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)) * RAD2DEG;
+
+	if (bearing < 0)
+		bearing += 360;
+
+	return bearing;
+}
+
+/**
+ * calc_roll_ail
+ *
+ * Calculates the roll ailerons control output
+ *
+ * @return Roll ailerons control output (-1,1)
+ */
+
+static float calc_roll_ail()
+{
+	float ret = pid((plane_data.roll_setpoint - plane_data.roll) / scaler, plane_data.rollspeed, PID_DT, PID_SCALER,
+			plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att);
+
+	if (ret < -1)
+		return -1;
+
+	if (ret > 1)
+		return 1;
+
+	return ret;
+}
+
+/**
+ * calc_pitch_elev
+ *
+ * Calculates the pitch elevators control output
+ *
+ * @return Pitch elevators control output (-1,1)
+ */
+
+static float calc_pitch_elev()
+{
+	float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER,
+			plane_data.Kp_att, plane_data.Ki_att, plane_data.Kd_att, plane_data.intmax_att);
+
+	if (ret < -1)
+		return -1;
+
+	if (ret > 1)
+		return 1;
+
+	return ret;
+}
+
+/**
+ * calc_yaw_rudder
+ *
+ * Calculates the yaw rudder control output (only if yaw rudder exists on the model)
+ *
+ * @return Yaw rudder control output (-1,1)
+ *
+ */
+
+static float calc_yaw_rudder(float hdg)
+{
+	float ret = pid((plane_data.yaw / RAD2DEG - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER,
+			plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
+
+	if (ret < -1)
+		return -1;
+
+	if (ret > 1)
+		return 1;
+
+	return ret;
+}
+
+/**
+ * calc_throttle
+ *
+ * Calculates the throttle control output
+ *
+ * @return Throttle control output (0,1)
+ */
+
+static float calc_throttle()
+{
+	float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER,
+			plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos);
+
+	if (ret < 0.2)
+		return 0.2;
+
+	if (ret > 1)
+		return 1;
+
+	return ret;
+}
+
+/**
+ * calc_gnd_speed
+ *
+ * Calculates the ground speed using the x and y components
+ *
+ * Input: none (operation on global data)
+ *
+ * Output: Ground speed of the plane
+ *
+ */
+
+static float calc_gnd_speed()
+{
+	float gnd_speed = sqrtf(plane_data.vx * plane_data.vx + plane_data.vy * plane_data.vy);
+	return gnd_speed;
+}
+
+/**
+ * calc_wp_distance
+ *
+ * Calculates the distance to the next waypoint
+ *
+ * @return the distance to the next waypoint
+ *
+ */
+
+static float calc_wp_distance()
+{
+	return sqrtf((plane_data.lat - plane_data.wp_y) * (plane_data.lat - plane_data.wp_y) +
+		     (plane_data.lon - plane_data.wp_x) * (plane_data.lon - plane_data.wp_x));
+}
+
+/**
+ * calc_roll_setpoint
+ *
+ * Calculates the offset angle for the roll plane,
+ * saturates at +- 35 deg.
+ *
+ * @return setpoint on which attitude control should stabilize while changing heading
+ *
+ */
+
+static float calc_roll_setpoint()
+{
+	float setpoint = 0;
+
+	if (plane_data.mode == TAKEOFF) {
+		setpoint = 0;
+
+	} else {
+		setpoint = calc_bearing() - plane_data.yaw;
+
+		if (setpoint < -35)
+			return -35;
+
+		if (setpoint > 35)
+			return 35;
+	}
+
+	return setpoint;
+}
+
+/**
+ * calc_pitch_setpoint
+ *
+ * Calculates the offset angle for the pitch plane
+ * saturates at +- 35 deg.
+ *
+ * @return setpoint on which attitude control should stabilize while changing altitude
+ *
+ */
+
+static float calc_pitch_setpoint()
+{
+	float setpoint = 0;
+
+	if (plane_data.mode == TAKEOFF) {
+		setpoint = 35;
+
+	} else {
+		setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()) * RAD2DEG;
+
+		if (setpoint < -35)
+			return -35;
+
+		if (setpoint > 35)
+			return 35;
+	}
+
+	return setpoint;
+}
+
+/**
+ * calc_throttle_setpoint
+ *
+ * Calculates the throttle setpoint for different flight modes
+ *
+ * @return throttle output setpoint
+ *
+ */
+
+static float calc_throttle_setpoint()
+{
+	float setpoint = 0;
+
+	// if TAKEOFF full throttle
+	if (plane_data.mode == TAKEOFF) {
+		setpoint = 60;
+	}
+
+	// if CRUISE - parameter value
+	if (plane_data.mode == CRUISE) {
+		setpoint = plane_data.airspeed;
+	}
+
+	// if LAND no throttle
+	if (plane_data.mode == LAND) {
+		setpoint = 0;
+	}
+
+	return setpoint;
+}
+
+/**
+ * set_plane_mode
+ *
+ * Sets the plane mode
+ * (TAKEOFF, CRUISE, LOITER or LAND)
+ *
+ */
+
+static void set_plane_mode()
+{
+	if (plane_data.alt < 10) {
+		plane_data.mode = TAKEOFF;
+
+	} else {
+		plane_data.mode = CRUISE;
+		// TODO: if reached waypoint and no further waypoint exists, go to LOITER mode
+	}
+
+	// Debug override - don't need TAKEOFF mode for now
+	plane_data.mode = CRUISE;
+}
+
+/*
+ * fixedwing_control_main
+ *
+ * @param argc number of arguments
+ * @param argv argument array
+ *
+ * @return 0
+ *
+ */
+
+int fixedwing_control_main(int argc, char *argv[])
+{
+	/* print text */
+	printf("Fixedwing control started\n");
+	usleep(100000);
+
+	/* default values for arguments */
+	char *fixedwing_uart_name = "/dev/ttyS1";
+	char *commandline_usage = "\tusage: fixedwing_control -d fixedwing-devicename\n";
+
+	/* read arguments */
+	int i;
+
+	if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
+		if (argc > i + 1) {
+			fixedwing_uart_name = argv[i + 1];
+
+		} else {
+			printf(commandline_usage);
+			return 0;
+		}
+	}
+
+	/* Set up to publish fixed wing control messages */
+	struct fixedwing_control_s control;
+	int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
+
+	/* Subscribe to global position, attitude and rc */
+	struct vehicle_global_position_s global_pos;
+	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	struct vehicle_attitude_s att;
+	int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	struct rc_channels_s rc;
+	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
+
+	/* Control constants */
+	control_outputs.mode = HIL_MODE;
+	control_outputs.nav_mode = 0;
+
+	/* Servo setup */
+
+	int fd;
+	servo_position_t data[PWM_SERVO_MAX_CHANNELS];
+
+	fd = open("/dev/pwm_servo", O_RDWR);
+
+	if (fd < 0) {
+		printf("failed opening /dev/pwm_servo\n");
+	}
+
+	ioctl(fd, PWM_SERVO_ARM, 0);
+
+	int16_t buffer_rc[3];
+	int16_t buffer_servo[3];
+	mixer_data_t mixer_buffer;
+	mixer_buffer.input  = buffer_rc;
+	mixer_buffer.output = buffer_servo;
+
+	mixer_conf_t mixers[3];
+
+	mixers[0].source = PITCH;
+	mixers[0].nr_actuators = 2;
+	mixers[0].dest[0] = AIL_1;
+	mixers[0].dest[1] = AIL_2;
+	mixers[0].dual_rate[0] = 1;
+	mixers[0].dual_rate[1] = 1;
+
+	mixers[1].source = ROLL;
+	mixers[1].nr_actuators = 2;
+	mixers[1].dest[0] = AIL_1;
+	mixers[1].dest[1] = AIL_2;
+	mixers[1].dual_rate[0] = 1;
+	mixers[1].dual_rate[1] = -1;
+
+	mixers[2].source = THROTTLE;
+	mixers[2].nr_actuators = 1;
+	mixers[2].dest[0] = MOT;
+	mixers[2].dual_rate[0] = 1;
+
+	/*
+	 * Main control, navigation and servo routine
+	 */
+
+	while(1)
+	{
+		/*
+		 * DATA Handling
+		 * Fetch current flight data
+		 */
+
+		/* get position, attitude and rc inputs */
+		// XXX add error checking
+		orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+		orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
+		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
+
+		/* scaling factors are defined by the data from the APM Planner
+		 * TODO: ifdef for other parameters (HIL/Real world switch)
+		 */
+
+		/* position values*/
+		plane_data.lat = global_pos.lat / 10000000;
+		plane_data.lon = global_pos.lon / 10000000;
+		plane_data.alt = global_pos.alt / 1000;
+		plane_data.vx = global_pos.vx / 100;
+		plane_data.vy = global_pos.vy / 100;
+		plane_data.vz = global_pos.vz / 100;
+
+		/* attitude values*/
+		plane_data.roll = att.roll;
+		plane_data.pitch = att.pitch;
+		plane_data.yaw = att.yaw;
+		plane_data.rollspeed = att.rollspeed;
+		plane_data.pitchspeed = att.pitchspeed;
+		plane_data.yawspeed = att.yawspeed;
+
+		/* parameter values */
+		get_parameters();
+
+		/* Attitude control part */
+
+//#define MUTE
+#ifndef MUTE
+		/******************************** DEBUG OUTPUT ************************************************************/
+
+		printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att,
+				(int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos,
+				(int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed,
+				(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z);
+
+//		printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint);
+//		printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint);
+//		printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint());
+
+//		printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed()));
+
+//		printf("Current Altitude: %i\n\n", (int)plane_data.alt);
+
+		printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
+				(int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw),
+				(int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed));
+
+//		printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n",
+//				(int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r));
+
+		printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
+				(int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator),
+				(int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle));
+
+		/************************************************************************************************************/
+
+#endif
+
+		/*
+		 * Computation section
+		 *
+		 * The function calls to compute the required control values happen
+		 * in this section.
+		 */
+
+		/* Set plane mode */
+		set_plane_mode();
+
+		/* Calculate the P,Q,R body rates of the aircraft */
+		calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG,
+				plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed);
+
+		/* Calculate the body frame angles of the aircraft */
+		//calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG);
+
+		/* Calculate the output values */
+		control_outputs.roll_ailerons = calc_roll_ail();
+		control_outputs.pitch_elevator = calc_pitch_elev();
+		//control_outputs.yaw_rudder = calc_yaw_rudder();
+		control_outputs.throttle = calc_throttle();
+
+		if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
+
+			if (plane_data.mode == TAKEOFF) {
+				control.attitude_control_output[ROLL] = 0;
+				control.attitude_control_output[PITCH] = 5000;
+				control.attitude_control_output[THROTTLE] = 10000;
+				//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
+			}
+
+			if (plane_data.mode == CRUISE) {
+				control.attitude_control_output[ROLL] = (int16_t)(10000 * control_outputs.roll_ailerons);
+				control.attitude_control_output[PITCH] = (int16_t)(10000 * control_outputs.pitch_elevator);
+				control.attitude_control_output[THROTTLE] = (int16_t)(10000 * control_outputs.throttle);
+				//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
+			}
+
+			control.counter++;
+			control.timestamp = hrt_absolute_time();
+		}
+
+		/* Navigation part */
+
+		// Get GPS Waypoint
+
+		//		if(global_data_wait(&global_data_position_setpoint->access_conf) == 0)
+		//		{
+		//			plane_data.wp_x = global_data_position_setpoint->x;
+		//			plane_data.wp_y = global_data_position_setpoint->y;
+		//			plane_data.wp_z = global_data_position_setpoint->z;
+		//		}
+		//		global_data_unlock(&global_data_position_setpoint->access_conf);
+
+		if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) {	// AUTO mode
+			// AUTO/HYBRID switch
+
+			if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) {
+				plane_data.roll_setpoint = calc_roll_setpoint();
+				plane_data.pitch_setpoint = calc_pitch_setpoint();
+				plane_data.throttle_setpoint = calc_throttle_setpoint();
+
+			} else {
+				plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200;
+				plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
+				plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
+			}
+
+			//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
+
+			// 10 Hz loop
+			usleep(100000);
+
+		} else {
+			control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale;
+			control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale;
+			control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale;
+			// since we don't have a yaw rudder
+			//control->attitude_control_output[3] = global_data_rc_channels->chan[YAW].scale;
+
+			control.counter++;
+			control.timestamp = hrt_absolute_time();
+		}
+
+		/* publish the control data */
+
+		orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
+
+		/* Servo part */
+
+		buffer_rc[ROLL] = control.attitude_control_output[ROLL];
+		buffer_rc[PITCH] = control.attitude_control_output[PITCH];
+		buffer_rc[THROTTLE] = control.attitude_control_output[THROTTLE];
+
+		//mix_and_link(mixers, 3, 2, &mixer_buffer);
+
+		// Scaling and saturation of servo outputs happens here
+
+		data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+		                                  + global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];
+
+		if (data[AIL_1] > SERVO_MAX)
+			data[AIL_1] = SERVO_MAX;
+
+		if (data[AIL_1] < SERVO_MIN)
+			data[AIL_1] = SERVO_MIN;
+
+		data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+		                                  + global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];
+
+		if (data[AIL_2] > SERVO_MAX)
+			data[AIL_2] = SERVO_MAX;
+
+		if (data[AIL_2] < SERVO_MIN)
+			data[AIL_2] = SERVO_MIN;
+
+		data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+		                              + global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];
+
+		if (data[MOT] > SERVO_MAX)
+			data[MOT] = SERVO_MAX;
+
+		if (data[MOT] < SERVO_MIN)
+			data[MOT] = SERVO_MIN;
+
+		int result = write(fd, &data, sizeof(data));
+
+		if (result != sizeof(data)) {
+			printf("failed writing servo outputs\n");
+		}
+
+		/* 20Hz loop*/
+		usleep(50000);
+	}
+
+	return 0;
+}
diff --git a/apps/fixedwing_control/fixedwing_control.h b/apps/fixedwing_control/fixedwing_control.h
new file mode 100644
index 000000000..6023e3967
--- /dev/null
+++ b/apps/fixedwing_control/fixedwing_control.h
@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Ivan Ovinnikov <oivan@ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file Definition of attitude controller
+ */
+
+#ifndef FIXEDWING_CONTROL_H_
+#define FIXEDWING_CONTROL_H_
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/fixedwing_control.h>
+
+/****************************************************************************
+ * Public Data
+ ****************************************************************************/
+
+/****************************************************************************
+ * Internal definitions
+ ****************************************************************************/
+
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+#endif /* FIXEDWING_CONTROL_H_ */
diff --git a/apps/fixedwing_control/fixedwing_control.zip b/apps/fixedwing_control/fixedwing_control.zip
new file mode 100644
index 000000000..edaac33d5
--- /dev/null
+++ b/apps/fixedwing_control/fixedwing_control.zip
diff --git a/apps/fixedwing_control/pid.c b/apps/fixedwing_control/pid.c
new file mode 100644
index 000000000..81b4deac1
--- /dev/null
+++ b/apps/fixedwing_control/pid.c
@@ -0,0 +1,118 @@
+/****************************************************************************
+ * pid.c
+ *
+ *   Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
+ *   Authors: Ivan Ovinnikov <oivan@ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include "pid.h"
+#include "fixedwing_control.h"
+
+/*******************************************************************************
+ * pid()
+ *
+ * Calculates the PID control output given an error
+ *
+ * Input: float error, uint16_t dt, float scaler, float K_p, float K_i, float K_d
+ *
+ * Output: PID control value
+ *
+ ******************************************************************************/
+
+static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax)
+{
+	// PID parameters
+
+	float Kp = K_p;
+	float Ki = K_i;
+	float Kd = K_d;
+	float delta_time = dt;	// delta time
+	float lerror;		// last error value
+	float imax = intmax;			// max integral value
+	float integrator;
+	float derivative;
+	float lderiv;
+	int fCut = 20;		// anything above 20 Hz is considered noise - low pass filter for the derivative
+	float output = 0;		// the output of the PID controller
+
+	output += error * Kp;
+
+	if ((fabs(Kd) > 0) && (dt > 0)) {
+
+		if (PID_DERIVMODE_CALC) {
+			derivative = (error - lerror) / delta_time;
+
+			// discrete low pass filter, cuts out the
+			// high frequency noise that can drive the controller crazy
+			float RC = 1 / (2 * M_PI * fCut);
+			derivative = lderiv +
+				     (delta_time / (RC + delta_time)) * (derivative - lderiv);
+
+			// update state
+			lerror 	= error;
+			lderiv  = derivative;
+
+		} else {
+			derivative = error_deriv;
+		}
+
+		// add in derivative component
+		output 	+= Kd * derivative;
+	}
+
+	printf("PID derivative %i\n", (int)(1000 * derivative));
+
+	// scale the P and D components
+	output *= scaler;
+
+	// Compute integral component if time has elapsed
+	if ((fabs(Ki) > 0) && (dt > 0)) {
+		integrator 		+= (error * Ki) * scaler * delta_time;
+
+		if (integrator < -imax) {
+			integrator = -imax;
+
+		} else if (integrator > imax) {
+			integrator = imax;
+		}
+
+		output += integrator;
+	}
+
+	printf("PID Integrator %i\n", (int)(1000 * integrator));
+
+	return output;
+}
+
diff --git a/apps/fixedwing_control/pid.h b/apps/fixedwing_control/pid.h
new file mode 100644
index 000000000..2f85c6c30
--- /dev/null
+++ b/apps/fixedwing_control/pid.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ * pid.h
+ *
+ *   Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
+ *   Authors: Ivan Ovinnikov <oivan@ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+
+#ifndef PID_H_
+#define PID_H_
+
+static float pid(float error, float error_deriv, uint16_t dt, float scaler, float K_p, float K_i, float K_d, float intmax);
+
+#endif /* PID_H_ */