Fresh import of the PX4 firmware sources.

Debug/NuttX

@@ -0,0 +1,211 @@
+#
+# Generic GDB macros for working with NuttX
+#
+
+echo Loading NuttX GDB macros.  Use 'help nuttx' for more information.\n
+
+define nuttx
+	echo Use 'help nuttx' for more information.\n
+end
+
+document nuttx
+. Various macros for working with NuttX.
+.
+.    showheap
+.        Prints the contents of the malloc heap(s).
+.    showtasks
+.        Prints a list of all tasks.
+.    showtask <address>
+.        Prints information about the task at <address>
+.
+. Use 'help <macro>' for more specific help.
+end
+
+################################################################################
+# Heap display
+################################################################################
+
+define _showheap
+	set $index = $arg0
+	if (sizeof(struct mm_allocnode_s) == 4)
+		set $MM_ALLOC_BIT = 0x8000
+	else
+		set $MM_ALLOC_BIT = 0x80000000
+	end
+	printf "HEAP %d  %p - %p\n", $index, g_heapstart[$index], g_heapend[$index]
+	printf "ptr      size\n"
+	set $node = (char *)g_heapstart[$index] + sizeof(struct mm_allocnode_s)
+	while $node < g_heapend[$index]
+		printf "  %p", $node
+		set $nodestruct = (struct mm_allocnode_s *)$node
+		printf "  %u", $nodestruct->size
+		if !($nodestruct->preceding & $MM_ALLOC_BIT)
+			printf " FREE"
+		end
+		if ($nodestruct->size > g_heapsize) || (($node + $nodestruct->size) > g_heapend[$index])
+			printf "  (BAD SIZE)"
+		end
+		printf "\n"
+		set $node = $node + $nodestruct->size
+	end
+end
+
+define showheap
+	set $nheaps = sizeof(g_heapstart) / sizeof(g_heapstart[0])
+	printf "Printing %d heaps\n", $nheaps
+	set $heapindex = (int)0
+	while $heapindex < $nheaps
+		showheap $heapindex
+		set $heapindex = $heapindex + 1
+	end
+end
+
+document showheap
+.    showheap
+.        Prints the contents of the malloc heap(s).
+end
+
+################################################################################
+# Task display
+################################################################################
+
+define _showtask_oneline
+	set $task = (struct _TCB *)$arg0
+	printf "    %p  %.2d %.3d %s\n", $task, $task->pid, $task->sched_priority, $task->name
+end
+
+define _showtasklist
+	set $queue = (dq_queue_t *)$arg0
+	set $cursor = (dq_entry_t *)$queue->head
+
+	if $cursor != 0
+		printf "    TCB        PID PRI\n"
+	else
+		printf "    <none>\n"
+	end
+
+	while $cursor != 0
+
+		_showtask_oneline $cursor
+
+		if $cursor == $queue->tail
+			set $cursor = 0
+		else
+			set $next = $cursor->flink
+
+			if $next->blink != $cursor
+				printf "task linkage corrupt\n"
+				set $cursor = 0
+			else
+				set $cursor = $next
+			end
+		end
+	end
+end
+
+#
+# Print task registers for a NuttX v7em target with FPU enabled.
+#
+define _showtaskregs_v7em
+	set $task = (struct _TCB *)$arg0
+	set $regs = (uint32_t *)&($task->xcp.regs[0])
+
+	printf "    r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[27], $regs[28], $regs[29], $regs[30], $regs[2], $regs[3], $regs[4], $regs[5]
+	printf "    r8: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $regs[6], $regs[7], $regs[8], $regs[9], $regs[31], $regs[0], $regs[32], $regs[33]
+	printf "    XPSR 0x%08x EXC_RETURN 0x%08x PRIMASK 0x%08x\n", $regs[34], $regs[10], $regs[1]
+end
+
+#
+# Print current registers for a NuttX v7em target with FPU enabled.
+#
+define _showcurrentregs_v7em
+	printf "    r0: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $r0, $r1, $r2, $r3, $r4, $r5, $r6, $r7
+	printf "    r8: 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x 0x%08x\n", $r8, $r9, $r10, $r11, $r12, $r13, $r14, $r15
+	printf "    XPSR 0x%08x\n", $xpsr
+end
+
+#
+# Print details of a semaphore
+#
+define _showsemaphore
+	printf "count %d ", $arg0->semcount
+	if $arg0->hlist.holder != 0
+		set $_task = (struct _TCB *)$arg0->hlist.holder
+		printf "held by %s", $_task->name
+	end
+	printf "\n"
+end
+
+define showtask
+	set $task = (struct _TCB *)$arg0
+
+	printf "%p %.2d ", $task, $task->pid
+	_showtaskstate $task
+	printf " %s\n", $task->name
+	set $stack_free = 0
+	while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
+		set $stack_free = $stack_free + 1
+	end
+	printf"    stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
+
+	if $task->task_state == TSTATE_WAIT_SEM
+		printf "    waiting on %p ", $task->waitsem
+		_showsemaphore $task->waitsem
+	end
+
+	if $task->task_state != TSTATE_TASK_RUNNING
+		_showtaskregs_v7em $task
+	else
+		_showcurrentregs_v7em
+	end
+
+	# XXX print registers here
+end
+
+document showtask
+.    showtask <TCB pointer>
+.        Print details of a task.
+end
+
+define _showtaskstate
+	if $arg0->task_state == TSTATE_TASK_INVALID
+		printf "INVALID"
+	end
+	if $arg0->task_state == TSTATE_TASK_PENDING
+		printf "PENDING"
+	end
+	if $arg0->task_state == TSTATE_TASK_READYTORUN
+		printf "READYTORUN"
+	end
+	if $arg0->task_state == TSTATE_TASK_RUNNING
+		printf "RUNNING"
+	end
+	if $arg0->task_state == TSTATE_TASK_INACTIVE
+		printf "INACTIVE"
+	end
+	if $arg0->task_state == TSTATE_WAIT_SEM
+		printf "WAIT_SEM"
+	end
+	if $arg0->task_state == TSTATE_WAIT_SIG
+		printf "WAIT_SIG"
+	end
+	if $arg0->task_state > TSTATE_WAIT_SIG
+		printf "%d", $arg0->task_state
+	end
+end
+
+define showtasks
+	printf "PENDING\n"
+	_showtasklist &g_pendingtasks
+	printf "RUNNABLE\n"
+	_showtasklist &g_readytorun
+	printf "WAITING\n"
+	_showtasklist &g_waitingforsemaphore
+	printf "INACTIVE\n"
+	_showtasklist &g_inactivetasks
+end
+
+document showtasks
+.    showtasks
+.        Print a list of all tasks in the system, separated into their respective queues.
+end

Debug/NuttX_BMP

@@ -0,0 +1,6 @@
+#
+# Setup macros for the BlackMagic debug probe and NuttX.
+#
+
+mon swdp_scan
+attach 1

Debug/memdump

@@ -0,0 +1,11 @@
+define f4_memdump
+	shell mkdir -p /tmp/dump
+	printf "Dumping CCSRAM to /tmp/dump/ccsram\n"
+	dump memory /tmp/dump/ccsram 0x10000000 0x10010000
+	printf "Dumping SRAM to /tmp/dump/sram\n"
+	dump memory /tmp/dump/sram 0x20000000 0x20020000
+end
+
+document f4_memdump
+Dumps the STM32F4 memory to files in /tmp/dump.
+end

Debug/stm32f4x.cfg

@@ -0,0 +1,64 @@
+# script for stm32f2xxx
+
+if { [info exists CHIPNAME] } {
+   set  _CHIPNAME $CHIPNAME
+} else {
+   set  _CHIPNAME stm32f4xxx
+}
+
+if { [info exists ENDIAN] } {
+   set  _ENDIAN $ENDIAN
+} else {
+   set  _ENDIAN little
+}
+
+# Work-area is a space in RAM used for flash programming
+# By default use 64kB
+if { [info exists WORKAREASIZE] } {
+   set  _WORKAREASIZE $WORKAREASIZE
+} else {
+   set  _WORKAREASIZE 0x10000
+}
+
+# JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz
+#
+# Since we may be running of an RC oscilator, we crank down the speed a
+# bit more to be on the safe side. Perhaps superstition, but if are
+# running off a crystal, we can run closer to the limit. Note
+# that there can be a pretty wide band where things are more or less stable.
+jtag_khz 1000
+
+jtag_nsrst_delay 100
+jtag_ntrst_delay 100
+
+#jtag scan chain
+if { [info exists CPUTAPID ] } {
+   set _CPUTAPID $CPUTAPID
+} else {
+  # See STM Document RM0033
+  # Section 32.6.3 - corresponds to Cortex-M3 r2p0
+   set _CPUTAPID 0x4ba00477
+}
+jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID
+
+if { [info exists BSTAPID ] } {
+   set _BSTAPID $BSTAPID
+} else {
+  # See STM Document RM0033
+  # Section 32.6.2
+  # 
+  set _BSTAPID 0x06413041
+}
+jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID
+
+set _TARGETNAME $_CHIPNAME.cpu
+target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto
+
+$_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0
+
+set _FLASHNAME $_CHIPNAME.flash
+flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME
+
+# if srst is not fitted use SYSRESETREQ to
+# perform a soft reset
+cortex_m3 reset_config sysresetreq

Documentation/README

@@ -0,0 +1,37 @@
+Linux/Mac OS X
+==============
+To install doxygen: 
+$sudo apt-get install doxygen
+
+If the above does not work go to:
+http://www.stack.nl/~dimitri/doxygen/download.html for the correct download.
+
+Then go to the following website for inforamtion on the install:
+http://www.stack.nl/~dimitri/doxygen/install.html
+
+Then to generate the html, run the following code while you are in the qgroundcontrol/doc directory:
+$doxygen Doxyfile
+
+The html file index.html should be in doc/html unless you changed the output directory. 
+
+The other option for generating the documentation is to use the wizard:
+$doxywizard &
+
+doxywizard information:
+http://www.stack.nl/~dimitri/doxygen/doxywizard_usage.html
+
+Or go to the Doxygen Manual for information at the website noted below.
+
+Windows
+=======
+Go to the following website for the correct download and follow the wizard to install:
+http://www.stack.nl/~dimitri/doxygen/download.html
+
+Run the wizard to generate the documentation. 
+Go to the website below or the Doxygen Manual for information on running doxywizard.
+http://www.stack.nl/~dimitri/doxygen/doxywizard_usage.html
+
+Doxygen Manual
+==============
+http://www.stack.nl/~dimitri/doxygen/
+

Documentation/doxygen.sh

@@ -0,0 +1,3 @@
+#!/bin/sh
+rm -rf html
+doxygen

Documentation/versionfilter.sh

@@ -0,0 +1,2 @@
+#!/bin/sh
+git log --pretty=format:"Last change: commit %h - %aN, %ar : %s" -1 $1 || echo no git

Firmware.sublime-project

@@ -0,0 +1,22 @@
+{
+	"folders":
+	[
+		{
+			"path": "."
+		}
+	],
+	"settings":
+	{
+		"tab_size": 8,
+		"translate_tabs_to_spaces": false
+	},
+	"build_systems":
+	[
+		{
+			"name": "PX4",
+			"working_dir": "${project_path}",
+			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
+			"cmd": ["make"]
+		}
+	]
+}

Images/px4fmu.prototype

@@ -0,0 +1,12 @@
+{
+    "board_id": 5, 
+    "magic": "PX4FWv1", 
+    "description": "Firmware for the PX4FMU board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4FMU",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}

Images/px4io.prototype

@@ -0,0 +1,12 @@
+{
+    "board_id": 7, 
+    "magic": "PX4FWv1", 
+    "description": "Firmware for the PX4IO board", 
+    "image": "", 
+    "build_time": 0, 
+    "summary": "PX4IO",
+    "version": "0.1",
+    "image_size": 0,
+    "git_identity": "",
+    "board_revision": 0
+}

Makefile

@@ -0,0 +1,142 @@
+#
+# Top-level Makefile for building PX4 firmware images.
+#
+#
+# Note that this is a transitional process; the eventual goal is for this
+# project to slim down and simply generate PX4 link kits via the NuttX
+# 'make export' mechanism.
+#
+#
+
+#
+# Some useful paths.
+#
+export PX4BASE		 = $(realpath $(dir $(lastword $(MAKEFILE_LIST))))
+export NUTTX_SRC	 = $(PX4BASE)/nuttx
+export NUTTX_APPS	 = $(PX4BASE)/apps
+export MAVLINK_SRC	 = $(PX4BASE)/mavlink
+export ROMFS_SRC	 = $(PX4BASE)/ROMFS
+export IMAGE_DIR	 = $(PX4BASE)/Images
+
+#
+# Tools
+#
+MKFW			 = $(PX4BASE)/Tools/px_mkfw.py
+UPLOADER		 = $(PX4BASE)/Tools/px_uploader.py
+
+#
+# What are we currently configured for?
+#
+CONFIGURED		 = $(PX4BASE)/.configured
+ifeq ($(wildcard $(CONFIGURED)),)
+# the $(CONFIGURED) target will make this a reality before building
+export TARGET		 = px4fmu
+$(shell echo $(TARGET) > $(CONFIGURED))
+else
+export TARGET		 = $(shell cat $(CONFIGURED))
+endif
+
+#
+# What we will build
+#
+FIRMWARE_BUNDLE		 = $(IMAGE_DIR)/$(TARGET).px4
+FIRMWARE_BINARY		 = $(IMAGE_DIR)/$(TARGET).bin
+FIRMWARE_PROTOTYPE	 = $(IMAGE_DIR)/$(TARGET).prototype
+
+#
+# Debugging
+#
+MQUIET			 = --no-print-directory
+#MQUIET			 = --print-directory
+
+all:			$(FIRMWARE_BUNDLE)
+
+#
+# Generate a wrapped .px4 file from the built binary
+#
+$(FIRMWARE_BUNDLE):	$(FIRMWARE_BINARY) $(MKFW) $(FIRMWARE_PROTOTYPE)
+	@echo Generating $@
+	@$(MKFW) --prototype $(FIRMWARE_PROTOTYPE) \
+		--git_identity $(PX4BASE) \
+		--image $(FIRMWARE_BINARY) > $@
+#
+# Build the firmware binary.
+#
+.PHONY:			$(FIRMWARE_BINARY)
+$(FIRMWARE_BINARY):	configure_$(TARGET) setup_$(TARGET)
+	@echo Building $@
+	@make -C $(NUTTX_SRC) -r $(MQUIET) all
+	@cp $(NUTTX_SRC)/nuttx.bin $@
+
+#
+# The 'configure' targets select one particular firmware configuration
+# and makes it current.
+#
+configure_px4fmu:
+ifneq ($(TARGET),px4fmu)
+	@make -C $(PX4BASE) distclean
+endif
+	@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4fmu/nsh
+	@echo px4fmu > $(CONFIGURED)
+
+configure_px4io:
+ifneq ($(TARGET),px4io)
+	@make -C $(PX4BASE) distclean
+endif
+	@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4io/io
+	@echo px4io > $(CONFIGURED)
+
+#
+# Per-configuration additional targets
+#
+.PHONY:			px4fmu_setup
+setup_px4fmu:
+	@echo Generating ROMFS
+	@make -C $(ROMFS_SRC) all
+
+setup_px4io:
+
+#
+# Firmware uploading.
+#
+
+# serial port defaults by operating system.
+SYSTYPE			 = $(shell uname)
+ifeq ($(SYSTYPE),Darwin)
+SERIAL_PORTS		?= "/dev/tty.usbmodemPX1,/dev/tty.usbmodemPX2,/dev/tty.usbmodemPX3,/dev/tty.usbmodemPX4"
+endif
+ifeq ($(SYSTYPE),Linux)
+SERIAL_PORTS		?= "/dev/ttyACM5,/dev/ttyACM4,/dev/ttyACM3,/dev/ttyACM2,/dev/ttyACM1,/dev/ttyACM0"
+endif
+ifeq ($(SERIAL_PORTS),)
+SERIAL_PORTS		 = "\\\\.\\COM18,\\\\.\\COM17,\\\\.\\COM16,\\\\.\\COM15,\\\\.\\COM14,\\\\.\\COM13,\\\\.\\COM12,\\\\.\\COM11,\\\\.\\COM10,\\\\.\\COM9,\\\\.\\COM8,\\\\.\\COM7,\\\\.\\COM6,\\\\.\\COM5,\\\\.\\COM4,\\\\.\\COM3,\\\\.\\COM2,\\\\.\\COM1,\\\\.\\COM0"
+endif
+
+upload:		$(FIRMWARE_BUNDLE) $(UPLOADER)
+	@python -u $(UPLOADER) --port $(SERIAL_PORTS) $(FIRMWARE_BUNDLE)
+
+#
+# Hacks and fixups
+#
+
+ifeq ($(SYSTYPE),Darwin)
+# PATH inherited by Eclipse may not include toolchain install location 
+export PATH			 := $(PATH):/usr/local/bin
+endif
+
+#
+# Cleanup targets.  'clean' should remove all built products and force
+# a complete re-compilation, 'distclean' should remove everything 
+# that's generated leaving only files that are in source control.
+#
+.PHONY:	clean
+clean:
+	@make -C $(NUTTX_SRC) -r $(MQUIET) clean
+	@make -C $(ROMFS_SRC) -r $(MQUIET) clean
+
+.PHONY:	distclean
+distclean:
+	@rm -f $(CONFIGURED)
+	@make -C $(NUTTX_SRC) -r $(MQUIET) distclean
+	@make -C $(ROMFS_SRC) -r $(MQUIET) distclean
+

ROMFS/.gitignore

@@ -0,0 +1 @@
+/img

ROMFS/Makefile

@@ -0,0 +1,102 @@
+#
+# Makefile to generate a PX4FMU ROMFS image.
+#
+# In normal use, 'make install' will generate a new ROMFS header and place it
+# into the px4fmu configuration in the appropriate location.
+#
+
+#
+# Directories of interest
+#
+SRCROOT		?= $(dir $(lastword $(MAKEFILE_LIST)))
+BUILDROOT	?= $(SRCROOT)/img
+ROMFS_HEADER	?= $(SRCROOT)/../nuttx/configs/px4fmu/include/nsh_romfsimg.h
+
+#
+# List of files to install in the ROMFS, specified as <source>~<destination>
+#
+ROMFS_FSSPEC	:= $(SRCROOT)/scripts/rcS~init.d/rcS \
+		   $(SRCROOT)/scripts/rc.sensors~init.d/rc.sensors \
+		   $(SRCROOT)/scripts/rc.logging~init.d/rc.logging \
+		   $(SRCROOT)/scripts/rc.standalone~init.d/rc.standalone \
+		   $(SRCROOT)/scripts/rc.PX4IO~init.d/rc.PX4IO \
+		   $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR
+
+#
+# Add the PX4IO firmware to the spec if someone has dropped it into the
+# source directory, or otherwise specified its location.
+#
+# Normally this is only something you'd do when working on PX4IO; most
+# users will upgrade with firmware off the microSD card.
+#
+PX4IO_FIRMWARE	?= $(SRCROOT)/px4io.bin
+ifneq ($(wildcard $(PX4IO_FIRMWARE)),)
+ROMFS_FSSPEC	+=  $(PX4IO_FIRMWARE)~px4io.bin
+endif
+
+################################################################################
+# No user-serviceable parts below
+################################################################################
+
+#
+# Just the source files from the ROMFS spec, so that we can fail cleanly if they don't
+# exist
+#
+ROMFS_SRCFILES	 = $(foreach spec,$(ROMFS_FSSPEC),$(firstword $(subst ~, ,$(spec))))
+
+#
+# Just the destination directories from the ROMFS spec
+#
+ROMFS_DIRS	 = $(sort $(dir $(foreach spec,$(ROMFS_FSSPEC),$(lastword $(subst ~, ,$(spec))))))
+
+
+#
+# Intermediate products
+#
+ROMFS_IMG	 = $(BUILDROOT)/romfs.img
+ROMFS_WORKDIR	 = $(BUILDROOT)/romfs
+
+#
+# Convenience target for rebuilding the ROMFS header
+#
+all:		$(ROMFS_HEADER)
+
+$(ROMFS_HEADER): $(ROMFS_IMG) $(dir $(ROMFS_HEADER))
+	@echo Generating the ROMFS header...
+	@(cd $(dir $(ROMFS_IMG)) && xxd -i $(notdir $(ROMFS_IMG))) > $@
+
+$(ROMFS_IMG):	$(ROMFS_WORKDIR)
+	@echo Generating the ROMFS image...
+	@genromfs -f $@ -d $(ROMFS_WORKDIR) -V "NSHInitVol"
+
+$(ROMFS_WORKDIR): $(ROMFS_SRCFILES)
+	@echo Rebuilding the ROMFS work area...
+	@rm -rf $(ROMFS_WORKDIR)
+	@mkdir -p $(ROMFS_WORKDIR)
+	@for dir in $(ROMFS_DIRS) ; do mkdir -p $(ROMFS_WORKDIR)/$$dir; done
+	@for spec in $(ROMFS_FSSPEC) ; do \
+		echo $$spec | sed -e 's%^.*~%  %' ;\
+		`echo "cp $$spec" | sed -e 's%~% $(ROMFS_WORKDIR)/%'` ;\
+	 done
+
+$(BUILDROOT):
+	@mkdir -p $(BUILDROOT)
+
+clean:
+	@rm -rf $(BUILDROOT)
+
+distclean:	clean
+	@rm -f $(PX4IO_FIRMWARE) $(ROMFS_HEADER)
+
+.PHONY:	all install clean distclean
+
+#
+# Hacks and fixups
+#
+SYSTYPE			 = $(shell uname)
+
+ifeq ($(SYSTYPE),Darwin)
+# PATH inherited by Eclipse may not include toolchain install location 
+export PATH			 := $(PATH):/usr/local/bin
+endif
+

ROMFS/scripts/rc.PX4IO

@@ -0,0 +1,74 @@
+#!nsh
+#
+# Flight startup script for PX4FMU with PX4IO carrier board.
+#
+
+echo "[init] doing PX4IO startup..."
+
+#
+# Start the ORB
+#
+uorb start
+
+#
+# Start the sensors.
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start MAVLink
+#
+mavlink -d /dev/ttyS0 -b 57600 &
+
+#
+# Start the commander.
+#
+# XXX this should be '<command> start'.
+#
+commander &
+
+#
+# Start the attitude estimator
+#
+# XXX this should be '<command> start'.
+#
+attitude_estimator_bm &
+#position_estimator &
+
+#
+# Configure PX4FMU for operation with PX4IO
+#
+# XXX arguments?
+#
+px4fmu start
+
+#
+# Start the fixed-wing controller
+#
+# XXX this should be '<command> start'.
+#
+fixedwing_control &
+
+#
+# Fire up the PX4IO interface.
+#
+px4io start
+
+#
+# Start looking for a GPS.
+#
+# XXX this should not need to be backgrounded
+#
+gps -d /dev/ttyS3 -m all &
+
+#
+# Start logging to microSD if we can
+#
+sh /etc/init.d/rc.logging
+
+#
+# startup is done; we don't want the shell because we
+# use the same UART for telemetry (dumb).
+#
+echo "[init] startup done, exiting."
+exit

ROMFS/scripts/rc.PX4IOAR

@@ -0,0 +1,69 @@
+#!nsh
+#
+# Flight startup script for PX4FMU on PX4IOAR carrier board.
+#
+
+echo "[init] doing PX4IOAR startup..."
+
+#
+# Start the ORB
+#
+uorb start
+
+#
+# Start the sensors.
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start MAVLink
+#
+mavlink -d /dev/ttyS0 -b 57600 &
+
+#
+# Start the commander.
+#
+# XXX this should be '<command> start'.
+#
+commander &
+
+#
+# Start the attitude estimator
+#
+# XXX this should be '<command> start'.
+#
+attitude_estimator_bm &
+#position_estimator &
+
+#
+# Configure PX4FMU for operation with PX4IOAR
+#
+# XXX arguments?
+#
+px4fmu start
+
+#
+# Fire up the AR.Drone controller.
+#
+# XXX this should be '<command> start'.
+#
+ardrone_control -d /dev/ttyS1 -m attitude &
+
+#
+# Start looking for a GPS.
+#
+# XXX this should not need to be backgrounded
+#
+gps -d /dev/ttyS3 -m all &
+
+#
+# Start logging to microSD if we can
+#
+sh /etc/init.d/rc.logging
+
+#
+# startup is done; we don't want the shell because we
+# use the same UART for telemetry (dumb).
+#
+echo "[init] startup done, exiting."
+exit

ROMFS/scripts/rc.jig

@@ -0,0 +1,10 @@
+#!nsh
+#
+# Test jig startup script
+#
+
+echo "[testing] doing production test.."
+
+tests jig
+
+echo "[testing] testing done"

ROMFS/scripts/rc.logging

@@ -0,0 +1,10 @@
+#!nsh
+#
+# Initialise logging services.
+#
+
+if [ -d /fs/microsd ]
+then
+	# XXX this should be '<command> start'.
+	# sdlog &
+fi

ROMFS/scripts/rc.sensors

@@ -0,0 +1,28 @@
+#!nsh
+#
+# Standard startup script for PX4FMU onboard sensor drivers.
+#
+
+#
+# Start sensor drivers here.
+#
+
+#ms5611 start
+
+#
+# Start the sensor collection task.
+#
+# XXX should be 'sensors start'
+#
+sensors &
+
+#
+# Test sensor functionality
+#
+# XXX integrate with 'sensors start' ?
+#
+#if sensors quicktest
+#then
+#	echo "[init] sensor initialisation FAILED."
+#	reboot
+#fi

ROMFS/scripts/rc.standalone

@@ -0,0 +1,67 @@
+#!nsh
+#
+# Flight startup script for PX4FMU standalone configuration.
+#
+
+echo "[init] doing standalone PX4FMU startup..."
+
+#
+# Start the ORB
+#
+#uorb start
+
+#
+# Start the sensors.
+#
+#sh /etc/init.d/rc.sensors
+
+#
+# Start MAVLink
+#
+# mavlink -d /dev/ttyS0 -b 57600 &
+
+#
+# Start the commander.
+#
+# XXX this should be 'commander start'.
+#
+#commander &
+
+#
+# Start the attitude estimator
+#
+# XXX this should be '<command> start'.
+#
+#attitude_estimator_bm &
+#position_estimator &
+
+#
+# Start the fixed-wing controller.
+#
+# XXX should this be looking for configuration to decide
+# whether the board is configured for fixed-wing use?
+#
+# XXX this should be 'fixedwing_control start'.
+#
+#fixedwing_control &
+
+#
+# Configure FMU for standalone mode
+#
+# XXX arguments?
+#
+#px4fmu start
+
+#
+# Start looking for a GPS.
+#
+# XXX this should not need to be backgrounded
+#
+#gps -d /dev/ttyS3 -m all &
+
+#
+# Start logging to microSD if we can
+#
+sh /etc/init.d/rc.logging
+
+echo "[init] startup done"

ROMFS/scripts/rcS

@@ -0,0 +1,120 @@
+#!nsh
+#
+# PX4FMU startup script.
+#
+# This script is responsible for:
+#
+# - mounting the microSD card (if present)
+# - running the user startup script from the microSD card (if present)
+# - detecting the configuration of the system and picking a suitable
+#   startup script to continue with
+#
+# Note: DO NOT add configuration-specific commands to this script;
+#       add them to the per-configuration scripts instead.
+#
+
+#
+# Default to auto-start mode.  An init script on the microSD card
+# can change this to prevent automatic startup of the flight script.
+#
+set MODE autostart
+set USB_ALLOWED yes
+set USB no
+
+#
+# Try to mount the microSD card.
+#
+echo "[init] looking for microSD..."
+if mount -t vfat /dev/mmcsd0 /fs/microsd
+then
+	echo "[init] card mounted at /fs/microsd"
+else
+	echo "[init] no microSD card found"
+fi
+
+#
+# Look for an init script on the microSD card.
+#
+# To prevent automatic startup in the current flight mode,
+# the script should set MODE to some other value.
+#
+if [ -f /fs/microsd/etc/rc ]
+then
+	echo "[init] reading /fs/microsd/etc/rc"
+	sh /fs/microsd/etc/rc
+fi
+
+#
+# Check for USB host
+#
+if [ $USB_ALLOWED == yes ]
+then
+	if sercon
+	then
+		echo "[init] USB interface connected"
+	fi
+fi
+
+#
+# If we are still in flight mode, work out what airframe 
+# configuration we have and start up accordingly.
+#
+if [ $MODE != autostart ]
+then
+	echo "[init] automatic startup cancelled by user script"
+else
+	echo "[init] detecting attached hardware..."
+
+	#
+	# Assume that we are PX4FMU in standalone mode
+	#
+	set BOARD PX4FMU
+
+	#
+	# Are we attached to a PX4IOAR (AR.Drone carrier board)?
+	#
+	if boardinfo -t 7
+	then
+		set BOARD PX4IOAR
+		if [ -f /etc/init.d/rc.PX4IOAR ]
+		then
+			echo "[init] reading /etc/init.d/rc.PX4IOAR"
+			sh /etc/init.d/rc.PX4IOAR
+		fi
+	else
+		echo "[init] PX4IOAR not detected"
+	fi
+
+	#
+	# Are we attached to a PX4IO?
+	#
+	if boardinfo -t 6
+	then
+		set BOARD PX4IO
+		if [ -f /etc/init.d/rc.PX4IO ]
+		then
+			echo "[init] reading /etc/init.d/rc.PX4IO"
+			sh /etc/init.d/rc.PX4IO
+		fi
+	else
+		echo "[init] PX4IO not detected"
+	fi
+
+	#
+	# Looks like we are stand-alone
+	#
+	if [ $BOARD == PX4FMU ]
+	then
+		echo "[init] no expansion board detected"
+		if [ -f /etc/init.d/rc.standalone ]
+		then
+			echo "[init] reading /etc/init.d/rc.standalone"
+			sh /etc/init.d/rc.standalone
+		fi
+	fi
+
+	#
+	# We may not reach here if the airframe-specific script exits the shell.
+	#
+	echo "[init] startup done."
+fi

Tools/fix_code_style.sh

@@ -0,0 +1,19 @@
+#!/bin/sh
+astyle \
+    --style=linux		\
+    --indent=force-tab=8	\
+    --indent-cases		\
+    --indent-preprocessor	\
+    --break-blocks=all		\
+    --pad-oper			\
+    --pad-header		\
+    --unpad-paren		\
+    --keep-one-line-blocks	\
+    --keep-one-line-statements	\
+    --align-pointer=name	\
+    --align-reference=name	\
+    --suffix=none		\
+    --ignore-exclude-errors-x	\
+    --lineend=linux		\
+    --exclude=EASTL		\
+    $*

Tools/px_mkfw.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# PX4 firmware image generator
+#
+# The PX4 firmware file is a JSON-encoded Python object, containing
+# metadata fields and a zlib-compressed base64-encoded firmware image.
+#
+
+import sys
+import argparse
+import json
+import base64
+import zlib
+import time
+import subprocess
+
+#
+# Construct a basic firmware description
+#
+def mkdesc():
+	proto = {}
+	proto['magic']		= "PX4FWv1"
+	proto['board_id']	= 0
+	proto['board_revision']	= 0
+	proto['version']	= ""
+	proto['summary']	= ""
+	proto['description']	= ""
+	proto['git_identity']	= ""
+	proto['build_time']	= 0
+	proto['image']		= base64.b64encode(bytearray())
+	proto['image_size']	= 0
+	return proto
+
+# Parse commandline
+parser = argparse.ArgumentParser(description="Firmware generator for the PX autopilot system.")
+parser.add_argument("--prototype",	action="store", help="read a prototype description from a file")
+parser.add_argument("--board_id",	action="store", help="set the board ID required")
+parser.add_argument("--board_revision",	action="store", help="set the board revision required")
+parser.add_argument("--version",	action="store", help="set a version string")
+parser.add_argument("--summary",	action="store", help="set a brief description")
+parser.add_argument("--description",	action="store", help="set a longer description")
+parser.add_argument("--git_identity",	action="store", help="the working directory to check for git identity")
+parser.add_argument("--image",		action="store", help="the firmware image")
+args = parser.parse_args()
+
+# Fetch the firmware descriptor prototype if specified
+if args.prototype != None:
+	f = open(args.prototype,"r")
+	desc = json.load(f)
+	f.close()
+else:
+	desc = mkdesc()
+
+desc['build_time'] 		= int(time.time())
+
+if args.board_id != None:
+	desc['board_id']	= int(args.board_id)
+if args.board_revision != None:
+	desc['board_revision']	= int(args.board_revision)
+if args.version != None:
+	desc['version']		= str(args.version)
+if args.summary != None:
+	desc['summary']		= str(args.summary)
+if args.description != None:
+	desc['description']	= str(args.description)
+if args.git_identity != None:
+	cmd = " ".join(["git", "--git-dir", args.git_identity + "/.git", "describe", "--always", "--dirty"])
+	p = subprocess.Popen(cmd, shell=True, stdout=subprocess.PIPE).stdout
+	desc['git_identity']	= p.read().strip()
+	p.close()
+if args.image != None:
+	f = open(args.image, "rb")
+	bytes = f.read()
+	desc['image_size'] = len(bytes)
+	desc['image'] = base64.b64encode(zlib.compress(bytes,9))
+
+print json.dumps(desc, indent=4)

Tools/px_uploader.py

@@ -0,0 +1,318 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Serial firmware uploader for the PX4FMU bootloader
+#
+# The PX4 firmware file is a JSON-encoded Python object, containing
+# metadata fields and a zlib-compressed base64-encoded firmware image.
+#
+# The uploader uses the following fields from the firmware file:
+#
+# image
+#	The firmware that will be uploaded.
+# image_size
+#	The size of the firmware in bytes.
+# board_id
+#	The board for which the firmware is intended.
+# board_revision
+#	Currently only used for informational purposes.
+#
+
+import sys
+import argparse
+import binascii
+import serial
+import os
+import struct
+import json
+import zlib
+import base64
+import time
+
+from sys import platform as _platform
+
+class firmware(object):
+	'''Loads a firmware file'''
+
+	desc = {}
+	image = bytearray()
+
+	def __init__(self, path):
+
+		# read the file
+		f = open(path, "r")
+		self.desc = json.load(f)
+		f.close()
+
+		self.image = zlib.decompress(base64.b64decode(self.desc['image']))
+
+	def property(self, propname):
+		return self.desc[propname]
+
+
+class uploader(object):
+	'''Uploads a firmware file to the PX FMU bootloader'''
+
+	NOP		= chr(0x00)
+	OK		= chr(0x10)
+	FAILED		= chr(0x11)
+	INSYNC		= chr(0x12)
+	EOC		= chr(0x20)
+	GET_SYNC	= chr(0x21)
+	GET_DEVICE	= chr(0x22)
+	CHIP_ERASE	= chr(0x23)
+	CHIP_VERIFY	= chr(0x24)
+	PROG_MULTI	= chr(0x27)
+	READ_MULTI	= chr(0x28)
+	REBOOT		= chr(0x30)
+	
+	INFO_BL_REV	= chr(1)	# bootloader protocol revision
+	BL_REV		= 2		# supported bootloader protocol 
+	INFO_BOARD_ID	= chr(2)	# board type
+	INFO_BOARD_REV	= chr(3)	# board revision
+	INFO_FLASH_SIZE	= chr(4)	# max firmware size in bytes
+
+	PROG_MULTI_MAX	= 60		# protocol max is 255, must be multiple of 4
+	READ_MULTI_MAX	= 60		# protocol max is 255, something overflows with >= 64
+
+	def __init__(self, portname, baudrate):
+		# open the port
+		self.port = serial.Serial(portname, baudrate, timeout=10)
+
+	def close(self):
+		if self.port is not None:
+			self.port.close()
+
+	def __send(self, c):
+#		print("send " + binascii.hexlify(c))
+		self.port.write(str(c))
+
+	def __recv(self, count = 1):
+		c = self.port.read(count)
+		if (len(c) < 1):
+			raise RuntimeError("timeout waiting for data")
+#		print("recv " + binascii.hexlify(c))
+		return c
+
+	def __getSync(self):
+		self.port.flush()
+		c = self.__recv()
+		if (c != self.INSYNC):
+			raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
+		c = self.__recv()
+		if (c != self.OK):
+			raise RuntimeError("unexpected 0x%x instead of OK" % ord(c))
+
+	# attempt to get back into sync with the bootloader
+	def __sync(self):
+		# send a stream of ignored bytes longer than the longest possible conversation
+		# that we might still have in progress
+#		self.__send(uploader.NOP * (uploader.PROG_MULTI_MAX + 2))
+		self.port.flushInput()
+		self.__send(uploader.GET_SYNC 
+				+ uploader.EOC)
+		self.__getSync()
+		
+	def __trySync(self):
+		c = self.__recv()
+		if (c != self.INSYNC):
+			#print("unexpected 0x%x instead of INSYNC" % ord(c))
+			return False;
+		c = self.__recv()
+		if (c != self.OK):
+			#print("unexpected 0x%x instead of OK" % ord(c))
+			return False
+		return True
+
+	# send the GET_DEVICE command and wait for an info parameter
+	def __getInfo(self, param):
+		self.__send(uploader.GET_DEVICE + param + uploader.EOC)
+		raw = self.__recv(4)
+		self.__getSync()
+		value = struct.unpack_from('<I', raw)
+		return value[0]
+
+	# send the CHIP_ERASE command and wait for the bootloader to become ready
+	def __erase(self):
+		self.__send(uploader.CHIP_ERASE 
+				+ uploader.EOC)
+		self.__getSync()
+
+	# send a PROG_MULTI command to write a collection of bytes
+	def __program_multi(self, data):
+		self.__send(uploader.PROG_MULTI
+				+ chr(len(data)))
+		self.__send(data)
+		self.__send(uploader.EOC)
+		self.__getSync()
+		
+	# verify multiple bytes in flash
+	def __verify_multi(self, data):
+		self.__send(uploader.READ_MULTI
+				+ chr(len(data))
+				+ uploader.EOC)
+		programmed = self.__recv(len(data))
+		if (programmed != data):
+			print("got    " + binascii.hexlify(programmed))
+			print("expect " + binascii.hexlify(data))
+			return False
+		self.__getSync()
+		return True
+		
+	# send the reboot command
+	def __reboot(self):
+		self.__send(uploader.REBOOT)
+		self.port.flush()
+
+	# split a sequence into a list of size-constrained pieces
+	def __split_len(self, seq, length):
+    		return [seq[i:i+length] for i in range(0, len(seq), length)]
+
+	# upload code
+	def __program(self, fw):
+		code = fw.image
+		groups = self.__split_len(code, uploader.PROG_MULTI_MAX)
+		for bytes in groups:
+			self.__program_multi(bytes)
+
+	# verify code
+	def __verify(self, fw):
+		self.__send(uploader.CHIP_VERIFY
+				+ uploader.EOC)
+		self.__getSync()
+		code = fw.image
+		groups = self.__split_len(code, uploader.READ_MULTI_MAX)
+		for bytes in groups:
+			if (not self.__verify_multi(bytes)):
+				raise RuntimeError("Verification failed")
+
+	# get basic data about the board
+	def identify(self):
+		# make sure we are in sync before starting
+		self.__sync()
+
+		# get the bootloader protocol ID first
+		bl_rev = self.__getInfo(uploader.INFO_BL_REV)
+		if bl_rev != uploader.BL_REV:
+			raise RuntimeError("Bootloader protocol mismatch")
+
+		self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
+		self.board_rev = self.__getInfo(uploader.INFO_BOARD_REV)
+		self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
+
+	# upload the firmware
+	def upload(self, fw):
+		# Make sure we are doing the right thing
+		if self.board_type != fw.property('board_id'):
+			raise RuntimeError("Firmware not suitable for this board")
+		if self.fw_maxsize < fw.property('image_size'):
+			raise RuntimeError("Firmware image is too large for this board")
+
+		print("erase...")
+		self.__erase()
+
+		print("program...")
+		self.__program(fw)
+
+		print("verify...")
+		self.__verify(fw)
+
+		print("done, rebooting.")
+		self.__reboot()
+		self.port.close()
+	
+
+# Parse commandline arguments
+parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
+parser.add_argument('--port', action="store", required=True, help="Serial port(s) to which the FMU may be attached")
+parser.add_argument('--baud', action="store", type=int, default=115200, help="Baud rate of the serial port (default is 115200), only required for true serial ports.")
+parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
+args = parser.parse_args()
+
+# Load the firmware file
+fw = firmware(args.firmware)
+print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
+
+# Spin waiting for a device to show up
+while True:
+	for port in args.port.split(","):
+
+		#print("Trying %s" % port)
+
+		# create an uploader attached to the port
+		try:
+			if "linux" in _platform:
+			# Linux, don't open Mac OS and Win ports
+				if not "COM" in port and not "tty.usb" in port:
+					up = uploader(port, args.baud)
+			elif "darwin" in _platform:
+				# OS X, don't open Windows and Linux ports
+				if not "COM" in port and not "ACM" in port:
+					up = uploader(port, args.baud)
+			elif "win" in _platform:
+				# Windows, don't open POSIX ports
+				if not "/" in port:
+					up = uploader(port, args.baud)
+		except:
+			# open failed, rate-limit our attempts
+			time.sleep(0.05)
+
+			# and loop to the next port
+			continue
+
+		# port is open, try talking to it
+		try:
+			# identify the bootloader
+			up.identify()
+			print("Found board %x,%x on %s" % (up.board_type, up.board_rev, port))
+
+		except:
+			# most probably a timeout talking to the port, no bootloader
+			continue
+
+		try:
+			# ok, we have a bootloader, try flashing it
+			up.upload(fw)
+
+		except RuntimeError as ex:
+
+			# print the error
+			print("ERROR: %s" % ex.args)
+
+		finally:
+			# always close the port
+			up.close()
+
+		# we could loop here if we wanted to wait for more boards...
+		sys.exit(0)

apps/ChangeLog.txt

@@ -0,0 +1,272 @@
+5.19 2011-03-12 Gregory Nutt <gnutt@nuttx.org>
+
+	* Initial version of the apps/ directory was released as contributed by
+	  Uros Platise.
+
+6.0 2011-03-21 Gregory Nutt <gnutt@nuttx.org>
+
+	* README.txt -- README cosmetics
+	* hello/ -- hello world minor changes
+	* Makefile -- Makefile cosmetics (I am slowly adding the Darjeeling JVM)
+	* Make.defs -- New file adds common make definitions for applications.
+	* hello/Makefile -- Now uses new Make.defs definitions.  Added README.txt.
+	* apps/poweroff -- New application to turn off board power.
+	* Moved NSH library, netutils, and examples from the nuttx/ directory to
+	  the apps/ directory
+	* Moved exec_nuttapp machinery into the nuttapp/ directory.
+
+6.1 2011-04-10 Gregory Nutt <gnutt@nuttx.org>
+
+	* Creation of auto-generated header files now occurs during the context
+	  build phase.
+	* Added sdcard insert and eject, nsh command '?' and some code remarks
+	* Renamed nuttapp to namedapp
+	* namedapp/binfs.c -- Create a tiny filesystem that can be used
+	  to show the internal named apps under /bin.
+	* Numerous fixes to build system required to support building with native
+	  Windows toolchain.
+
+6.2 2011-05-06 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/nxffs:  Add a test a a configuration that will be used to
+	  verify NXFFS.
+
+6.3 2011-05-15 Gregory Nutt <gnutt@nuttx.org>
+
+    * apps/interpreter: Add a directory to hold interpreters.  The Pascal add-
+	  on module now installs and builds under this directory.
+	* apps/interpreter/ficl: Added logic to build Ficl (the "Forth Inspired
+	  Command Language").  See http://ficl.sourceforge.net/.
+	* apps/netutils/dhcpc, dhcpcd, and tftp.  If these directories are included
+	  in the configuration but CONFIG_NET_UDP is disable (which is not very wise),
+	  then a make error occurs because tools/mkdep.sh is called with no files.
+	* system/free: Move Uros' custom free command from vsn/free
+	* system/install: Add a new install command submitted by Uros Platise.
+	* examples/rgmp.  Add a placeholder for an RGMP build example.
+	  RGMP is a project for running GPOS and RTOS simultaneously on
+	  multi-processor platforms. See http://rgmp.sourceforge.net/wiki/index.php/Main_Page
+	  for further information about RGMP.  NOTE:  This is an empty example
+	  on initial check-in.
+
+6.4 2011-06-06 Gregory Nutt <gnutt@nuttx.org>
+
+	* nshlib/nsh_netcmds.c: If a network device name and IP address are provided
+	  with the ifconfig command, then this command will now set the network address.
+	  (Contributed by Yu Qiang).
+	* netutils/ftpc: A library to support client-side FTP.
+	* examples/ftpc: A simple add-on to the NSH.  From NSH, you can start
+	  this simple FTP shell to transfer files to/from a remote FTP server.
+
+6.5 2011-06-21 Gregory Nutt <gnutt@nuttx.org>
+
+	* netutils/ftpc:  Simpflication and size reduction.
+
+6.6 2011-07-11 Gregory Nutt <gnutt@nuttx.org>
+
+	* Make.defs, namedapp/namedapp.c: Several structural changes made to get a
+	  clean compile under the ez80 ZDS-II toolchain (no design changes).
+	* apps/examples/buttons: Add a test for the new standardized button interfaces
+	* apps/examples/nxtext: Add another NX graphics test.  This one focus on
+	  placing text on the background while pop-up windows occur.  Text should
+	  continue to update normally with or without the popup windows present.
+
+6.7 2011-08-02 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/nx and nxtext:  These examples can now be built as NSH
+	  "built-in" commands.
+	* apps/examples/nxhello:  The simplest graphics example:  It just says
+	  "Hello, World!" in the center of the display. This example can also be
+	  built as an NSH "built-in" command.
+	* apps/examples/nx, ntext, and nxhello:  All updated to use the new
+	  NuttX font interfaces.
+	* apps/examples/nximage:  Another super simple  graphics example:  It just puts
+	  the NuttX logo in the center of the display. This example can also be
+	  built as an NSH "built-in" command.
+	* apps/examples/usbstorage: Can now be built as two NSH "built-in" commands:
+	  'msconn' will connect the USB mass storage device; 'msdis' will disconnect
+	  the USB storage device.
+	* apps/examples/nx*:  All NX header files moved from nuttx/include/nuttx to 
+	  nuttx/include/nuttx/nx.
+	* apps/examples/usbstorage: Added instrumentation to monitor memory usage
+	  to check for memory leaks in the USB storage driver.
+	* apps/examples/nxhello/nxhello_bkgd.c:  Fix handling of allocated glyph
+	  memory.
+
+6.8 2011-08-11 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/nxlines:  Added a test for NX line drawing capabilities.
+
+6.9 2011-09-11 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/nxlines:  Extend the line drawing text to include drawing
+	  of circles.
+	* apps/system/i2c:  Add an I2C test tool that should help to bring up I2C
+	  devices (when it is fully functional).
+	* apps/nshlib/nsh_timcmds.c:  Add the date command that can be used to
+	  show or set the time (only if CONFIG_RTC is set).
+
+6.10 2011-10-06 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/system/i2c:  Add repitition and address auto-incrementing so that
+	  and command can be executed numerous times.  Add a new verify command
+	  that will write to a register, read from register, and verify that
+	  returned value.
+	* apps/graphics/tiff:  Add a library that can be used to create TIFF files.
+	* apps/examples/tiff:  Add a unit test for the TIFF file creation logic
+	* apps/examples/lcdrw:  Add a test to verify if you can or can or read
+	  data from an LCD correctly.
+	* apps/examples/usbterm:  A USB terminal example.. more of a USB chat or
+	  serial bridge:  Data received on local console echoed via USB serial; 
+	  data received on USB serial is echoed on the local console.
+	* apps/examples/touchscreen:  Add a simple, generic test for any
+	  touschscreen driver.
+	* Makefile:  The apps/ Makefile now checks for an apps/external directory
+	  or symbolic link.  If such a directory/link exists (and has a Makefile),
+	  it will be added to the apps/ build.  This allows external directories
+	  to be included into the apps/ build by simply creating a symbolic link.
+
+6.11 2011-11-12 Gregory Nutt <gnutt@nuttx.org>
+
+	(No major changes from 6.10)
+
+6.12 2011-12-06 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/buttons:  The button test can now be executed as an NSH
+	  built in command.
+
+6.13 2012-12-26 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/dhcpd: May now be built as an NSH built-in application
+	  by setting CONFIG_NSH_BUILTIN_APPS.
+	* apps/netutils/dhcpd/dhcpd.c: Fix several problems using host order address
+	  where network addresses expected (and vice versa).
+	* apps/examples/nettest: May now be built as an NSH built-in application
+	  by setting CONFIG_NSH_BUILTIN_APPS.
+	* apps/examples/nettest: Correct some build issues with the nettest is
+	  built for performance evaluation.
+	* apps/examples/adc:  Add a very simple test to drive and test an ADC
+	  driver.
+	* apps/examples/pwm:  Add an NSH PWM command to drive and test a PWM
+	  driver.
+	* apps/examples/can:  Add an NSH CAN command to drive and test a CAN
+	  driver in loopback mode.
+
+6.14 2012-01-15 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/buttons/main.c:  The test needs to call up_buttoninit() to
+	  properly configure the button interrupt GPIOs.
+	* apps/examples/pwm:  Add support to test the pulse count option recently
+	  added to the PWM interface.
+
+6.15 2012-02-12 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/nshlib/nsh_serial.c and nsh_usbdev.c:  If NuttX is configured to use
+	  a USB serial console, then NSH needs to wait until the USB console is
+	  connected and available.
+	* apps/examples/composite:  Add a test of the USB composite device.
+	* apps/examples/Telnetd:  Move the tiny uIP shell example from
+	  netutils/Telnetd to examples/Telnetd.  Enhanced the Telnetd daemon so that
+	  it supports Telnetd via a TTY device driver: A new TTY device driver is
+	  created when each new Telnet connection is created.  The shell thread
+	  is started with stdin, stdout, and stderr mapped to the TTY device.
+	* netutils/Telnetd:  The old uIP Telnet demo is gone.  In its place is a new
+	  Telnet infrastructure. The new Telnet daemon creates sessions that are
+	  "wrapped" as character devices and mapped to stdin, stdout, and stderr.
+	  Now the Telnet session can be inherited by spawned tasks.
+	* examples/Telnetd:  Add a test for the new Telnet daemon.
+	* examples/Telnetd/telnetd_driver.c: Move the internal socket structure from
+	  the daemon's socket array into the driver's state data so that it will be
+	  independent from the the Telnetd daemon.
+	* apps/system/readline:  Moved the old nuttx/lib/stdio/lib_fgets.c here
+	  and renamed it as readline().  The old fgets was simplied and the overloaded
+	  readline functionality was removed.
+	* apps/netutils/ftpd:  Add an FTPD server (does not even compile on initial
+	  checkin).
+	* apps/examples/ftpd:  Add a test for the FTPD server (untest on initial
+	  check-in).
+	* apps/nshlib/nsh_fscmds.c:  Add support for a 'dmesg' command that will
+	  dump the system log if CONFIG_SYSLOG is selected.
+
+6.16 2012-03-10 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/qencoder:  Add a quadrature driver test.
+	* apps/examples/ostest/fpu.c:  Add a test to verify that FPU registers
+	  are properly saved and restored on context switches.
+	* apps/system/readline/readline.c:  readline() will now treat either a
+	  backspace or a DEL character as a backspace (i.e., deleting the character
+	  to the left of the cursor).  This makes NSH less dependent on particular
+	  keyboard mappings of the Backspace key.  Submitted by Mike Smith.
+	* apps/examples/cdcacm:  An example that illustrates how the CDC/ACM driver
+	  may to connected and disconnected through software control.
+	* apps/examples/nsh/nsh_main.c:  If available, call up_cxxinitialize() to
+	  initialize all statically defined C++ classes.
+	* apps/nshlib:  Now supports a USB serial device for NSH console I/O.  This
+	  allows NSH to be used on boards that have USB but no serial connectors.
+
+6.17 2012-04-14 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/can:  Add conditional compilation so that the test can be
+	  configured to only send messages or to only receive messages.  This will
+	  let the test work in other modes than simple loopback testing.
+	* apps/examples/hello and apps/examples/ostest:  Can now be built as NSH
+	  built-int functions.
+	* vsn/hello:  Removed.  The modified apps/examples/hello is enough "Hello,
+	  World!"
+	* apps/examples/nxconsole:  Add a test of the NX console device.
+	* apps/examples/nxconsole:  The NX console example now supports running
+	  the NuttShell (NSH) within an NX window.
+	* apps/system/readline:  Now uses standard definitions from
+	  include/nuttx/ascii.h and vt100.h
+	* Kconfig, */Kconfig:  Added skeleton Kconfig files to all directories that
+	  may need them.
+
+6.18 2012-05-19 Gregory Nutt <gnutt@nuttx.org>
+
+	* Kconfig:  Continued Kconfig file updates (no longer tracking on a per-file
+	  basis in the ChangeLog)
+	* apps/examples/watchdog:  Add a watchdog timer example.
+	* apps/examples/tiff:  Fix wrong path used for temporary file.
+	* apps/examples/touchscreen:  Standardize the board-specific, touchscreen
+	  initialization interfaces.
+
+6.19 2012-06-15 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/nshlib/nsh_usbdev.c:  Add the capability to use an arbitrary USB
+	  device as the console (not necessarily /dev/console).  This is a useful
+	  option because then you can still use the serial console to debug with.
+	* apps/nshlib/nsh_usbdev.c:  User now has to press ENTER 3 times before
+	  USB console will start.  Otherwise, the USB console starts before there
+	  is anyone at the other end to listen.
+	* apps/nshlib/nsh_usbdev.c and nsh_consolemain.c:  Add support for the USB
+	  capability when a USB console is used.
+	* apps/nshlib/nsh_fscmds.c:  Add the 'mv' command
+
+6.20 2012-07-12 Gregory Nutt <gnutt@nuttx.org>
+
+	* namedapp/exec_namedapp.c - Correct an error when round robin scheduling
+	  is enabled.  The priority of the new, named application was erroneously
+	  being set to the priority of the parent thread; losing its configured
+	  priority.  Reported by Mike Smith.
+
+6.21 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/include/: Stylistic clean-up of all header files.
+	* apps/modbus and apps/include/modbus:  A port of freemodbus-v1.5.0
+	  has been added to the NuttX apps/ source tree.
+	* apps/examples/modbus:  A port of the freemodbus-v1.5.0 "demo"
+	  program that will be used to verify the FreeModBus port
+	* apps/modbus:  Don't use strerror().  It is just too big.
+	* apps/modbus:  Add CONFIG_MB_TERMIOS.  If the driver doesn't support
+	  termios ioctls, then don't bother trying to configure the baud, parity
+	  etc.
+	* apps/nslib:  If waitpid() is supported, then NSH not catches the
+	  return value from spawned applications (provided by Mike Smith)
+	* apps/nslib:  Lock the schedule while starting built-in applications
+	  in order to eliminate race conditions (also from Mike Smith).
+	* apps/examples/adc, pwm, and qencoder:  Add support for testing
+	  devices with multiple ADC, PWM, and QE devices.
+	* apps/nshlib/nsh_mntcmds.c:  Separated mount-related commands out of
+	  nsh_fscmds.c.  Extended to the mount command so that if no arguments
+	  are provided, then the current mountpoints are enumerated.
+	* apps/nshlib/nsh_mntcmds.c:  Add an NSH df command to list the
+	  properties of mounted file systems.

apps/Kconfig

@@ -0,0 +1,36 @@
+#
+# For a description of the syntax of this configuration file,
+# see misc/tools/kconfig-language.txt.
+#
+
+menu "Named Applications"
+source "$APPSDIR/namedapp/Kconfig"
+endmenu
+
+menu "Examples"
+source "$APPSDIR/examples/Kconfig"
+endmenu
+
+menu "Interpreters"
+source "$APPSDIR/interpreters/Kconfig"
+endmenu
+
+menu "Network Utilities"
+source "$APPSDIR/netutils/Kconfig"
+endmenu
+
+menu "ModBus"
+source "$APPSDIR/modbus/Kconfig"
+endmenu
+
+menu "NSH Library"
+source "$APPSDIR/nshlib/Kconfig"
+endmenu
+
+menu "System NSH Add-Ons"
+source "$APPSDIR/system/Kconfig"
+endmenu
+
+menu "VSN board Add-Ons"
+source "$APPSDIR/vsn/Kconfig"
+endmenu

apps/Make.defs

@@ -0,0 +1,41 @@
+############################################################################
+# apps/Make.defs
+# Common make definitions provided to all applications
+#
+#   Copyright (C) 2011 Gregory Nutt. All rights reserved.
+#   Author: Gregory Nutt <spudmonkey@racsa.co.cr>
+#
+# 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.
+#
+############################################################################
+
+define REGISTER
+	@echo "Register: $1"
+	@echo "{ \"$1\", $2, $3, $4 }," >> "$(APPDIR)/namedapp/namedapp_list.h"
+	@echo "EXTERN int $4(int argc, char *argv[]);" >> "$(APPDIR)/namedapp/namedapp_proto.h"
+endef

apps/Makefile

@@ -0,0 +1,176 @@
+############################################################################
+# 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

@@ -0,0 +1,211 @@
+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

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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

@@ -0,0 +1,272 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,12 @@
+/*
+ * 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

@@ -0,0 +1,60 @@
+#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

@@ -0,0 +1,21 @@
+/*
+ * 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

@@ -0,0 +1,277 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,61 @@
+/****************************************************************************
+ * 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

@@ -0,0 +1,435 @@
+/****************************************************************************
+ *
+ *   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

@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   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.c

@@ -0,0 +1,109 @@
+#include "pid.h"
+
+#include <px4/attitude_estimator_bm/matrix.h> //TODO: move matrix.h to somewhere else?
+
+void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
+	      uint8_t mode, uint8_t plot_i)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->intmax = intmax;
+	pid->mode = mode;
+	pid->plot_i = plot_i;
+	pid->count = 0;
+	pid->saturated = 0;
+
+	pid->sp = 0;
+	pid->error_previous = 0;
+	pid->integral = 0;
+}
+void pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax)
+{
+	pid->kp = kp;
+	pid->ki = ki;
+	pid->kd = kd;
+	pid->intmax = intmax;
+	//	pid->mode = mode;
+
+	//	pid->sp = 0;
+	//	pid->error_previous = 0;
+	//	pid->integral = 0;
+}
+
+//void pid_set(PID_t *pid, float sp)
+//{
+//	pid->sp = sp;
+//	pid->error_previous = 0;
+//	pid->integral = 0;
+//}
+
+/**
+ *
+ * @param pid
+ * @param val
+ * @param dt
+ * @return
+ */
+float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
+{
+	/*  error = setpoint - actual_position
+	 integral = integral + (error*dt)
+	 derivative = (error - previous_error)/dt
+	 output = (Kp*error) + (Ki*integral) + (Kd*derivative)
+	 previous_error = error
+	 wait(dt)
+	 goto start
+	 */
+
+	float i, d;
+	pid->sp = sp;
+	float error = pid->sp - val;
+
+	if (pid->saturated && (pid->integral * error > 0)) {
+		//Output is saturated and the integral would get bigger (positive or negative)
+		i = pid->integral;
+
+		//Reset saturation. If we are still saturated this will be set again at output limit check.
+		pid->saturated = 0;
+
+	} else {
+		i = pid->integral + (error * dt);
+	}
+
+	// Anti-Windup. Needed if we don't use the saturation above.
+	if (pid->intmax != 0.0) {
+		if (i > pid->intmax) {
+			pid->integral = pid->intmax;
+
+		} else if (i < -pid->intmax) {
+
+			pid->integral = -pid->intmax;
+
+		} else {
+			pid->integral = i;
+		}
+
+		//Send Controller integrals
+		//		Disabled because of new possibilities with debug_vect.
+		//		Now sent in Main Loop at 5 Hz. 26.06.2010 Laurens
+		//		if (pid->plot_i && (pid->count++ % 16 == 0)&&(global_data.param[PARAM_SEND_SLOT_DEBUG_2] == 1))
+		//		{
+		//			mavlink_msg_debug_send(MAVLINK_COMM_1, pid->plot_i, pid->integral);
+		//		}
+	}
+
+	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
+		d = (error - pid->error_previous) / dt;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
+		d = -val_dot;
+
+	} else {
+		d = 0;
+	}
+
+	pid->error_previous = error;
+
+	return (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
+}

apps/ardrone_control/pid.h

@@ -0,0 +1,40 @@
+/*
+ * 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_ */

apps/ardrone_control/position_control.c

@@ -0,0 +1,308 @@
+/****************************************************************************
+ * ardrone_control/position_control.c
+ *
+ *   Copyright (C) 2008, 2012 Thomas Gubler, Julian Oes, Lorenz Meier. All rights reserved.
+ *   Author: Based on the pixhawk quadrotor controller
+ *
+ * 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 "position_control.h"
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+#include <stdbool.h>
+#include <float.h>
+#include "pid.h"
+
+#ifndef FM_PI
+#define FM_PI 3.1415926535897932384626433832795f
+#endif
+
+
+
+#define CONTROL_PID_POSITION_INTERVAL	0.020
+
+int read_lock_position(global_data_position_t *position)
+{
+	static int ret;
+	ret = global_data_wait(&global_data_position->access_conf);
+
+	if (ret == 0) {
+		memcpy(&position->lat, &global_data_position->lat, sizeof(global_data_position->lat));
+		memcpy(&position->lon, &global_data_position->lon, sizeof(global_data_position->lon));
+		memcpy(&position->alt, &global_data_position->alt, sizeof(global_data_position->alt));
+		memcpy(&position->relative_alt, &global_data_position->relative_alt, sizeof(global_data_position->relative_alt));
+		memcpy(&position->vx, &global_data_position->vx, sizeof(global_data_position->vx));
+		memcpy(&position->vy, &global_data_position->vy, sizeof(global_data_position->vy));
+		memcpy(&position->vz, &global_data_position->vz, sizeof(global_data_position->vz));
+		memcpy(&position->hdg, &global_data_position->hdg, sizeof(global_data_position->hdg));
+
+
+	} else {
+		printf("Controller timeout, no new position values available\n");
+	}
+
+	global_data_unlock(&global_data_position->access_conf);
+	return ret;
+}
+
+float get_distance_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next)
+{
+	double lat_now_rad = lat_now / 180.0 * M_PI;
+	double lon_now_rad = lon_now / 180.0 * M_PI;
+	double lat_next_rad = lat_next / 180.0 * M_PI;
+	double lon_next_rad = lon_next / 180.0 * M_PI;
+
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(lat_now_rad) * cos(lat_next_rad);
+	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+	const double radius_earth = 6371000.0;
+
+	return radius_earth * c;
+}
+
+float get_bearing_to_next_waypoint(float lat_now, float lon_now, float lat_next, float lon_next)
+{
+	double lat_now_rad = lat_now / 180.0 * M_PI;
+	double lon_now_rad = lon_now / 180.0 * M_PI;
+	double lat_next_rad = lat_next / 180.0 * M_PI;
+	double lon_next_rad = lon_next / 180.0 * M_PI;
+
+	double d_lat = lat_next_rad - lat_now_rad;
+	double d_lon = lon_next_rad - lon_now_rad;
+
+
+	float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+
+	if (theta < 0) {
+		theta = theta + 2 * FM_PI;
+	}
+
+	return theta;
+}
+
+void control_position(void)
+{
+	static PID_t distance_controller;
+
+	static int read_ret;
+	static global_data_position_t position_estimated;
+
+	static uint16_t counter;
+
+	static bool initialized;
+	static uint16_t pm_counter;
+
+	static float lat_next;
+	static float lon_next;
+
+	static float pitch_current;
+
+	static float thrust_total;
+
+
+	if (initialized == false) {
+
+		global_data_lock(&global_data_parameter_storage->access_conf);
+
+		pid_init(&distance_controller,
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
+			 PID_MODE_DERIVATIV_CALC, 150);//150
+
+//		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+//		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+		pm_counter = global_data_parameter_storage->counter;
+
+		global_data_unlock(&global_data_parameter_storage->access_conf);
+
+		thrust_total = 0.0f;
+
+		/* Position initialization */
+		/* Wait for new position estimate */
+		do {
+			read_ret = read_lock_position(&position_estimated);
+		} while (read_ret != 0);
+
+		lat_next = position_estimated.lat;
+		lon_next = position_estimated.lon;
+
+		/* attitude initialization */
+		global_data_lock(&global_data_attitude->access_conf);
+		pitch_current = global_data_attitude->pitch;
+		global_data_unlock(&global_data_attitude->access_conf);
+
+		initialized = true;
+	}
+
+	/* load new parameters with 10Hz */
+	if (counter % 50 == 0) {
+		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
+			/* check whether new parameters are available */
+			if (global_data_parameter_storage->counter > pm_counter) {
+				pid_set_parameters(&distance_controller,
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
+
+//
+//				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+//				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+
+				pm_counter = global_data_parameter_storage->counter;
+				printf("Position controller changed pid parameters\n");
+			}
+		}
+
+		global_data_unlock(&global_data_parameter_storage->access_conf);
+	}
+
+
+	/* Wait for new position estimate */
+	do {
+		read_ret = read_lock_position(&position_estimated);
+	} while (read_ret != 0);
+
+	/* Get next waypoint */ //TODO: add local copy
+
+	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
+		lat_next = global_data_position_setpoint->x;
+		lon_next = global_data_position_setpoint->y;
+		global_data_unlock(&global_data_position_setpoint->access_conf);
+	}
+
+	/* Get distance to waypoint */
+	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
+//	if(counter % 5 == 0)
+//		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
+
+	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
+	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
+
+	if (counter % 5 == 0)
+		printf("bearing: %.4f\n", bearing);
+
+	/* Calculate speed in direction of bearing (needed for controller) */
+	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
+//	if(counter % 5 == 0)
+//		printf("speed_norm: %.4f\n", speed_norm);
+	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
+
+	/* Control Thrust in bearing direction  */
+	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
+				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
+
+//	if(counter % 5 == 0)
+//		printf("horizontal thrust: %.4f\n", horizontal_thrust);
+
+	/* Get total thrust (from remote for now) */
+	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
+		global_data_unlock(&global_data_rc_channels->access_conf);
+	}
+
+	const float max_gas = 500.0f;
+	thrust_total *= max_gas / 20000.0f; //TODO: check this
+	thrust_total += max_gas / 2.0f;
+
+
+	if (horizontal_thrust > thrust_total) {
+		horizontal_thrust = thrust_total;
+
+	} else if (horizontal_thrust < -thrust_total) {
+		horizontal_thrust = -thrust_total;
+	}
+
+
+
+	//TODO: maybe we want to add a speed controller later...
+
+	/* Calclulate thrust in east and north direction */
+	float thrust_north = cosf(bearing) * horizontal_thrust;
+	float thrust_east = sinf(bearing) * horizontal_thrust;
+
+	if (counter % 10 == 0) {
+		printf("thrust north: %.4f\n", thrust_north);
+		printf("thrust east: %.4f\n", thrust_east);
+		fflush(stdout);
+	}
+
+	/* Get current attitude */
+	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
+		pitch_current = global_data_attitude->pitch;
+		global_data_unlock(&global_data_attitude->access_conf);
+	}
+
+	/* Get desired pitch & roll */
+	float pitch_desired = 0.0f;
+	float roll_desired = 0.0f;
+
+	if (thrust_total != 0) {
+		float pitch_fraction = -thrust_north / thrust_total;
+		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
+
+		if (roll_fraction < -1) {
+			roll_fraction = -1;
+
+		} else if (roll_fraction > 1) {
+			roll_fraction = 1;
+		}
+
+//		if(counter % 5 == 0)
+//		{
+//			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
+//			fflush(stdout);
+//		}
+
+		pitch_desired = asinf(pitch_fraction);
+		roll_desired = asinf(roll_fraction);
+	}
+
+	/*Broadcast desired angles */
+	global_data_lock(&global_data_ardrone_control->access_conf);
+	global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[0] = roll_desired;
+	global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[1] = pitch_desired;
+	global_data_ardrone_control->attitude_setpoint_navigationframe_from_positioncontroller[2] = bearing; //TODO: add yaw setpoint
+	global_data_unlock(&global_data_ardrone_control->access_conf);
+	global_data_broadcast(&global_data_ardrone_control->access_conf);
+
+
+	counter++;
+}

apps/ardrone_control/position_control.h

@@ -0,0 +1,13 @@
+/*
+ * position_control.h
+ *
+ *  Created on: May 29, 2012
+ *      Author: thomasgubler
+ */
+
+#ifndef POSITION_CONTROL_H_
+#define POSITION_CONTROL_H_
+
+void control_position(void);
+
+#endif /* POSITION_CONTROL_H_ */

apps/ardrone_control/rate_control.c

@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <nagelit@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 rate control
+ */
+
+#include "rate_control.h"
+#include "ardrone_control_helper.h"
+#include "ardrone_motor_control.h"
+#include <arch/board/up_hrt.h>
+
+extern int ardrone_write;
+extern int gpios;
+
+typedef struct  {
+	uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration
+	uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration
+	uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration
+	uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration
+	uint8_t target_system; ///< System ID of the system that should set these motor commands
+} quad_motors_setpoint_t;
+
+
+void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints)
+{
+	static quad_motors_setpoint_t actuators_desired;
+	//static quad_motors_setpoint_t quad_motors_setpoint_desired;
+
+	static int16_t outputBand = 0;
+
+//	static uint16_t control_counter;
+	static hrt_abstime now_time;
+	static hrt_abstime last_time;
+
+	static float setpointXrate;
+	static float setpointYrate;
+	static float setpointZrate;
+
+	static float setpointRateCast[3];
+	static float Kp;
+//	static float Ki;
+	static float setpointThrustCast;
+	static float startpointFullControll;
+	static float maxThrustSetpoints;
+
+	static float gyro_filtered[3];
+	static float gyro_filtered_offset[3];
+	static float gyro_alpha;
+	static float gyro_alpha_offset;
+//	static float errXrate;
+	static float attRatesScaled[3];
+
+	static uint16_t offsetCnt;
+//	static float antiwindup;
+	static int motor_skip_counter;
+
+	static int read_ret;
+
+	static bool initialized;
+
+	if (initialized == false) {
+		initialized = true;
+
+		/* Read sensors for initial values */
+
+		gyro_filtered_offset[0] = 0.00026631611f * (float)raw->gyro_raw[0];
+		gyro_filtered_offset[1] = 0.00026631611f * (float)raw->gyro_raw[1];
+		gyro_filtered_offset[2] = 0.00026631611f * (float)raw->gyro_raw[2];
+
+		gyro_filtered[0] = 0.00026631611f * (float)raw->gyro_raw[0];
+		gyro_filtered[1] = 0.00026631611f * (float)raw->gyro_raw[1];
+		gyro_filtered[2] = 0.00026631611f * (float)raw->gyro_raw[2];
+
+		outputBand = 0;
+		startpointFullControll = 150.0f;
+		maxThrustSetpoints = 511.0f;
+		//Kp=60;
+		//Kp=40.0f;
+		//Kp=45;
+		Kp = 30.0f;
+//		Ki=0.0f;
+//		antiwindup=50.0f;
+	}
+
+	/* Get setpoint */
+
+
+	//Rate Controller
+	setpointRateCast[0] = -((float)setpoints->motor_right_ne - 9999.0f) * 0.01f / 180.0f * 3.141f;
+	setpointRateCast[1] = -((float)setpoints->motor_front_nw - 9999.0f) * 0.01f / 180.0f * 3.141f;
+	setpointRateCast[2] = 0; //-((float)setpoints->motor_back_se-9999.0f)*0.01f;
+	//Ki=actuatorDesired.motorRight_NE*0.001f;
+	setpointThrustCast = setpoints->motor_left_sw;
+
+	attRatesScaled[0] = 0.000317603994f * (float)raw->gyro_raw[0];
+	attRatesScaled[1] = 0.000317603994f * (float)raw->gyro_raw[1];
+	attRatesScaled[2] = 0.000317603994f * (float)raw->gyro_raw[2];
+
+	//filtering of the gyroscope values
+
+	//compute filter coefficient alpha
+
+	//gyro_alpha=0.005/(2.0f*3.1415f*200.0f+0.005f);
+	//gyro_alpha=0.009;
+	gyro_alpha = 0.09f;
+	gyro_alpha_offset = 0.001f;
+	//gyro_alpha=0.001;
+	//offset estimation and filtering
+	offsetCnt++;
+	uint8_t i;
+
+	for (i = 0; i < 3; i++) {
+		if (offsetCnt < 5000) {
+			gyro_filtered_offset[i] = attRatesScaled[i] * gyro_alpha_offset +  gyro_filtered_offset[i] * (1 - gyro_alpha_offset);
+		}
+
+		gyro_filtered[i] = 1.0f * ((attRatesScaled[i] - gyro_filtered_offset[i]) * gyro_alpha + gyro_filtered[i] * (1 - gyro_alpha)) - 0 * setpointRateCast[i];
+	}
+
+	// //START DEBUG
+	// /* write filtered values to global_data_attitude */
+	// global_data_attitude->rollspeed = gyro_filtered[0];
+	// global_data_attitude->pitchspeed = gyro_filtered[1];
+	// global_data_attitude->yawspeed = gyro_filtered[2];
+	// //END DEBUG
+
+	//rate controller
+
+	//X-axis
+	setpointXrate = -Kp * (setpointRateCast[0] - gyro_filtered[0]);
+	//Y-axis
+	setpointYrate = -Kp * (setpointRateCast[1] - gyro_filtered[1]);
+	//Z-axis
+	setpointZrate = -Kp * (setpointRateCast[2] - gyro_filtered[2]);
+
+
+
+
+	//Mixing
+	if (setpointThrustCast <= 0) {
+		setpointThrustCast = 0;
+		outputBand = 0;
+	}
+
+	if ((setpointThrustCast < startpointFullControll) && (setpointThrustCast > 0)) {
+		outputBand = 0.75f * setpointThrustCast;
+	}
+
+	if ((setpointThrustCast >= startpointFullControll) && (setpointThrustCast < maxThrustSetpoints - 0.75f * startpointFullControll)) {
+		outputBand = 0.75f * startpointFullControll;
+	}
+
+	if (setpointThrustCast >= maxThrustSetpoints - 0.75f * startpointFullControll) {
+		setpointThrustCast = 0.75f * startpointFullControll;
+		outputBand = 0.75f * startpointFullControll;
+	}
+
+	actuators_desired.motor_front_nw = setpointThrustCast + (setpointXrate + setpointYrate + setpointZrate);
+	actuators_desired.motor_right_ne = setpointThrustCast + (-setpointXrate + setpointYrate - setpointZrate);
+	actuators_desired.motor_back_se = setpointThrustCast + (-setpointXrate - setpointYrate + setpointZrate);
+	actuators_desired.motor_left_sw = setpointThrustCast + (setpointXrate - setpointYrate - setpointZrate);
+
+
+	if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_front_nw = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + setpointXrate + setpointYrate + setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_front_nw = setpointThrustCast - outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_right_ne = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + setpointYrate - setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_right_ne = setpointThrustCast - outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_back_se = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + (-setpointXrate) + (-setpointYrate) + setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_back_se = setpointThrustCast - outputBand;
+	}
+
+	if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) > (setpointThrustCast + outputBand)) {
+		actuators_desired.motor_left_sw = setpointThrustCast + outputBand;
+	}
+
+	if ((setpointThrustCast + setpointXrate + (-setpointYrate) - setpointZrate) < (setpointThrustCast - outputBand)) {
+		actuators_desired.motor_left_sw = setpointThrustCast - outputBand;
+	}
+
+	//printf("%lu,%lu,%lu,%lu\n",actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
+
+	if (motor_skip_counter % 5 == 0) {
+		uint8_t motorSpeedBuf[5];
+		ar_get_motor_packet(motorSpeedBuf, actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
+//		uint8_t* motorSpeedBuf = ar_get_motor_packet(1, 1, 1, 1);
+//			if(motor_skip_counter %50 == 0)
+//			{
+//				if(0==actuators_desired.motor_front_nw || 0 == actuators_desired.motor_right_ne || 0 == actuators_desired.motor_back_se || 0 == actuators_desired.motor_left_sw)
+//					printf("Motors set: %u, %u, %u, %u\n", actuators_desired.motor_front_nw, actuators_desired.motor_right_ne, actuators_desired.motor_back_se, actuators_desired.motor_left_sw);
+//			printf("input: %u\n", setpoints->motor_front_nw);
+//			printf("Roll casted desired: %f, Pitch casted desired: %f, Yaw casted desired: %f\n", setpointRateCast[0], setpointRateCast[1], setpointRateCast[2]);
+//			}
+		write(ardrone_write, motorSpeedBuf, 5);
+//			motor_skip_counter = 0;
+	}
+
+	motor_skip_counter++;
+
+	//START DEBUG
+//	global_data_lock(&global_data_ardrone_control->access_conf);
+//	global_data_ardrone_control->timestamp = hrt_absolute_time();
+//	global_data_ardrone_control->gyro_scaled[0] = attRatesScaled[0];
+//	global_data_ardrone_control->gyro_scaled[1] = attRatesScaled[1];
+//	global_data_ardrone_control->gyro_scaled[2] = attRatesScaled[2];
+//	global_data_ardrone_control->gyro_filtered[0] = gyro_filtered[0];
+//	global_data_ardrone_control->gyro_filtered[1] = gyro_filtered[1];
+//	global_data_ardrone_control->gyro_filtered[2] = gyro_filtered[2];
+//	global_data_ardrone_control->gyro_filtered_offset[0] = gyro_filtered_offset[0];
+//	global_data_ardrone_control->gyro_filtered_offset[1] = gyro_filtered_offset[1];
+//	global_data_ardrone_control->gyro_filtered_offset[2] = gyro_filtered_offset[2];
+//	global_data_ardrone_control->setpoint_rate_cast[0] = setpointRateCast[0];
+//	global_data_ardrone_control->setpoint_rate_cast[1] = setpointRateCast[1];
+//	global_data_ardrone_control->setpoint_rate_cast[2] = setpointRateCast[2];
+//	global_data_ardrone_control->setpoint_thrust_cast = setpointThrustCast;
+//	global_data_ardrone_control->setpoint_rate[0] = setpointXrate;
+//	global_data_ardrone_control->setpoint_rate[1] = setpointYrate;
+//	global_data_ardrone_control->setpoint_rate[2] = setpointZrate;
+//	global_data_ardrone_control->motor_front_nw = actuators_desired.motor_front_nw;
+//	global_data_ardrone_control->motor_right_ne = actuators_desired.motor_right_ne;
+//	global_data_ardrone_control->motor_back_se = actuators_desired.motor_back_se;
+//	global_data_ardrone_control->motor_left_sw = actuators_desired.motor_left_sw;
+//	global_data_unlock(&global_data_ardrone_control->access_conf);
+//	global_data_broadcast(&global_data_ardrone_control->access_conf);
+	//END DEBUG
+
+
+
+//	gettimeofday(&tv, NULL);
+//	now = ((uint32_t)tv.tv_sec) * 1000 + tv.tv_usec/1000;
+//	time_elapsed = now - last_run;
+//	if (time_elapsed*1000 > CONTROL_LOOP_USLEEP)
+//	{
+//		sleep_time = (int32_t)CONTROL_LOOP_USLEEP - ((int32_t)time_elapsed*1000 - (int32_t)CONTROL_LOOP_USLEEP);
+//
+//		if(motor_skip_counter %500 == 0)
+//		{
+//			printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run);
+//		}
+//	}
+//
+//	if (sleep_time <= 0)
+//	{
+//		printf("WARNING: CPU Overload!\n");
+//		printf("Desired: %u, New usleep: %i, Time elapsed: %u, Now: %u, Last run: %u\n",(uint32_t)CONTROL_LOOP_USLEEP, sleep_time, time_elapsed*1000, now, last_run);
+//		usleep(CONTROL_LOOP_USLEEP);
+//	}
+//	else
+//	{
+//		usleep(sleep_time);
+//	}
+//	last_run = now;
+//
+//	now_time = hrt_absolute_time();
+//	if(control_counter % 500 == 0)
+//	{
+//		printf("Now: %lu\n",(unsigned long)now_time);
+//		printf("Last: %lu\n",(unsigned long)last_time);
+//		printf("Difference: %lu\n", (unsigned long)(now_time - last_time));
+//		printf("now seconds: %lu\n", (unsigned long)(now_time / 1000000));
+//	}
+//	last_time = now_time;
+//
+//	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;
+//	}
+//	control_counter++;
+}

apps/ardrone_control/rate_control.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <nagelit@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 Definition of attitude rate control
+ */
+#ifndef RATE_CONTROL_H_
+#define RATE_CONTROL_H_
+
+#include <uORB/uORB.h>
+#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/sensor_combined.h>
+
+void control_rates(struct sensor_combined_s *raw, struct ardrone_motors_setpoint_s *setpoints);
+
+#endif /* RATE_CONTROL_H_ */

apps/attitude_estimator_ekf/Makefile

@@ -0,0 +1,52 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+APPNAME		 = attitude_estimator_ekf
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 20000
+
+CSRCS		 = attitude_estimator_ekf_main.c \
+		   codegen/eye.c \
+		   codegen/attitudeKalmanfilter.c \
+		   codegen/mrdivide.c \
+		   codegen/attitudeKalmanfilter_initialize.c \
+		   codegen/attitudeKalmanfilter_terminate.c \
+		   codegen/rt_nonfinite.c \
+		   codegen/rtGetInf.c \
+		   codegen/rtGetNaN.c \
+		   codegen/norm.c
+
+# XXX this is *horribly* broken
+INCLUDES	+= $(TOPDIR)/../mavlink/include/mavlink
+
+include $(APPDIR)/mk/app.mk

apps/attitude_estimator_ekf/attitudeKalmanfilter.m

@@ -0,0 +1,108 @@
+function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+%#codegen
+
+
+%Extended Attitude Kalmanfilter
+%   
+    %state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]'
+    %measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]'
+    %knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW]
+    %
+    %[x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+    %
+    %Example.... 
+    %
+    % $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $
+   
+
+    %%define the matrices
+    acc_ProcessNoise=knownConst(1);
+    mag_ProcessNoise=knownConst(2); 
+    ratesOffset_ProcessNoise=knownConst(3);
+    rates_ProcessNoise=knownConst(4);
+   
+    
+    acc_MeasurementNoise=knownConst(5);
+    mag_MeasurementNoise=knownConst(6);
+    rates_MeasurementNoise=knownConst(7);
+
+     %process noise covariance matrix
+     Q = [      eye(3)*acc_ProcessNoise,    zeros(3),                   zeros(3),                           zeros(3);
+                zeros(3),                   eye(3)*mag_ProcessNoise,    zeros(3),                           zeros(3);
+                zeros(3),                   zeros(3),                   eye(3)*ratesOffset_ProcessNoise,    zeros(3);
+                zeros(3),                   zeros(3),                   zeros(3),                           eye(3)*rates_ProcessNoise];
+    
+     %measurement noise covariance matrix
+     R = [   eye(3)*acc_MeasurementNoise,       zeros(3),                       zeros(3);
+                 zeros(3),                          eye(3)*mag_MeasurementNoise,    zeros(3);
+                 zeros(3),                          zeros(3),                       eye(3)*rates_MeasurementNoise];
+    
+
+    %observation matrix
+    H_k=[   eye(3),     zeros(3),   zeros(3),   zeros(3);
+            zeros(3),   eye(3),     zeros(3),   zeros(3);
+            zeros(3),   zeros(3),   eye(3),     eye(3)];
+        
+    %compute A(t,w)
+    
+    %x_aposteriori_k[10,11,12] should be [p,q,r]
+    %R_temp=[1,-r, q
+    %        r, 1, -p
+    %       -q, p, 1]
+
+    R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11);
+        dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10);
+        -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1];
+    
+    %strange, should not be transposed
+    A_pred=[R_temp',     zeros(3),   zeros(3),   zeros(3);
+        zeros(3),   R_temp',     zeros(3),   zeros(3);
+        zeros(3),   zeros(3),   eye(3),     zeros(3);
+        zeros(3),   zeros(3),   zeros(3),   eye(3)];
+    
+    %%prediction step
+    x_apriori=A_pred*x_aposteriori_k;
+
+    %linearization
+    acc_temp_mat=[0,              dt*x_aposteriori_k(3),    -dt*x_aposteriori_k(2);
+        -dt*x_aposteriori_k(3), 0,                  dt*x_aposteriori_k(1);
+        dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1),    0];
+    
+    mag_temp_mat=[0,              dt*x_aposteriori_k(6),    -dt*x_aposteriori_k(5);
+        -dt*x_aposteriori_k(6), 0,                  dt*x_aposteriori_k(4);
+        dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4),    0];
+    
+    A_lin=[R_temp',     zeros(3),   zeros(3),   acc_temp_mat';
+        zeros(3),   R_temp',     zeros(3),   mag_temp_mat';
+        zeros(3),   zeros(3),   eye(3),     zeros(3);
+        zeros(3),   zeros(3),   zeros(3),   eye(3)];
+    
+    
+    P_apriori=A_lin*P_aposteriori_k*A_lin'+Q;
+    
+    
+    %%update step
+
+    y_k=z_k-H_k*x_apriori;
+    S_k=H_k*P_apriori*H_k'+R;
+    K_k=(P_apriori*H_k'/(S_k));
+
+    
+    x_aposteriori=x_apriori+K_k*y_k;
+    P_aposteriori=(eye(12)-K_k*H_k)*P_apriori;
+
+
+    %%Rotation matrix generation
+    
+    earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3));
+    earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6)));
+    earth_y=cross(earth_x,earth_z);
+    
+    Rot_matrix=[earth_x,earth_y,earth_z];
+    
+
+
+
+
+
+

apps/attitude_estimator_ekf/attitudeKalmanfilter.prj

@@ -0,0 +1,270 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project>
+  <configuration target="target.matlab.ecoder" target-name="MATLAB Embedded Coder Target" name="attitudeKalmanfilter" location="F:\codegenerationMatlabAttFilter" file="F:\codegenerationMatlabAttFilter\attitudeKalmanfilter.prj" build-checksum="2344418414">
+    <param.mex.general.TargetLang>option.general.TargetLang.C</param.mex.general.TargetLang>
+    <param.mex.general.IntegrityChecks>true</param.mex.general.IntegrityChecks>
+    <param.mex.general.ResponsivenessChecks>true</param.mex.general.ResponsivenessChecks>
+    <param.mex.general.EnableBLAS>true</param.mex.general.EnableBLAS>
+    <param.mex.general.ExtrinsicCalls>true</param.mex.general.ExtrinsicCalls>
+    <param.mex.general.EchoExpressions>true</param.mex.general.EchoExpressions>
+    <param.mex.general.EnableDebugging>true</param.mex.general.EnableDebugging>
+    <param.mex.general.SaturateOnIntegerOverflow>true</param.mex.general.SaturateOnIntegerOverflow>
+    <param.mex.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.mex.general.FilePartitionMethod>
+    <param.mex.general.GlobalDataSyncMethod>option.general.GlobalDataSyncMethod.SyncAlways</param.mex.general.GlobalDataSyncMethod>
+    <param.mex.general.EnableVariableSizing>true</param.mex.general.EnableVariableSizing>
+    <param.mex.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.mex.general.DynamicMemoryAllocation>
+    <param.mex.paths.working>option.paths.working.project</param.mex.paths.working>
+    <param.mex.paths.working.specified />
+    <param.mex.paths.build>option.paths.build.project</param.mex.paths.build>
+    <param.mex.paths.build.specified />
+    <param.mex.paths.search />
+    <param.mex.report.GenerateReport>true</param.mex.report.GenerateReport>
+    <param.mex.report.LaunchReport>false</param.mex.report.LaunchReport>
+    <param.mex.comments.GenerateComments>true</param.mex.comments.GenerateComments>
+    <param.mex.comments.MATLABSourceComments>true</param.mex.comments.MATLABSourceComments>
+    <param.mex.symbols.ReservedNameArray />
+    <param.mex.customcode.CustomSourceCode />
+    <param.mex.customcode.CustomHeaderCode />
+    <param.mex.customcode.CustomInitializer />
+    <param.mex.customcode.CustomTerminator />
+    <param.mex.customcode.CustomInclude />
+    <param.mex.customcode.CustomSource />
+    <param.mex.customcode.CustomLibrary />
+    <param.mex.PostCodeGenCommand />
+    <param.mex.EnableMemcpy>true</param.mex.EnableMemcpy>
+    <param.mex.MemcpyThreshold>64</param.mex.MemcpyThreshold>
+    <param.mex.InitFltsAndDblsToZero>true</param.mex.InitFltsAndDblsToZero>
+    <param.mex.InlineThreshold>10</param.mex.InlineThreshold>
+    <param.mex.InlineThresholdMax>200</param.mex.InlineThresholdMax>
+    <param.mex.InlineStackLimit>4000</param.mex.InlineStackLimit>
+    <param.mex.StackUsageMax>200000</param.mex.StackUsageMax>
+    <param.mex.ConstantFoldingTimeout>10000</param.mex.ConstantFoldingTimeout>
+    <param.grt.general.TargetLang>option.general.TargetLang.C</param.grt.general.TargetLang>
+    <param.general.target.description>MATLAB Embedded Coder Target</param.general.target.description>
+    <param.grt.CCompilerOptimization>option.CCompilerOptimization.On</param.grt.CCompilerOptimization>
+    <param.grt.CCompilerCustomOptimizations />
+    <param.grt.general.GenerateMakefile>true</param.grt.general.GenerateMakefile>
+    <param.grt.general.MakeCommand>make_rtw</param.grt.general.MakeCommand>
+    <param.grt.general.TemplateMakefile>default_tmf</param.grt.general.TemplateMakefile>
+    <param.grt.general.SaturateOnIntegerOverflow>true</param.grt.general.SaturateOnIntegerOverflow>
+    <param.grt.general.FilePartitionMethod>option.general.FilePartitionMethod.MapMFileToCFile</param.grt.general.FilePartitionMethod>
+    <param.grt.general.EnableVariableSizing>true</param.grt.general.EnableVariableSizing>
+    <param.grt.general.DynamicMemoryAllocation>option.general.DynamicMemoryAllocation.Disabled</param.grt.general.DynamicMemoryAllocation>
+    <param.grt.paths.working>option.paths.working.project</param.grt.paths.working>
+    <param.grt.paths.working.specified />
+    <param.grt.paths.build>option.paths.build.project</param.grt.paths.build>
+    <param.grt.paths.build.specified />
+    <param.grt.paths.search />
+    <param.grt.report.GenerateReport>true</param.grt.report.GenerateReport>
+    <param.grt.report.LaunchReport>false</param.grt.report.LaunchReport>
+    <param.grt.GenerateComments>true</param.grt.GenerateComments>
+    <param.grt.MATLABSourceComments>true</param.grt.MATLABSourceComments>
+    <param.ert.MATLABFcnDesc>false</param.ert.MATLABFcnDesc>
+    <param.ert.CustomSymbolStrGlobalVar>$M$N</param.ert.CustomSymbolStrGlobalVar>
+    <param.ert.CustomSymbolStrType>$M$N</param.ert.CustomSymbolStrType>
+    <param.ert.CustomSymbolStrField>$M$N</param.ert.CustomSymbolStrField>
+    <param.ert.CustomSymbolStrFcn>$M$N</param.ert.CustomSymbolStrFcn>
+    <param.ert.CustomSymbolStrTmpVar>$M$N</param.ert.CustomSymbolStrTmpVar>
+    <param.ert.CustomSymbolStrMacro>$M$N</param.ert.CustomSymbolStrMacro>
+    <param.ert.CustomSymbolStrEMXArray>emxArray_$M$N</param.ert.CustomSymbolStrEMXArray>
+    <param.grt.MaxIdLength>32</param.grt.MaxIdLength>
+    <param.grt.ReservedNameArray />
+    <param.grt.customcode.CustomSourceCode />
+    <param.grt.customcode.CustomHeaderCode />
+    <param.grt.customcode.CustomInitializer />
+    <param.grt.customcode.CustomTerminator />
+    <param.grt.customcode.CustomInclude />
+    <param.grt.customcode.CustomSource />
+    <param.grt.customcode.CustomLibrary />
+    <param.grt.PostCodeGenCommand />
+    <param.grt.Verbose>false</param.grt.Verbose>
+    <param.grt.TargetFunctionLibrary>C89/C90 (ANSI)</param.grt.TargetFunctionLibrary>
+    <param.grt.SupportNonFinite>true</param.grt.SupportNonFinite>
+    <param.ert.TargetFunctionLibrary>C99 (ISO)</param.ert.TargetFunctionLibrary>
+    <param.ert.PurelyIntegerCode>false</param.ert.PurelyIntegerCode>
+    <param.ert.SupportNonFinite>true</param.ert.SupportNonFinite>
+    <param.ert.IncludeTerminateFcn>true</param.ert.IncludeTerminateFcn>
+    <param.ert.MultiInstanceCode>false</param.ert.MultiInstanceCode>
+    <param.ert.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ert.ParenthesesLevel>
+    <param.ert.ConvertIfToSwitch>false</param.ert.ConvertIfToSwitch>
+    <param.ert.PreserveExternInFcnDecls>true</param.ert.PreserveExternInFcnDecls>
+    <param.grt.EnableMemcpy>true</param.grt.EnableMemcpy>
+    <param.grt.MemcpyThreshold>64</param.grt.MemcpyThreshold>
+    <param.grt.InitFltsAndDblsToZero>true</param.grt.InitFltsAndDblsToZero>
+    <param.grt.InlineThreshold>10</param.grt.InlineThreshold>
+    <param.grt.InlineThresholdMax>200</param.grt.InlineThresholdMax>
+    <param.grt.InlineStackLimit>4000</param.grt.InlineStackLimit>
+    <param.grt.StackUsageMax>200000</param.grt.StackUsageMax>
+    <param.grt.ConstantFoldingTimeout>10000</param.grt.ConstantFoldingTimeout>
+    <param.UseECoderFeatures>true</param.UseECoderFeatures>
+    <param.mex.outputfile>attitudeKalmanfilter_mex</param.mex.outputfile>
+    <param.grt.outputfile>attitudeKalmanfilter</param.grt.outputfile>
+    <param.artifact>option.target.artifact.lib</param.artifact>
+    <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
+    <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+    <param.version>R2011a</param.version>
+    <param.HasECoderFeatures>true</param.HasECoderFeatures>
+    <param.mex.mainhtml />
+    <param.grt.mainhtml>F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter\html\index.html</param.grt.mainhtml>
+    <unset>
+      <param.mex.general.TargetLang />
+      <param.mex.general.IntegrityChecks />
+      <param.mex.general.ResponsivenessChecks />
+      <param.mex.general.EnableBLAS />
+      <param.mex.general.ExtrinsicCalls />
+      <param.mex.general.EchoExpressions />
+      <param.mex.general.EnableDebugging />
+      <param.mex.general.SaturateOnIntegerOverflow />
+      <param.mex.general.FilePartitionMethod />
+      <param.mex.general.GlobalDataSyncMethod />
+      <param.mex.general.EnableVariableSizing />
+      <param.mex.general.DynamicMemoryAllocation />
+      <param.mex.paths.working />
+      <param.mex.paths.working.specified />
+      <param.mex.paths.build />
+      <param.mex.paths.build.specified />
+      <param.mex.paths.search />
+      <param.mex.report.GenerateReport />
+      <param.mex.report.LaunchReport />
+      <param.mex.comments.GenerateComments />
+      <param.mex.comments.MATLABSourceComments />
+      <param.mex.symbols.ReservedNameArray />
+      <param.mex.customcode.CustomSourceCode />
+      <param.mex.customcode.CustomHeaderCode />
+      <param.mex.customcode.CustomInitializer />
+      <param.mex.customcode.CustomTerminator />
+      <param.mex.customcode.CustomInclude />
+      <param.mex.customcode.CustomSource />
+      <param.mex.customcode.CustomLibrary />
+      <param.mex.PostCodeGenCommand />
+      <param.mex.EnableMemcpy />
+      <param.mex.MemcpyThreshold />
+      <param.mex.InitFltsAndDblsToZero />
+      <param.mex.InlineThreshold />
+      <param.mex.InlineThresholdMax />
+      <param.mex.InlineStackLimit />
+      <param.mex.StackUsageMax />
+      <param.mex.ConstantFoldingTimeout />
+      <param.grt.general.TargetLang />
+      <param.grt.CCompilerCustomOptimizations />
+      <param.grt.general.GenerateMakefile />
+      <param.grt.general.MakeCommand />
+      <param.grt.general.TemplateMakefile />
+      <param.grt.general.SaturateOnIntegerOverflow />
+      <param.grt.general.FilePartitionMethod />
+      <param.grt.general.EnableVariableSizing />
+      <param.grt.general.DynamicMemoryAllocation />
+      <param.grt.paths.working />
+      <param.grt.paths.working.specified />
+      <param.grt.paths.build />
+      <param.grt.paths.build.specified />
+      <param.grt.paths.search />
+      <param.grt.report.GenerateReport />
+      <param.grt.report.LaunchReport />
+      <param.grt.GenerateComments />
+      <param.ert.MATLABFcnDesc />
+      <param.ert.CustomSymbolStrGlobalVar />
+      <param.ert.CustomSymbolStrType />
+      <param.ert.CustomSymbolStrField />
+      <param.ert.CustomSymbolStrFcn />
+      <param.ert.CustomSymbolStrTmpVar />
+      <param.ert.CustomSymbolStrMacro />
+      <param.ert.CustomSymbolStrEMXArray />
+      <param.grt.MaxIdLength />
+      <param.grt.ReservedNameArray />
+      <param.grt.customcode.CustomHeaderCode />
+      <param.grt.customcode.CustomInitializer />
+      <param.grt.customcode.CustomTerminator />
+      <param.grt.customcode.CustomInclude />
+      <param.grt.customcode.CustomSource />
+      <param.grt.customcode.CustomLibrary />
+      <param.grt.PostCodeGenCommand />
+      <param.grt.Verbose />
+      <param.grt.SupportNonFinite />
+      <param.ert.PurelyIntegerCode />
+      <param.ert.SupportNonFinite />
+      <param.ert.IncludeTerminateFcn />
+      <param.ert.MultiInstanceCode />
+      <param.ert.ParenthesesLevel />
+      <param.ert.ConvertIfToSwitch />
+      <param.ert.PreserveExternInFcnDecls />
+      <param.grt.EnableMemcpy />
+      <param.grt.MemcpyThreshold />
+      <param.grt.InitFltsAndDblsToZero />
+      <param.grt.InlineThreshold />
+      <param.grt.InlineThresholdMax />
+      <param.grt.InlineStackLimit />
+      <param.grt.StackUsageMax />
+      <param.grt.ConstantFoldingTimeout />
+      <param.UseECoderFeatures />
+      <param.mex.outputfile />
+      <param.grt.outputfile />
+      <param.mex.GenCodeOnly />
+      <param.version />
+      <param.HasECoderFeatures />
+      <param.mex.mainhtml />
+    </unset>
+    <fileset.entrypoints>
+      <file value="${PROJECT_ROOT}\attitudeKalmanfilter.m" custom-data-expanded="true">
+        <Inputs fileName="attitudeKalmanfilter.m" functionName="attitudeKalmanfilter">
+          <Input Name="dt">
+            <Class>single</Class>
+            <Size>1 x 1</Size>
+            <Value />
+            <InitialValue />
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="z_k">
+            <Class>single</Class>
+            <Size>9 x 1</Size>
+            <Value />
+            <InitialValue />
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="x_aposteriori_k">
+            <Class>single</Class>
+            <Size>12 x 1</Size>
+            <Value />
+            <InitialValue />
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="P_aposteriori_k">
+            <Class>single</Class>
+            <Size>12 x 12</Size>
+            <Value />
+            <InitialValue />
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="knownConst">
+            <Class>single</Class>
+            <Size>7 x 1</Size>
+            <Value />
+            <InitialValue />
+            <Complex>false</Complex>
+          </Input>
+        </Inputs>
+      </file>
+    </fileset.entrypoints>
+    <fileset.package />
+    <build-deliverables />
+    <matlab>
+      <root>C:\Program Files\MATLAB\R2011a</root>
+    </matlab>
+    <platform>
+      <unix>false</unix>
+      <mac>false</mac>
+      <windows>true</windows>
+      <win2k>false</win2k>
+      <winxp>false</winxp>
+      <vista>false</vista>
+      <linux>false</linux>
+      <solaris>false</solaris>
+      <osver>6.1</osver>
+      <os32>false</os32>
+      <os64>true</os64>
+      <arch>win64</arch>
+      <matlab>true</matlab>
+    </platform>
+  </configuration>
+</deployment-project>
+

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -0,0 +1,263 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Laurens Mackay <mackayl@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 Extended Kalman Filter for Attitude Estimation
+ */
+
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <v1.0/common/mavlink.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <arch/board/up_hrt.h>
+
+#include "codegen/attitudeKalmanfilter_initialize.h"
+#include "codegen/attitudeKalmanfilter.h"
+
+__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
+
+
+// #define N_STATES 6
+
+// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
+// #define REPROJECTION_COUNTER_LIMIT 125
+
+static unsigned int loop_interval_alarm = 4500;	// loop interval in microseconds
+
+static float dt = 1;
+/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
+/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+static float z_k[9] = {0};					/**< Measurement vector */
+static float x_aposteriori[12] = {0};		/**< */
+static float P_aposteriori[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,
+				   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0, 100.f
+				  };	/**< init: diagonal matrix with big values */
+static float knownConst[7] = {1, 1, 1, 1, 1, 1, 1};			/**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+static float Rot_matrix[9] = {1.f,  0,  0,
+			      0,  1.f,  0,
+			      0,  0,  1.f
+			     };		/**< init: identity matrix */
+
+// static float x_aposteriori_k[12] = {0};
+// static float P_aposteriori_k[144] = {0};
+
+/*
+ * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ */
+
+/*
+ * EKF Attitude Estimator main function.
+ *
+ * Estimates the attitude recursively once started.
+ *
+ * @param argc number of commandline arguments (plus command name)
+ * @param argv strings containing the arguments
+ */
+int attitude_estimator_ekf_main(int argc, char *argv[])
+{
+	// print text
+	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
+	fflush(stdout);
+
+	int overloadcounter = 19;
+
+	/* Initialize filter */
+	attitudeKalmanfilter_initialize();
+
+	/* store start time to guard against too slow update rates */
+	uint64_t last_run = hrt_absolute_time();
+
+	struct sensor_combined_s raw = {0};
+	struct vehicle_attitude_s att = {};
+
+	uint64_t last_data = 0;
+	uint64_t last_measurement = 0;
+
+	/* subscribe to raw data */
+	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
+	/* advertise attitude */
+	int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
+
+	int loopcounter = 0;
+	int printcounter = 0;
+
+	/* Main loop*/
+	while (true) {
+
+		struct pollfd fds[1] = {
+			{ .fd = sub_raw,   .events = POLLIN },
+		};
+		int ret = poll(fds, 1, 1000);
+
+		/* check for a timeout */
+		if (ret == 0) {
+			/* */
+
+			/* update successful, copy data on every 2nd run and execute filter */
+		} else if (loopcounter & 0x01) {
+
+			orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
+
+			/* Calculate data time difference in seconds */
+			dt = (raw.timestamp - last_measurement) / 1000000.0f;
+			last_measurement = raw.timestamp;
+
+			// XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here
+			float range_g = 4.0f;
+			float mag_offset[3] = {0};
+			/* scale from 14 bit to m/s2 */
+			z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f;  // = accel * (1 / 32768.0f / 8.0f * 9.81f);
+			z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f;  // = accel * (1 / 32768.0f / 8.0f * 9.81f);
+			z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f;  // = accel * (1 / 32768.0f / 8.0f * 9.81f);
+
+			// XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here
+			z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f;
+			z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f;
+			z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f;
+
+			/* Fill in gyro measurements */
+			z_k[6] =  raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
+			z_k[7] =  raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
+			z_k[8] =  raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */;
+
+
+			uint64_t now = hrt_absolute_time();
+			unsigned int time_elapsed = now - last_run;
+			last_run = now;
+
+			if (time_elapsed > loop_interval_alarm) {
+				//TODO: add warning, cpu overload here
+				if (overloadcounter == 20) {
+					printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+					overloadcounter = 0;
+				}
+
+				overloadcounter++;
+			}
+
+//			now = hrt_absolute_time();
+			/* filter values */
+			/*
+			 * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+			 */
+			uint64_t timing_start = hrt_absolute_time();
+			attitudeKalmanfilter(dt, z_k, x_aposteriori, P_aposteriori, knownConst, Rot_matrix, x_aposteriori, P_aposteriori);
+			uint64_t timing_diff = hrt_absolute_time() - timing_start;
+
+			/* print rotation matrix every 200th time */
+			if (printcounter % 200 == 0) {
+				printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
+				printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
+				       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
+				       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
+			}
+
+			printcounter++;
+
+//			time_elapsed = hrt_absolute_time() - now;
+//			if (blubb == 20)
+//			{
+//				printf("Estimator: %lu\n", time_elapsed);
+//				blubb = 0;
+//			}
+//			blubb++;
+
+			if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data);
+
+//			printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data);
+			last_data = raw.timestamp;
+
+			/* send out */
+			att.timestamp = raw.timestamp;
+			// att.roll = euler.x;
+			// att.pitch = euler.y;
+			// att.yaw = euler.z + F_M_PI;
+
+			// if (att.yaw > 2 * F_M_PI) {
+			// 	att.yaw -= 2 * F_M_PI;
+			// }
+
+			// att.rollspeed = rates.x;
+			// att.pitchspeed = rates.y;
+			// att.yawspeed = rates.z;
+
+			// memcpy()R
+
+			// Broadcast
+			orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+		}
+
+		loopcounter++;
+	}
+
+	/* Should never reach here */
+	return 0;
+}
+
+
+
+
+

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c

@@ -0,0 +1,635 @@
+/*
+ * attitudeKalmanfilter.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+#include "eye.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ */
+void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T
+  x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T
+  knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T
+  P_aposteriori[144])
+{
+  real32_T R_temp[9];
+  real_T dv0[9];
+  real_T dv1[9];
+  int32_T i;
+  int32_T i0;
+  real32_T A_pred[144];
+  real32_T x_apriori[12];
+  real32_T b_A_pred[144];
+  int32_T i1;
+  real32_T c_A_pred[144];
+  static const int8_T iv0[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
+
+  real32_T P_apriori[144];
+  real32_T b_P_apriori[108];
+  static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0,
+    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1 };
+
+  real32_T K_k[108];
+  static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
+    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
+    0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
+    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0,
+    0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 };
+
+  real32_T fv0[81];
+  real32_T fv1[81];
+  real32_T fv2[81];
+  real32_T B;
+  real_T dv2[144];
+  real32_T b_B;
+  real32_T earth_z[3];
+  real32_T y[3];
+  real32_T earth_x[3];
+
+  /* Extended Attitude Kalmanfilter */
+  /*     */
+  /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+  /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */
+  /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+  /*  */
+  /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */
+  /*  */
+  /* Example....  */
+  /*  */
+  /*  $Author: Tobias Naegeli $    $Date: 2012 $    $Revision: 1 $ */
+  /* %define the matrices */
+  /* 'attitudeKalmanfilter:19' acc_ProcessNoise=knownConst(1); */
+  /* 'attitudeKalmanfilter:20' mag_ProcessNoise=knownConst(2); */
+  /* 'attitudeKalmanfilter:21' ratesOffset_ProcessNoise=knownConst(3); */
+  /* 'attitudeKalmanfilter:22' rates_ProcessNoise=knownConst(4); */
+  /* 'attitudeKalmanfilter:25' acc_MeasurementNoise=knownConst(5); */
+  /* 'attitudeKalmanfilter:26' mag_MeasurementNoise=knownConst(6); */
+  /* 'attitudeKalmanfilter:27' rates_MeasurementNoise=knownConst(7); */
+  /* process noise covariance matrix */
+  /* 'attitudeKalmanfilter:30' Q = [      eye(3)*acc_ProcessNoise,    zeros(3),                   zeros(3),                           zeros(3); */
+  /* 'attitudeKalmanfilter:31'                 zeros(3),                   eye(3)*mag_ProcessNoise,    zeros(3),                           zeros(3); */
+  /* 'attitudeKalmanfilter:32'                 zeros(3),                   zeros(3),                   eye(3)*ratesOffset_ProcessNoise,    zeros(3); */
+  /* 'attitudeKalmanfilter:33'                 zeros(3),                   zeros(3),                   zeros(3),                           eye(3)*rates_ProcessNoise]; */
+  /* measurement noise covariance matrix */
+  /* 'attitudeKalmanfilter:36' R = [   eye(3)*acc_MeasurementNoise,       zeros(3),                       zeros(3); */
+  /* 'attitudeKalmanfilter:37'                  zeros(3),                          eye(3)*mag_MeasurementNoise,    zeros(3); */
+  /* 'attitudeKalmanfilter:38'                  zeros(3),                          zeros(3),                       eye(3)*rates_MeasurementNoise]; */
+  /* observation matrix */
+  /* 'attitudeKalmanfilter:42' H_k=[   eye(3),     zeros(3),   zeros(3),   zeros(3); */
+  /* 'attitudeKalmanfilter:43'             zeros(3),   eye(3),     zeros(3),   zeros(3); */
+  /* 'attitudeKalmanfilter:44'             zeros(3),   zeros(3),   eye(3),     eye(3)]; */
+  /* compute A(t,w) */
+  /* x_aposteriori_k[10,11,12] should be [p,q,r] */
+  /* R_temp=[1,-r, q */
+  /*         r, 1, -p */
+  /*        -q, p, 1] */
+  /* 'attitudeKalmanfilter:53' R_temp=[1,-dt*x_aposteriori_k(12),dt*x_aposteriori_k(11); */
+  /* 'attitudeKalmanfilter:54'         dt*x_aposteriori_k(12),1,-dt*x_aposteriori_k(10); */
+  /* 'attitudeKalmanfilter:55'         -dt*x_aposteriori_k(11), dt*x_aposteriori_k(10),1]; */
+  R_temp[0] = 1.0F;
+  R_temp[3] = -dt * x_aposteriori_k[11];
+  R_temp[6] = dt * x_aposteriori_k[10];
+  R_temp[1] = dt * x_aposteriori_k[11];
+  R_temp[4] = 1.0F;
+  R_temp[7] = -dt * x_aposteriori_k[9];
+  R_temp[2] = -dt * x_aposteriori_k[10];
+  R_temp[5] = dt * x_aposteriori_k[9];
+  R_temp[8] = 1.0F;
+
+  /* strange, should not be transposed */
+  /* 'attitudeKalmanfilter:58' A_pred=[R_temp',     zeros(3),   zeros(3),   zeros(3); */
+  /* 'attitudeKalmanfilter:59'         zeros(3),   R_temp',     zeros(3),   zeros(3); */
+  /* 'attitudeKalmanfilter:60'         zeros(3),   zeros(3),   eye(3),     zeros(3); */
+  /* 'attitudeKalmanfilter:61'         zeros(3),   zeros(3),   zeros(3),   eye(3)]; */
+  eye(dv0);
+  eye(dv1);
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * (i + 3)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * (i + 6)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * (i + 9)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * i) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * i) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * i) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+    }
+  }
+
+  /* %prediction step */
+  /* 'attitudeKalmanfilter:64' x_apriori=A_pred*x_aposteriori_k; */
+  for (i = 0; i < 12; i++) {
+    x_apriori[i] = 0.0F;
+    for (i0 = 0; i0 < 12; i0++) {
+      x_apriori[i] += A_pred[i + 12 * i0] * x_aposteriori_k[i0];
+    }
+  }
+
+  /* linearization */
+  /* 'attitudeKalmanfilter:67' acc_temp_mat=[0,              dt*x_aposteriori_k(3),    -dt*x_aposteriori_k(2); */
+  /* 'attitudeKalmanfilter:68'         -dt*x_aposteriori_k(3), 0,                  dt*x_aposteriori_k(1); */
+  /* 'attitudeKalmanfilter:69'         dt*x_aposteriori_k(2), -dt*x_aposteriori_k(1),    0]; */
+  /* 'attitudeKalmanfilter:71' mag_temp_mat=[0,              dt*x_aposteriori_k(6),    -dt*x_aposteriori_k(5); */
+  /* 'attitudeKalmanfilter:72'         -dt*x_aposteriori_k(6), 0,                  dt*x_aposteriori_k(4); */
+  /* 'attitudeKalmanfilter:73'         dt*x_aposteriori_k(5), -dt*x_aposteriori_k(4),    0]; */
+  /* 'attitudeKalmanfilter:75' A_lin=[R_temp',     zeros(3),   zeros(3),   acc_temp_mat'; */
+  /* 'attitudeKalmanfilter:76'         zeros(3),   R_temp',     zeros(3),   mag_temp_mat'; */
+  /* 'attitudeKalmanfilter:77'         zeros(3),   zeros(3),   eye(3),     zeros(3); */
+  /* 'attitudeKalmanfilter:78'         zeros(3),   zeros(3),   zeros(3),   eye(3)]; */
+  eye(dv0);
+  eye(dv1);
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * i] = R_temp[i + 3 * i0];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * (i + 3)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[i0 + 12 * (i + 6)] = 0.0F;
+    }
+  }
+
+  A_pred[108] = 0.0F;
+  A_pred[109] = dt * x_aposteriori_k[2];
+  A_pred[110] = -dt * x_aposteriori_k[1];
+  A_pred[120] = -dt * x_aposteriori_k[2];
+  A_pred[121] = 0.0F;
+  A_pred[122] = dt * x_aposteriori_k[0];
+  A_pred[132] = dt * x_aposteriori_k[1];
+  A_pred[133] = -dt * x_aposteriori_k[0];
+  A_pred[134] = 0.0F;
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * i) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 3)) + 3] = R_temp[i + 3 * i0];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+    }
+  }
+
+  A_pred[111] = 0.0F;
+  A_pred[112] = dt * x_aposteriori_k[5];
+  A_pred[113] = -dt * x_aposteriori_k[4];
+  A_pred[123] = -dt * x_aposteriori_k[5];
+  A_pred[124] = 0.0F;
+  A_pred[125] = dt * x_aposteriori_k[3];
+  A_pred[135] = dt * x_aposteriori_k[4];
+  A_pred[136] = -dt * x_aposteriori_k[3];
+  A_pred[137] = 0.0F;
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * i) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)dv0[i0 + 3 * i];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * i) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)dv1[i0 + 3 * i];
+    }
+  }
+
+  /* 'attitudeKalmanfilter:81' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */
+  for (i = 0; i < 12; i++) {
+    for (i0 = 0; i0 < 12; i0++) {
+      b_A_pred[i + 12 * i0] = 0.0F;
+      for (i1 = 0; i1 < 12; i1++) {
+        b_A_pred[i + 12 * i0] += A_pred[i + 12 * i1] * P_aposteriori_k[i1 + 12 *
+          i0];
+      }
+    }
+
+    for (i0 = 0; i0 < 12; i0++) {
+      c_A_pred[i + 12 * i0] = 0.0F;
+      for (i1 = 0; i1 < 12; i1++) {
+        c_A_pred[i + 12 * i0] += b_A_pred[i + 12 * i1] * A_pred[i0 + 12 * i1];
+      }
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[0];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[i0 + 12 * (i + 3)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[i0 + 12 * (i + 6)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[i0 + 12 * (i + 9)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * i) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] *
+        knownConst[1];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 6)) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 9)) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * i) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 3)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] *
+        knownConst[2];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 9)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * i) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 3)) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 6)) + 9] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      b_A_pred[(i0 + 12 * (i + 9)) + 9] = (real32_T)iv0[i0 + 3 * i] *
+        knownConst[3];
+    }
+  }
+
+  for (i = 0; i < 12; i++) {
+    for (i0 = 0; i0 < 12; i0++) {
+      P_apriori[i0 + 12 * i] = c_A_pred[i0 + 12 * i] + b_A_pred[i0 + 12 * i];
+    }
+  }
+
+  /* %update step */
+  /* 'attitudeKalmanfilter:86' y_k=z_k-H_k*x_apriori; */
+  /* 'attitudeKalmanfilter:87' S_k=H_k*P_apriori*H_k'+R; */
+  /* 'attitudeKalmanfilter:88' K_k=(P_apriori*H_k'/(S_k)); */
+  for (i = 0; i < 12; i++) {
+    for (i0 = 0; i0 < 9; i0++) {
+      b_P_apriori[i + 12 * i0] = 0.0F;
+      for (i1 = 0; i1 < 12; i1++) {
+        b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 +
+          12 * i0];
+      }
+    }
+  }
+
+  for (i = 0; i < 9; i++) {
+    for (i0 = 0; i0 < 12; i0++) {
+      K_k[i + 9 * i0] = 0.0F;
+      for (i1 = 0; i1 < 12; i1++) {
+        K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0];
+      }
+    }
+
+    for (i0 = 0; i0 < 9; i0++) {
+      fv0[i + 9 * i0] = 0.0F;
+      for (i1 = 0; i1 < 12; i1++) {
+        fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0];
+      }
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[i0 + 9 * i] = (real32_T)iv0[i0 + 3 * i] * knownConst[4];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[i0 + 9 * (i + 3)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[i0 + 9 * (i + 6)] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[(i0 + 9 * i) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[(i0 + 9 * (i + 3)) + 3] = (real32_T)iv0[i0 + 3 * i] * knownConst[5];
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[(i0 + 9 * (i + 6)) + 3] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[(i0 + 9 * i) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[(i0 + 9 * (i + 3)) + 6] = 0.0F;
+    }
+  }
+
+  for (i = 0; i < 3; i++) {
+    for (i0 = 0; i0 < 3; i0++) {
+      fv1[(i0 + 9 * (i + 6)) + 6] = (real32_T)iv0[i0 + 3 * i] * knownConst[6];
+    }
+  }
+
+  for (i = 0; i < 9; i++) {
+    for (i0 = 0; i0 < 9; i0++) {
+      fv2[i0 + 9 * i] = fv0[i0 + 9 * i] + fv1[i0 + 9 * i];
+    }
+  }
+
+  mrdivide(b_P_apriori, fv2, K_k);
+
+  /* 'attitudeKalmanfilter:91' x_aposteriori=x_apriori+K_k*y_k; */
+  for (i = 0; i < 9; i++) {
+    B = 0.0F;
+    for (i0 = 0; i0 < 12; i0++) {
+      B += (real32_T)iv2[i + 9 * i0] * x_apriori[i0];
+    }
+
+    R_temp[i] = z_k[i] - B;
+  }
+
+  for (i = 0; i < 12; i++) {
+    B = 0.0F;
+    for (i0 = 0; i0 < 9; i0++) {
+      B += K_k[i + 12 * i0] * R_temp[i0];
+    }
+
+    x_aposteriori[i] = x_apriori[i] + B;
+  }
+
+  /* 'attitudeKalmanfilter:92' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */
+  b_eye(dv2);
+  for (i = 0; i < 12; i++) {
+    for (i0 = 0; i0 < 12; i0++) {
+      B = 0.0F;
+      for (i1 = 0; i1 < 9; i1++) {
+        B += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0];
+      }
+
+      A_pred[i + 12 * i0] = (real32_T)dv2[i + 12 * i0] - B;
+    }
+  }
+
+  for (i = 0; i < 12; i++) {
+    for (i0 = 0; i0 < 12; i0++) {
+      P_aposteriori[i + 12 * i0] = 0.0F;
+      for (i1 = 0; i1 < 12; i1++) {
+        P_aposteriori[i + 12 * i0] += A_pred[i + 12 * i1] * P_apriori[i1 + 12 *
+          i0];
+      }
+    }
+  }
+
+  /* %Rotation matrix generation */
+  /* 'attitudeKalmanfilter:97' earth_z=x_aposteriori(1:3)/norm(x_aposteriori(1:3)); */
+  B = norm(*(real32_T (*)[3])&x_aposteriori[0]);
+
+  /* 'attitudeKalmanfilter:98' earth_x=cross(earth_z,x_aposteriori(4:6)/norm(x_aposteriori(4:6))); */
+  b_B = norm(*(real32_T (*)[3])&x_aposteriori[3]);
+  for (i = 0; i < 3; i++) {
+    earth_z[i] = x_aposteriori[i] / B;
+    y[i] = x_aposteriori[i + 3] / b_B;
+  }
+
+  earth_x[0] = earth_z[1] * y[2] - earth_z[2] * y[1];
+  earth_x[1] = earth_z[2] * y[0] - earth_z[0] * y[2];
+  earth_x[2] = earth_z[0] * y[1] - earth_z[1] * y[0];
+
+  /* 'attitudeKalmanfilter:99' earth_y=cross(earth_x,earth_z); */
+  /* 'attitudeKalmanfilter:101' Rot_matrix=[earth_x,earth_y,earth_z]; */
+  y[0] = earth_x[1] * earth_z[2] - earth_x[2] * earth_z[1];
+  y[1] = earth_x[2] * earth_z[0] - earth_x[0] * earth_z[2];
+  y[2] = earth_x[0] * earth_z[1] - earth_x[1] * earth_z[0];
+  for (i = 0; i < 3; i++) {
+    Rot_matrix[i] = earth_x[i];
+    Rot_matrix[3 + i] = y[i];
+    Rot_matrix[6 + i] = earth_z[i];
+  }
+}
+
+/* End of code generation (attitudeKalmanfilter.c) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h

@@ -0,0 +1,32 @@
+/*
+ * attitudeKalmanfilter.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_H__
+#define __ATTITUDEKALMANFILTER_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter(real32_T dt, const real32_T z_k[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T knownConst[7], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+#endif
+/* End of code generation (attitudeKalmanfilter.h) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c

@@ -0,0 +1,31 @@
+/*
+ * attitudeKalmanfilter_initialize.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (attitudeKalmanfilter_initialize.c) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h

@@ -0,0 +1,32 @@
+/*
+ * attitudeKalmanfilter_initialize.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
+#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter_initialize(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_initialize.h) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.bat

@@ -0,0 +1,11 @@
+@echo off
+call "%VS100COMNTOOLS%..\..\VC\vcvarsall.bat" AMD64
+
+cd .
+nmake -f attitudeKalmanfilter_rtw.mk  GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
+@if errorlevel 1 goto error_exit
+exit /B 0
+
+:error_exit
+echo The make command returned an error of %errorlevel%
+An_error_occurred_during_the_call_to_make

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_rtw.mk

@@ -0,0 +1,328 @@
+# Copyright 1994-2010 The MathWorks, Inc.
+#
+# File    : xrt_vcx64.tmf   $Revision: 1.1.6.1 $
+#
+# Abstract:
+#       Code generation template makefile for building a Windows-based,
+#       stand-alone real-time version of MATLAB-code using generated C/C++
+#       code and the
+#          Microsoft Visual C/C++ compiler version 8, 9, 10 for x64
+#
+#       Note that this template is automatically customized by the
+#       code generation build procedure to create "<model>.mk"
+#
+#       The following defines can be used to modify the behavior of the
+#       build:
+#
+#    	  OPT_OPTS       - Optimization option. See DEFAULT_OPT_OPTS in
+#                          vctools.mak for default.
+#    	  OPTS           - User specific options.
+#	  CPP_OPTS       - C++ compiler options.
+#    	  USER_SRCS      - Additional user sources, such as files needed by
+#    			   S-functions.
+#    	  USER_INCLUDES  - Additional include paths
+#    			   (i.e. USER_INCLUDES="-Iwhere-ever -Iwhere-ever2")
+#
+#       To enable debugging:
+#         set OPT_OPTS=-Zi (may vary with compiler version, see compiler doc) 
+#         modify tmf LDFLAGS to include /DEBUG
+#
+
+#------------------------ Macros read by make_rtw -----------------------------
+#
+# The following macros are read by the code generation build procedure:
+#
+#  MAKECMD         - This is the command used to invoke the make utility
+#  HOST            - What platform this template makefile is targeted for
+#                    (i.e. PC or UNIX)
+#  BUILD           - Invoke make from the code generation build procedure
+#                    (yes/no)?
+#  SYS_TARGET_FILE - Name of system target file.
+
+MAKECMD         = nmake
+HOST            = PC
+BUILD           = yes
+SYS_TARGET_FILE = ert.tlc
+BUILD_SUCCESS	= ^#^#^# Created
+COMPILER_TOOL_CHAIN = vcx64
+
+#---------------------- Tokens expanded by make_rtw ---------------------------
+#
+# The following tokens, when wrapped with "|>" and "<|" are expanded by the
+# code generation build procedure.
+#
+#  MODEL_NAME          - Name of the Simulink block diagram
+#  MODEL_MODULES       - Any additional generated source modules
+#  MAKEFILE_NAME       - Name of makefile created from template makefile <model>.mk
+#  MATLAB_ROOT         - Path to where MATLAB is installed.
+#  MATLAB_BIN          - Path to MATLAB executable.
+#  S_FUNCTIONS         - List of S-functions.
+#  S_FUNCTIONS_LIB     - List of S-functions libraries to link.
+#  SOLVER              - Solver source file name
+#  NUMST               - Number of sample times
+#  TID01EQ             - yes (1) or no (0): Are sampling rates of continuous task
+#                        (tid=0) and 1st discrete task equal.
+#  NCSTATES            - Number of continuous states
+#  BUILDARGS           - Options passed in at the command line.
+#  MULTITASKING        - yes (1) or no (0): Is solver mode multitasking
+#  EXT_MODE            - yes (1) or no (0): Build for external mode
+#  TMW_EXTMODE_TESTING - yes (1) or no (0): Build ext_test.c for external mode
+#                        testing.
+#  EXTMODE_TRANSPORT   - Index of transport mechanism (e.g. tcpip, serial) for extmode
+#  EXTMODE_STATIC      - yes (1) or no (0): Use static instead of dynamic mem alloc.
+#  EXTMODE_STATIC_SIZE - Size of static memory allocation buffer.
+
+MODEL                = attitudeKalmanfilter
+MODULES              = attitudeKalmanfilter.c eye.c mrdivide.c norm.c attitudeKalmanfilter_initialize.c attitudeKalmanfilter_terminate.c rt_nonfinite.c rtGetNaN.c rtGetInf.c 
+MAKEFILE             = attitudeKalmanfilter_rtw.mk
+MATLAB_ROOT          = C:\Program Files\MATLAB\R2011a
+ALT_MATLAB_ROOT      = C:\PROGRA~1\MATLAB\R2011a
+MATLAB_BIN           = C:\Program Files\MATLAB\R2011a\bin
+ALT_MATLAB_BIN       = C:\PROGRA~1\MATLAB\R2011a\bin
+S_FUNCTIONS          = 
+S_FUNCTIONS_LIB      = 
+SOLVER               = 
+NUMST                = 1
+TID01EQ              = 0
+NCSTATES             = 0
+BUILDARGS            =  GENERATE_REPORT=1 ADD_MDL_NAME_TO_GLOBALS=0
+MULTITASKING         = 0
+EXT_MODE             = 0
+TMW_EXTMODE_TESTING  = 0
+EXTMODE_TRANSPORT    = 0
+EXTMODE_STATIC       = 0
+EXTMODE_STATIC_SIZE  = 1000000
+
+MODELREFS            = 
+SHARED_SRC           = 
+SHARED_SRC_DIR       = 
+SHARED_BIN_DIR       = 
+SHARED_LIB           = 
+TARGET_LANG_EXT      = c
+OPTIMIZATION_FLAGS   = /O2 /Oy-
+ADDITIONAL_LDFLAGS   = 
+
+#--------------------------- Model and reference models -----------------------
+MODELLIB                  = attitudeKalmanfilter.lib
+MODELREF_LINK_LIBS        = 
+MODELREF_LINK_RSPFILE     = attitudeKalmanfilter_ref.rsp
+MODELREF_INC_PATH         = 
+RELATIVE_PATH_TO_ANCHOR   = F:\CODEGE~1
+MODELREF_TARGET_TYPE      = RTW
+
+!if "$(MATLAB_ROOT)" != "$(ALT_MATLAB_ROOT)"
+MATLAB_ROOT = $(ALT_MATLAB_ROOT)
+!endif
+!if "$(MATLAB_BIN)" != "$(ALT_MATLAB_BIN)"
+MATLAB_BIN = $(ALT_MATLAB_BIN)
+!endif
+
+#--------------------------- Tool Specifications ------------------------------
+
+CPU = AMD64
+!include $(MATLAB_ROOT)\rtw\c\tools\vctools.mak
+APPVER = 5.02
+
+PERL = $(MATLAB_ROOT)\sys\perl\win32\bin\perl
+#------------------------------ Include/Lib Path ------------------------------
+
+MATLAB_INCLUDES =                    $(MATLAB_ROOT)\simulink\include
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\extern\include
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);$(MATLAB_ROOT)\rtw\c\src\ext_mode\common
+
+# Additional file include paths
+
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter\codegen\lib\attitudeKalmanfilter
+MATLAB_INCLUDES = $(MATLAB_INCLUDES);F:\codegenerationMatlabAttFilter
+
+INCLUDE = .;$(RELATIVE_PATH_TO_ANCHOR);$(MATLAB_INCLUDES);$(INCLUDE);$(MODELREF_INC_PATH)
+
+!if "$(SHARED_SRC_DIR)" != ""
+INCLUDE = $(INCLUDE);$(SHARED_SRC_DIR)
+!endif
+
+#------------------------ External mode ---------------------------------------
+# Uncomment -DVERBOSE to have information printed to stdout
+# To add a new transport layer, see the comments in
+#   <matlabroot>/toolbox/simulink/simulink/extmode_transports.m
+!if $(EXT_MODE) == 1
+EXT_CC_OPTS = -DEXT_MODE # -DVERBOSE
+!if $(EXTMODE_TRANSPORT) == 0 #tcpip
+EXT_SRC = updown.c rtiostream_interface.c rtiostream_tcpip.c
+EXT_LIB = wsock32.lib
+!endif
+!if $(EXTMODE_TRANSPORT) == 1 #serial_win32
+EXT_SRC = ext_svr.c updown.c ext_work.c ext_svr_serial_transport.c
+EXT_SRC = $(EXT_SRC) ext_serial_pkt.c ext_serial_win32_port.c
+EXT_LIB =
+!endif
+!if $(TMW_EXTMODE_TESTING) == 1
+EXT_SRC     = $(EXT_SRC) ext_test.c
+EXT_CC_OPTS = $(EXT_CC_OPTS) -DTMW_EXTMODE_TESTING
+!endif
+!if $(EXTMODE_STATIC) == 1
+EXT_SRC     = $(EXT_SRC) mem_mgr.c
+EXT_CC_OPTS = $(EXT_CC_OPTS) -DEXTMODE_STATIC -DEXTMODE_STATIC_SIZE=$(EXTMODE_STATIC_SIZE)
+!endif
+!else
+EXT_SRC     =
+EXT_CC_OPTS =
+EXT_LIB     =
+!endif
+
+#------------------------ rtModel ----------------------------------------------
+
+RTM_CC_OPTS = -DUSE_RTMODEL
+
+#----------------- Compiler and Linker Options --------------------------------
+
+# Optimization Options
+#   Set OPT_OPTS=-Zi for debugging (may depend on compiler version)
+OPT_OPTS = $(DEFAULT_OPT_OPTS)
+
+# General User Options
+OPTS =
+
+!if "$(OPTIMIZATION_FLAGS)" != ""
+CC_OPTS = $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS) $(OPTIMIZATION_FLAGS)
+!else
+CC_OPTS = $(OPT_OPTS) $(OPTS) $(EXT_CC_OPTS) $(RTM_CC_OPTS)
+!endif
+
+CPP_REQ_DEFINES = -DMODEL=$(MODEL) -DRT -DNUMST=$(NUMST) \
+		  -DTID01EQ=$(TID01EQ) -DNCSTATES=$(NCSTATES) \
+		  -DMT=$(MULTITASKING) -DHAVESTDIO 
+
+# Uncomment this line to move warning level to W4
+# cflags = $(cflags:W3=W4)
+CFLAGS   = $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
+	   $(cflags) $(cvarsmt) /wd4996
+CPPFLAGS = $(CPP_OPTS) $(CC_OPTS) $(CPP_REQ_DEFINES) $(USER_INCLUDES) \
+	   $(cflags) $(cvarsmt) /wd4996 /EHsc-
+LDFLAGS  = $(ldebug) $(conflags) $(EXT_LIB) $(conlibs) libcpmt.lib $(ADDITIONAL_LDFLAGS)
+
+# libcpmt.lib is the multi-threaded, static lib version of the C++ standard lib
+
+#----------------------------- Source Files -----------------------------------
+
+
+#Standalone executable
+!if "$(MODELREF_TARGET_TYPE)" == "NONE"
+PRODUCT   = $(RELATIVE_PATH_TO_ANCHOR)\$(MODEL).exe
+REQ_SRCS  = $(MODULES) $(EXT_SRC)
+
+#Model Reference Target
+!else
+PRODUCT   = $(MODELLIB)
+REQ_SRCS  = $(MODULES)  $(EXT_SRC)
+!endif
+
+USER_SRCS =
+
+SRCS = $(REQ_SRCS) $(USER_SRCS) $(S_FUNCTIONS)
+OBJS_CPP_UPPER = $(SRCS:.CPP=.obj)
+OBJS_CPP_LOWER = $(OBJS_CPP_UPPER:.cpp=.obj)
+OBJS_C_UPPER = $(OBJS_CPP_LOWER:.C=.obj)
+OBJS = $(OBJS_C_UPPER:.c=.obj)
+SHARED_OBJS = $(SHARED_SRC:.c=.obj)
+# ------------------------- Additional Libraries ------------------------------
+
+LIBS =
+
+
+LIBS = $(LIBS)
+
+# ---------------------------- Linker Script ----------------------------------
+
+CMD_FILE = $(MODEL).lnk
+GEN_LNK_SCRIPT = $(MATLAB_ROOT)\rtw\c\tools\mkvc_lnk.pl
+
+#--------------------------------- Rules --------------------------------------
+all: set_environment_variables $(PRODUCT)
+
+!if "$(MODELREF_TARGET_TYPE)" == "NONE"
+#--- Stand-alone model ---
+$(PRODUCT) : $(OBJS) $(SHARED_LIB) $(LIBS) $(MODELREF_LINK_LIBS)
+	@cmd /C "echo ### Linking ..."
+	$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
+	$(LD) $(LDFLAGS) $(S_FUNCTIONS_LIB) $(SHARED_LIB) $(LIBS) $(MAT_LIBS) @$(CMD_FILE) @$(MODELREF_LINK_RSPFILE) -out:$@
+	@del $(CMD_FILE)
+	@cmd /C "echo $(BUILD_SUCCESS) executable $(MODEL).exe"
+!else
+#--- Model reference code generation Target ---
+$(PRODUCT) : $(OBJS) $(SHARED_LIB)
+	@cmd /C "echo ### Linking ..."
+	$(PERL) $(GEN_LNK_SCRIPT) $(CMD_FILE) $(OBJS)
+	$(LD) -lib /OUT:$(PRODUCT) @$(CMD_FILE) $(S_FUNCTIONS_LIB)
+	@cmd /C "echo $(BUILD_SUCCESS) static library $(MODELLIB)"
+!endif
+
+{$(MATLAB_ROOT)\rtw\c\src}.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\ext_mode\common}.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\rtiostream\rtiostreamtcpip}.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\ext_mode\serial}.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+{$(MATLAB_ROOT)\rtw\c\src\ext_mode\custom}.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+# Additional sources
+
+
+
+
+
+# Put these rule last, otherwise nmake will check toolboxes first
+
+{$(RELATIVE_PATH_TO_ANCHOR)}.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+{$(RELATIVE_PATH_TO_ANCHOR)}.cpp.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CPPFLAGS) $<
+
+.c.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CFLAGS) $<
+
+.cpp.obj :
+	@cmd /C "echo ### Compiling $<"
+	$(CC) $(CPPFLAGS) $<
+
+!if "$(SHARED_LIB)" != ""
+$(SHARED_LIB) : $(SHARED_SRC)
+	@cmd /C "echo ### Creating $@"
+	@$(CC) $(CFLAGS) -Fo$(SHARED_BIN_DIR)\ @<<
+$?
+<<
+	@$(LIBCMD) /nologo /out:$@ $(SHARED_OBJS)
+	@cmd /C "echo ### $@ Created"
+!endif
+
+
+set_environment_variables:
+	@set INCLUDE=$(INCLUDE)
+	@set LIB=$(LIB)
+
+# Libraries:
+
+
+
+
+
+#----------------------------- Dependencies -----------------------------------
+
+$(OBJS) : $(MAKEFILE) rtw_proj.tmw

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c

@@ -0,0 +1,31 @@
+/*
+ * attitudeKalmanfilter_terminate.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (attitudeKalmanfilter_terminate.c) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h

@@ -0,0 +1,32 @@
+/*
+ * attitudeKalmanfilter_terminate.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
+#define __ATTITUDEKALMANFILTER_TERMINATE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void attitudeKalmanfilter_terminate(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_terminate.h) */

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h

@@ -0,0 +1,16 @@
+/*
+ * attitudeKalmanfilter_types.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
+#define __ATTITUDEKALMANFILTER_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (attitudeKalmanfilter_types.h) */

apps/attitude_estimator_ekf/codegen/eye.c

@@ -0,0 +1,51 @@
+/*
+ * eye.c
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "eye.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_eye(real_T I[144])
+{
+  int32_T i;
+  memset((void *)&I[0], 0, 144U * sizeof(real_T));
+  for (i = 0; i < 12; i++) {
+    I[i + 12 * i] = 1.0;
+  }
+}
+
+/*
+ *
+ */
+void eye(real_T I[9])
+{
+  int32_T i;
+  memset((void *)&I[0], 0, 9U * sizeof(real_T));
+  for (i = 0; i < 3; i++) {
+    I[i + 3 * i] = 1.0;
+  }
+}
+
+/* End of code generation (eye.c) */

apps/attitude_estimator_ekf/codegen/eye.h

@@ -0,0 +1,33 @@
+/*
+ * eye.h
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __EYE_H__
+#define __EYE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void b_eye(real_T I[144]);
+extern void eye(real_T I[9]);
+#endif
+/* End of code generation (eye.h) */

apps/attitude_estimator_ekf/codegen/mrdivide.c

@@ -0,0 +1,158 @@
+/*
+ * mrdivide.c
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+{
+  int32_T jy;
+  int32_T iy;
+  real32_T b_A[81];
+  int8_T ipiv[9];
+  int32_T j;
+  int32_T mmj;
+  int32_T jj;
+  int32_T jp1j;
+  int32_T c;
+  int32_T ix;
+  real32_T temp;
+  int32_T k;
+  real32_T s;
+  int32_T loop_ub;
+  real32_T Y[108];
+  for (jy = 0; jy < 9; jy++) {
+    for (iy = 0; iy < 9; iy++) {
+      b_A[iy + 9 * jy] = B[jy + 9 * iy];
+    }
+
+    ipiv[jy] = (int8_T)(1 + jy);
+  }
+
+  for (j = 0; j < 8; j++) {
+    mmj = -j;
+    jj = j * 10;
+    jp1j = jj + 1;
+    c = mmj + 9;
+    jy = 0;
+    ix = jj;
+    temp = fabsf(b_A[jj]);
+    for (k = 2; k <= c; k++) {
+      ix++;
+      s = fabsf(b_A[ix]);
+      if (s > temp) {
+        jy = k - 1;
+        temp = s;
+      }
+    }
+
+    if ((real_T)b_A[jj + jy] != 0.0) {
+      if (jy != 0) {
+        ipiv[j] = (int8_T)((j + jy) + 1);
+        ix = j;
+        iy = j + jy;
+        for (k = 0; k < 9; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 9;
+          iy += 9;
+        }
+      }
+
+      loop_ub = (jp1j + mmj) + 8;
+      for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
+        b_A[iy] /= b_A[jj];
+      }
+    }
+
+    c = 8 - j;
+    jy = jj + 9;
+    for (iy = 1; iy <= c; iy++) {
+      if ((real_T)b_A[jy] != 0.0) {
+        temp = b_A[jy] * -1.0F;
+        ix = jp1j;
+        loop_ub = (mmj + jj) + 18;
+        for (k = 10 + jj; k + 1 <= loop_ub; k++) {
+          b_A[k] += b_A[ix] * temp;
+          ix++;
+        }
+      }
+
+      jy += 9;
+      jj += 9;
+    }
+  }
+
+  for (jy = 0; jy < 12; jy++) {
+    for (iy = 0; iy < 9; iy++) {
+      Y[iy + 9 * jy] = A[jy + 12 * iy];
+    }
+  }
+
+  for (iy = 0; iy < 9; iy++) {
+    if (ipiv[iy] != iy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[iy + 9 * j];
+        Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
+        Y[(ipiv[iy] + 9 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 0; k < 9; k++) {
+      jy = 9 * k;
+      if ((real_T)Y[k + c] != 0.0) {
+        for (iy = k + 2; iy < 10; iy++) {
+          Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 8; k > -1; k += -1) {
+      jy = 9 * k;
+      if ((real_T)Y[k + c] != 0.0) {
+        Y[k + c] /= b_A[k + jy];
+        for (iy = 0; iy + 1 <= k; iy++) {
+          Y[iy + c] -= Y[k + c] * b_A[iy + jy];
+        }
+      }
+    }
+  }
+
+  for (jy = 0; jy < 9; jy++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * jy] = Y[jy + 9 * iy];
+    }
+  }
+}
+
+/* End of code generation (mrdivide.c) */

apps/attitude_estimator_ekf/codegen/mrdivide.h

@@ -0,0 +1,32 @@
+/*
+ * mrdivide.h
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __MRDIVIDE_H__
+#define __MRDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
+#endif
+/* End of code generation (mrdivide.h) */

apps/attitude_estimator_ekf/codegen/norm.c

@@ -0,0 +1,62 @@
+/*
+ * norm.c
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+real32_T norm(const real32_T x[3])
+{
+  real32_T y;
+  real32_T scale;
+  boolean_T firstNonZero;
+  int32_T k;
+  real32_T absxk;
+  real32_T t;
+  y = 0.0F;
+  scale = 0.0F;
+  firstNonZero = TRUE;
+  for (k = 0; k < 3; k++) {
+    if (x[k] != 0.0F) {
+      absxk = fabsf(x[k]);
+      if (firstNonZero) {
+        scale = absxk;
+        y = 1.0F;
+        firstNonZero = FALSE;
+      } else if (scale < absxk) {
+        t = scale / absxk;
+        y = 1.0F + y * t * t;
+        scale = absxk;
+      } else {
+        t = absxk / scale;
+        y += t * t;
+      }
+    }
+  }
+
+  return scale * sqrtf(y);
+}
+
+/* End of code generation (norm.c) */

apps/attitude_estimator_ekf/codegen/norm.h

@@ -0,0 +1,32 @@
+/*
+ * norm.h
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __NORM_H__
+#define __NORM_H__
+/* Include files */
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern real32_T norm(const real32_T x[3]);
+#endif
+/* End of code generation (norm.h) */

apps/attitude_estimator_ekf/codegen/rtGetInf.c

@@ -0,0 +1,139 @@
+/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf
+ */
+#include "rtGetInf.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetInf ==================================================
+ * Abstract:
+ * Initialize rtInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetInf(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T inf = 0.0;
+  if (bitsPerReal == 32U) {
+    inf = rtGetInfF();
+  } else {
+    uint16_T one = 1U;
+    enum {
+      LittleEndian,
+      BigEndian
+    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+    switch (machByteOrder) {
+     case LittleEndian:
+      {
+        union {
+          LittleEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        inf = tmpVal.fltVal;
+        break;
+      }
+
+     case BigEndian:
+      {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        inf = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return inf;
+}
+
+/* Function: rtGetInfF ==================================================
+ * Abstract:
+ * Initialize rtInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetInfF(void)
+{
+  IEEESingle infF;
+  infF.wordL.wordLuint = 0x7F800000U;
+  return infF.wordL.wordLreal;
+}
+
+/* Function: rtGetMinusInf ==================================================
+ * Abstract:
+ * Initialize rtMinusInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetMinusInf(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T minf = 0.0;
+  if (bitsPerReal == 32U) {
+    minf = rtGetMinusInfF();
+  } else {
+    uint16_T one = 1U;
+    enum {
+      LittleEndian,
+      BigEndian
+    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+    switch (machByteOrder) {
+     case LittleEndian:
+      {
+        union {
+          LittleEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0xFFF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        minf = tmpVal.fltVal;
+        break;
+      }
+
+     case BigEndian:
+      {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0xFFF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        minf = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return minf;
+}
+
+/* Function: rtGetMinusInfF ==================================================
+ * Abstract:
+ * Initialize rtMinusInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetMinusInfF(void)
+{
+  IEEESingle minfF;
+  minfF.wordL.wordLuint = 0xFF800000U;
+  return minfF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetInf.c) */

apps/attitude_estimator_ekf/codegen/rtGetInf.h

@@ -0,0 +1,23 @@
+/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RTGETINF_H__
+#define __RTGETINF_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetInf(void);
+extern real32_T rtGetInfF(void);
+extern real_T rtGetMinusInf(void);
+extern real32_T rtGetMinusInfF(void);
+
+#endif
+/* End of code generation (rtGetInf.h) */

apps/attitude_estimator_ekf/codegen/rtGetNaN.c

@@ -0,0 +1,96 @@
+/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T nan = 0.0;
+  if (bitsPerReal == 32U) {
+    nan = rtGetNaNF();
+  } else {
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      union {
+        LittleEndianIEEEDouble bitVal;
+        real_T fltVal;
+      } tmpVal;
+
+      tmpVal.bitVal.words.wordH = 0xFFF80000U;
+      tmpVal.bitVal.words.wordL = 0x00000000U;
+      nan = tmpVal.fltVal;
+      break;
+    }
+
+   case BigEndian:
+    {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+        nan = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+  IEEESingle nanF = { { 0 } };
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      nanF.wordL.wordLuint = 0xFFC00000U;
+      break;
+    }
+
+   case BigEndian:
+    {
+      nanF.wordL.wordLuint = 0x7FFFFFFFU;
+      break;
+    }
+  }
+
+  return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */

apps/attitude_estimator_ekf/codegen/rtGetNaN.h

@@ -0,0 +1,21 @@
+/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RTGETNAN_H__
+#define __RTGETNAN_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetNaN(void);
+extern real32_T rtGetNaNF(void);
+
+#endif
+/* End of code generation (rtGetNaN.h) */

apps/attitude_estimator_ekf/codegen/rt_nonfinite.c

@@ -0,0 +1,87 @@
+/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+/*
+ * Abstract:
+ *      MATLAB for code generation function to initialize non-finites,
+ *      (Inf, NaN and -Inf).
+ */
+#include "rt_nonfinite.h"
+#include "rtGetNaN.h"
+#include "rtGetInf.h"
+
+real_T rtInf;
+real_T rtMinusInf;
+real_T rtNaN;
+real32_T rtInfF;
+real32_T rtMinusInfF;
+real32_T rtNaNF;
+
+/* Function: rt_InitInfAndNaN ==================================================
+ * Abstract:
+ * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
+ * generated code. NaN is initialized as non-signaling. Assumes IEEE.
+ */
+void rt_InitInfAndNaN(size_t realSize)
+{
+  (void) (realSize);
+  rtNaN = rtGetNaN();
+  rtNaNF = rtGetNaNF();
+  rtInf = rtGetInf();
+  rtInfF = rtGetInfF();
+  rtMinusInf = rtGetMinusInf();
+  rtMinusInfF = rtGetMinusInfF();
+}
+
+/* Function: rtIsInf ==================================================
+ * Abstract:
+ * Test if value is infinite
+ */
+boolean_T rtIsInf(real_T value)
+{
+  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+}
+
+/* Function: rtIsInfF =================================================
+ * Abstract:
+ * Test if single-precision value is infinite
+ */
+boolean_T rtIsInfF(real32_T value)
+{
+  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+}
+
+/* Function: rtIsNaN ==================================================
+ * Abstract:
+ * Test if value is not a number
+ */
+boolean_T rtIsNaN(real_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+  return _isnan(value)? TRUE:FALSE;
+#else
+  return (value!=value)? 1U:0U;
+#endif
+}
+
+/* Function: rtIsNaNF =================================================
+ * Abstract:
+ * Test if single-precision value is not a number
+ */
+boolean_T rtIsNaNF(real32_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+  return _isnan((real_T)value)? true:false;
+#else
+  return (value!=value)? 1U:0U;
+#endif
+}
+
+
+/* End of code generation (rt_nonfinite.c) */

apps/attitude_estimator_ekf/codegen/rt_nonfinite.h

@@ -0,0 +1,53 @@
+/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RT_NONFINITE_H__
+#define __RT_NONFINITE_H__
+
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+#include <float.h>
+#endif
+#include <stddef.h>
+#include "rtwtypes.h"
+
+extern real_T rtInf;
+extern real_T rtMinusInf;
+extern real_T rtNaN;
+extern real32_T rtInfF;
+extern real32_T rtMinusInfF;
+extern real32_T rtNaNF;
+extern void rt_InitInfAndNaN(size_t realSize);
+extern boolean_T rtIsInf(real_T value);
+extern boolean_T rtIsInfF(real32_T value);
+extern boolean_T rtIsNaN(real_T value);
+extern boolean_T rtIsNaNF(real32_T value);
+
+typedef struct {
+  struct {
+    uint32_T wordH;
+    uint32_T wordL;
+  } words;
+} BigEndianIEEEDouble;
+
+typedef struct {
+  struct {
+    uint32_T wordL;
+    uint32_T wordH;
+  } words;
+} LittleEndianIEEEDouble;
+
+typedef struct {
+  union {
+    real32_T wordLreal;
+    uint32_T wordLuint;
+  } wordL;
+} IEEESingle;
+
+#endif
+/* End of code generation (rt_nonfinite.h) */

apps/attitude_estimator_ekf/codegen/rtw_proj.tmw

@@ -0,0 +1 @@
+Code generation project for attitudeKalmanfilter using C:\Program Files\MATLAB\R2011a\toolbox\coder\coder\rtw\c\xrt\xrt_vcx64.tmf. MATLAB root = C:\Program Files\MATLAB\R2011a.

apps/attitude_estimator_ekf/codegen/rtwtypes.h

@@ -0,0 +1,159 @@
+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Wed Jul 11 08:38:35 2012
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================* 
+ * Target hardware information
+ *   Device type: Generic->MATLAB Host Computer
+ *   Number of bits:     char:   8    short:   16    int:  32
+ *                       long:  32      native word size:  32
+ *   Byte ordering: LittleEndian
+ *   Signed integer division rounds to: Zero
+ *   Shift right on a signed integer as arithmetic shift: on
+ *=======================================================================*/
+
+/*=======================================================================* 
+ * Fixed width word size data types:                                     * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================* 
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
+ *                           ulong_T, char_T and byte_T.                     * 
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================* 
+ * Complex number type definitions                                           * 
+ *===========================================================================*/
+#define CREAL_T	
+   typedef struct {  
+     real32_T re;  
+     real32_T im;  
+   } creal32_T;  
+
+   typedef struct {  
+     real64_T re;  
+     real64_T im;  
+   } creal64_T;  
+
+   typedef struct {  
+     real_T re;  
+     real_T im;  
+   } creal_T;  
+
+   typedef struct {  
+     int8_T re;  
+     int8_T im;  
+   } cint8_T;  
+
+   typedef struct {  
+     uint8_T re;  
+     uint8_T im;  
+   } cuint8_T;  
+
+   typedef struct {  
+     int16_T re;  
+     int16_T im;  
+   } cint16_T;  
+
+   typedef struct {  
+     uint16_T re;  
+     uint16_T im;  
+   } cuint16_T;  
+
+   typedef struct {  
+     int32_T re;  
+     int32_T im;  
+   } cint32_T;  
+
+   typedef struct {  
+     uint32_T re;  
+     uint32_T im;  
+   } cuint32_T;  
+
+
+/*=======================================================================* 
+ * Min and Max:                                                          * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *=======================================================================*/
+
+#define MAX_int8_T  	((int8_T)(127))
+#define MIN_int8_T  	((int8_T)(-128))
+#define MAX_uint8_T 	((uint8_T)(255))
+#define MIN_uint8_T 	((uint8_T)(0))
+#define MAX_int16_T 	((int16_T)(32767))
+#define MIN_int16_T 	((int16_T)(-32768))
+#define MAX_uint16_T	((uint16_T)(65535))
+#define MIN_uint16_T	((uint16_T)(0))
+#define MAX_int32_T 	((int32_T)(2147483647))
+#define MIN_int32_T 	((int32_T)(-2147483647-1))
+#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T	((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+#  ifndef false
+#   define false (0U)
+#  endif
+#  ifndef true
+#   define true (1U)
+#  endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX	64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */

apps/commander/Makefile

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Commander application
+#
+
+APPNAME		 = commander
+PRIORITY	 = SCHED_PRIORITY_MAX - 10
+STACKSIZE	 = 3072
+
+INCLUDES	= $(TOPDIR)/../mavlink/include/mavlink
+
+include $(APPDIR)/mk/app.mk
+

apps/commander/commander.c

@@ -0,0 +1,862 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author 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 commander.c
+ * Main system state machine implementation.
+ */
+
+#include "commander.h"
+
+#include <nuttx/config.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <debug.h>
+#include <sys/prctl.h>
+#include <v1.0/common/mavlink.h>
+#include <string.h>
+#include <arch/board/drv_led.h>
+#include <arch/board/up_hrt.h>
+#include <arch/board/drv_tone_alarm.h>
+#include <arch/board/up_hrt.h>
+#include "state_machine_helper.h"
+#include "systemlib/systemlib.h"
+#include <math.h>
+#include <poll.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/rc_channels.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/vehicle_command.h>
+#include <mavlink/mavlink_log.h>
+
+#include <systemlib/systemlib.h>
+
+#include <arch/board/up_cpuload.h>
+extern struct system_load_s system_load;
+
+/* Decouple update interval and hysteris counters, all depends on intervals */
+#define COMMANDER_MONITORING_INTERVAL 50000
+#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
+#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define STICK_ON_OFF_LIMIT 7500
+#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
+#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
+
+#define GPS_FIX_TYPE_2D 2
+#define GPS_FIX_TYPE_3D 3
+#define GPS_QUALITY_GOOD_COUNTER_LIMIT 50
+
+/* File descriptors */
+static int leds;
+static int buzzer;
+static int mavlink_fd;
+static bool commander_initialized = false;
+static struct vehicle_status_s current_status = {
+	.state_machine = SYSTEM_STATE_PREFLIGHT,
+	.mode = 0
+}; /**< Main state machine */
+static int stat_pub;
+
+static uint16_t nofix_counter = 0;
+static uint16_t gotfix_counter = 0;
+
+static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd);
+
+/* pthread loops */
+static void *command_handling_loop(void *arg);
+// static void *subsystem_info_loop(void *arg);
+
+__EXPORT int commander_main(int argc, char *argv[]);
+
+#ifdef CONFIG_TONE_ALARM
+static int buzzer_init(void);
+static void buzzer_deinit(void);
+
+static int buzzer_init()
+{
+	buzzer = open("/dev/tone_alarm", O_WRONLY);
+
+	if (buzzer < 0) {
+		fprintf(stderr, "[commander] Buzzer: open fail\n");
+		return ERROR;
+	}
+
+	return 0;
+}
+
+static void buzzer_deinit()
+{
+	close(buzzer);
+}
+#endif
+
+static int led_init(void);
+static void led_deinit(void);
+static int led_toggle(int led);
+static int led_on(int led);
+static int led_off(int led);
+
+static int led_init()
+{
+	leds = open("/dev/led", O_RDONLY | O_NONBLOCK);
+
+	if (leds < 0) {
+		fprintf(stderr, "[commander] LED: open fail\n");
+		return ERROR;
+	}
+
+	if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) {
+		fprintf(stderr, "[commander] LED: ioctl fail\n");
+		return ERROR;
+	}
+
+	return 0;
+}
+
+static void led_deinit()
+{
+	close(leds);
+}
+
+static int led_toggle(int led)
+{
+	static int last_blue = LED_ON;
+	static int last_amber = LED_ON;
+
+	if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON;
+
+	if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON;
+
+	return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led);
+}
+
+static int led_on(int led)
+{
+	return ioctl(leds, LED_ON, led);
+}
+
+static int led_off(int led)
+{
+	return ioctl(leds, LED_OFF, led);
+}
+
+enum AUDIO_PATTERN {
+	AUDIO_PATTERN_ERROR = 1,
+	AUDIO_PATTERN_NOTIFY_POSITIVE = 2,
+	AUDIO_PATTERN_NOTIFY_NEUTRAL = 3,
+	AUDIO_PATTERN_NOTIFY_NEGATIVE = 4,
+	AUDIO_PATTERN_TETRIS = 5
+};
+
+int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) {
+
+	/* Trigger alarm if going into any error state */
+	if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) ||
+		((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) {
+		ioctl(buzzer, TONE_SET_ALARM, 0);
+		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR);
+	}
+
+	/* Trigger neutral on arming / disarming */
+	if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) {
+		ioctl(buzzer, TONE_SET_ALARM, 0);
+		ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL);
+	}
+
+	/* Trigger Tetris on being bored */
+
+	return 0;
+}
+
+void do_gyro_calibration(void)
+{
+
+	const int calibration_count = 3000;
+
+	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s raw;
+
+	int calibration_counter = 0;
+	float gyro_offset[3] = {0, 0, 0};
+
+	while (calibration_counter < calibration_count) {
+
+		/* wait blocking for new data */
+		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
+
+		if (poll(fds, 1, 1000)) {
+			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
+			gyro_offset[0] += raw.gyro_raw[0];
+			gyro_offset[1] += raw.gyro_raw[1];
+			gyro_offset[2] += raw.gyro_raw[2];
+			calibration_counter++;
+		}
+	}
+
+	gyro_offset[0] = gyro_offset[0] / calibration_count;
+	gyro_offset[1] = gyro_offset[1] / calibration_count;
+	gyro_offset[2] = gyro_offset[2] / calibration_count;
+
+	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_XOFFSET] = gyro_offset[0];
+	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_YOFFSET] = gyro_offset[1];
+	global_data_parameter_storage->pm.param_values[PARAM_SENSOR_GYRO_ZOFFSET] = gyro_offset[2];
+
+	char offset_output[50];
+	sprintf(offset_output, "[commander] gyro calibration finished, offsets: x:%d, y:%d, z:%d", (int)gyro_offset[0], (int)gyro_offset[1], (int)gyro_offset[2]);
+	mavlink_log_info(mavlink_fd, offset_output);
+
+	close(sub_sensor_combined);
+
+	// XXX Add a parameter changed broadcast notification
+}
+
+
+
+void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
+{
+	/* result of the command */
+	uint8_t result = MAV_RESULT_UNSUPPORTED;
+
+
+	/* supported command handling start */
+
+	/* request to set different system mode */
+	switch (cmd->command) {
+
+
+		case MAV_CMD_DO_SET_MODE:
+		{
+			update_state_machine_mode_request(status_pub, current_vehicle_status, (uint8_t)cmd->param1);
+		}
+		break;
+//
+//		case MAV_CMD_COMPONENT_ARM_DISARM:
+//		{
+//			/* request to arm */
+//			if (cmd->param1 == 1.0f) {
+//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED))
+//					result = MAV_RESULT_ACCEPTED;
+//			/* request to disarm */
+//			} else if (cmd->param1 == 0.0f) {
+//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_STANDBY))
+//					result = MAV_RESULT_ACCEPTED;
+//			}
+//		}
+//		break;
+//
+//		/* request for an autopilot reboot */
+//		case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
+//		{
+//			if (cmd->param1 == 1.0f) {
+//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_HALT)) {
+//					result = MAV_RESULT_ACCEPTED;//TODO: this has no effect
+//				}
+//			}
+//
+//		}
+//		break;
+//
+//		/* request to land */
+//		case MAV_CMD_NAV_LAND:
+//		 {
+//				//TODO: add check if landing possible
+//				//TODO: add landing maneuver
+//
+//				if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) {
+//					result = MAV_RESULT_ACCEPTED;
+//		}		}
+//		break;
+//
+//		/* request to takeoff */
+//		case MAV_CMD_NAV_TAKEOFF:
+//		{
+//			//TODO: add check if takeoff possible
+//			//TODO: add takeoff maneuver
+//
+//			if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) {
+//				result = MAV_RESULT_ACCEPTED;
+//			}
+//		}
+//		break;
+//
+		/* preflight calibration */
+	case MAV_CMD_PREFLIGHT_CALIBRATION: {
+			if (cmd->param1 == 1.0)	{
+				mavlink_log_info(mavlink_fd, "[commander] starting gyro calibration");
+				do_gyro_calibration();
+				result = MAV_RESULT_ACCEPTED;
+
+			} else {
+				fprintf(stderr, "[commander] refusing unsupported calibration request\n");
+				mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported calibration request");
+				result = MAV_RESULT_UNSUPPORTED;
+			}
+		}
+		break;
+
+		/* preflight parameter load / store */
+	case MAV_CMD_PREFLIGHT_STORAGE: {
+			/* Read all parameters from EEPROM to RAM */
+
+			if (cmd->param1 == 0.0)	{
+
+				if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
+					printf("[commander] Loaded EEPROM params in RAM\n");
+					mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM");
+					result = MAV_RESULT_ACCEPTED;
+
+				} else {
+					fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
+					mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
+					result = MAV_RESULT_FAILED;
+				}
+
+				/* Write all parameters from RAM to EEPROM */
+
+			} else if (cmd->param1 == 1.0)	{
+
+				if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
+					printf("[commander] RAM params written to EEPROM\n");
+					mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM");
+					result = MAV_RESULT_ACCEPTED;
+
+				} else {
+					fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
+					mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM");
+					result = MAV_RESULT_FAILED;
+				}
+
+			} else {
+				fprintf(stderr, "[commander] refusing unsupported storage request\n");
+				mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request");
+				result = MAV_RESULT_UNSUPPORTED;
+			}
+		}
+		break;
+
+	default: {
+			mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
+			result = MAV_RESULT_UNSUPPORTED;
+		}
+		break;
+	}
+
+	/* supported command handling stop */
+
+
+	/* send any requested ACKs */
+	if (cmd->confirmation > 0) {
+		/* send acknowledge command */
+		mavlink_message_t msg;
+		mavlink_msg_command_ack_pack(0, 0, &msg, cmd->command, result);
+		//global_data_send_mavlink_message_out(&msg);
+
+	}
+
+}
+
+
+static void *command_handling_loop(void *arg)  //handles commands which come from the mavlink app
+{
+	/* Set thread name */
+	prctl(PR_SET_NAME, "commander cmd handler", getpid());
+
+	/* Subscribe to command topic */
+	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
+	struct vehicle_command_s cmd;
+
+	while (1) {
+		struct pollfd fds[1] = { { .fd = cmd_sub, .events = POLLIN } };
+
+		if (poll(fds, 1, 5000) == 0) {
+			/* timeout, but this is no problem */
+		} else {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(stat_pub, &current_status, &cmd);
+		}
+	}
+
+	return NULL;
+}
+
+// static void *subsystem_info_loop(void *arg)  //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal
+// {
+// 	/* Set thread name */
+// 	prctl(PR_SET_NAME, "commander subsys", getpid());
+
+// 	uint8_t current_info_local = SUBSYSTEM_INFO_BUFFER_SIZE;
+// 	uint16_t total_counter = 0;
+
+// 	while (1) {
+
+// 		if (0 == global_data_wait(&global_data_subsystem_info->access_conf)) {
+// //				printf("got subsystem_info\n");
+
+// 			while (current_info_local != global_data_subsystem_info->current_info) {
+// //					printf("current_info_local = %d, current_info = %d \n", current_info_local, global_data_subsystem_info->current_info);
+
+// 				current_info_local++;
+
+// 				if (current_info_local >= SUBSYSTEM_INFO_BUFFER_SIZE)
+// 					current_info_local = 0;
+
+// 				/* Handle the new subsystem info and write updated version of global_data_sys_status */
+// 				subsystem_info_t *info = &(global_data_subsystem_info->info[current_info_local]);
+
+// //					printf("Commander got subsystem info: %d %d %d\n", info->present, info->enabled, info->health);
+
+
+// 				if (info->present != 0) {
+// 					update_state_machine_subsystem_present(stat_pub, &current_status, &info->subsystem_type);
+
+// 				} else {
+// 					update_state_machine_subsystem_notpresent(stat_pub, &current_status, &info->subsystem_type);
+// 				}
+
+// 				if (info->enabled != 0) {
+// 					update_state_machine_subsystem_enabled(stat_pub, &current_status, &info->subsystem_type);
+
+// 				} else {
+// 					update_state_machine_subsystem_disabled(stat_pub, &current_status, &info->subsystem_type);
+// 				}
+
+// 				if (info->health != 0) {
+// 					update_state_machine_subsystem_healthy(stat_pub, &current_status, &info->subsystem_type);
+
+// 				} else {
+// 					update_state_machine_subsystem_unhealthy(stat_pub, &current_status, &info->subsystem_type);
+// 				}
+
+// 				total_counter++;
+// 			}
+
+// 			if (global_data_subsystem_info->counter - total_counter > SUBSYSTEM_INFO_BUFFER_SIZE) {
+// 				printf("[commander] Warning: Too many subsystem status updates, subsystem_info buffer full\n"); //TODO: add to error queue
+// 				global_data_subsystem_info->counter = total_counter; //this makes sure we print the warning only once
+// 			}
+
+// 			global_data_unlock(&global_data_subsystem_info->access_conf);
+// 		}
+// 	}
+
+// 	return NULL;
+// }
+
+
+
+enum BAT_CHEM {
+	BAT_CHEM_LITHIUM_POLYMERE = 0,
+};
+
+/*
+ * Provides a coarse estimate of remaining battery power.
+ *
+ * The estimate is very basic and based on decharging voltage curves.
+ *
+ * @return the estimated remaining capacity in 0..1
+ */
+float battery_remaining_estimate_voltage(int cells, int chemistry, float voltage)
+{
+	float ret = 0;
+	// XXX do this properly
+	// XXX rebase on parameters
+	const float chemistry_voltage_empty[] = {3.2f};
+	const float chemistry_voltage_full[] = {4.05f};
+
+	ret = (voltage - cells * chemistry_voltage_empty[chemistry]) / (cells * (chemistry_voltage_full[chemistry] - chemistry_voltage_empty[chemistry]));
+
+	/* limit to sane values */
+	ret = (ret < 0) ? 0 : ret;
+	ret = (ret > 1) ? 1 : ret;
+	return ret;
+}
+
+/****************************************************************************
+ * Name: commander
+ ****************************************************************************/
+
+int commander_main(int argc, char *argv[])
+{
+	/* not yet initialized */
+	commander_initialized = false;
+
+	/* welcome user */
+	printf("[commander] I am in command now!\n");
+
+	/* Pthreads */
+	pthread_t command_handling_thread;
+	// pthread_t subsystem_info_thread;
+
+	/* initialize */
+	if (led_init() != 0) {
+		fprintf(stderr, "[commander] ERROR: Failed to initialize leds\n");
+	}
+
+	if (buzzer_init() != 0) {
+		fprintf(stderr, "[commander] ERROR: Failed to initialize buzzer\n");
+	}
+
+	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	if (mavlink_fd < 0) {
+		fprintf(stderr, "[commander] ERROR: Failed to open MAVLink log stream, start mavlink app first.\n");
+	}
+
+	/* advertise to ORB */
+	stat_pub = orb_advertise(ORB_ID(vehicle_status), &current_status);
+
+	if (stat_pub < 0) {
+		printf("[commander] ERROR: orb_advertise failed.\n");
+	}
+
+	/* make sure we are in preflight state */
+	//do_state_update(stat_pub, &current_status, (commander_state_machine_t)SYSTEM_STATE_PREFLIGHT);
+
+	mavlink_log_info(mavlink_fd, "[commander] system is running");
+
+	/* load EEPROM parameters */
+	if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
+		printf("[commander] Loaded EEPROM params in RAM\n");
+		mavlink_log_info(mavlink_fd, "[commander] Loaded EEPROM params in RAM");
+
+	} else {
+		fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
+		mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
+	}
+
+	/* create pthreads */
+	pthread_attr_t command_handling_attr;
+	pthread_attr_init(&command_handling_attr);
+	pthread_attr_setstacksize(&command_handling_attr, 3072);
+	pthread_create(&command_handling_thread, &command_handling_attr, command_handling_loop, NULL);
+
+	// pthread_attr_t subsystem_info_attr;
+	// pthread_attr_init(&subsystem_info_attr);
+	// pthread_attr_setstacksize(&subsystem_info_attr, 2048);
+	// pthread_create(&subsystem_info_thread, &subsystem_info_attr, subsystem_info_loop, NULL);
+
+	/* Start monitoring loop */
+	uint16_t counter = 0;
+	uint8_t flight_env;
+	// uint8_t fix_type;
+	/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
+	float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
+	bool battery_voltage_valid = true;
+	bool low_battery_voltage_actions_done = false;
+	bool critical_battery_voltage_actions_done = false;
+	uint8_t low_voltage_counter = 0;
+	uint16_t critical_voltage_counter = 0;
+	int16_t mode_switch_rc_value;
+	float bat_remain = 1.0f;
+
+//	bool arm_done = false;
+//	bool disarm_done = false;
+
+	uint16_t stick_off_counter = 0;
+	uint16_t stick_on_counter = 0;
+
+	float hdop = 65535.0f;
+
+	int gps_quality_good_counter = 0;
+
+	/* Subscribe to RC data */
+	int rc_sub = orb_subscribe(ORB_ID(rc_channels));
+	struct rc_channels_s rc = {0};
+
+	int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	struct vehicle_gps_position_s gps = {0};
+
+	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	struct sensor_combined_s sensors = {0};
+
+	uint8_t vehicle_state_previous = current_status.state_machine;
+
+	uint64_t last_idle_time = 0;
+
+	/* now initialized */
+	commander_initialized = true;
+
+	while (1) { //TODO: this while loop needs cleanup, split into sub-functions
+
+		/* Get current values */
+		orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
+		orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
+		orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
+
+		battery_voltage = sensors.battery_voltage_v;
+		battery_voltage_valid = sensors.battery_voltage_valid;
+		bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
+
+		flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+		/* Slow but important 5 Hz checks */
+		if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
+			/* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */
+			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || current_status.state_machine == SYSTEM_STATE_AUTO  || current_status.state_machine == SYSTEM_STATE_MANUAL)) {
+				/* armed */
+				led_toggle(LED_BLUE);
+
+			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				/* not armed */
+				led_toggle(LED_BLUE);
+			}
+
+			/* toggle error led at 5 Hz in HIL mode */
+			if ((current_status.mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+				/* armed */
+				led_toggle(LED_AMBER);
+
+			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
+				/* toggle error (red) at 5 Hz on low battery or error */
+				led_toggle(LED_AMBER);
+
+			} else {
+				/* Constant error indication in standby mode without GPS */
+				if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR && !current_status.gps_valid) {
+					led_on(LED_AMBER);
+
+				} else {
+					led_off(LED_AMBER);
+				}
+			}
+
+			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+				//compute system load
+				uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
+
+				if (last_idle_time > 0)
+					current_status.load = 1000 - (interval_runtime / 1000);	//system load is time spent in non-idle
+
+				last_idle_time = system_load.tasks[0].total_runtime;
+			}
+		}
+
+		// // XXX Export patterns and threshold to parameters
+		/* Trigger audio event for low battery */
+		if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) {
+			/* For less than 10%, start be really annoying at 5 Hz */
+			ioctl(buzzer, TONE_SET_ALARM, 0);
+			ioctl(buzzer, TONE_SET_ALARM, 3);
+
+		} else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) {
+			ioctl(buzzer, TONE_SET_ALARM, 0);
+
+		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) {
+			/* For less than 20%, start be slightly annoying at 1 Hz */
+			ioctl(buzzer, TONE_SET_ALARM, 0);
+			ioctl(buzzer, TONE_SET_ALARM, 2);
+
+		} else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) {
+			ioctl(buzzer, TONE_SET_ALARM, 0);
+		}
+
+		/* Check if last transition deserved an audio event */
+#warning This code depends on state that is no longer? maintained
+#if 0
+		trigger_audio_alarm(vehicle_mode_previous, vehicle_state_previous, current_status.mode, current_status.state_machine);
+#endif
+
+		/* only check gps fix if we are outdoor */
+//		if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+//
+//			hdop = (float)(gps.eph) / 100.0f;
+//
+//			/* check if gps fix is ok */
+//			if (gps.fix_type == GPS_FIX_TYPE_3D) { //TODO: is 2d-fix ok? //see http://en.wikipedia.org/wiki/Dilution_of_precision_%28GPS%29
+//
+//				if (gotfix_counter >= GPS_GOTFIX_COUNTER_REQUIRED) { //TODO: add also a required time?
+//					update_state_machine_got_position_fix(stat_pub, &current_status);
+//					gotfix_counter = 0;
+//				} else {
+//					gotfix_counter++;
+//				}
+//				nofix_counter = 0;
+//
+//				if (hdop < 5.0f) { //TODO: this should be a parameter
+//					if (gps_quality_good_counter > GPS_QUALITY_GOOD_COUNTER_LIMIT) {
+//						current_status.gps_valid = true;//--> position estimator can use the gps measurements
+//					}
+//
+//					gps_quality_good_counter++;
+//
+//
+////					if(counter%10 == 0)//for testing only
+////						printf("gps_quality_good_counter = %u\n", gps_quality_good_counter);//for testing only
+//
+//				} else {
+//					gps_quality_good_counter = 0;
+//					current_status.gps_valid = false;//--> position estimator can not use the gps measurements
+//				}
+//
+//			} else {
+//				gps_quality_good_counter = 0;
+//				current_status.gps_valid = false;//--> position estimator can not use the gps measurements
+//
+//				if (nofix_counter > GPS_NOFIX_COUNTER_LIMIT) { //TODO: add also a timer limit?
+//					update_state_machine_no_position_fix(stat_pub, &current_status);
+//					nofix_counter = 0;
+//				} else {
+//					nofix_counter++;
+//				}
+//				gotfix_counter = 0;
+//			}
+//
+//		}
+//
+//
+//		if (flight_env == PX4_FLIGHT_ENVIRONMENT_TESTING) //simulate position fix for quick indoor tests
+		update_state_machine_got_position_fix(stat_pub, &current_status);
+		/* end: check gps */
+
+		/* Check battery voltage */
+		/* write to sys_status */
+		current_status.voltage_battery = battery_voltage;
+		orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+
+		/* if battery voltage is getting lower, warn using buzzer, etc.  */
+		if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
+
+			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				low_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
+			}
+
+			low_voltage_counter++;
+		}
+
+		/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
+		else if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_CRITICAL_VOLTS && false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
+			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
+				critical_battery_voltage_actions_done = true;
+				mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
+				state_machine_emergency(stat_pub, &current_status);
+			}
+
+			critical_voltage_counter++;
+
+		} else {
+			low_voltage_counter = 0;
+			critical_voltage_counter = 0;
+		}
+
+		/* End battery voltage check */
+
+		/* Start RC state check */
+
+		int16_t chan3_scale =  rc.chan[rc.function[YAW]].scale;
+		int16_t chan2_scale =  rc.chan[rc.function[THROTTLE]].scale;
+
+		/* check if left stick is in lower left position --> switch to standby state */
+		if (chan3_scale > STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values
+			if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+				update_state_machine_disarm(stat_pub, &current_status);
+				stick_on_counter = 0;
+
+			} else {
+				stick_off_counter++;
+				stick_on_counter = 0;
+			}
+		}
+
+		/* check if left stick is in lower right position --> arm */
+		if (chan3_scale < -STICK_ON_OFF_LIMIT && chan2_scale < -STICK_ON_OFF_LIMIT) { //TODO: remove hardcoded values
+			if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
+				update_state_machine_arm(stat_pub, &current_status);
+				stick_on_counter = 0;
+
+			} else {
+				stick_on_counter++;
+				stick_off_counter = 0;
+			}
+		}
+
+		/* Check the value of the rc channel of the mode switch */
+		mode_switch_rc_value = rc.chan[rc.function[OVERRIDE]].scale;
+
+		if (mode_switch_rc_value > STICK_ON_OFF_LIMIT) {
+			update_state_machine_mode_manual(stat_pub, &current_status);
+
+		} else if (mode_switch_rc_value < -STICK_ON_OFF_LIMIT) {
+			update_state_machine_mode_auto(stat_pub, &current_status);
+
+		} else {
+			update_state_machine_mode_stabilized(stat_pub, &current_status);
+		}
+
+		/* End mode switch */
+
+		/* END RC state check */
+
+
+		current_status.counter++;
+		current_status.timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_status), stat_pub, &current_status);
+
+
+
+		/* Store old modes to detect and act on state transitions */
+		vehicle_state_previous = current_status.state_machine;
+
+		fflush(stdout);
+		counter++;
+		usleep(COMMANDER_MONITORING_INTERVAL);
+	}
+
+	/* wait for threads to complete */
+	pthread_join(command_handling_thread, NULL);
+	// pthread_join(subsystem_info_thread, NULL);
+
+	/* close fds */
+	led_deinit();
+	buzzer_deinit();
+
+	return 0;
+}
+

apps/commander/commander.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ *   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 Main system state machine definition */
+
+#ifndef COMMANDER_H_
+#define COMMANDER_H_
+
+#define VOLTAGE_BATTERY_HIGH_VOLTS 12.0f
+#define VOLTAGE_BATTERY_LOW_VOLTS 10.5f
+#define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
+#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
+#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
+
+#endif /* COMMANDER_H_ */

apps/commander/state_machine_helper.c

@@ -0,0 +1,564 @@
+/****************************************************************************
+ *
+ *   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 State machine helper functions implementations */
+
+#include <stdio.h>
+#include "state_machine_helper.h"
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <systemlib/systemlib.h>
+#include <arch/board/up_hrt.h>
+
+static const char* system_state_txt[] = {
+	"SYSTEM_STATE_PREFLIGHT",
+	"SYSTEM_STATE_STANDBY",
+	"SYSTEM_STATE_GROUND_READY",
+	"SYSTEM_STATE_MANUAL",
+	"SYSTEM_STATE_STABILIZED",
+	"SYSTEM_STATE_AUTO",
+	"SYSTEM_STATE_MISSION_ABORT",
+	"SYSTEM_STATE_EMCY_LANDING",
+	"SYSTEM_STATE_EMCY_CUTOFF",
+	"SYSTEM_STATE_GROUND_ERROR",
+	"SYSTEM_STATE_REBOOT",
+
+};
+
+
+void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state)
+{
+	int invalid_state = false;
+
+	switch (new_state) {
+	case SYSTEM_STATE_MISSION_ABORT: {
+			/* Indoor or outdoor */
+			uint8_t flight_environment_parameter = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+			if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+				do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
+
+			} else {
+				do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
+			}
+
+			return;
+		}
+		break;
+
+	case SYSTEM_STATE_EMCY_LANDING:
+		/* Tell the controller to land */
+		//TODO: add emcy landing code here
+
+		fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
+		//global_data_send_mavlink_statustext_message_out("Commander: state: emergency landing", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_EMCY_CUTOFF:
+		/* Tell the controller to cutoff the motors (thrust = 0), make sure that this is not overwritten by another app and stays at 0 */
+		//TODO: add emcy cutoff code here
+
+		fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
+		//global_data_send_mavlink_statustext_message_out("Commander: state: emergency cutoff", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_GROUND_ERROR:
+		fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
+		//global_data_send_mavlink_statustext_message_out("Commander: state: ground error", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_PREFLIGHT:
+		//global_data_send_mavlink_statustext_message_out("Commander: state: preflight", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_REBOOT:
+		usleep(500000);
+		reboot();
+		break;
+
+	case SYSTEM_STATE_STANDBY:
+		//global_data_send_mavlink_statustext_message_out("Commander: state: standby", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_GROUND_READY:
+		//global_data_send_mavlink_statustext_message_out("Commander: state: armed", MAV_SEVERITY_INFO);
+
+		//if in manual mode switch to manual state
+//		if (current_status->remote_manual) {
+//			printf("[commander] manual mode\n");
+//			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+//			return;
+//		}
+
+		break;
+
+	case SYSTEM_STATE_AUTO:
+		//global_data_send_mavlink_statustext_message_out("Commander: state: auto", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_STABILIZED:
+		//global_data_send_mavlink_statustext_message_out("Commander: state: stabilized", MAV_SEVERITY_INFO);
+		break;
+
+	case SYSTEM_STATE_MANUAL:
+		//global_data_send_mavlink_statustext_message_out("Commander: state: manual", MAV_SEVERITY_INFO);
+		break;
+
+	default:
+		invalid_state = true;
+		break;
+	}
+
+	if (invalid_state == false) {
+		//publish the new state
+		current_status->state_machine = new_state;
+		current_status->counter++;
+		current_status->timestamp = hrt_absolute_time();
+		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+		printf("[commander] new state: %s\n", system_state_txt[new_state]);
+	}
+}
+
+
+/*
+ * Private functions, update the state machine
+ */
+void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status)
+{
+	fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
+	/* Depending on the current state go to one of the error states */
+
+	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
+
+	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
+
+	} else {
+		fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
+	}
+}
+
+void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
+{
+	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
+		state_machine_emergency_always_critical(status_pub, current_status);
+
+	} else {
+		//global_data_send_mavlink_statustext_message_out("[commander] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
+	}
+
+}
+
+
+
+// /*
+//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
+//  */
+
+// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
+//  *
+//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
+//  * */
+
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was removed something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was disabled something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//TODO state machine change (recovering)
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//TODO state machine change
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//TODO state machine change
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		//TODO state machine change
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+
+// }
+
+
+// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
+// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
+
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency_always_critical(status_pub, current_status);
+
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
+
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency_always_critical(status_pub, current_status);
+
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
+
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency_always_critical(status_pub, current_status);
+
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// //			//TODO: remove this block
+// //			break;
+// //			///////////////////
+// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
+
+// //				printf("previosly_healthy = %u\n", previosly_healthy);
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency(status_pub, current_status);
+
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+
+
+void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status)
+{
+	/* Depending on the current state switch state */
+	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+	}
+}
+
+void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status)
+{
+	/* Depending on the current state switch state */
+	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		state_machine_emergency(status_pub, current_status);
+	}
+}
+
+void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status)
+{
+	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
+		printf("[commander] arming\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+	} /*else if (current_status->state_machine == SYSTEM_STATE_AUTO) {
+
+		printf("[commander] landing\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+	}*/
+}
+
+void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status)
+{
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+		printf("[commander] going standby\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+
+	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[commander] MISSION ABORT!\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+	}
+}
+
+void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status)
+{
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[commander] manual mode\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+	}
+}
+
+void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status)
+{
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[commander] stabilized mode\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+	}
+}
+
+void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status)
+{
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
+		printf("[commander] auto mode\n");
+		do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+	}
+}
+
+
+uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode)
+{
+	commander_state_machine_t current_system_state = current_status->state_machine;
+
+	printf("in update state request\n");
+	uint8_t ret = 1;
+
+	/* Switch on HIL if in standby */
+	if ((current_system_state == SYSTEM_STATE_STANDBY) && (mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+		/* Enable HIL on request */
+		current_status->mode |= MAV_MODE_FLAG_HIL_ENABLED;
+	}
+
+	/* NEVER actually switch off HIL without reboot */
+	if ((current_status->mode & MAV_MODE_FLAG_HIL_ENABLED) && !(mode & MAV_MODE_FLAG_HIL_ENABLED)) {
+		fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
+		ret = ERROR;
+	}
+
+	//TODO: clarify mapping between mavlink enum MAV_MODE and the state machine, then add more decisions to the switch (see also the system_state_loop function in mavlink.c)
+	switch (mode) {
+	case MAV_MODE_MANUAL_ARMED: //= SYSTEM_STATE_ARMED
+		if (current_system_state == SYSTEM_STATE_STANDBY) {
+			/* Set armed flag */
+			current_status->mode |= MAV_MODE_FLAG_SAFETY_ARMED;
+			/* Set manual input enabled flag */
+			current_status->mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+			ret = OK;
+		}
+
+		break;
+
+	case MAV_MODE_MANUAL_DISARMED:
+		if (current_system_state == SYSTEM_STATE_GROUND_READY) {
+			/* Clear armed flag, leave manual input enabled */
+			current_status->mode &= ~MAV_MODE_FLAG_SAFETY_ARMED;
+			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+			ret = OK;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	#warning STATE MACHINE IS INCOMPLETE HERE
+
+//	if(ret != 0) //Debug
+//	{
+//		strcpy(mavlink_statustext_msg_content.values[0].string_value, "Commander: command rejected");
+//		global_data_send_mavlink_message_out(&mavlink_statustext_msg_content);
+//	}
+
+	return ret;
+}
+
+uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
+{
+	commander_state_machine_t current_system_state = current_status->state_machine;
+
+	uint8_t ret = 1;
+
+	switch (custom_mode) {
+	case SYSTEM_STATE_GROUND_READY:
+		break;
+
+	case SYSTEM_STATE_STANDBY:
+		break;
+
+	case SYSTEM_STATE_REBOOT:
+		printf("try to reboot\n");
+
+		if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
+			printf("system will reboot\n");
+			//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
+			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
+			ret = 0;
+		}
+
+		break;
+
+	case SYSTEM_STATE_AUTO:
+		printf("try to switch to auto/takeoff\n");
+
+		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
+			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+			printf("state: auto\n");
+			ret = 0;
+		}
+
+		break;
+
+	case SYSTEM_STATE_MANUAL:
+		printf("try to switch to manual\n");
+
+		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
+			do_state_update(status_pub, current_status, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+			printf("state: manual\n");
+			ret = 0;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+

apps/commander/state_machine_helper.h

@@ -0,0 +1,133 @@
+/****************************************************************************
+ *
+ *   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 State machine helper functions definitions */
+
+#ifndef STATE_MACHINE_HELPER_H_
+#define STATE_MACHINE_HELPER_H_
+
+#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
+#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <v1.0/common/mavlink.h>
+
+
+/*
+ * @brief switch to new state with no checking *
+ *
+ * do_state_update: this is the functions that all other functions have to call in order to update the state.
+ * the function does not question the state change, this must be done before
+ * The function performs actions that are connected with the new state (buzzer, reboot, ...)
+ *
+ *
+ */
+void do_state_update(int status_pub, struct vehicle_status_s *current_status, commander_state_machine_t new_state);
+
+/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+
+// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+
+
+/*
+ * @brief handle state machine if got position fix
+ */
+void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status);
+
+/*
+ * @brief handle state machine if position fix lost
+ */
+void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status);
+
+/*
+ * @brief handle state machine if user wants to arm
+ */
+void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status);
+
+/*
+ * @brief handle state machine if user wants to disarm
+ */
+void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status);
+
+/*
+ * @brief handle state machine if mode switch is manual
+ */
+void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status);
+
+/*
+ * @brief handle state machine if mode switch is stabilized
+ */
+void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status);
+
+/*
+ * @brief handle state machine if mode switch is auto
+ */
+void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status);
+
+
+
+
+/*
+ *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
+ *  If the request is obeyed the functions return 0
+ *
+ */
+
+/*
+ * @brief handles *incoming request* to switch to a specific state, if state change is successful returns 0
+ */
+uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t mode);
+
+/*
+ * @brief handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
+ */
+uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, uint8_t custom_mode);
+
+
+void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status);
+void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status);
+
+
+
+
+#endif /* STATE_MACHINE_HELPER_H_ */

apps/doxygen_mainpage.dox

@@ -0,0 +1,49 @@
+/** @mainpage PX4 Codebase Overview
+
+PX4 is an open-source, open-hardware project aiming at providing a high-end autopilot to the academic, hobby and industrial communities (BSD licensed). For full documentation, refer to the official project website:
+
+https://pixhawk.ethz.ch/px4/
+
+Please follow the toolchain installation instructions at https://pixhawk.ethz.ch/px4/dev/px4_quickstart.
+
+*/
+
+/**
+@defgroup topics uORB Topics
+
+Small and efficient object request broker.
+
+The micro object request broker (uORB) is a very efficient implementation of an object
+request broker following the publisher/subscriber design pattern.
+
+It is in detail described here:
+https://pixhawk.ethz.ch/px4/dev/shared_object_communication
+*/
+
+/**
+@defgroup apps Onboard Applications
+
+This is the list of the main functions of all onboard applications. The use of these
+applications is documented here:
+https://pixhawk.ethz.ch/px4/users/apps/start
+
+Developer documentation of individual applications and the uORB data sharing mechanisms
+are described here:
+https://pixhawk.ethz.ch/px4/dev/apps/start
+
+*/
+
+/**
+@defgroup attitude_estimation Attitude Estimation
+
+*/
+
+/**
+@defgroup position_estimation Position Estimation
+
+*/
+
+/**
+ * @addtogroup topics
+ * @{
+ */

apps/drivers/bma180/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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 the BMA180 driver.
+#
+
+APPNAME		 = bma180
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+
+include $(APPDIR)/mk/app.mk

apps/drivers/bma180/bma180.cpp

@@ -0,0 +1,774 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Driver for the Bosch BMA 180 MEMS accelerometer connected via SPI.
+ */
+
+#include <nuttx/config.h>
+
+#include <device/spi.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/up_hrt.h>
+
+#include <drivers/drv_accel.h>
+
+extern "C" { __EXPORT int bma180_main(int argc, char *argv[]); }
+
+class BMA180 : public device::SPI
+{
+public:
+	BMA180(int bus, spi_dev_e device);
+	~BMA180();
+
+	virtual int		init();
+
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int		ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	virtual int		open_first(struct file *filp);
+	virtual int		close_last(struct file *filp);
+
+	/**
+	 * Diagnostics - print some basic information about the driver.
+	 */
+	void			print_info();
+
+protected:
+	virtual int		probe();
+
+private:
+
+	struct hrt_call		_call;
+	unsigned		_call_interval;
+
+	unsigned		_num_reports;
+	volatile unsigned	_next_report;
+	volatile unsigned	_oldest_report;
+	struct accel_report	*_reports;
+
+	struct accel_scale	_scale;
+	float			_range_scale;
+
+	unsigned		_reads;
+
+	/**
+	 * Start automatic measurement.
+	 */
+	void			start();
+
+	/**
+	 * Stop automatic measurement.
+	 */
+	void			stop();
+
+	/**
+	 * Static trampoline from the hrt_call context; because we don't have a
+	 * generic hrt wrapper yet.
+	 *
+	 * Called by the HRT in interrupt context at the specified rate if
+	 * automatic polling is enabled.
+	 *
+	 * @param arg		Instance pointer for the driver that is polling.
+	 */
+	static void		measure_trampoline(void *arg);
+
+	/**
+	 * Fetch measurements from the sensor and update the report ring.
+	 */
+	void			measure();
+
+	/**
+	 * Read a register from the BMA180
+	 *
+	 * @param		The register to read.
+	 * @return		The value that was read.
+	 */
+	uint8_t			read_reg(unsigned reg);
+
+	/**
+	 * Write a register in the BMA180
+	 *
+	 * @param reg		The register to write.
+	 * @param value		The new value to write.
+	 */
+	void			write_reg(unsigned reg, uint8_t value);
+
+	/**
+	 * Modify a register in the BMA180
+	 *
+	 * Bits are cleared before bits are set.
+	 *
+	 * @param reg		The register to modify.
+	 * @param clearbits	Bits in the register to clear.
+	 * @param setbits	Bits in the register to set.
+	 */
+	void			modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
+
+	/**
+	 * Set the BMA180 measurement range.
+	 *
+	 * @param max_g		The maximum G value the range must support.
+	 * @return		OK if the value can be supported, -ERANGE otherwise.
+	 */
+	int			set_range(unsigned max_g);
+
+	/**
+	 * Set the BMA180 lowpass filter.
+	 *
+	 * @param frequency	Set the lowpass filter cutoff frequency to no less than
+	 *			this frequency.
+	 * @return		OK if the value can be supported.
+	 */
+	int			set_bandwidth(unsigned frequency);
+};
+
+/* helper macro for handling report buffer indices */
+#define INCREMENT(_x, _lim)	do { _x++; if (_x >= _lim) _x = 0; } while(0)
+
+#define DIR_READ			(1<<7)
+#define DIR_WRITE			(0<<7)
+
+#define ADDR_CHIP_ID			0x00
+#define CHIP_ID				0x03
+
+#define ADDR_ACC_X_LSB			0x02
+#define ADDR_ACC_Y_LSB			0x04
+#define ADDR_ACC_Z_LSB			0x06
+#define ADDR_TEMPERATURE		0x08
+
+#define ADDR_RESET			0x10
+#define SOFT_RESET				0xB6
+
+#define ADDR_BW_TCS			0x20
+#define BW_TCS_BW_MASK				(0xf<<4)
+#define BW_TCS_BW_10HZ				(0<<4)
+#define BW_TCS_BW_20HZ				(1<<4)
+#define BW_TCS_BW_40HZ				(2<<4)
+#define BW_TCS_BW_75HZ				(3<<4)
+#define BW_TCS_BW_150HZ				(4<<4)
+#define BW_TCS_BW_300HZ				(5<<4)
+#define BW_TCS_BW_600HZ				(6<<4)
+#define BW_TCS_BW_1200HZ			(7<<4)
+
+#define ADDR_HIGH_DUR			0x27
+#define HIGH_DUR_DIS_I2C			(1<<0)
+
+#define ADDR_TCO_Z			0x30
+#define TCO_Z_MODE_MASK				0x3
+
+#define ADDR_GAIN_Y			0x33
+#define GAIN_Y_SHADOW_DIS			(1<<0)
+
+#define ADDR_OFFSET_LSB1		0x35
+#define OFFSET_LSB1_RANGE_MASK			(7<<1)
+#define OFFSET_LSB1_RANGE_1G			(0<<1)
+#define OFFSET_LSB1_RANGE_2G			(2<<1)
+#define OFFSET_LSB1_RANGE_3G			(3<<1)
+#define OFFSET_LSB1_RANGE_4G			(4<<1)
+#define OFFSET_LSB1_RANGE_8G			(5<<1)
+#define OFFSET_LSB1_RANGE_16G			(6<<1)
+
+#define ADDR_OFFSET_T			0x37
+#define OFFSET_T_READOUT_12BIT			(1<<0)
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" { int bma180_main(int argc, char *argv[]); }
+
+
+BMA180::BMA180(int bus, spi_dev_e device) :
+	SPI("BMA180", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 8000000),
+	_num_reports(0),
+	_next_report(0),
+	_oldest_report(0),
+	_reports(nullptr),
+	_reads(0)
+{
+	// enable debug() calls
+	_debug_enabled = true;
+
+	// default scale factors
+	_scale.x_offset = 0;
+	_scale.x_scale  = 1.0f;
+	_scale.y_offset = 0;
+	_scale.y_scale  = 1.0f;
+	_scale.z_offset = 0;
+	_scale.z_scale  = 1.0f;
+}
+
+BMA180::~BMA180()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete[] _reports;
+}
+
+int
+BMA180::init()
+{
+	int ret;
+
+	/* do SPI init (and probe) first */
+	ret = SPI::init();
+
+	/* if probe/setup successful, finish chip init */
+	if (ret == OK) {
+
+		/* perform soft reset (p48) */
+		write_reg(ADDR_RESET, SOFT_RESET);
+
+		/* wait 10us (p49) */
+		usleep(10);
+
+		/* disable I2C interface */
+		modify_reg(ADDR_HIGH_DUR, HIGH_DUR_DIS_I2C, 0);
+
+		/* switch to low-noise mode */
+		modify_reg(ADDR_TCO_Z, TCO_Z_MODE_MASK, 0);
+
+		/* disable 12-bit mode */
+		modify_reg(ADDR_OFFSET_T, OFFSET_T_READOUT_12BIT, 0);
+
+		/* disable shadow-disable mode */
+		modify_reg(ADDR_GAIN_Y, GAIN_Y_SHADOW_DIS, 0);
+	}
+
+	return ret;
+}
+
+int
+BMA180::open_first(struct file *filp)
+{
+	/* reset to manual-poll mode */
+	_call_interval = 0;
+
+	/* allocate basic report buffers */
+	_num_reports = 2;
+	_reports = new struct accel_report[_num_reports];
+	_oldest_report = _next_report = 0;
+
+	/* set default range and lowpass */
+	set_range(4);		/* 4G */
+	set_bandwidth(600);	/* 600Hz */
+
+	return OK;
+}
+
+int
+BMA180::close_last(struct file *filp)
+{
+	/* stop measurement */
+	stop();
+
+	/* free report buffers */
+	if (_reports != nullptr) {
+		delete[] _reports;
+		_num_reports = 0;
+	}
+
+	return OK;
+}
+
+int
+BMA180::probe()
+{
+	if (read_reg(ADDR_CHIP_ID) == CHIP_ID)
+		return OK;
+
+	return -EIO;
+}
+
+ssize_t
+BMA180::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct accel_report);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_call_interval > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the measurement code while we are doing this;
+		 * we are careful to avoid racing with it.
+		 */
+		while (count--) {
+			if (_oldest_report != _next_report) {
+				memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
+				ret += sizeof(_reports[0]);
+				INCREMENT(_oldest_report, _num_reports);
+			}
+		}
+
+		_reads++;
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement */
+	_oldest_report = _next_report = 0;
+	measure();
+
+	/* measurement will have generated a report, copy it out */
+	memcpy(buffer, _reports, sizeof(*_reports));
+	ret = sizeof(*_reports);
+
+	return ret;
+}
+
+int
+BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case ACCELIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case ACC_POLLRATE_MANUAL:
+				stop();
+				_call_interval = 0;
+				return OK;
+
+				/* external signalling not supported */
+			case ACC_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_call_interval == 0);
+
+					/* convert hz to hrt interval via microseconds */
+					unsigned ticks = 1000000 / arg;
+
+					/* check against maximum sane rate */
+					if (ticks < 1000)
+						return -EINVAL;
+
+					/* update interval for next measurement */
+					/* XXX this is a bit shady, but no other way to adjust... */
+					_call.period = _call_interval;
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case ACCELIOCSQUEUEDEPTH: {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 2) || (arg > 100))
+				return -EINVAL;
+
+			/* allocate new buffer */
+			struct accel_report *buf = new struct accel_report[arg];
+
+			if (nullptr == buf)
+				return -ENOMEM;
+
+			/* reset the measurement state machine with the new buffer, free the old */
+			stop();
+			delete[] _reports;
+			_num_reports = arg;
+			_reports = buf;
+			start();
+
+			return OK;
+		}
+
+	case ACCELIOCSLOWPASS:
+		return set_bandwidth(arg);
+
+	case ACCELIORANGE:
+		return set_range(arg);
+
+	case ACCELIOCSSAMPLERATE:	/* sensor sample rate is not (really) adjustable */
+	case ACCELIOCSREPORTFORMAT:	/* no alternate report formats */
+		return -EINVAL;
+
+	default:
+		/* give it to the superclass */
+		return SPI::ioctl(filp, cmd, arg);
+	}
+}
+
+uint8_t
+BMA180::read_reg(unsigned reg)
+{
+	uint8_t cmd[2];
+
+	cmd[0] = reg | DIR_READ;
+
+	transfer(cmd, cmd, sizeof(cmd));
+
+	return cmd[1];
+}
+
+void
+BMA180::write_reg(unsigned reg, uint8_t value)
+{
+	uint8_t	cmd[2];
+
+	cmd[0] = reg | DIR_WRITE;
+	cmd[1] = value;
+
+	transfer(cmd, nullptr, sizeof(cmd));
+}
+
+void
+BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
+{
+	uint8_t	val;
+
+	val = read_reg(reg);
+	val &= ~clearbits;
+	val |= setbits;
+	write_reg(reg, val);
+}
+
+int
+BMA180::set_range(unsigned max_g)
+{
+	uint8_t rangebits;
+	float rangescale;
+
+	if (max_g > 16) {
+		return -ERANGE;
+
+	} else if (max_g > 8) {		/* 16G */
+		rangebits = OFFSET_LSB1_RANGE_16G;
+		rangescale = 1.98;
+
+	} else if (max_g > 4) {		/* 8G */
+		rangebits = OFFSET_LSB1_RANGE_8G;
+		rangescale = 0.99;
+
+	} else if (max_g > 3) {		/* 4G */
+		rangebits = OFFSET_LSB1_RANGE_4G;
+		rangescale = 0.5;
+
+	} else if (max_g > 2) {		/* 3G */
+		rangebits = OFFSET_LSB1_RANGE_3G;
+		rangescale = 0.38;
+
+	} else if (max_g > 1) {		/* 2G */
+		rangebits = OFFSET_LSB1_RANGE_2G;
+		rangescale = 0.25;
+
+	} else {			/* 1G */
+		rangebits = OFFSET_LSB1_RANGE_1G;
+		rangescale = 0.13;
+	}
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_OFFSET_LSB1, OFFSET_LSB1_RANGE_MASK, rangebits);
+	_range_scale = rangescale;
+
+	return OK;
+}
+
+int
+BMA180::set_bandwidth(unsigned frequency)
+{
+	uint8_t	bwbits;
+
+	if (frequency > 1200) {
+		return -ERANGE;
+
+	} else if (frequency > 600) {
+		bwbits = BW_TCS_BW_1200HZ;
+
+	} else if (frequency > 300) {
+		bwbits = BW_TCS_BW_600HZ;
+
+	} else if (frequency > 150) {
+		bwbits = BW_TCS_BW_300HZ;
+
+	} else if (frequency > 75) {
+		bwbits = BW_TCS_BW_150HZ;
+
+	} else if (frequency > 40) {
+		bwbits = BW_TCS_BW_75HZ;
+
+	} else if (frequency > 20) {
+		bwbits = BW_TCS_BW_40HZ;
+
+	} else if (frequency > 10) {
+		bwbits = BW_TCS_BW_20HZ;
+
+	} else {
+		bwbits = BW_TCS_BW_10HZ;
+	}
+
+	/* adjust sensor configuration */
+	modify_reg(ADDR_BW_TCS, BW_TCS_BW_MASK, bwbits);
+
+	return OK;
+}
+
+void
+BMA180::start()
+{
+	/* make sure we are stopped first */
+	stop();
+
+	/* reset the report ring */
+	_oldest_report = _next_report = 0;
+
+	/* start polling at the specified rate */
+	hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&BMA180::measure_trampoline, this);
+}
+
+void
+BMA180::stop()
+{
+	hrt_cancel(&_call);
+}
+
+void
+BMA180::measure_trampoline(void *arg)
+{
+	BMA180 *dev = (BMA180 *)arg;
+
+	/* make another measurement */
+	dev->measure();
+}
+
+void
+BMA180::measure()
+{
+	/*
+	 * This evil is to deal with the stupid layout of the BMA180
+	 * measurement registers vs. the SPI transaction model.
+	 */
+	union {
+		uint8_t bytes[10];
+		uint16_t words[5];
+	} buf;
+
+	/*
+	 * Fetch the full set of measurements from the BMA180 in one pass;
+	 * 7 bytes starting from the X LSB.
+	 */
+	buf.bytes[1] = ADDR_ACC_X_LSB;
+	transfer(&buf.bytes[1], &buf.bytes[1], 8);
+
+	/*
+	 * Adjust and scale results to mg.
+	 *
+	 * Note that we ignore the "new data" bits.  At any time we read, each
+	 * of the axis measurements are the "most recent", even if we've seen
+	 * them before.  There is no good way to synchronise with the internal
+	 * measurement flow without using the external interrupt.
+	 */
+	_reports[_next_report].timestamp = hrt_absolute_time();
+	_reports[_next_report].x = (buf.words[1] >> 2) * _range_scale;
+	_reports[_next_report].y = (buf.words[2] >> 2) * _range_scale;
+	_reports[_next_report].z = (buf.words[3] >> 2) * _range_scale;
+
+	/*
+	 * @todo Apply additional scaling / calibration factors here.
+	 */
+
+	/* post a report to the ring - note, not locked */
+	INCREMENT(_next_report, _num_reports);
+
+	/* if we are running up against the oldest report, fix it */
+	if (_next_report == _oldest_report)
+		INCREMENT(_oldest_report, _num_reports);
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+}
+
+void
+BMA180::print_info()
+{
+	printf("reads:          %u\n", _reads);
+	printf("report queue:   %u (%u/%u @ %p)\n",
+	       _num_reports, _oldest_report, _next_report, _reports);
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace
+{
+
+BMA180	*g_dev;
+
+/*
+ * XXX this should just be part of the generic sensors test...
+ */
+
+int
+test()
+{
+	int fd = -1;
+	struct accel_report report;
+	ssize_t sz;
+	const char *reason = "test OK";
+
+	do {
+
+		/* get the driver */
+		fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+		if (fd < 0) {
+			reason = "can't open driver";
+			break;
+		}
+
+		/* do a simple demand read */
+		sz = read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			reason = "immediate read failed";
+			break;
+		}
+
+		printf("single read\n");
+		fflush(stdout);
+		printf("time:        %lld\n", report.timestamp);
+		printf("x:           %f\n", report.x);
+		printf("y:           %f\n", report.y);
+		printf("z:           %f\n", report.z);
+
+	} while (0);
+
+	printf("BMA180: %s\n", reason);
+
+	return OK;
+}
+
+int
+info()
+{
+	if (g_dev == nullptr) {
+		fprintf(stderr, "BMA180: driver not running\n");
+		return -ENOENT;
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	return OK;
+}
+
+
+} // namespace
+
+int
+bma180_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 *
+	 * XXX it would be nice to have a wrapper for this...
+	 */
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_dev != nullptr) {
+			fprintf(stderr, "BMA180: already loaded\n");
+			return -EBUSY;
+		}
+
+		/* create the driver */
+		g_dev = new BMA180(CONFIG_BMA180_SPI_BUS, (spi_dev_e)CONFIG_BMA180_SPI_DEVICE);
+
+		if (g_dev == nullptr) {
+			fprintf(stderr, "BMA180: driver alloc failed\n");
+			return -ENOMEM;
+		}
+
+		if (OK != g_dev->init()) {
+			fprintf(stderr, "BMA180: driver init failed\n");
+			usleep(100000);
+			delete g_dev;
+			g_dev = nullptr;
+			return -EIO;
+		}
+
+		printf("BMA180: driver started\n");
+		return OK;
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		return test();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info"))
+		return info();
+
+	fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
+	return -EINVAL;
+}

apps/drivers/device/Makefile

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build the device driver framework.
+#
+
+include $(APPDIR)/mk/app.mk

apps/drivers/device/cdev.cpp

@@ -0,0 +1,396 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Character device base class.
+ */
+
+#include "device.h"
+
+#include <sys/ioctl.h>
+#include <arch/irq.h>
+
+#include <stdlib.h>
+#include <stdio.h>
+
+#ifdef CONFIG_DISABLE_POLL
+# error This driver is not compatible with CONFIG_DISABLE_POLL
+#endif
+
+namespace device
+{
+
+/* how much to grow the poll waiter set each time it has to be increased */
+static const unsigned pollset_increment = 0;
+
+/*
+ * The standard NuttX operation dispatch table can't call C++ member functions
+ * directly, so we have to bounce them through this dispatch table.
+ */
+static int	cdev_open(struct file *filp);
+static int	cdev_close(struct file *filp);
+static ssize_t	cdev_read(struct file *filp, char *buffer, size_t buflen);
+static ssize_t	cdev_write(struct file *filp, const char *buffer, size_t buflen);
+static off_t	cdev_seek(struct file *filp, off_t offset, int whence);
+static int	cdev_ioctl(struct file *filp, int cmd, unsigned long arg);
+static int	cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
+
+/**
+ * Character device indirection table.
+ *
+ * Every cdev we register gets the same function table; we use the private data
+ * field in the inode to store the instance pointer.
+ *
+ * Note that we use the GNU extension syntax here because we don't get designated
+ * initialisers in gcc 4.6.
+ */
+static const struct file_operations cdev_fops = {
+open	: cdev_open,
+close	: cdev_close,
+read	: cdev_read,
+write	: cdev_write,
+seek	: cdev_seek,
+ioctl	: cdev_ioctl,
+poll	: cdev_poll,
+};
+
+CDev::CDev(const char *name,
+	   const char *devname,
+	   int irq) :
+	// base class
+	Device(name, irq),
+	// public
+	// protected
+	// private
+	_devname(devname),
+	_registered(false),
+	_open_count(0)
+{
+	for (unsigned i = 0; i < _max_pollwaiters; i++)
+		_pollset[i] = nullptr;
+}
+
+CDev::~CDev()
+{
+	if (_registered)
+		unregister_driver(_devname);
+}
+
+int
+CDev::init()
+{
+	int ret = OK;
+
+	// base class init first
+	ret = Device::init();
+
+	if (ret != OK)
+		goto out;
+
+	// now register the driver
+	ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
+
+	if (ret != OK)
+		goto out;
+
+	_registered = true;
+
+out:
+	return ret;
+}
+
+/*
+ * Default implementations of the character device interface
+ */
+int
+CDev::open(struct file *filp)
+{
+	int ret = OK;
+
+	lock();
+	/* increment the open count */
+	_open_count++;
+
+	if (_open_count == 1) {
+
+		/* first-open callback may decline the open */
+		ret = open_first(filp);
+
+		if (ret != OK)
+			_open_count--;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::open_first(struct file *filp)
+{
+	return OK;
+}
+
+int
+CDev::close(struct file *filp)
+{
+	int ret = OK;
+
+	lock();
+
+	if (_open_count > 0) {
+		/* decrement the open count */
+		_open_count--;
+
+		/* callback cannot decline the close */
+		if (_open_count == 0)
+			ret = close_last(filp);
+
+	} else {
+		ret = -EBADF;
+	}
+
+	unlock();
+
+	return ret;
+}
+
+int
+CDev::close_last(struct file *filp)
+{
+	return OK;
+}
+
+ssize_t
+CDev::read(struct file *filp, char *buffer, size_t buflen)
+{
+	return -ENOSYS;
+}
+
+ssize_t
+CDev::write(struct file *filp, const char *buffer, size_t buflen)
+{
+	return -ENOSYS;
+}
+
+off_t
+CDev::seek(struct file *filp, off_t offset, int whence)
+{
+	return -ENOSYS;
+}
+
+int
+CDev::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+		/* fetch a pointer to the driver's private data */
+	case DIOC_GETPRIV:
+		*(void **)(uintptr_t)arg = (void *)this;
+		return OK;
+	}
+
+	return -ENOTTY;
+}
+
+int
+CDev::poll(struct file *filp, struct pollfd *fds, bool setup)
+{
+	int ret = OK;
+
+	/*
+	 * Lock against pollnotify() (and possibly other callers)
+	 */
+	lock();
+
+	if (setup) {
+		/*
+		 * Save the file pointer in the pollfd for the subclass'
+		 * benefit.
+		 */
+		fds->priv = (void *)filp;
+
+		/*
+		 * Handle setup requests.
+		 */
+		ret = store_poll_waiter(fds);
+
+		if (ret == OK) {
+
+			/*
+			 * Check to see whether we should send a poll notification
+			 * immediately.
+			 */
+			fds->revents |= fds->events & poll_state(filp);
+
+			/* yes? post the notification */
+			if (fds->revents != 0)
+				sem_post(fds->sem);
+		}
+
+	} else {
+		/*
+		 * Handle a teardown request.
+		 */
+		ret = remove_poll_waiter(fds);
+	}
+
+	unlock();
+
+	return ret;
+}
+
+void
+CDev::poll_notify(pollevent_t events)
+{
+	/* lock against poll() as well as other wakeups */
+	irqstate_t state = irqsave();
+
+	for (unsigned i = 0; i < _max_pollwaiters; i++)
+		if (nullptr != _pollset[i])
+			poll_notify_one(_pollset[i], events);
+
+	irqrestore(state);
+}
+
+void
+CDev::poll_notify_one(struct pollfd *fds, pollevent_t events)
+{
+	/* update the reported event set */
+	fds->revents |= fds->events & events;
+
+	/* if the state is now interesting, wake the waiter if it's still asleep */
+	/* XXX semcount check here is a vile hack; counting semphores should not be abused as cvars */
+	if ((fds->revents != 0) && (fds->sem->semcount <= 0))
+		sem_post(fds->sem);
+}
+
+pollevent_t
+CDev::poll_state(struct file *filp)
+{
+	/* by default, no poll events to report */
+	return 0;
+}
+
+int
+CDev::store_poll_waiter(struct pollfd *fds)
+{
+	/*
+	 * Look for a free slot.
+	 */
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (nullptr == _pollset[i]) {
+
+			/* save the pollfd */
+			_pollset[i] = fds;
+
+			return OK;
+		}
+	}
+
+	return ENOMEM;
+}
+
+int
+CDev::remove_poll_waiter(struct pollfd *fds)
+{
+	for (unsigned i = 0; i < _max_pollwaiters; i++) {
+		if (fds == _pollset[i]) {
+
+			_pollset[i] = nullptr;
+			return OK;
+
+		}
+	}
+
+	puts("poll: bad fd state");
+	return -EINVAL;
+}
+
+static int
+cdev_open(struct file *filp)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->open(filp);
+}
+
+static int
+cdev_close(struct file *filp)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->close(filp);
+}
+
+static ssize_t
+cdev_read(struct file *filp, char *buffer, size_t buflen)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->read(filp, buffer, buflen);
+}
+
+static ssize_t
+cdev_write(struct file *filp, const char *buffer, size_t buflen)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->write(filp, buffer, buflen);
+}
+
+static off_t
+cdev_seek(struct file *filp, off_t offset, int whence)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->seek(filp, offset, whence);
+}
+
+static int
+cdev_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->ioctl(filp, cmd, arg);
+}
+
+static int
+cdev_poll(struct file *filp, struct pollfd *fds, bool setup)
+{
+	CDev *cdev = (CDev *)(filp->f_inode->i_private);
+
+	return cdev->poll(filp, fds, setup);
+}
+
+} // namespace device