Fresh import of the PX4 firmware sources.

apps/Makefile

+############################################################################
+# apps/Makefile
+#
+#   Copyright (C) 2011-2012 Uros Platise. All rights reserved.
+#   Authors: Uros Platise <uros.platise@isotel.eu>
+#            Gregory Nutt <gnutt@nuttx.org>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name NuttX nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+-include $(TOPDIR)/Make.defs
+-include $(TOPDIR)/.config
+
+APPDIR = ${shell pwd}
+
+# Application Directories
+
+# CONFIGURED_APPS is the list of all configured built-in directories/built
+#   action. It is created by the configured appconfig file (a copy of which
+#   appears in this directory as .config)
+# SUBDIRS is the list of all directories containing Makefiles.  It is used
+#   only for cleaning. namedapp must always be the first in the list.  This
+#   list can be extended by the .config file as well
+
+CONFIGURED_APPS =
+#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system vsn
+ALL_SUBDIRS = $(dir $(shell /usr/bin/find . -name Makefile))
+SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
+
+
+# There are two different mechanisms for obtaining the list of configured
+# directories:
+#
+# (1) In the legacy method, these paths are all provided in the appconfig
+#     file that is copied to the top-level apps/ directory as .config
+# (2) With the development of the NuttX configuration tool, however, the
+#     selected applications are now enabled by the configuration tool.
+#     The apps/.config file is no longer used.  Instead, the set of
+#     configured build directories can be found by including a Make.defs
+#     file contained in each of the apps/subdirectories.
+#
+# When the NuttX configuration tools executes, it will always define the
+# configure CONFIG_NUTTX_NEWCONFIG to select between these two cases.  Then
+# legacy appconfig files will still work but newly configuration files will
+# also work.  Eventually the CONFIG_NUTTX_NEWCONFIG option will be phased
+# out.
+
+ifeq ($(CONFIG_NUTTX_NEWCONFIG),y)
+
+-include examples/Make.defs
+-include graphics/Make.defs
+-include interpreters/Make.defs
+-include modbus/Make.defs
+-include namedapp/Make.defs
+-include netutils/Make.defs
+-include nshlib/Make.defs
+-include system/Make.defs
+-include vsn/Make.defs
+
+# INSTALLED_APPS is the list of currently available application directories.  It
+# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent
+# application directory. namedapp is always in the list of applications to be
+# built.
+
+INSTALLED_APPS =
+
+# The legacy case:
+
+else
+-include .config
+
+# INSTALLED_APPS is the list of currently available application directories.  It
+# is the same as CONFIGURED_APPS, but filtered to exclude any non-existent
+# application directory. namedapp is always in the list of applications to be
+# built.
+
+INSTALLED_APPS = namedapp
+endif
+
+# Create the list of available applications (INSTALLED_APPS)
+
+define ADD_BUILTIN
+INSTALLED_APPS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi}
+endef
+
+$(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
+
+# The external/ directory may also be added to the INSTALLED_APPS.  But there
+# is no external/ directory in the repository.  Rather, this directory may be
+# provided by the user (possibly as a symbolic link) to add libraries and
+# applications to the standard build from the repository.
+
+INSTALLED_APPS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
+SUBDIRS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
+
+# The final build target
+
+BIN = libapps$(LIBEXT)
+
+# Build targets
+
+all: $(BIN)
+.PHONY: $(INSTALLED_APPS) context depend clean distclean
+
+$(INSTALLED_APPS):
+	@$(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)";
+
+$(BIN):	$(INSTALLED_APPS)
+	@( for obj in $(OBJS) ; do \
+		$(call ARCHIVE, $@, $${obj}); \
+	done ; )
+
+.context:
+	@for dir in $(INSTALLED_APPS) ; do \
+		rm -f $$dir/.context ; \
+		$(MAKE) -C $$dir TOPDIR="$(TOPDIR)"  APPDIR="$(APPDIR)" context ; \
+	done
+	@touch $@
+
+context: .context
+
+.depend: context Makefile $(SRCS)
+	@for dir in $(INSTALLED_APPS) ; do \
+		rm -f $$dir/.depend ; \
+		$(MAKE) -C $$dir TOPDIR="$(TOPDIR)"  APPDIR="$(APPDIR)" depend ; \
+	done
+	@touch $@
+
+depend: .depend
+
+clean:
+	@for dir in $(SUBDIRS) ; do \
+		$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
+	done
+	@rm -f $(BIN) *~ .*.swp *.o
+	$(call CLEAN)
+
+distclean: # clean
+	@for dir in $(SUBDIRS) ; do \
+		$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
+	done
+	@rm -f .config .context .depend
+	@( if [ -e external ]; then \
+		echo "********************************************************"; \
+		echo "* The external directory/link must be removed manually *"; \
+		echo "********************************************************"; \
+	   fi; \
+	) 
+
+

apps/README.txt

+Application Folder
+==================
+
+Contents
+--------
+
+  General
+  Directory Location
+  Named Applications
+  Named Startup main() function
+  NuttShell (NSH) Built-In Commands
+  Synchronous Built-In Commands
+  Application Configuration File
+  Example Named Application
+  Building NuttX with Board-Specific Pieces Outside the Source Tree
+
+General
+-------
+This folder provides various applications found in sub-directories.  These
+applications are not inherently a part of NuttX but are provided you help
+you develop your own applications.  The apps/ directory is a "break away"
+part of the configuration that you may chose to use or not.
+
+Directory Location
+------------------
+The default application directory used by the NuttX build should be named
+apps/ (or apps-x.y/ where x.y is the NuttX version number).  This apps/
+directory should appear in the directory tree at the same level as the
+NuttX directory.  Like:
+
+ .
+ |- nuttx
+ |
+ `- apps
+
+If all of the above conditions are TRUE, then NuttX will be able to
+find the application directory.  If your application directory has a 
+different name or is location at a different position, then you will
+have to inform the NuttX build system of that location.  There are several
+ways to do that:
+
+1) You can define CONFIG_APPS_DIR to be the full path to your application
+   directory in the NuttX configuration file.
+2) You can provide the path to the application directory on the command line
+   like:  make APPDIR=<path> or make CONFIG_APPS_DIR=<path>
+3) When you configure NuttX using tools/configure.sh, you can provide that
+   path to the application directory on the configuration command line
+   like: ./configure.sh -a <app-dir> <board-name>/<config-name>
+
+Named Applications
+------------------
+NuttX also supports applications that can be started using a name string.
+In this case, application entry points with their requirements are gathered
+together in two files:
+
+  - namedapp/namedapp_proto.h  Entry points, prototype function
+  - namedapp/namedapp_list.h  Application specific information and requirements
+
+The build occurs in several phases as different build targets are executed:
+(1) context, (2) depend, and (3) default (all). Application information is
+collected during the make context build phase.
+
+To execute an application function:
+
+  exec_namedapp() is defined in the nuttx/include/apps/apps.h 
+
+NuttShell (NSH) Built-In Commands
+---------------------------------
+One use of named applications is to provide a way of invoking your custom
+application through the NuttShell (NSH) command line.  NSH will support
+a seamless method invoking the applications, when the following option is
+enabled in the NuttX configuration file:
+
+  CONFIG_NSH_BUILTIN_APPS=y
+
+Applications registered in the apps/namedapp/namedapp_list.h file will then
+be accessible from the NSH command line.  If you type 'help' at the NSH
+prompt, you will see a list of the registered commands.
+
+Synchronous Built-In Commands
+-----------------------------
+By default, built-in commands started from the NSH command line will run
+asynchronously with NSH.  If you want to force NSH to execute commands
+then wait for the command to execute, you can enable that feature by
+adding the following to the NuttX configuration file:
+
+CONFIG_SCHED_WAITPID=y
+
+The configuration option enables support for the waitpid() RTOS interface.
+When that interface is enabled, NSH will use it to wait, sleeping until
+the built-in command executes to completion.
+
+Of course, even with CONFIG_SCHED_WAITPID=y defined, specific commands
+can still be forced to run asynchronously by adding the ampersand (&)
+after the NSH command.
+
+Application Configuration File
+------------------------------
+A special configuration file is used to configure which applications
+are to be included in the build.  The source for this file is 
+configs/<board>/<configuration>/appconfig.  The existence of the appconfig
+file in the board configuration directory is sufficient to enable building
+of applications.
+
+The appconfig file is copied into the apps/ directory as .config when
+NuttX is configured.  .config is included in the toplevel apps/Makefile.
+As a minimum, this configuration file must define files to add to the
+CONFIGURED_APPS list like:
+
+  CONFIGURED_APPS  += examples/hello vsn/poweroff
+
+Named Start-Up main() function
+------------------------------
+A named application can even be used as the main, start-up entry point
+into your embedded software.  When the user defines this option in
+the NuttX configuration file:
+
+  CONFIG_BUILTIN_APP_START=<application name>
+  
+that application shall be invoked immediately after system starts
+*instead* of the normal, default "user_start" entry point.
+Note that <application name> must be provided as: "hello", 
+will call:
+
+  int hello_main(int argc, char *argv[])
+
+Example Named Application
+-------------------------
+An example application skeleton can be found under the examples/hello
+sub-directory.  This example shows how a named application can be added
+to the project. One must define:
+
+ 1. create sub-directory as: appname
+ 2. provide entry point: appname_main()
+ 3. set the requirements in the file: Makefile, specially the lines:
+
+  APPNAME    = appname
+  PRIORITY  = SCHED_PRIORITY_DEFAULT
+  STACKSIZE  = 768
+  ASRCS    = asm source file list as a.asm b.asm ...
+  CSRCS    = C source file list as foo1.c foo2.c ..
+
+ 4. add application in the apps/.config
+
+Building NuttX with Board-Specific Pieces Outside the Source Tree
+-----------------------------------------------------------------
+
+Q: Has anyone come up with a tidy way to build NuttX with board-
+   specific pieces outside the source tree?
+A: Here are four:
+
+   1) There is a make target called 'make export'. It will build
+      NuttX, then bundle all of the header files, libaries, startup
+      objects, and other build components into a .zip file. You
+      can can move that .zip file into any build environment you
+      want. You even build NuttX under a DOS CMD window.
+
+      This make target is documented in the top level nuttx/README.txt.
+
+   2) You can replace the entire apps/ directory. If there is
+      nothing in the apps/ directory that you need, you can define
+      CONFIG_APPS_DIR in your .config file so that it points to a
+      different, custom application directory.
+
+      You can copy any pieces that you like from the old apps/directory
+      to your custom apps directory as necessary.
+
+      This is documented in NuttX/configs/README.txt and
+      nuttx/Documentation/NuttxPortingGuide.html (Online at
+      http://nuttx.sourceforge.net/NuttxPortingGuide.html#apndxconfigs
+      under Build options). And in the apps/README.txt file.
+
+   3) If you like the random collection of stuff in the apps/ directory
+      but just want to expand the existing components with your own,
+      external sub-directory then there is an easy way to that too:
+      You just create the sympolic link at apps/external that
+      redirects to your application sub-directory. The apps/Makefile
+      will always automatically check for the existence of an
+      apps/external directory and if it exists, it will automatically
+      incorporate it into the build.
+
+      This feature of the apps/Makefile is documented only here.
+
+      You can, for example, create a script called install.sh that
+      installs a custom application, configuration, and board specific
+      directory:
+
+      a) Copy 'MyBoard' directory to configs/MyBoard.
+      b) Add a symbolic link to MyApplication at apps/external
+      c) Configure NuttX (usually by:
+      
+         tools/configure.sh MyBoard/MyConfiguration
+
+         or simply by copying defconfig->nutt/.config,
+         setenv.sh->nuttx/setenv.sh, Make.defs->nuttx/Make.defs,
+         appconfig->apps/.config
+
+      Using the 'external' link makes it especially easy to add a
+      'built-in' application an existing configuration.
+
+   4) Add any link to apps/
+
+      a) Add symbolic links apps/ to as many other directories as you
+         want.
+      b) Then just add the (relative) paths to the links in your
+         appconfig file (that becomes the apps/.config file).
+
+      That is basically the same as my option #3 but doesn't use the
+      magic 'external' link. The toplevel apps/Makefile will always
+      to build whatever in finds in the apps/.config file (plus the
+      external link if present).

apps/ardrone_control/Makefile

+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build uORB
+#
+
+APPNAME		 = ardrone_control
+PRIORITY	 = SCHED_PRIORITY_MAX - 15
+STACKSIZE	 = 2048
+
+# explicit list of sources - not everything is built currently
+CSRCS		 = ardrone_control.c ardrone_motor_control.c ardrone_control_helper.c rate_control.c attitude_control.c pid.c
+
+include $(APPDIR)/mk/app.mk

apps/ardrone_control/ardrone_control.c

+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file Implementation of AR.Drone 1.0 / 2.0 control interface
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <termios.h>
+#include <time.h>
+#include <sys/prctl.h>
+#include <arch/board/up_hrt.h>
+#include "ardrone_control.h"
+#include "attitude_control.h"
+#include "rate_control.h"
+#include "ardrone_motor_control.h"
+#include "position_control.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/ardrone_control.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/sensor_combined.h>
+
+#include "ardrone_control_helper.h"
+
+__EXPORT int ardrone_control_main(int argc, char *argv[]);
+
+/****************************************************************************
+ * Internal Definitions
+ ****************************************************************************/
+
+
+enum {
+	CONTROL_MODE_RATES = 0,
+	CONTROL_MODE_ATTITUDE = 1,
+} control_mode;
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/*File descriptors */
+int ardrone_write;
+int gpios;
+
+bool position_control_thread_started;
+
+/****************************************************************************
+ * pthread loops
+ ****************************************************************************/
+static void *position_control_loop(void *arg)
+{
+	struct vehicle_status_s *state = (struct vehicle_status_s *)arg;
+	// Set thread name
+	prctl(PR_SET_NAME, "ardrone pos ctrl", getpid());
+
+	while (1) {
+		if (state->state_machine == SYSTEM_STATE_AUTO) {
+//			control_position();   //FIXME TODO XXX
+			/* temporary 50 Hz execution */
+			usleep(20000);
+
+		} else {
+			position_control_thread_started = false;
+			break;
+		}
+	}
+
+	return NULL;
+}
+
+/****************************************************************************
+ * main
+ ****************************************************************************/
+
+int ardrone_control_main(int argc, char *argv[])
+{
+	/* welcome user */
+	printf("[ardrone_control] Control started, taking over motors\n");
+
+	/* default values for arguments */
+	char *ardrone_uart_name = "/dev/ttyS1";
+	control_mode = CONTROL_MODE_RATES;
+
+	char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
+
+	/* read commandline arguments */
+	int i;
+
+	for (i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
+			if (argc > i + 1) {
+				ardrone_uart_name = argv[i + 1];
+
+			} else {
+				printf(commandline_usage);
+				return 0;
+			}
+
+		} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
+			if (argc > i + 1) {
+				if (strcmp(argv[i + 1], "rates") == 0) {
+					control_mode = CONTROL_MODE_RATES;
+
+				} else if (strcmp(argv[i + 1], "attitude") == 0) {
+					control_mode = CONTROL_MODE_ATTITUDE;
+
+				} else {
+					printf(commandline_usage);
+					return 0;
+				}
+
+			} else {
+				printf(commandline_usage);
+				return 0;
+			}
+		}
+	}
+
+	/* open uarts */
+	printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name);
+	ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY);
+
+	/* initialize motors */
+	ar_init_motors(ardrone_write, &gpios);
+	int counter = 0;
+
+	/* pthread for position control */
+	pthread_t position_control_thread;
+	position_control_thread_started = false;
+
+	/* structures */
+	struct vehicle_status_s state;
+	struct vehicle_attitude_s att;
+	struct ardrone_control_s ar_control;
+	struct rc_channels_s rc;
+	struct sensor_combined_s raw;
+	struct ardrone_motors_setpoint_s setpoint;
+
+	/* subscribe to attitude, motor setpoints and system state */
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
+
+	/* publish AR.Drone motor control state */
+	int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control);
+
+	while (1) {
+		/* get a local copy of the vehicle state */
+		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+		/* get a local copy of rc */
+		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
+		/* get a local copy of attitude */
+		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+
+		if (state.state_machine == SYSTEM_STATE_AUTO) {
+			if (false == position_control_thread_started) {
+				pthread_attr_t position_control_thread_attr;
+				pthread_attr_init(&position_control_thread_attr);
+				pthread_attr_setstacksize(&position_control_thread_attr, 2048);
+				pthread_create(&position_control_thread, &position_control_thread_attr, position_control_loop, &state);
+				position_control_thread_started = true;
+			}
+
+			control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
+
+			//No check for remote sticks to disarm in auto mode, land/disarm with ground station
+
+		} else if (state.state_machine == SYSTEM_STATE_MANUAL) {
+			if (control_mode == CONTROL_MODE_RATES) {
+				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+				orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
+				control_rates(&raw, &setpoint);
+
+			} else if (control_mode == CONTROL_MODE_ATTITUDE) {
+				control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
+			}
+
+		} else {
+
+		}
+
+		//fancy led animation...
+		static int blubb = 0;
+
+		if (counter % 20 == 0) {
+			if (blubb == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
+
+			if (blubb == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
+
+			if (blubb == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
+
+			if (blubb == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
+
+			if (blubb == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
+
+			if (blubb == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
+
+			if (blubb == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
+
+			if (blubb == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
+
+			if (blubb == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
+
+			if (blubb == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
+
+			if (blubb == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
+
+			if (blubb == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
+
+			blubb++;
+
+			if (blubb == 12) blubb = 0;
+		}
+
+		/* run at approximately 200 Hz */
+		usleep(5000);
+		counter++;
+	}
+
+	/* close uarts */
+	close(ardrone_write);
+	ar_multiplexing_deinit(gpios);
+
+	printf("[ardrone_control] ending now...\r\n");
+	fflush(stdout);
+	return 0;
+}
+

apps/ardrone_control/ardrone_control.h

+/*
+ * ardrone_control.h
+ *
+ *  Created on: Mar 23, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef ARDRONE_CONTROL_H_
+#define ARDRONE_CONTROL_H_
+
+
+#endif /* ARDRONE_CONTROL_H_ */

apps/ardrone_control/ardrone_control_helper.c

+#include "ardrone_control_helper.h"
+#include <unistd.h>
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+// int read_sensors_raw(sensors_raw_t *sensors_raw)
+// {
+// 	static int ret;
+// 	ret = global_data_wait(&global_data_sensors_raw->access_conf_rate_full);
+
+// 	if (ret == 0) {
+// 		memcpy(sensors_raw->gyro_raw, global_data_sensors_raw->gyro_raw, sizeof(sensors_raw->gyro_raw));
+// //		printf("Timestamp %d\n", &global_data_sensors_raw->timestamp);
+
+// 	} else {
+// 		printf("Controller timeout, no new sensor values available\n");
+// 	}
+
+// 	global_data_unlock(&global_data_sensors_raw->access_conf_rate_full);
+// 	return ret;
+// }
+
+// int read_attitude(global_data_attitude_t *attitude)
+// {
+
+// 	static int ret;
+// 	ret = global_data_wait(&global_data_attitude->access_conf);
+
+// 	if (ret == 0) {
+// 		memcpy(&attitude->roll, &global_data_attitude->roll, sizeof(global_data_attitude->roll));
+// 		memcpy(&attitude->pitch, &global_data_attitude->pitch, sizeof(global_data_attitude->pitch));
+// 		memcpy(&attitude->yaw, &global_data_attitude->yaw, sizeof(global_data_attitude->yaw));
+// 		memcpy(&attitude->rollspeed, &global_data_attitude->rollspeed, sizeof(global_data_attitude->rollspeed));
+// 		memcpy(&attitude->pitchspeed, &global_data_attitude->pitchspeed, sizeof(global_data_attitude->pitchspeed));
+// 		memcpy(&attitude->yawspeed, &global_data_attitude->yawspeed, sizeof(global_data_attitude->yawspeed));
+
+// 	} else {
+// 		printf("Controller timeout, no new attitude values available\n");
+// 	}
+
+// 	global_data_unlock(&global_data_attitude->access_conf);
+
+
+
+// 	return ret;
+// }
+
+// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint)
+// {
+
+// 	if (0 == global_data_trylock(&global_data_quad_motors_setpoint->access_conf)) { //TODO: check if trylock is the right choice, maybe only lock?
+// 		rate_setpoint->motor_front_nw = global_data_quad_motors_setpoint->motor_front_nw;
+// 		rate_setpoint->motor_right_ne = global_data_quad_motors_setpoint->motor_right_ne;
+// 		rate_setpoint->motor_back_se = global_data_quad_motors_setpoint->motor_back_se;
+// 		rate_setpoint->motor_left_sw = global_data_quad_motors_setpoint->motor_left_sw;
+
+// 		global_data_unlock(&global_data_quad_motors_setpoint->access_conf);
+// 	}
+// }

apps/ardrone_control/ardrone_control_helper.h

+/*
+ * ardrone_control_helper.h
+ *
+ *  Created on: May 15, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef ARDRONE_CONTROL_HELPER_H_
+#define ARDRONE_CONTROL_HELPER_H_
+
+#include <stdint.h>
+
+// typedef struct {
+// 	int16_t	gyro_raw[3]; // l3gd20
+// } sensors_raw_t;
+
+// /* Copy quad_motors_setpoint values from global memory to private variables */ //TODO: change this once the new mavlink message for rates is available
+// void read_quad_motors_setpoint(quad_motors_setpoint_t *rate_setpoint);
+
+
+#endif /* ARDRONE_CONTROL_HELPER_H_ */

apps/ardrone_control/ardrone_motor_control.c

+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file Implementation of AR.Drone 1.0 / 2.0 motor control interface
+ */
+
+
+
+#include "ardrone_motor_control.h"
+
+static const unsigned long motor_gpios = GPIO_EXT_1 | GPIO_EXT_2 | GPIO_MULTI_1 | GPIO_MULTI_2;
+static const unsigned long motor_gpio[4] = { GPIO_EXT_1, GPIO_EXT_2, GPIO_MULTI_1, GPIO_MULTI_2 };
+
+typedef union {
+	uint16_t motor_value;
+	uint8_t bytes[2];
+} motor_union_t;
+
+/**
+ * @brief Generate the 8-byte motor set packet
+ *
+ * @return the number of bytes (8)
+ */
+void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4)
+{
+	motor_buf[0] = 0x20;
+	motor_buf[1] = 0x00;
+	motor_buf[2] = 0x00;
+	motor_buf[3] = 0x00;
+	motor_buf[4] = 0x00;
+	/*
+	 * {0x20, 0x00, 0x00, 0x00, 0x00};
+	 * 0x20 is start sign / motor command
+	 */
+	motor_union_t curr_motor;
+	uint16_t nineBitMask = 0x1FF;
+
+	/* Set motor 1 */
+	curr_motor.motor_value = (motor1 & nineBitMask) << 4;
+	motor_buf[0] |= curr_motor.bytes[1];
+	motor_buf[1] |= curr_motor.bytes[0];
+
+	/* Set motor 2 */
+	curr_motor.motor_value = (motor2 & nineBitMask) << 3;
+	motor_buf[1] |= curr_motor.bytes[1];
+	motor_buf[2] |= curr_motor.bytes[0];
+
+	/* Set motor 3 */
+	curr_motor.motor_value = (motor3 & nineBitMask) << 2;
+	motor_buf[2] |= curr_motor.bytes[1];
+	motor_buf[3] |= curr_motor.bytes[0];
+
+	/* Set motor 4 */
+	curr_motor.motor_value = (motor4 & nineBitMask) << 1;
+	motor_buf[3] |= curr_motor.bytes[1];
+	motor_buf[4] |= curr_motor.bytes[0];
+}
+
+void ar_enable_broadcast(int fd)
+{
+	ar_select_motor(fd, 0);
+}
+
+int ar_multiplexing_init()
+{
+	int		fd;
+	
+	fd = open("/dev/gpio", O_RDONLY | O_NONBLOCK);
+
+	if (fd < 0) {
+		printf("GPIO: open fail\n");
+		return fd;
+	}
+
+	if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
+		printf("GPIO: output set fail\n");
+		close(fd);
+		return -1;
+	}
+
+	/* deactivate all outputs */
+	int ret = 0;
+	ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+	if (ret < 0) {
+		printf("GPIO: clearing pins fail\n");
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+int ar_multiplexing_deinit(int fd)
+{
+	if (fd < 0) {
+		printf("GPIO: no valid descriptor\n");
+		return fd;
+	}
+
+	int ret = 0;
+
+	/* deselect motor 1-4 */
+	ret += ioctl(fd, GPIO_SET, motor_gpios);
+
+	if (ret != 0) {
+		printf("GPIO: clear failed %d times\n", ret);
+	}
+
+	if (ioctl(fd, GPIO_SET_INPUT, motor_gpios) != 0) {
+		printf("GPIO: input set fail\n");
+		return -1;
+	}
+
+	close(fd);
+
+	return ret;
+}
+
+int ar_select_motor(int fd, uint8_t motor)
+{
+	int ret = 0;
+	unsigned long gpioset;
+	/*
+	 *  Four GPIOS:
+	 *		GPIO_EXT1
+	 *		GPIO_EXT2
+	 *		GPIO_UART2_CTS
+	 *		GPIO_UART2_RTS
+	 */
+
+	/* select motor 0 to enable broadcast */
+	if (motor == 0) {
+		/* select motor 1-4 */
+		ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
+
+	} else {
+		/* select reqested motor */	
+		ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
+
+		/* deselect all others */
+		gpioset = motor_gpios ^ motor_gpio[motor - 1];
+		ret += ioctl(fd, GPIO_SET, gpioset);
+	}
+
+	return ret;
+}
+
+void ar_init_motors(int ardrone_uart, int *gpios_pin)
+{
+	/* Initialize multiplexing */
+	*gpios_pin = ar_multiplexing_init();
+
+	/* Write ARDrone commands on UART2 */
+	uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
+	uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
+
+	/* initialize all motors
+	 * - select one motor at a time
+	 * - configure motor
+	 */
+	int i;
+	int errcounter = 0;
+
+	for (i = 1; i < 5; ++i) {
+		/* Initialize motors 1-4 */
+		initbuf[3] = i;
+		errcounter += ar_select_motor(*gpios_pin, i);
+
+		write(ardrone_uart, initbuf + 0, 1);
+
+		/* sleep 400 ms */
+		usleep(200000);
+		usleep(200000);
+
+		write(ardrone_uart, initbuf + 1, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		write(ardrone_uart, initbuf + 2, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		write(ardrone_uart, initbuf + 3, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		write(ardrone_uart, initbuf + 4, 1);
+		/* wait 50 ms */
+		usleep(50000);
+
+		/* enable multicast */
+		write(ardrone_uart, multicastbuf + 0, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 1, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 2, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 3, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 4, 1);
+		/* wait 1 ms */
+		usleep(1000);
+
+		write(ardrone_uart, multicastbuf + 5, 1);
+		/* wait 5 ms */
+		usleep(50000);
+	}
+
+	/* start the multicast part */
+	errcounter += ar_select_motor(*gpios_pin, 0);
+
+	if (errcounter != 0) {
+		printf("AR: init sequence incomplete, failed %d times", -errcounter);
+		fflush(stdout);
+	}
+}
+
+/*
+ * Sets the leds on the motor controllers, 1 turns led on, 0 off.
+ */
+void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green)
+{
+	/*
+	 * 2 bytes are sent. The first 3 bits describe the command: 011 means led control
+	 * the following 4 bits are the red leds for motor 4, 3, 2, 1
+	 * then 4 bits with unknown function, then 4 bits for green leds for motor 4, 3, 2, 1
+	 * the last bit is unknown.
+	 *
+	 * The packet is therefore:
+	 * 011 rrrr 0000 gggg 0
+	 */
+	uint8_t leds[2];
+	leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1);
+	leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
+	write(ardrone_uart, leds, 2);
+}

apps/ardrone_control/ardrone_motor_control.h

+/****************************************************************************
+ * px4/ardrone_offboard_control.h
+ *
+ *   Copyright (C) 2012 PX4 Autopilot Project. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <poll.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <drivers/drv_gpio.h>
+
+
+/**
+ * @brief Generate the 8-byte motor set packet
+ *
+ * @return the number of bytes (8)
+ */
+void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
+
+int ar_select_motor(int fd, uint8_t motor);
+
+void ar_enable_broadcast(int fd);
+
+int ar_multiplexing_init(void);
+int ar_multiplexing_deinit(int fd);
+
+void ar_init_motors(int ardrone_uart, int *gpios_uart);
+
+void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
+

apps/ardrone_control/attitude_control.c

+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file Implementation of attitude controller
+ */
+
+#include "attitude_control.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
+#include "ardrone_motor_control.h"
+#include <float.h>
+#include <math.h>
+#include "pid.h"
+#include <arch/board/up_hrt.h>
+
+extern int ardrone_write;
+extern int gpios;
+
+#define CONTROL_PID_ATTITUDE_INTERVAL	5e-3
+
+void turn_xy_plane(const float_vect3 *vector, float yaw,
+		   float_vect3 *result)
+{
+	//turn clockwise
+	static uint16_t counter;
+
+	result->x = (cos(yaw) * vector->x + sin(yaw) * vector->y);
+	result->y = (-sin(yaw) * vector->x + cos(yaw) * vector->y);
+	result->z = vector->z; //leave direction normal to xy-plane untouched
+
+	counter++;
+}
+
+void navi2body_xy_plane(const float_vect3 *vector, const float yaw,
+			float_vect3 *result)
+{
+	turn_xy_plane(vector, yaw, result);
+//	result->x = vector->x;
+//	result->y = vector->y;
+//	result->z = vector->z;
+	//	result->x = cos(yaw) * vector->x + sin(yaw) * vector->y;
+	//	result->y = -sin(yaw) * vector->x + cos(yaw) * vector->y;
+	//	result->z = vector->z; //leave direction normal to xy-plane untouched
+}
+
+void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status, int ardrone_pub, struct ardrone_control_s *ar_control)
+{
+	static int motor_skip_counter = 0;
+
+	static PID_t yaw_pos_controller;
+	static PID_t yaw_speed_controller;
+	static PID_t nick_controller;
+	static PID_t roll_controller;
+
+	static const float min_gas = 1;
+	static const float max_gas = 512;
+	static uint16_t motor_pwm[4] = {0, 0, 0, 0};
+	static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+//	static float remote_control_weight_z = 1;
+//	static float position_control_weight_z = 0;
+
+	static float pid_yawpos_lim;
+	static float pid_yawspeed_lim;
+	static float pid_att_lim;
+
+	static bool initialized;
+
+	static float_vect3 attitude_setpoint_navigationframe_from_positioncontroller;
+
+	static hrt_abstime now_time;
+	static hrt_abstime last_time;
+
+	static commander_state_machine_t current_state;
+
+	/* initialize the pid controllers when the function is called for the first time */
+	if (initialized == false) {
+
+		pid_init(&yaw_pos_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU],
+			 PID_MODE_DERIVATIV_CALC, 154);
+
+		pid_init(&yaw_speed_controller,
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
+			 PID_MODE_DERIVATIV_CALC, 155);
+
+		pid_init(&nick_controller,
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
+			 PID_MODE_DERIVATIV_SET, 156);
+
+		pid_init(&roll_controller,
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+			 (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU],
+			 PID_MODE_DERIVATIV_SET, 157);
+
+		pid_yawpos_lim = 	global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
+		pid_yawspeed_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim =	(max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+
+		//TODO: true initialization? get gps while on ground?
+		attitude_setpoint_navigationframe_from_positioncontroller.x = 0.0f;
+		attitude_setpoint_navigationframe_from_positioncontroller.y = 0.0f;
+		attitude_setpoint_navigationframe_from_positioncontroller.z = 0.0f;
+
+		last_time = 0;
+		initialized = true;
+	}
+
+	/* load new parameters with lower rate */
+	if (motor_skip_counter % 50 == 0) {
+		pid_set_parameters(&yaw_pos_controller,
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_P],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_I],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_D],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_AWU]);
+
+		pid_set_parameters(&yaw_speed_controller,
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_P],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_I],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
+
+		pid_set_parameters(&nick_controller,
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
+
+		pid_set_parameters(&roll_controller,
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
+				   (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_AWU]);
+
+		pid_yawpos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_YAWPOS_LIM];
+		pid_yawspeed_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_LIM];
+		pid_att_lim = (max_gas - min_gas) * global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_LIM];
+	}
+
+	current_state = status->state_machine;
+	float_vect3 attitude_setpoint_bodyframe = {}; //this is the setpoint in the bodyframe "mixed" together from the setpoint from the remote and the setpoint from the position controller
+
+	if (current_state == SYSTEM_STATE_AUTO) {
+
+		attitude_setpoint_navigationframe_from_positioncontroller.x = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[0];
+		attitude_setpoint_navigationframe_from_positioncontroller.y = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[1];
+		attitude_setpoint_navigationframe_from_positioncontroller.z = ar_control->attitude_setpoint_navigationframe_from_positioncontroller[2];
+
+		float yaw_e = att->yaw - attitude_setpoint_navigationframe_from_positioncontroller.z;
+
+		// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
+		if (yaw_e > M_PI) {
+			yaw_e -= 2.0f * M_PI;
+		}
+
+		if (yaw_e < -M_PI) {
+			yaw_e += 2.0f * M_PI;
+		}
+
+		attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL);
+
+
+		/* limit control output */
+		if (attitude_setpoint_navigationframe_from_positioncontroller.z > pid_yawpos_lim) {
+			attitude_setpoint_navigationframe_from_positioncontroller.z = pid_yawpos_lim;
+			yaw_pos_controller.saturated = 1;
+		}
+
+		if (attitude_setpoint_navigationframe_from_positioncontroller.z < -pid_yawpos_lim) {
+			attitude_setpoint_navigationframe_from_positioncontroller.z = -pid_yawpos_lim;
+			yaw_pos_controller.saturated = 1;
+		}
+
+		//transform attitude setpoint from position controller from navi to body frame on xy_plane
+		float_vect3 attitude_setpoint_bodyframe_from_positioncontroller;
+		navi2body_xy_plane(&attitude_setpoint_navigationframe_from_positioncontroller, att->yaw , &attitude_setpoint_bodyframe_from_positioncontroller); //yaw angle= att->yaw
+		//now everything is in body frame
+
+
+		//TODO: here we decide which input (position controller or ppm) we use. For now we have only the ppm, this should be decided dpending on the state machione (manula or auto) ppm should always overwrite auto (?)
+		attitude_setpoint_bodyframe.x = attitude_setpoint_bodyframe_from_positioncontroller.x;
+		attitude_setpoint_bodyframe.y = attitude_setpoint_bodyframe_from_positioncontroller.y;
+		attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
+
+	} else if (current_state == SYSTEM_STATE_MANUAL) {
+		attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f;
+		attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f;
+		attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265;
+	}
+
+	/* add an attitude offset which needs to be estimated somewhere */
+	attitude_setpoint_bodyframe.x += global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET];
+	attitude_setpoint_bodyframe.y += global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET];
+
+	/*Calculate Controllers*/
+	//control Nick
+	float nick = pid_calculate(&nick_controller, attitude_setpoint_bodyframe.y, att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	//control Roll
+	float roll = pid_calculate(&roll_controller, attitude_setpoint_bodyframe.x, att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	//control Yaw Speed
+	float yaw = pid_calculate(&yaw_speed_controller, attitude_setpoint_bodyframe.z, att->yawspeed, 0, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
+
+	//compensation to keep force in z-direction
+	float zcompensation;
+
+	if (fabs(att->roll) > 0.5f) {
+		zcompensation = 1.13949393f;
+
+	} else {
+		zcompensation = 1.0f / cosf(att->roll);
+	}
+
+	if (fabs(att->pitch) > 0.5f) {
+		zcompensation *= 1.13949393f;
+
+	} else {
+		zcompensation *= 1.0f / cosf(att->pitch);
+	}
+
+	// use global_data.position_control_output.z and mix parameter global_data.param[PARAM_MIX_POSITION_Z_WEIGHT]
+	// to compute thrust for Z position control
+	//
+	//	float motor_thrust = min_gas +
+	//			( ( 1 - global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] ) * ( max_gas - min_gas ) * global_data.gas_remote * zcompensation )
+	//		   + ( global_data.param[PARAM_MIX_POSITION_Z_WEIGHT] * ( max_gas - min_gas ) * controlled_thrust * zcompensation );
+	//calculate the basic thrust
+
+
+
+	float motor_thrust = 0;
+
+	// FLYING MODES
+	if (current_state == SYSTEM_STATE_MANUAL) {
+		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;
+
+	} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
+		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;	//TODO
+
+	} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
+		motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;	//TODO
+
+	} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
+		motor_thrust = 0;	//immediately cut off thrust!
+
+	} else {
+		motor_thrust = 0;	// Motor thrust must be zero in any other mode!
+	}
+
+	// Convertion to motor-step units
+	motor_thrust *= zcompensation;
+	motor_thrust *= max_gas / 20000.0f; //TODO: check this
+	motor_thrust += (max_gas - min_gas) / 2.f;
+
+	//limit control output
+	//yawspeed
+	if (yaw > pid_yawspeed_lim) {
+		yaw = pid_yawspeed_lim;
+		yaw_speed_controller.saturated = 1;
+	}
+
+	if (yaw < -pid_yawspeed_lim) {
+		yaw = -pid_yawspeed_lim;
+		yaw_speed_controller.saturated = 1;
+	}
+
+	if (nick > pid_att_lim) {
+		nick = pid_att_lim;
+		nick_controller.saturated = 1;
+	}
+
+	if (nick < -pid_att_lim) {
+		nick = -pid_att_lim;
+		nick_controller.saturated = 1;
+	}
+
+
+	if (roll > pid_att_lim) {
+		roll = pid_att_lim;
+		roll_controller.saturated = 1;
+	}
+
+	if (roll < -pid_att_lim) {
+		roll = -pid_att_lim;
+		roll_controller.saturated = 1;
+	}
+
+	/* Emit controller values */
+	ar_control->setpoint_thrust_cast = motor_thrust;
+	ar_control->setpoint_attitude[0] = attitude_setpoint_bodyframe.x;
+	ar_control->setpoint_attitude[1] = attitude_setpoint_bodyframe.y;
+	ar_control->setpoint_attitude[2] = attitude_setpoint_bodyframe.z;
+	ar_control->attitude_control_output[0] = roll;
+	ar_control->attitude_control_output[1] = nick;
+	ar_control->attitude_control_output[2] = yaw;
+	ar_control->zcompensation = zcompensation;
+	orb_publish(ORB_ID(ardrone_control), ardrone_pub, ar_control);
+
+	static float output_band = 0.f;
+	static float band_factor = 0.75f;
+	static float startpoint_full_control = 150.0f;		//TODO
+	static float yaw_factor = 1.0f;
+
+	if (motor_thrust <= min_gas) {
+		motor_thrust = min_gas;
+		output_band = 0.f;
+
+	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_gas) {
+		output_band = band_factor * (motor_thrust - min_gas);
+
+	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_gas - band_factor * startpoint_full_control) {
+		output_band = band_factor * startpoint_full_control;
+
+	} else if (motor_thrust >= max_gas - band_factor * startpoint_full_control) {
+		output_band = band_factor * (max_gas - motor_thrust);
+	}
+
+	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
+
+	// FRONT (MOTOR 1)
+	motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw);
+
+	// RIGHT (MOTOR 2)
+	motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw);
+
+	// BACK (MOTOR 3)
+	motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw);
+
+	// LEFT (MOTOR 4)
+	motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw);
+
+	// if we are not in the output band
+	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
+	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
+	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
+	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
+
+		yaw_factor = 0.5f;
+		// FRONT (MOTOR 1)
+		motor_calc[0] = motor_thrust + (roll / 2 + nick / 2 - yaw * yaw_factor);
+
+		// RIGHT (MOTOR 2)
+		motor_calc[1] = motor_thrust + (-roll / 2 + nick / 2 + yaw * yaw_factor);
+
+		// BACK (MOTOR 3)
+		motor_calc[2] = motor_thrust + (-roll / 2 - nick / 2 - yaw * yaw_factor);
+
+		// LEFT (MOTOR 4)
+		motor_calc[3] = motor_thrust + (roll / 2 - nick / 2 + yaw * yaw_factor);
+	}
+
+	uint8_t i;
+
+	for (i = 0; i < 4; i++) {
+		//check for limits
+		if (motor_calc[i] < motor_thrust - output_band) {
+			motor_calc[i] = motor_thrust - output_band;
+		}
+
+		if (motor_calc[i] > motor_thrust + output_band) {
+			motor_calc[i] = motor_thrust + output_band;
+		}
+	}
+
+	// Write out actual thrust
+	motor_pwm[0] = (uint16_t) motor_calc[0];
+	motor_pwm[1] = (uint16_t) motor_calc[1];
+	motor_pwm[2] = (uint16_t) motor_calc[2];
+	motor_pwm[3] = (uint16_t) motor_calc[3];
+
+	//SEND MOTOR COMMANDS
+	uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
+	ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
+	write(ardrone_write, motorSpeedBuf, 5);
+
+	motor_skip_counter++;
+
+//		now_time = hrt_absolute_time() / 1000000;
+//		if(now_time - last_time > 0)
+//		{
+//			printf("Counter: %ld\n",control_counter);
+//			last_time = now_time;
+//			control_counter = 0;
+//		}
+}

apps/ardrone_control/attitude_control.h

+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* @file attitude control for quadrotors */
+
+#ifndef ATTITUDE_CONTROL_H_
+#define ATTITUDE_CONTROL_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/ardrone_control.h>
+#include <uORB/topics/vehicle_status.h>
+
+void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attitude_s *att,
+		      const struct vehicle_status_s *status, int ardrone_pub,
+		      struct ardrone_control_s *ar_control);
+
+#endif /* ATTITUDE_CONTROL_H_ */

apps/ardrone_control/pid.h

+/*
+ * pid.h
+ *
+ *  Created on: May 29, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+#include <stdint.h>
+
+/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
+ * val_dot in pid_calculate() will be ignored */
+#define PID_MODE_DERIVATIV_CALC	0
+/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
+#define PID_MODE_DERIVATIV_SET	1
+
+typedef struct {
+	float kp;
+	float ki;
+	float kd;
+	float intmax;
+	float sp;
+	float integral;
+	float error_previous;
+	uint8_t mode;
+	uint8_t plot_i;
+	uint8_t count;
+	uint8_t saturated;
+} PID_t;
+
+void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, uint8_t mode, uint8_t plot_i);
+void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax);
+//void pid_set(PID_t *pid, float sp);
+float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
+
+
+
+#endif /* PID_H_ */