Merged

.gitignore

@@ -8,9 +8,12 @@
 apps/namedapp/namedapp_list.h
 apps/namedapp/namedapp_proto.h
 Make.dep
+*.pyc
 *.o
 *.a
+*.d
 *~
+*.dSYM
 Images/*.bin
 Images/*.px4
 nuttx/Make.defs
@@ -41,3 +44,10 @@ cscope.out
 .configX-e
 nuttx-export.zip
 .~lock.*
+dot.gdbinit
+mavlink/include/mavlink/v0.9/
+.*.swp
+.swp
+core
+.gdbinit
+mkdeps

Debug/PX4

@@ -0,0 +1,54 @@
+#
+# Various PX4-specific macros
+#
+source Debug/NuttX
+
+echo Loading PX4 GDB macros.  Use 'help px4' for more information.\n
+
+define px4
+	echo Use 'help px4' for more information.\n
+end
+
+document px4
+. Various macros for working with the PX4 firmware.
+.
+.    perf
+.        Prints the state of all performance counters.
+.
+. Use 'help <macro>' for more specific help.
+end
+
+
+define _perf_print
+	set $hdr = (struct perf_ctr_header *)$arg0
+	printf "%p\n", $hdr
+	printf "%s: ", $hdr->name
+	# PC_COUNT
+	if $hdr->type == 0
+		set $count = (struct perf_ctr_count *)$hdr
+		printf "%llu events,\n", $count->event_count;
+	end
+	# PC_ELPASED
+	if $hdr->type == 1
+		set $elapsed = (struct perf_ctr_elapsed *)$hdr
+		printf "%llu events, %lluus elapsed, min %lluus, max %lluus\n", $elapsed->event_count, $elapsed->time_total, $elapsed->time_least, $elapsed->time_most
+	end
+	# PC_INTERVAL
+	if $hdr->type == 2
+		set $interval = (struct perf_ctr_interval *)$hdr
+		printf "%llu events, %llu avg, min %lluus max %lluus\n", $interval->event_count, ($interval->time_last - $interval->time_first) / $interval->event_count, $interval->time_least, $interval->time_most
+	end
+end
+
+define perf
+	set $ctr = (sq_entry_t *)(perf_counters.head)
+	while $ctr != 0
+		_perf_print $ctr
+		set $ctr = $ctr->flink
+	end
+end
+
+document perf
+.    perf
+.        Prints performance counters.
+end

Makefile

@@ -28,12 +28,8 @@ UPLOADER		 = $(PX4BASE)/Tools/px_uploader.py
 # What are we currently configured for?
 #
 CONFIGURED		 = $(PX4BASE)/.configured
-ifeq ($(wildcard $(CONFIGURED)),)
+ifneq ($(wildcard $(CONFIGURED)),)
-# the $(CONFIGURED) target will make this a reality before building
+export TARGET		:= $(shell cat $(CONFIGURED))
-export TARGET		 = px4fmu
-$(shell echo $(TARGET) > $(CONFIGURED))
-else
-export TARGET		 = $(shell cat $(CONFIGURED))
 endif
 
 #
@@ -59,12 +55,13 @@ $(FIRMWARE_BUNDLE):	$(FIRMWARE_BINARY) $(MKFW) $(FIRMWARE_PROTOTYPE)
 	@$(MKFW) --prototype $(FIRMWARE_PROTOTYPE) \
 		--git_identity $(PX4BASE) \
 		--image $(FIRMWARE_BINARY) > $@
+
 #
 # Build the firmware binary.
 #
 .PHONY:			$(FIRMWARE_BINARY)
-$(FIRMWARE_BINARY):	configure_$(TARGET) setup_$(TARGET)
+$(FIRMWARE_BINARY):	setup_$(TARGET) configure-check
-	@echo Building $@
+	@echo Building $@ for $(TARGET)
 	@make -C $(NUTTX_SRC) -r $(MQUIET) all
 	@cp $(NUTTX_SRC)/nuttx.bin $@
 
@@ -73,19 +70,26 @@ $(FIRMWARE_BINARY):	configure_$(TARGET) setup_$(TARGET)
 # and makes it current.
 #
 configure_px4fmu:
-ifneq ($(TARGET),px4fmu)
+	@echo Configuring for px4fmu
 	@make -C $(PX4BASE) distclean
-endif
 	@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4fmu/nsh
 	@echo px4fmu > $(CONFIGURED)
 
 configure_px4io:
-ifneq ($(TARGET),px4io)
+	@echo Configuring for px4io
 	@make -C $(PX4BASE) distclean
-endif
 	@cd $(NUTTX_SRC)/tools && /bin/sh configure.sh px4io/io
 	@echo px4io > $(CONFIGURED)
 
+configure-check:
+ifeq ($(wildcard $(CONFIGURED)),)
+	@echo
+	@echo "Not configured - use 'make configure_px4fmu' or 'make configure_px4io' first"
+	@echo
+	@exit 1
+endif
+
+
 #
 # Per-configuration additional targets
 #
@@ -96,6 +100,9 @@ setup_px4fmu:
 
 setup_px4io:
 
+# fake target to make configure-check happy if TARGET is not set
+setup_:
+
 #
 # Firmware uploading.
 #

ROMFS/logging/logconv.m

@@ -9,6 +9,13 @@ close all
 % Set the path to your sysvector.bin file here
 filePath = 'sysvector.bin';
 
+% Work around a Matlab bug (not related to PX4)
+% where timestamps from 1.1.1970 do not allow to
+% read the file's size
+if ismac
+    system('touch -t 201212121212.12 sysvector.bin');
+end
+
 %%%%%%%%%%%%%%%%%%%%%%%
 % SYSTEM VECTOR
 %
@@ -24,6 +31,8 @@ filePath = 'sysvector.bin';
 % float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
 % float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
 % float vbat; //battery voltage in [volt]
+% float bat_current - current drawn from battery at this time instant
+% float bat_discharged - discharged energy in mAh
 % float adc[3]; //remaining auxiliary ADC ports [volt]
 % float local_position[3]; //tangent plane mapping into x,y,z [m]
 % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
@@ -31,27 +40,34 @@ filePath = 'sysvector.bin';
 % float rotMatrix[9]; //unitvectors
 % float actuator_control[4]; //unitvector
 % float optical_flow[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+% float diff_pressure; - pressure difference in millibar
+% float ind_airspeed;
+% float true_airspeed;
 
 % Definition of the logged values
-logFormat{1} = struct('name', 'timestamp',         'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64'); 
+logFormat{1} = struct('name', 'timestamp',         'bytes', 8, 'array', 1, 'precision', 'uint64', 'machineformat', 'ieee-le.l64');
-logFormat{2} = struct('name', 'gyro',              'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{2} = struct('name', 'gyro',              'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{3} = struct('name', 'accel',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{3} = struct('name', 'accel',             'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{4} = struct('name', 'mag',               'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{4} = struct('name', 'mag',               'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{5} = struct('name', 'baro',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{5} = struct('name', 'baro',              'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{6} = struct('name', 'baro_alt',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{6} = struct('name', 'baro_alt',          'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{7} = struct('name', 'baro_temp',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{7} = struct('name', 'baro_temp',         'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
 logFormat{8} = struct('name', 'control',           'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{9} = struct('name', 'actuators',         'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{9} = struct('name', 'actuators',         'bytes', 4, 'array', 8, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{10} = struct('name', 'vbat',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{10} = struct('name', 'vbat',             'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{11} = struct('name', 'adc',              'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{11} = struct('name', 'bat_current',      'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{12} = struct('name', 'local_position',   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le'); 
+logFormat{12} = struct('name', 'bat_discharged',   'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{13} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le'); 
+logFormat{13} = struct('name', 'adc',              'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{14} = struct('name', 'attitude',         'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{14} = struct('name', 'local_position',   'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{15} = struct('name', 'rot_matrix',       'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32',  'machineformat', 'ieee-le');
-logFormat{16} = struct('name', 'vicon_position',   'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{16} = struct('name', 'attitude',         'bytes', 4, 'array', 3, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{17} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{17} = struct('name', 'rot_matrix',       'bytes', 4, 'array', 9, 'precision', 'float',   'machineformat', 'ieee-le');
-logFormat{18} = struct('name', 'optical_flow',     'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{18} = struct('name', 'vicon_position',   'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
-
+logFormat{19} = struct('name', 'actuator_control', 'bytes', 4, 'array', 4, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{20} = struct('name', 'optical_flow',     'bytes', 4, 'array', 6, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{21} = struct('name', 'diff_pressure',    'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{22} = struct('name', 'ind_airspeed',     'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
+logFormat{23} = struct('name', 'true_airspeed',    'bytes', 4, 'array', 1, 'precision', 'float',   'machineformat', 'ieee-le');
 
 % First get length of one line
 columns = length(logFormat);
@@ -96,5 +112,3 @@ if exist(filePath, 'file')
 else
     disp(['file: ' filePath ' does not exist' char(10)]);
 end
-
-

ROMFS/scripts/rc.PX4IO

@@ -1,12 +1,9 @@
 #!nsh
-#
-# Flight startup script for PX4FMU with PX4IO carrier board.
-#
 
-echo "[init] doing PX4IO startup..."
+set USB no
 
 #
-# Start the ORB
+# Start the object request broker
 #
 uorb start
 
@@ -20,15 +17,27 @@ then
 	param load
 fi
 
+#
+# Enable / connect to PX4IO
+#
+px4io start
+
+#
+# Load an appropriate mixer. FMU_pass.mix is a passthru mixer
+# which is good for testing. See ROMFS/mixers for a full list of mixers.
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_pass.mix
+
 #
 # Start the sensors.
 #
 sh /etc/init.d/rc.sensors
 
 #
-# Start MAVLink
+# Start MAVLink on UART1 (dev/ttyS0) at 57600 baud (CLI is now unusable)
 #
 mavlink start -d /dev/ttyS0 -b 57600
+usleep 5000
 
 #
 # Start the commander.
@@ -41,35 +50,24 @@ commander start
 attitude_estimator_ekf start
 
 #
-# Configure PX4FMU for operation with PX4IO
+# Start the attitude and position controller
 #
-# XXX arguments?
+fixedwing_att_control start
-#
+fixedwing_pos_control start
-px4fmu start
 
 #
-# Start the fixed-wing controller
+# Start GPS capture. Comment this out if you do not have a GPS.
-#
-fixedwing_control start
-
-#
-# Fire up the PX4IO interface.
-#
-px4io start
-
-#
-# Start looking for a GPS.
 #
 gps start
 
 #
 # 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).
+# use the same UART for telemetry
 #
-echo "[init] startup done, exiting."
+echo "[init] startup done"
 exit

ROMFS/scripts/rc.sensors

@@ -8,6 +8,7 @@
 #
 
 ms5611 start
+adc start
 
 if mpu6000 start
 then

ROMFS/scripts/rcS

@@ -46,8 +46,12 @@ if [ -f /fs/microsd/etc/rc ]
 then
 	echo "[init] reading /fs/microsd/etc/rc"
 	sh /fs/microsd/etc/rc
-else
+fi
-	echo "[init] script /fs/microsd/etc/rc not present"
+# Also consider rc.txt files
+if [ -f /fs/microsd/etc/rc.txt ]
+then
+	echo "[init] reading /fs/microsd/etc/rc.txt"
+	sh /fs/microsd/etc/rc.txt
 fi
 
 #

Tools/fix_code_style_ubuntu.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	\
+    --suffix=none		\
+    --lineend=linux		\
+    $*
+    #--ignore-exclude-errors-x	\
+    #--exclude=EASTL		\
+    #--align-reference=name	\

apps/ChangeLog.txt

@@ -33,7 +33,7 @@
 
 6.3 2011-05-15 Gregory Nutt <gnutt@nuttx.org>
 
-    * apps/interpreter: Add a directory to hold interpreters.  The Pascal add-
+	* 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/.
@@ -349,7 +349,7 @@
 	* apps/NxWidgets/Kconfig:  Add option to turn on the memory monitor
 	  feature of the NxWidgets/NxWM unit tests.
 
-6.23 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
+6.23 2012-11-05 Gregory Nutt <gnutt@nuttx.org>
 
 	* vsn: Moved all NSH commands from vsn/ to system/.  Deleted the vsn/
 	  directory.
@@ -368,3 +368,70 @@
 	  recent check-ins (Darcy Gong).
 	* apps/netutils/webclient/webclient.c:  Fix another but that I introduced
 	  when I was trying to add correct handling for loss of connection (Darcy Gong)
+	* apps/nshlib/nsh_telnetd.c:  Add support for login to Telnet session via
+	  username and password (Darcy Gong).
+	* apps/netutils/resolv/resolv.c (and files using the DNS resolver): Various
+	  DNS address resolution improvements from Darcy Gong.
+	* apps/nshlib/nsh_netcmds.c:  The ping command now passes a maximum round
+	  trip time to uip_icmpping().  This allows pinging of hosts on complex
+	  networks where the ICMP ECHO round trip time may exceed the ping interval.
+	* apps/examples/nxtext/nxtext_main.c:  Fix bad conditional compilation
+	  when CONFIG_NX_KBD is not defined.  Submitted by Petteri Aimonen.
+	* apps/examples/nximage/nximage_main.c:  Add a 5 second delay after the
+	  NX logo is presented so that there is time for the image to be verified.
+	  Suggested by Petteri Aimonen.
+	* apps/Makefile: Small change that reduces the number of shell invocations
+	  by one (Mike Smith).
+	* apps/examples/elf:  Test example for the ELF loader.
+	* apps/examples/elf:  The ELF module test example appears fully functional.
+	* apps/netutils/json:  Add a snapshot of the cJSON project.  Contributed by
+	  Darcy Gong.
+	* apps/examples/json:  Test example for cJSON from Darcy Gong
+	* apps/nshlib/nsh_netinit.c: Fix static IP DNS problem (Darcy Gong)
+	* apps/netutils/resolv/resolv.c: DNS fixes from Darcy Gong.
+	* COPYING: Licensing information added.
+	* apps/netutils/codec and include/netutils/urldecode.h, base64.h, and md5.h:
+	  A port of the BASE46, MD5 and URL CODEC library from Darcy Gong.
+	* nsnlib/nsh_codeccmd.c:  NSH commands to use the CODEC library.
+	  Contributed by Darcy Gong.
+	* apps/examples/wgetjson: Test example contributed by Darcy Gong
+	* apps/examples/cxxtest:  A test for the uClibc++ library provided by
+	  Qiang Yu and the RGMP team.
+	* apps/netutils/webclient, apps/netutils.codes, and apps/examples/wgetjson:
+	  Add support for wget POST interface.  Contributed by Darcy Gong.
+	* apps/examples/relays:  A relay example contributed by Darcy Gong.
+	* apps/nshlib/nsh_netcmds: Add ifup and ifdown commands (from Darcy
+	  Gong).
+	* apps/nshlib/nsh_netcmds: Extend the ifconfig command so that it
+	  supports setting IP addresses, network masks, name server addresses,
+	  and hardware address (from Darcy Gong).
+
+6.24 2012-12-20 Gregory Nutt <gnutt@nuttx.org>
+
+	* apps/examples/ostest/roundrobin.c:  Replace large tables with
+	  algorithmic prime number generation.  This allows the roundrobin
+	  test to run on platforms with minimal SRAM (Freddie Chopin).
+	* apps/nshlib/nsh_dbgcmds.c:  Add hexdump command to dump the contents
+	  of a file (or character device) to the console  Contributed by Petteri
+	  Aimonen.
+	* apps/examples/modbus:  Fixes from Freddie Chopin
+	* apps/examples/modbus/Kconfig: Kconfig logic for FreeModBus contributed
+	  by Freddie Chopin.
+	* Makefile, */Makefile:  Various fixes for Windows native build.  Now uses
+	  make foreach loops instead of shell loops.
+	* apps/examples/elf/test/*/Makefile: OSX doesn't support install -D, use
+	  mkdir -p then install without the -D.  From Mike Smith.
+	* apps/examples/relays/Makefile: Reduced stack requirement (Darcy Gong).
+	* apps/nshlib and apps/netutils/dhcpc:  Extend the NSH ifconfig command plus
+	  various DHCPC improvements(Darcy Gong).
+	* apps/nshlib/nsh_apps.c: Fix compilation errors when CONFIG_NSH_DISABLEBG=y.
+	  From Freddie Chopin.
+	* Rename CONFIG_PCODE and CONFIG_FICL as CONFIG_INTERPRETERS_PCODE and
+	  CONFIG_INTERPRETERS_FICL for consistency with other configuration naming.
+	* apps/examples/keypadtest:  A keypad test example contributed by Denis
+	  Carikli.
+	* apps/examples/elf and nxflat:  If CONFIG_BINFMT_EXEPATH is defined, these
+	  tests will now use a relative path to the program and expect the binfmt/
+	  logic to find the absolute path to the program using the PATH variable.
+
+6.25 2013-xx-xx Gregory Nutt <gnutt@nuttx.org>

apps/Makefile

@@ -107,18 +107,23 @@ endif
 # Create the list of available applications (INSTALLED_APPS)
 
 define ADD_BUILTIN
-INSTALLED_APPS += ${shell if [ -r $1/Makefile ]; then echo "$1"; fi}
+  INSTALLED_APPS += $(if $(wildcard $1$(DELIM)Makefile),$1,)
 endef
 
 $(foreach BUILTIN, $(CONFIGURED_APPS), $(eval $(call ADD_BUILTIN,$(BUILTIN))))
 
+# EXTERNAL_APPS is used to add out of tree apps to the build
+INSTALLED_APPS += $(EXTERNAL_APPS)
+
 # 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}
+EXTERNAL_DIR := $(dir $(wildcard external$(DELIM)Makefile))
-SUBDIRS += ${shell if [ -r external/Makefile ]; then echo "external"; fi}
+
+INSTALLED_APPS += $(EXTERNAL_DIR)
+SUBDIRS += $(EXTERNAL_DIR)
 
 # The final build target
 
@@ -130,48 +135,81 @@ all: $(BIN)
 .PHONY: $(INSTALLED_APPS) context depend clean distclean
 
 $(INSTALLED_APPS):
-	@$(MAKE) -C $@ TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)";
+	$(Q) $(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 \
+ifeq ($(CONFIG_WINDOWS_NATIVE),y)
+	$(Q) for %%G in ($(INSTALLED_APPS)) do ( \
+		if exist %%G\.context del /f /q %%G\.context \
+		$(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context \
+	)
+else
+	$(Q) for dir in $(INSTALLED_APPS) ; do \
 		rm -f $$dir/.context ; \
-		$(MAKE) -C $$dir TOPDIR="$(TOPDIR)"  APPDIR="$(APPDIR)" context ; \
+		$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" context ; \
 	done
-	@touch $@
+endif
+	$(Q) touch $@
 
 context: .context
 
 .depend: context Makefile $(SRCS)
-	@for dir in $(INSTALLED_APPS) ; do \
+ifeq ($(CONFIG_WINDOWS_NATIVE),y)
+	$(Q) for %%G in ($(INSTALLED_APPS)) do ( \
+		if exist %%G\.depend del /f /q %%G\.depend \
+		$(MAKE) -C %%G TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend \
+	)
+else
+	$(Q) for dir in $(INSTALLED_APPS) ; do \
 		rm -f $$dir/.depend ; \
-		$(MAKE) -C $$dir TOPDIR="$(TOPDIR)"  APPDIR="$(APPDIR)" depend ; \
+		$(MAKE) -C $$dir TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" depend ; \
 	done
-	@touch $@
+endif
+	$(Q) touch $@
 
 depend: .depend
 
 clean:
-	@for dir in $(SUBDIRS) ; do \
+ifeq ($(CONFIG_WINDOWS_NATIVE),y)
+	$(Q) for %%G in ($(SUBDIRS)) do ( \
+		$(MAKE) -C %%G clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \
+	)
+else
+	$(Q) for dir in $(SUBDIRS) ; do \
 		$(MAKE) -C $$dir clean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
 	done
-	@rm -f $(BIN) *~ .*.swp *.o
+endif
+	$(call DELFILE, $(BIN))
 	$(call CLEAN)
 
 distclean: # clean
-	@for dir in $(SUBDIRS) ; do \
+ifeq ($(CONFIG_WINDOWS_NATIVE),y)
+	$(Q) for %%G in ($(SUBDIRS)) do ( \
+		$(MAKE) -C %%G distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)" \
+	)
+	$(call DELFILE, .config)
+	$(call DELFILE, .context)
+	$(call DELFILE, .depend)
+	$(Q) ( if exist  external ( \
+		echo ********************************************************" \
+		echo * The external directory/link must be removed manually *" \
+		echo ********************************************************" \
+	)
+else
+	$(Q) for dir in $(SUBDIRS) ; do \
 		$(MAKE) -C $$dir distclean TOPDIR="$(TOPDIR)" APPDIR="$(APPDIR)"; \
 	done
-	@rm -f .config .context .depend
+	$(call DELFILE, .config)
-	@( if [ -e external ]; then \
+	$(call DELFILE, .context)
+	$(call DELFILE, .depend)
+	$(Q) ( if [ -e external ]; then \
 		echo "********************************************************"; \
 		echo "* The external directory/link must be removed manually *"; \
 		echo "********************************************************"; \
 	   fi; \
 	) 
+endif
 
 

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -35,7 +35,7 @@
 
 /*
  * @file attitude_estimator_ekf_main.c
- * 
+ *
  * Extended Kalman Filter for Attitude Estimation.
  */
 
@@ -79,18 +79,18 @@ static float dt = 0.005f;
 static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
 static float x_aposteriori_k[12];		/**< states */
 static float P_aposteriori_k[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, 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, 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, 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,  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, 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, 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, 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, 100.f,   0,   0,   0,
-		0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
+				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
-		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
+				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
-		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
+				     0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
-}; /**< init: diagonal matrix with big values */
+				    }; /**< init: diagonal matrix with big values */
 
 static float x_aposteriori[12];
 static float P_aposteriori[144];
@@ -99,9 +99,9 @@ static float P_aposteriori[144];
 static float euler[3] = {0.0f, 0.0f, 0.0f};
 
 static float Rot_matrix[9] = {1.f,  0,  0,
-		0,  1.f,  0,
+			      0,  1.f,  0,
-		0,  0,  1.f
+			      0,  0,  1.f
-};		/**< init: identity matrix */
+			     };		/**< init: identity matrix */
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -123,6 +123,7 @@ usage(const char *reason)
 {
 	if (reason)
 		fprintf(stderr, "%s\n", reason);
+
 	fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
 }
@@ -131,7 +132,7 @@ usage(const char *reason)
  * The attitude_estimator_ekf app only briefly exists to start
  * the background job. The stack size assigned in the
  * Makefile does only apply to this management task.
- * 
+ *
  * The actual stack size should be set in the call
  * to task_create().
  */
@@ -150,11 +151,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
-				SCHED_DEFAULT,
+					      SCHED_DEFAULT,
-				SCHED_PRIORITY_MAX - 5,
+					      SCHED_PRIORITY_MAX - 5,
-				12000,
+					      12000,
-				attitude_estimator_ekf_thread_main,
+					      attitude_estimator_ekf_thread_main,
-				(argv) ? (const char **)&argv[2] : (const char **)NULL);
+					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
 	}
 
@@ -166,9 +167,11 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			printf("\tattitude_estimator_ekf app is running\n");
+
 		} else {
 			printf("\tattitude_estimator_ekf app not started\n");
 		}
+
 		exit(0);
 	}
 
@@ -235,7 +238,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* advertise debug value */
 	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	// orb_advert_t pub_dbg = -1;
-	
+
 	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 	// XXX write this out to perf regs
 
@@ -263,8 +266,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	while (!thread_should_exit) {
 
 		struct pollfd fds[2] = {
-				{ .fd = sub_raw,   .events = POLLIN },
+			{ .fd = sub_raw,   .events = POLLIN },
-				{ .fd = sub_params, .events = POLLIN }
+			{ .fd = sub_params, .events = POLLIN }
 		};
 		int ret = poll(fds, 2, 1000);
 
@@ -273,10 +276,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 		} else if (ret == 0) {
 			/* check if we're in HIL - not getting sensor data is fine then */
 			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+
 			if (!state.flag_hil_enabled) {
-				fprintf(stderr, 
+				fprintf(stderr,
 					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
 			}
+
 		} else {
 
 			/* only update parameters if they changed */
@@ -308,6 +313,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						gyro_offsets[1] /= offset_count;
 						gyro_offsets[2] /= offset_count;
 					}
+
 				} else {
 
 					perf_begin(ekf_loop_perf);
@@ -336,6 +342,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
 						sensor_last_timestamp[1] = raw.timestamp;
 					}
+
 					z_k[3] = raw.accelerometer_m_s2[0];
 					z_k[4] = raw.accelerometer_m_s2[1];
 					z_k[5] = raw.accelerometer_m_s2[2];
@@ -347,6 +354,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
 						sensor_last_timestamp[2] = raw.timestamp;
 					}
+
 					z_k[6] = raw.magnetometer_ga[0];
 					z_k[7] = raw.magnetometer_ga[1];
 					z_k[8] = raw.magnetometer_ga[2];
@@ -368,8 +376,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-					if (!const_initialized && dt < 0.05f && dt > 0.005f)
+					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
-					{
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
@@ -395,13 +402,15 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					}
 
 					uint64_t timing_start = hrt_absolute_time();
-					
+
 					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
-							euler, Rot_matrix, x_aposteriori, P_aposteriori);
+							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
+
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
 						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
 						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
+
 					} else {
 						/* due to inputs or numerical failure the output is invalid, skip it */
 						continue;
@@ -413,9 +422,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 					/* send out */
 					att.timestamp = raw.timestamp;
-					att.roll = euler[0];
+
-					att.pitch = euler[1];
+					// XXX Apply the same transformation to the rotation matrix
-					att.yaw = euler[2];
+					att.roll = euler[0] - ekf_params.roll_off;
+					att.pitch = euler[1] - ekf_params.pitch_off;
+					att.yaw = euler[2] - ekf_params.yaw_off;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
@@ -431,6 +442,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
 						// Broadcast
 						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+
 					} else {
 						warnx("NaN in roll/pitch/yaw estimate!");
 					}

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -35,7 +35,7 @@
 
 /*
  * @file attitude_estimator_ekf_params.c
- * 
+ *
  * Parameters for EKF filter
  */
 
@@ -65,13 +65,17 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
 /* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
 /* magnetometer measurement noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
+/* offsets in roll, pitch and yaw of sensor plane and body */
+PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
@@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r7 	=	param_find("EKF_ATT_R7");
 	h->r8 	=	param_find("EKF_ATT_R8");
 
+	h->roll_off  =	param_find("ATT_ROLL_OFFS");
+	h->pitch_off =	param_find("ATT_PITCH_OFFS");
+	h->yaw_off   =	param_find("ATT_YAW_OFFS");
+
 	return OK;
 }
 
@@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r7, &(p->r[7]));
 	param_get(h->r8, &(p->r[8]));
 
+	param_get(h->roll_off, &(p->roll_off));
+	param_get(h->pitch_off, &(p->pitch_off));
+	param_get(h->yaw_off, &(p->yaw_off));
+
 	return OK;
 }

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h

@@ -35,7 +35,7 @@
 
 /*
  * @file attitude_estimator_ekf_params.h
- * 
+ *
  * Parameters for EKF filter
  */
 
@@ -44,11 +44,15 @@
 struct attitude_estimator_ekf_params {
 	float r[9];
 	float q[12];
+	float roll_off;
+	float pitch_off;
+	float yaw_off;
 };
 
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
 	param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
+	param_t roll_off, pitch_off, yaw_off;
 };
 
 /**

apps/commander/calibration_routines.c

@@ -45,172 +45,175 @@
 
 
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
-    unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) {
+			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius)
+{
 
-    float x_sumplain = 0.0f;
+	float x_sumplain = 0.0f;
-    float x_sumsq = 0.0f;
+	float x_sumsq = 0.0f;
-    float x_sumcube = 0.0f;
+	float x_sumcube = 0.0f;
 
-    float y_sumplain = 0.0f;
+	float y_sumplain = 0.0f;
-    float y_sumsq = 0.0f;
+	float y_sumsq = 0.0f;
-    float y_sumcube = 0.0f;
+	float y_sumcube = 0.0f;
 
-    float z_sumplain = 0.0f;
+	float z_sumplain = 0.0f;
-    float z_sumsq = 0.0f;
+	float z_sumsq = 0.0f;
-    float z_sumcube = 0.0f;
+	float z_sumcube = 0.0f;
 
-    float xy_sum = 0.0f;
+	float xy_sum = 0.0f;
-    float xz_sum = 0.0f;
+	float xz_sum = 0.0f;
-    float yz_sum = 0.0f;
+	float yz_sum = 0.0f;
 
-    float x2y_sum = 0.0f;
+	float x2y_sum = 0.0f;
-    float x2z_sum = 0.0f;
+	float x2z_sum = 0.0f;
-    float y2x_sum = 0.0f;
+	float y2x_sum = 0.0f;
-    float y2z_sum = 0.0f;
+	float y2z_sum = 0.0f;
-    float z2x_sum = 0.0f;
+	float z2x_sum = 0.0f;
-    float z2y_sum = 0.0f;
+	float z2y_sum = 0.0f;
 
-    for (unsigned int i = 0; i < size; i++) {
+	for (unsigned int i = 0; i < size; i++) {
 
-        float x2 = x[i] * x[i];
+		float x2 = x[i] * x[i];
-        float y2 = y[i] * y[i];
+		float y2 = y[i] * y[i];
-        float z2 = z[i] * z[i];
+		float z2 = z[i] * z[i];
 
-        x_sumplain += x[i];
+		x_sumplain += x[i];
-        x_sumsq += x2;
+		x_sumsq += x2;
-        x_sumcube += x2 * x[i];
+		x_sumcube += x2 * x[i];
 
-        y_sumplain += y[i];
+		y_sumplain += y[i];
-        y_sumsq += y2;
+		y_sumsq += y2;
-        y_sumcube += y2 * y[i];
+		y_sumcube += y2 * y[i];
 
-        z_sumplain += z[i];
+		z_sumplain += z[i];
-        z_sumsq += z2;
+		z_sumsq += z2;
-        z_sumcube += z2 * z[i];
+		z_sumcube += z2 * z[i];
 
-        xy_sum += x[i] * y[i];
+		xy_sum += x[i] * y[i];
-        xz_sum += x[i] * z[i];
+		xz_sum += x[i] * z[i];
-        yz_sum += y[i] * z[i];
+		yz_sum += y[i] * z[i];
 
-        x2y_sum += x2 * y[i];
+		x2y_sum += x2 * y[i];
-        x2z_sum += x2 * z[i];
+		x2z_sum += x2 * z[i];
 
-        y2x_sum += y2 * x[i];
+		y2x_sum += y2 * x[i];
-        y2z_sum += y2 * z[i];
+		y2z_sum += y2 * z[i];
 
-        z2x_sum += z2 * x[i];
+		z2x_sum += z2 * x[i];
-        z2y_sum += z2 * y[i];
+		z2y_sum += z2 * y[i];
-    }
+	}
 
-    //
+	//
-    //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
+	//Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
-    //
+	//
-    //    P is a structure that has been computed with the data earlier.
+	//    P is a structure that has been computed with the data earlier.
-    //    P.npoints is the number of elements; the length of X,Y,Z are identical.
+	//    P.npoints is the number of elements; the length of X,Y,Z are identical.
-    //    P's members are logically named.
+	//    P's members are logically named.
-    //
+	//
-    //    X[n] is the x component of point n
+	//    X[n] is the x component of point n
-    //    Y[n] is the y component of point n
+	//    Y[n] is the y component of point n
-    //    Z[n] is the z component of point n
+	//    Z[n] is the z component of point n
-    //
+	//
-    //    A is the x coordiante of the sphere
+	//    A is the x coordiante of the sphere
-    //    B is the y coordiante of the sphere
+	//    B is the y coordiante of the sphere
-    //    C is the z coordiante of the sphere
+	//    C is the z coordiante of the sphere
-    //    Rsq is the radius squared of the sphere.
+	//    Rsq is the radius squared of the sphere.
-    //
+	//
-    //This method should converge; maybe 5-100 iterations or more.
+	//This method should converge; maybe 5-100 iterations or more.
-    //
+	//
-    float x_sum = x_sumplain / size;        //sum( X[n] )
+	float x_sum = x_sumplain / size;        //sum( X[n] )
-    float x_sum2 = x_sumsq / size;    //sum( X[n]^2 )
+	float x_sum2 = x_sumsq / size;    //sum( X[n]^2 )
-    float x_sum3 = x_sumcube / size;    //sum( X[n]^3 )
+	float x_sum3 = x_sumcube / size;    //sum( X[n]^3 )
-    float y_sum = y_sumplain / size;        //sum( Y[n] )
+	float y_sum = y_sumplain / size;        //sum( Y[n] )
-    float y_sum2 = y_sumsq / size;    //sum( Y[n]^2 )
+	float y_sum2 = y_sumsq / size;    //sum( Y[n]^2 )
-    float y_sum3 = y_sumcube / size;    //sum( Y[n]^3 )
+	float y_sum3 = y_sumcube / size;    //sum( Y[n]^3 )
-    float z_sum = z_sumplain / size;        //sum( Z[n] )
+	float z_sum = z_sumplain / size;        //sum( Z[n] )
-    float z_sum2 = z_sumsq / size;    //sum( Z[n]^2 )
+	float z_sum2 = z_sumsq / size;    //sum( Z[n]^2 )
-    float z_sum3 = z_sumcube / size;    //sum( Z[n]^3 )
+	float z_sum3 = z_sumcube / size;    //sum( Z[n]^3 )
 
-    float XY = xy_sum / size;        //sum( X[n] * Y[n] )
+	float XY = xy_sum / size;        //sum( X[n] * Y[n] )
-    float XZ = xz_sum / size;        //sum( X[n] * Z[n] )
+	float XZ = xz_sum / size;        //sum( X[n] * Z[n] )
-    float YZ = yz_sum / size;        //sum( Y[n] * Z[n] )
+	float YZ = yz_sum / size;        //sum( Y[n] * Z[n] )
-    float X2Y = x2y_sum / size;    //sum( X[n]^2 * Y[n] )
+	float X2Y = x2y_sum / size;    //sum( X[n]^2 * Y[n] )
-    float X2Z = x2z_sum / size;    //sum( X[n]^2 * Z[n] )
+	float X2Z = x2z_sum / size;    //sum( X[n]^2 * Z[n] )
-    float Y2X = y2x_sum / size;    //sum( Y[n]^2 * X[n] )
+	float Y2X = y2x_sum / size;    //sum( Y[n]^2 * X[n] )
-    float Y2Z = y2z_sum / size;    //sum( Y[n]^2 * Z[n] )
+	float Y2Z = y2z_sum / size;    //sum( Y[n]^2 * Z[n] )
-    float Z2X = z2x_sum / size;    //sum( Z[n]^2 * X[n] )
+	float Z2X = z2x_sum / size;    //sum( Z[n]^2 * X[n] )
-    float Z2Y = z2y_sum / size;    //sum( Z[n]^2 * Y[n] )
+	float Z2Y = z2y_sum / size;    //sum( Z[n]^2 * Y[n] )
 
-    //Reduction of multiplications
+	//Reduction of multiplications
-    float F0 = x_sum2 + y_sum2 + z_sum2;
+	float F0 = x_sum2 + y_sum2 + z_sum2;
-    float F1 =  0.5f * F0;
+	float F1 =  0.5f * F0;
-    float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
+	float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
-    float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
+	float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
-    float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
+	float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
 
-    //Set initial conditions:
+	//Set initial conditions:
-    float A = x_sum;
+	float A = x_sum;
-    float B = y_sum;
+	float B = y_sum;
-    float C = z_sum;
+	float C = z_sum;
 
-    //First iteration computation:
+	//First iteration computation:
-    float A2 = A*A;
+	float A2 = A * A;
-    float B2 = B*B;
+	float B2 = B * B;
-    float C2 = C*C;
+	float C2 = C * C;
-    float QS = A2 + B2 + C2;
+	float QS = A2 + B2 + C2;
-    float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
+	float QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
 
-    //Set initial conditions:
+	//Set initial conditions:
-    float Rsq = F0 + QB + QS;
+	float Rsq = F0 + QB + QS;
 
-    //First iteration computation:
+	//First iteration computation:
-    float Q0 = 0.5f * (QS - Rsq);
+	float Q0 = 0.5f * (QS - Rsq);
-    float Q1 = F1 + Q0;
+	float Q1 = F1 + Q0;
-    float Q2 = 8.0f * ( QS - Rsq + QB + F0 );
+	float Q2 = 8.0f * (QS - Rsq + QB + F0);
-    float aA,aB,aC,nA,nB,nC,dA,dB,dC;
+	float aA, aB, aC, nA, nB, nC, dA, dB, dC;
 
-    //Iterate N times, ignore stop condition.
+	//Iterate N times, ignore stop condition.
-    int n = 0;
+	int n = 0;
-    while( n < max_iterations ){
-        n++;
 
-        //Compute denominator:
+	while (n < max_iterations) {
-        aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2);
+		n++;
-        aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2);
-        aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2);
-        aA = (aA == 0.0f) ? 1.0f : aA;
-        aB = (aB == 0.0f) ? 1.0f : aB;
-        aC = (aC == 0.0f) ? 1.0f : aC;
 
-        //Compute next iteration
+		//Compute denominator:
-        nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA);
+		aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2);
-        nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB);
+		aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2);
-        nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC);
+		aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2);
+		aA = (aA == 0.0f) ? 1.0f : aA;
+		aB = (aB == 0.0f) ? 1.0f : aB;
+		aC = (aC == 0.0f) ? 1.0f : aC;
 
-        //Check for stop condition
+		//Compute next iteration
-        dA = (nA - A);
+		nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA);
-        dB = (nB - B);
+		nB = B - ((F3 + 16.0f * (A * XY + C * YZ + y_sum * (-B2 - Q0) + B * (y_sum2 + Q1 - A * x_sum - C * z_sum))) / aB);
-        dC = (nC - C);
+		nC = C - ((F4 + 16.0f * (A * XZ + B * YZ + z_sum * (-C2 - Q0) + C * (z_sum2 + Q1 - A * x_sum - B * y_sum))) / aC);
-        if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; }
 
-        //Compute next iteration's values
+		//Check for stop condition
-        A = nA;
+		dA = (nA - A);
-        B = nB;
+		dB = (nB - B);
-        C = nC;
+		dC = (nC - C);
-        A2 = A*A;
-        B2 = B*B;
-        C2 = C*C;
-        QS = A2 + B2 + C2;
-        QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
-        Rsq = F0 + QB + QS;
-        Q0 = 0.5f * (QS - Rsq);
-        Q1 = F1 + Q0;
-        Q2 = 8.0f * ( QS - Rsq + QB + F0 );
-    }
 
-    *sphere_x = A;
+		if ((dA * dA + dB * dB + dC * dC) <= delta) { break; }
-    *sphere_y = B;
-    *sphere_z = C;
-    *sphere_radius = sqrtf(Rsq);
 
-    return 0;
+		//Compute next iteration's values
+		A = nA;
+		B = nB;
+		C = nC;
+		A2 = A * A;
+		B2 = B * B;
+		C2 = C * C;
+		QS = A2 + B2 + C2;
+		QB = -2.0f * (A * x_sum + B * y_sum + C * z_sum);
+		Rsq = F0 + QB + QS;
+		Q0 = 0.5f * (QS - Rsq);
+		Q1 = F1 + Q0;
+		Q2 = 8.0f * (QS - Rsq + QB + F0);
+	}
+
+	*sphere_x = A;
+	*sphere_y = B;
+	*sphere_z = C;
+	*sphere_radius = sqrtf(Rsq);
+
+	return 0;
 }

apps/commander/calibration_routines.h

@@ -58,4 +58,4 @@
  * @return 0 on success, 1 on failure
  */
 int sphere_fit_least_squares(const float x[], const float y[], const float z[],
-    unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);
+			     unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);

apps/commander/commander.h

@@ -52,4 +52,7 @@
 #define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
 #define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
 
+void tune_confirm(void);
+void tune_error(void);
+
 #endif /* COMMANDER_H_ */

apps/commander/state_machine_helper.c

@@ -50,7 +50,7 @@
 
 #include "state_machine_helper.h"
 
-static const char* system_state_txt[] = {
+static const char *system_state_txt[] = {
 	"SYSTEM_STATE_PREFLIGHT",
 	"SYSTEM_STATE_STANDBY",
 	"SYSTEM_STATE_GROUND_READY",
@@ -79,7 +79,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 	case SYSTEM_STATE_MISSION_ABORT: {
 			/* Indoor or outdoor */
 			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
-				ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
+			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
 
 			// } else {
 			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
@@ -93,8 +93,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		fprintf(stderr, "[commander] EMERGENCY LANDING!\n");
+		warnx("EMERGENCY LANDING!\n");
-		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY LANDING!");
+		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
 		break;
 
 	case SYSTEM_STATE_EMCY_CUTOFF:
@@ -103,8 +103,8 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = false;
 
-		fprintf(stderr, "[commander] EMERGENCY MOTOR CUTOFF!\n");
+		warnx("EMERGENCY MOTOR CUTOFF!\n");
-		mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY MOTOR CUTOFF!");
+		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
 		break;
 
 	case SYSTEM_STATE_GROUND_ERROR:
@@ -114,36 +114,41 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* prevent actuators from arming */
 		current_status->flag_system_armed = false;
 
-		fprintf(stderr, "[commander] GROUND ERROR, locking down propulsion system\n");
+		warnx("GROUND ERROR, locking down propulsion system\n");
-		mavlink_log_critical(mavlink_fd, "[commander] GROUND ERROR, locking down propulsion system");
+		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
 		break;
 
 	case SYSTEM_STATE_PREFLIGHT:
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "[commander] Switched to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
+
 		} else {
 			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to switch to PREFLIGHT state");
+			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
 		}
+
 		break;
 
 	case SYSTEM_STATE_REBOOT:
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY
-		 || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
+			|| current_status->flag_hil_enabled) {
 			invalid_state = false;
 			/* set system flags according to state */
 			current_status->flag_system_armed = false;
-			mavlink_log_critical(mavlink_fd, "[commander] REBOOTING SYSTEM");
+			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
 			usleep(500000);
 			up_systemreset();
 			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+
 		} else {
 			invalid_state = true;
-			mavlink_log_critical(mavlink_fd, "[commander] REFUSED to REBOOT");
+			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
 		}
+
 		break;
 
 	case SYSTEM_STATE_STANDBY:
@@ -152,7 +157,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* standby enforces disarmed */
 		current_status->flag_system_armed = false;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to STANDBY state");
+		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
 		break;
 
 	case SYSTEM_STATE_GROUND_READY:
@@ -162,7 +167,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* ground ready has motors / actuators armed */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to GROUND READY state");
+		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
 		break;
 
 	case SYSTEM_STATE_AUTO:
@@ -172,7 +177,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* auto is airborne and in auto mode, motors armed */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / AUTO mode");
+		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
 		break;
 
 	case SYSTEM_STATE_STABILIZED:
@@ -180,7 +185,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / STABILIZED mode");
+		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
 		break;
 
 	case SYSTEM_STATE_MANUAL:
@@ -188,7 +193,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		/* set system flags according to state */
 		current_status->flag_system_armed = true;
 
-		mavlink_log_critical(mavlink_fd, "[commander] Switched to FLYING / MANUAL mode");
+		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
 		break;
 
 	default:
@@ -202,14 +207,17 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con
 		publish_armed_status(current_status);
 		ret = OK;
 	}
+
 	if (invalid_state) {
-		mavlink_log_critical(mavlink_fd, "[commander] REJECTING invalid state transition");
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
 		ret = ERROR;
 	}
+
 	return ret;
 }
 
-void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) {
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
 	/* publish the new state */
 	current_status->counter++;
 	current_status->timestamp = hrt_absolute_time();
@@ -217,9 +225,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	/* assemble state vector based on flag values */
 	if (current_status->flag_control_rates_enabled) {
 		current_status->onboard_control_sensors_present |= 0x400;
+
 	} else {
 		current_status->onboard_control_sensors_present &= ~0x400;
 	}
+
 	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
 	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
 	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
@@ -232,10 +242,11 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
 	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
 
 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]);
+	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
 }
 
-void publish_armed_status(const struct vehicle_status_s *current_status) {
+void publish_armed_status(const struct vehicle_status_s *current_status)
+{
 	struct actuator_armed_s armed;
 	armed.armed = current_status->flag_system_armed;
 	/* lock down actuators if required, only in HIL */
@@ -250,19 +261,19 @@ void publish_armed_status(const struct vehicle_status_s *current_status) {
  */
 void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	fprintf(stderr, "[commander] EMERGENCY HANDLER\n");
+	warnx("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, mavlink_fd, (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 NOT abort mission
 		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
 
 	} else {
-		fprintf(stderr, "[commander] Unknown system state: #%d\n", current_status->state_machine);
+		warnx("Unknown system state: #%d\n", current_status->state_machine);
 	}
 }
 
@@ -272,7 +283,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st
 		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
 
 	} 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);
+		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
 	}
 
 }
@@ -497,7 +508,7 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_
 void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
-		printf("[commander] arming\n");
+		printf("[cmd] arming\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 	}
 }
@@ -505,11 +516,11 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s
 void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
 	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");
+		printf("[cmd] going standby\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (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");
+		printf("[cmd] MISSION ABORT!\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 	}
 }
@@ -518,54 +529,139 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 {
 	int old_mode = current_status->flight_mode;
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
+
 	current_status->flag_control_manual_enabled = true;
-	/* enable attitude control per default */
+
-	current_status->flag_control_attitude_enabled = true;
+	/* set behaviour based on airframe */
-	current_status->flag_control_rates_enabled = true;
+	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
+
+		/* assuming a rotary wing, set to SAS */
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+
+	} else {
+
+		/* assuming a fixed wing, set to direct pass-through */
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+		current_status->flag_control_attitude_enabled = false;
+		current_status->flag_control_rates_enabled = false;
+	}
+
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	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");
+		printf("[cmd] manual mode\n");
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
 	}
 }
 
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	int old_mode = current_status->flight_mode;
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
+		int old_mode = current_status->flight_mode;
-	current_status->flag_control_manual_enabled = true;
+		int old_manual_control_mode = current_status->manual_control_mode;
-	current_status->flag_control_attitude_enabled = true;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
-	current_status->flag_control_rates_enabled = true;
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+		current_status->flag_control_manual_enabled = true;
+
+		if (old_mode != current_status->flight_mode ||
+		    old_manual_control_mode != current_status->manual_control_mode) {
+			printf("[cmd] att stabilized mode\n");
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+			state_machine_publish(status_pub, current_status, mavlink_fd);
+		}
+
+	}
+}
+
+void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
+		tune_error();
+		return;
+	}
 
 	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");
+		printf("[cmd] position guided mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+
 	}
 }
 
 void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
 {
-	int old_mode = current_status->flight_mode;
+	if (!current_status->flag_vector_flight_mode_ok) {
-	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
-	current_status->flag_control_manual_enabled = true;
+		return;
-	current_status->flag_control_attitude_enabled = true;
+	}
-	current_status->flag_control_rates_enabled = true;
-	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	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");
+		printf("[cmd] auto mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
 		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 	}
 }
 
 
 uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
 {
-	printf("[commander] Requested new mode: %d\n", (int)mode);
 	uint8_t ret = 1;
 
+	/* Switch on HIL if in standby and not already in HIL mode */
+	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+	    && !current_status->flag_hil_enabled) {
+		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+			/* Enable HIL on request */
+			current_status->flag_hil_enabled = true;
+			ret = OK;
+			state_machine_publish(status_pub, current_status, mavlink_fd);
+			publish_armed_status(current_status);
+			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+
+		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+			   current_status->flag_system_armed) {
+
+			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+
+		} else {
+
+			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+		}
+	}
+
+	/* switch manual / auto */
+	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+
+	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+
+	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+
+	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+	}
+
 	/* vehicle is disarmed, mode requests arming */
 	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
 		/* only arm in standby state */
@@ -573,7 +669,7 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
 			ret = OK;
-			printf("[commander] arming due to command request\n");
+			printf("[cmd] arming due to command request\n");
 		}
 	}
 
@@ -583,26 +679,14 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
 			ret = OK;
-			printf("[commander] disarming due to command request\n");
+			printf("[cmd] disarming due to command request\n");
 		}
 	}
 
-	/* Switch on HIL if in standby and not already in HIL mode */
-	if ((current_status->state_machine == SYSTEM_STATE_STANDBY) && (mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
-		&& !current_status->flag_hil_enabled) {
-		/* Enable HIL on request */
-		current_status->flag_hil_enabled = true;
-		ret = OK;
-		state_machine_publish(status_pub, current_status, mavlink_fd);
-		publish_armed_status(current_status);
-		printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
-	} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
-		mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
-	}
-
 	/* NEVER actually switch off HIL without reboot */
 	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
-		fprintf(stderr, "[commander] DENYING request to switch of HIL. Please power cycle (safety reasons)\n");
+		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
 		ret = ERROR;
 	}
 
@@ -625,9 +709,12 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_
 	case SYSTEM_STATE_REBOOT:
 		printf("try to reboot\n");
 
-		if (current_system_state == SYSTEM_STATE_STANDBY || current_system_state == SYSTEM_STATE_PREFLIGHT) {
+		if (current_system_state == SYSTEM_STATE_STANDBY
+				|| current_system_state == SYSTEM_STATE_PREFLIGHT
+				|| current_status->flag_hil_enabled) {
 			printf("system will reboot\n");
-			//global_data_send_mavlink_statustext_message_out("Rebooting autopilot.. ", MAV_SEVERITY_INFO);
+			mavlink_log_critical(mavlink_fd, "Rebooting..");
+			usleep(200000);
 			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
 			ret = 0;
 		}

apps/commander/state_machine_helper.h

@@ -127,6 +127,15 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
  */
 void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
 
+/**
+ * Handle state machine if mode switch is guided
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
 /**
  * Handle state machine if mode switch is auto
  *

apps/controllib/Makefile

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Control library
+#
+CSRCS		 = test_params.c
+
+CXXSRCS		 = block/Block.cpp \
+		   block/BlockParam.cpp \
+		   block/UOrbPublication.cpp \
+		   block/UOrbSubscription.cpp \
+		   blocks.cpp \
+		   fixedwing.cpp
+
+include $(APPDIR)/mk/app.mk

apps/controllib/block/Block.cpp

@@ -0,0 +1,210 @@
+/****************************************************************************
+ *
+ *   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 Block.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <string.h>
+#include <stdio.h>
+
+#include "Block.hpp"
+#include "BlockParam.hpp"
+#include "UOrbSubscription.hpp"
+#include "UOrbPublication.hpp"
+
+namespace control
+{
+
+Block::Block(SuperBlock *parent, const char *name) :
+	_name(name),
+	_parent(parent),
+	_dt(0),
+	_subscriptions(),
+	_params()
+{
+	if (getParent() != NULL) {
+		getParent()->getChildren().add(this);
+	}
+}
+
+void Block::getName(char *buf, size_t n)
+{
+	if (getParent() == NULL) {
+		strncpy(buf, _name, n);
+
+	} else {
+		char parentName[blockNameLengthMax];
+		getParent()->getName(parentName, n);
+
+		if (!strcmp(_name, "")) {
+			strncpy(buf, parentName, blockNameLengthMax);
+
+		} else {
+			snprintf(buf, blockNameLengthMax, "%s_%s", parentName, _name);
+		}
+	}
+}
+
+void Block::updateParams()
+{
+	BlockParamBase *param = getParams().getHead();
+	int count = 0;
+
+	while (param != NULL) {
+		if (count++ > maxParamsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max params for block: %s\n", name);
+			break;
+		}
+
+		//printf("updating param: %s\n", param->getName());
+		param->update();
+		param = param->getSibling();
+	}
+}
+
+void Block::updateSubscriptions()
+{
+	UOrbSubscriptionBase *sub = getSubscriptions().getHead();
+	int count = 0;
+
+	while (sub != NULL) {
+		if (count++ > maxSubscriptionsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max subscriptions for block: %s\n", name);
+			break;
+		}
+
+		sub->update();
+		sub = sub->getSibling();
+	}
+}
+
+void Block::updatePublications()
+{
+	UOrbPublicationBase *pub = getPublications().getHead();
+	int count = 0;
+
+	while (pub != NULL) {
+		if (count++ > maxPublicationsPerBlock) {
+			char name[blockNameLengthMax];
+			getName(name, blockNameLengthMax);
+			printf("exceeded max publications for block: %s\n", name);
+			break;
+		}
+
+		pub->update();
+		pub = pub->getSibling();
+	}
+}
+
+void SuperBlock::setDt(float dt)
+{
+	Block::setDt(dt);
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->setDt(dt);
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildParams()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updateParams();
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildSubscriptions()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updateSubscriptions();
+		child = child->getSibling();
+	}
+}
+
+void SuperBlock::updateChildPublications()
+{
+	Block *child = getChildren().getHead();
+	int count = 0;
+
+	while (child != NULL) {
+		if (count++ > maxChildrenPerBlock) {
+			char name[40];
+			getName(name, 40);
+			printf("exceeded max children for block: %s\n", name);
+			break;
+		}
+
+		child->updatePublications();
+		child = child->getSibling();
+	}
+}
+
+} // namespace control

apps/controllib/block/Block.hpp

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   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 Block.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <inttypes.h>
+
+#include "List.hpp"
+
+namespace control
+{
+
+static const uint16_t maxChildrenPerBlock = 100;
+static const uint16_t maxParamsPerBlock = 100;
+static const uint16_t maxSubscriptionsPerBlock = 100;
+static const uint16_t maxPublicationsPerBlock = 100;
+static const uint8_t blockNameLengthMax = 80;
+
+// forward declaration
+class BlockParamBase;
+class UOrbSubscriptionBase;
+class UOrbPublicationBase;
+class SuperBlock;
+
+/**
+ */
+class __EXPORT Block :
+	public ListNode<Block *>
+{
+public:
+	friend class BlockParamBase;
+// methods
+	Block(SuperBlock *parent, const char *name);
+	void getName(char *name, size_t n);
+	virtual ~Block() {};
+	virtual void updateParams();
+	virtual void updateSubscriptions();
+	virtual void updatePublications();
+	virtual void setDt(float dt) { _dt = dt; }
+// accessors
+	float getDt() { return _dt; }
+protected:
+// accessors
+	SuperBlock *getParent() { return _parent; }
+	List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
+	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<BlockParamBase *> & getParams() { return _params; }
+// attributes
+	const char *_name;
+	SuperBlock *_parent;
+	float _dt;
+	List<UOrbSubscriptionBase *> _subscriptions;
+	List<UOrbPublicationBase *> _publications;
+	List<BlockParamBase *> _params;
+};
+
+class __EXPORT SuperBlock :
+	public Block
+{
+public:
+	friend class Block;
+// methods
+	SuperBlock(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_children() {
+	}
+	virtual ~SuperBlock() {};
+	virtual void setDt(float dt);
+	virtual void updateParams() {
+		Block::updateParams();
+
+		if (getChildren().getHead() != NULL) updateChildParams();
+	}
+	virtual void updateSubscriptions() {
+		Block::updateSubscriptions();
+
+		if (getChildren().getHead() != NULL) updateChildSubscriptions();
+	}
+	virtual void updatePublications() {
+		Block::updatePublications();
+
+		if (getChildren().getHead() != NULL) updateChildPublications();
+	}
+protected:
+// methods
+	List<Block *> & getChildren() { return _children; }
+	void updateChildParams();
+	void updateChildSubscriptions();
+	void updateChildPublications();
+// attributes
+	List<Block *> _children;
+};
+
+} // namespace control

apps/controllib/block/BlockParam.cpp

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   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 Blockparam.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "BlockParam.hpp"
+
+namespace control
+{
+
+BlockParamBase::BlockParamBase(Block *parent, const char *name) :
+	_handle(PARAM_INVALID)
+{
+	char fullname[blockNameLengthMax];
+
+	if (parent == NULL) {
+		strncpy(fullname, name, blockNameLengthMax);
+
+	} else {
+		char parentName[blockNameLengthMax];
+		parent->getName(parentName, blockNameLengthMax);
+
+		if (!strcmp(name, "")) {
+			strncpy(fullname, parentName, blockNameLengthMax);
+
+		} else {
+			snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
+		}
+
+		parent->getParams().add(this);
+	}
+
+	_handle = param_find(fullname);
+
+	if (_handle == PARAM_INVALID)
+		printf("error finding param: %s\n", fullname);
+};
+
+} // namespace control

apps/controllib/block/BlockParam.hpp

@@ -0,0 +1,85 @@
+/****************************************************************************
+ *
+ *   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 BlockParam.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <systemlib/param/param.h>
+
+#include "Block.hpp"
+#include "List.hpp"
+
+namespace control
+{
+
+/**
+ * A base class for block params that enables traversing linked list.
+ */
+class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
+{
+public:
+	BlockParamBase(Block *parent, const char *name);
+	virtual ~BlockParamBase() {};
+	virtual void update() = 0;
+	const char *getName() { return param_name(_handle); }
+protected:
+	param_t _handle;
+};
+
+/**
+ * Parameters that are tied to blocks for updating and nameing.
+ */
+template<class T>
+class __EXPORT BlockParam : public BlockParamBase
+{
+public:
+	BlockParam(Block *block, const char *name) :
+		BlockParamBase(block, name),
+		_val() {
+		update();
+	}
+	T get() { return _val; }
+	void set(T val) { _val = val; }
+	void update() {
+		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+	}
+protected:
+	T _val;
+};
+
+} // namespace control

apps/controllib/block/List.hpp

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   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 Node.h
+ *
+ * A node of a linked list.
+ */
+
+#pragma once
+
+template<class T>
+class __EXPORT ListNode
+{
+public:
+	ListNode() : _sibling(NULL) {
+	}
+	void setSibling(T sibling) { _sibling = sibling; }
+	T getSibling() { return _sibling; }
+	T get() {
+		return _sibling;
+	}
+protected:
+	T _sibling;
+};
+
+template<class T>
+class __EXPORT List
+{
+public:
+	List() : _head() {
+	}
+	void add(T newNode) {
+		newNode->setSibling(getHead());
+		setHead(newNode);
+	}
+	T getHead() { return _head; }
+private:
+	void setHead(T &head) { _head = head; }
+	T _head;
+};

apps/controllib/block/UOrbPublication.cpp

@@ -0,0 +1,39 @@
+/****************************************************************************
+ *
+ *   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 UOrbPublication.cpp
+ *
+ */
+
+#include "UOrbPublication.hpp"

apps/controllib/block/UOrbPublication.hpp

@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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 UOrbPublication.h
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include "Block.hpp"
+#include "List.hpp"
+
+
+namespace control
+{
+
+class Block;
+
+/**
+ * Base publication warapper class, used in list traversal
+ * of various publications.
+ */
+class __EXPORT UOrbPublicationBase : public ListNode<control::UOrbPublicationBase *>
+{
+public:
+
+	UOrbPublicationBase(
+		List<UOrbPublicationBase *> * list,
+		const struct orb_metadata *meta) :
+		_meta(meta),
+		_handle() {
+		if (list != NULL) list->add(this);
+	}
+	void update() {
+		orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+	}
+	virtual void *getDataVoidPtr() = 0;
+	virtual ~UOrbPublicationBase() {
+		orb_unsubscribe(getHandle());
+	}
+	const struct orb_metadata *getMeta() { return _meta; }
+	int getHandle() { return _handle; }
+protected:
+	void setHandle(orb_advert_t handle) { _handle = handle; }
+	const struct orb_metadata *_meta;
+	orb_advert_t _handle;
+};
+
+/**
+ * UOrb Publication wrapper class
+ */
+template<class T>
+class UOrbPublication :
+	public T, // this must be first!
+	public UOrbPublicationBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param list      A list interface for adding to list during construction
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 */
+	UOrbPublication(
+		List<UOrbPublicationBase *> * list,
+		const struct orb_metadata *meta) :
+		T(), // initialize data structure to zero
+		UOrbPublicationBase(list, meta) {
+		// It is important that we call T()
+		// before we publish the data, so we
+		// call this here instead of the base class
+		setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+	}
+	virtual ~UOrbPublication() {}
+	/*
+	 * XXX
+	 * This function gets the T struct, assuming
+	 * the struct is the first base class, this
+	 * should use dynamic cast, but doesn't
+	 * seem to be available
+	 */
+	void *getDataVoidPtr() { return (void *)(T *)(this); }
+};
+
+} // namespace control

apps/controllib/block/UOrbSubscription.cpp

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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 UOrbSubscription.cpp
+ *
+ */
+
+#include "UOrbSubscription.hpp"
+
+namespace control
+{
+
+bool __EXPORT UOrbSubscriptionBase::updated()
+{
+	bool isUpdated = false;
+	orb_check(_handle, &isUpdated);
+	return isUpdated;
+}
+
+} // namespace control

apps/controllib/block/UOrbSubscription.hpp

@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   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 UOrbSubscription.h
+ *
+ */
+
+#pragma once
+
+#include <uORB/uORB.h>
+#include "Block.hpp"
+#include "List.hpp"
+
+
+namespace control
+{
+
+class Block;
+
+/**
+ * Base subscription warapper class, used in list traversal
+ * of various subscriptions.
+ */
+class __EXPORT UOrbSubscriptionBase :
+	public ListNode<control::UOrbSubscriptionBase *>
+{
+public:
+// methods
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 */
+	UOrbSubscriptionBase(
+		List<UOrbSubscriptionBase *> * list,
+		const struct orb_metadata *meta) :
+		_meta(meta),
+		_handle() {
+		if (list != NULL) list->add(this);
+	}
+	bool updated();
+	void update() {
+		if (updated()) {
+			orb_copy(_meta, _handle, getDataVoidPtr());
+		}
+	}
+	virtual void *getDataVoidPtr() = 0;
+	virtual ~UOrbSubscriptionBase() {
+		orb_unsubscribe(_handle);
+	}
+// accessors
+	const struct orb_metadata *getMeta() { return _meta; }
+	int getHandle() { return _handle; }
+protected:
+// accessors
+	void setHandle(int handle) { _handle = handle; }
+// attributes
+	const struct orb_metadata *_meta;
+	int _handle;
+};
+
+/**
+ * UOrb Subscription wrapper class
+ */
+template<class T>
+class __EXPORT UOrbSubscription :
+	public T, // this must be first!
+	public UOrbSubscriptionBase
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param list      A list interface for adding to list during construction
+	 * @param meta		The uORB metadata (usually from the ORB_ID() macro)
+	 *			for the topic.
+	 * @param interval  The minimum interval in milliseconds between updates
+	 */
+	UOrbSubscription(
+		List<UOrbSubscriptionBase *> * list,
+		const struct orb_metadata *meta, unsigned interval) :
+		T(), // initialize data structure to zero
+		UOrbSubscriptionBase(list, meta) {
+		setHandle(orb_subscribe(getMeta()));
+		orb_set_interval(getHandle(), interval);
+	}
+
+	/**
+	 * Deconstructor
+	 */
+	virtual ~UOrbSubscription() {}
+
+	/*
+	 * XXX
+	 * This function gets the T struct, assuming
+	 * the struct is the first base class, this
+	 * should use dynamic cast, but doesn't
+	 * seem to be available
+	 */
+	void *getDataVoidPtr() { return (void *)(T *)(this); }
+	T getData() { return T(*this); }
+};
+
+} // namespace control

apps/controllib/blocks.cpp

@@ -0,0 +1,486 @@
+/****************************************************************************
+ *
+ *   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 blocks.cpp
+ *
+ * Controller library code
+ */
+
+#include <math.h>
+#include <stdio.h>
+
+#include "blocks.hpp"
+
+namespace control
+{
+
+int basicBlocksTest()
+{
+	blockLimitTest();
+	blockLimitSymTest();
+	blockLowPassTest();
+	blockHighPassTest();
+	blockIntegralTest();
+	blockIntegralTrapTest();
+	blockDerivativeTest();
+	blockPTest();
+	blockPITest();
+	blockPDTest();
+	blockPIDTest();
+	blockOutputTest();
+	blockRandUniformTest();
+	blockRandGaussTest();
+	return 0;
+}
+
+float BlockLimit::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < getMin()) {
+		input = getMin();
+	}
+
+	return input;
+}
+
+int blockLimitTest()
+{
+	printf("Test BlockLimit\t\t\t: ");
+	BlockLimit limit(NULL, "TEST");
+	// initial state
+	ASSERT(equal(1.0f, limit.getMax()));
+	ASSERT(equal(-1.0f, limit.getMin()));
+	ASSERT(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT(equal(1.0f, limit.update(2.0f)));
+	ASSERT(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockLimitSym::update(float input)
+{
+	if (input > getMax()) {
+		input = _max.get();
+
+	} else if (input < -getMax()) {
+		input = -getMax();
+	}
+
+	return input;
+}
+
+int blockLimitSymTest()
+{
+	printf("Test BlockLimitSym\t\t: ");
+	BlockLimitSym limit(NULL, "TEST");
+	// initial state
+	ASSERT(equal(1.0f, limit.getMax()));
+	ASSERT(equal(0.0f, limit.getDt()));
+	// update
+	ASSERT(equal(-1.0f, limit.update(-2.0f)));
+	ASSERT(equal(1.0f, limit.update(2.0f)));
+	ASSERT(equal(0.0f, limit.update(0.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockLowPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = b / (1 + b);
+	setState(a * input + (1 - a)*getState());
+	return getState();
+}
+
+int blockLowPassTest()
+{
+	printf("Test BlockLowPass\t\t: ");
+	BlockLowPass lowPass(NULL, "TEST_LP");
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCut()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.8626974f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
+float BlockHighPass::update(float input)
+{
+	float b = 2 * float(M_PI) * getFCut() * getDt();
+	float a = 1 / (1 + b);
+	setY(a * (getY() + input - getU()));
+	setU(input);
+	return getY();
+}
+
+int blockHighPassTest()
+{
+	printf("Test BlockHighPass\t\t: ");
+	BlockHighPass highPass(NULL, "TEST_HP");
+	// test initial state
+	ASSERT(equal(10.0f, highPass.getFCut()));
+	ASSERT(equal(0.0f, highPass.getU()));
+	ASSERT(equal(0.0f, highPass.getY()));
+	ASSERT(equal(0.0f, highPass.getDt()));
+	// set dt
+	highPass.setDt(0.1f);
+	ASSERT(equal(0.1f, highPass.getDt()));
+	// set state
+	highPass.setU(1.0f);
+	ASSERT(equal(1.0f, highPass.getU()));
+	highPass.setY(1.0f);
+	ASSERT(equal(1.0f, highPass.getY()));
+	// test update
+	ASSERT(equal(0.2746051f, highPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		highPass.update(2.0f);
+	}
+
+	ASSERT(equal(0.0f, highPass.getY()));
+	ASSERT(equal(0.0f, highPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockIntegral::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() + input * getDt()));
+	return getY();
+}
+
+int blockIntegralTest()
+{
+	printf("Test BlockIntegral\t\t: ");
+	BlockIntegral integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT(equal(1.0f, integral.getMax()));
+	ASSERT(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT(equal(0.1f, integral.getDt()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setY(-0.9f);
+	ASSERT(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setY(0.1f);
+	ASSERT(equal(0.2f, integral.update(1.0)));
+	ASSERT(equal(0.2f, integral.getY()));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockIntegralTrap::update(float input)
+{
+	// trapezoidal integration
+	setY(_limit.update(getY() +
+			   (getU() + input) / 2.0f * getDt()));
+	setU(input);
+	return getY();
+}
+
+int blockIntegralTrapTest()
+{
+	printf("Test BlockIntegralTrap\t\t: ");
+	BlockIntegralTrap integral(NULL, "TEST_I");
+	// test initial state
+	ASSERT(equal(1.0f, integral.getMax()));
+	ASSERT(equal(0.0f, integral.getDt()));
+	// set dt
+	integral.setDt(0.1f);
+	ASSERT(equal(0.1f, integral.getDt()));
+	// set U
+	integral.setU(1.0f);
+	ASSERT(equal(1.0f, integral.getU()));
+	// set Y
+	integral.setY(0.9f);
+	ASSERT(equal(0.9f, integral.getY()));
+
+	// test exceed max
+	for (int i = 0; i < 100; i++) {
+		integral.update(1.0f);
+	}
+
+	ASSERT(equal(1.0f, integral.update(1.0f)));
+	// test exceed min
+	integral.setU(-1.0f);
+	integral.setY(-0.9f);
+	ASSERT(equal(-0.9f, integral.getY()));
+
+	for (int i = 0; i < 100; i++) {
+		integral.update(-1.0f);
+	}
+
+	ASSERT(equal(-1.0f, integral.update(-1.0f)));
+	// test update
+	integral.setU(2.0f);
+	integral.setY(0.1f);
+	ASSERT(equal(0.25f, integral.update(1.0)));
+	ASSERT(equal(0.25f, integral.getY()));
+	ASSERT(equal(1.0f, integral.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+float BlockDerivative::update(float input)
+{
+	float output = _lowPass.update((input - getU()) / getDt());
+	setU(input);
+	return output;
+}
+
+int blockDerivativeTest()
+{
+	printf("Test BlockDerivative\t\t: ");
+	BlockDerivative derivative(NULL, "TEST_D");
+	// test initial state
+	ASSERT(equal(0.0f, derivative.getU()));
+	ASSERT(equal(10.0f, derivative.getLP()));
+	// set dt
+	derivative.setDt(0.1f);
+	ASSERT(equal(0.1f, derivative.getDt()));
+	// set U
+	derivative.setU(1.0f);
+	ASSERT(equal(1.0f, derivative.getU()));
+	// test  update
+	ASSERT(equal(8.6269744f, derivative.update(2.0f)));
+	ASSERT(equal(2.0f, derivative.getU()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPTest()
+{
+	printf("Test BlockP\t\t\t: ");
+	BlockP blockP(NULL, "TEST_P");
+	// test initial state
+	ASSERT(equal(0.2f, blockP.getKP()));
+	ASSERT(equal(0.0f, blockP.getDt()));
+	// set dt
+	blockP.setDt(0.1f);
+	ASSERT(equal(0.1f, blockP.getDt()));
+	// test  update
+	ASSERT(equal(0.4f, blockP.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPITest()
+{
+	printf("Test BlockPI\t\t\t: ");
+	BlockPI blockPI(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPI.getKP()));
+	ASSERT(equal(0.1f, blockPI.getKI()));
+	ASSERT(equal(0.0f, blockPI.getDt()));
+	ASSERT(equal(1.0f, blockPI.getIntegral().getMax()));
+	// set dt
+	blockPI.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPI.getDt()));
+	// set integral state
+	blockPI.getIntegral().setY(0.1f);
+	ASSERT(equal(0.1f, blockPI.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43
+	ASSERT(equal(0.43f, blockPI.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPDTest()
+{
+	printf("Test BlockPD\t\t\t: ");
+	BlockPD blockPD(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPD.getKP()));
+	ASSERT(equal(0.01f, blockPD.getKD()));
+	ASSERT(equal(0.0f, blockPD.getDt()));
+	ASSERT(equal(10.0f, blockPD.getDerivative().getLP()));
+	// set dt
+	blockPD.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPD.getDt()));
+	// set derivative state
+	blockPD.getDerivative().setU(1.0f);
+	ASSERT(equal(1.0f, blockPD.getDerivative().getU()));
+	// test  update
+	// 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744
+	ASSERT(equal(0.486269744f, blockPD.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockPIDTest()
+{
+	printf("Test BlockPID\t\t\t: ");
+	BlockPID blockPID(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.2f, blockPID.getKP()));
+	ASSERT(equal(0.1f, blockPID.getKI()));
+	ASSERT(equal(0.01f, blockPID.getKD()));
+	ASSERT(equal(0.0f, blockPID.getDt()));
+	ASSERT(equal(10.0f, blockPID.getDerivative().getLP()));
+	ASSERT(equal(1.0f, blockPID.getIntegral().getMax()));
+	// set dt
+	blockPID.setDt(0.1f);
+	ASSERT(equal(0.1f, blockPID.getDt()));
+	// set derivative state
+	blockPID.getDerivative().setU(1.0f);
+	ASSERT(equal(1.0f, blockPID.getDerivative().getU()));
+	// set integral state
+	blockPID.getIntegral().setY(0.1f);
+	ASSERT(equal(0.1f, blockPID.getIntegral().getY()));
+	// test  update
+	// 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697
+	ASSERT(equal(0.5162697f, blockPID.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockOutputTest()
+{
+	printf("Test BlockOutput\t\t: ");
+	BlockOutput blockOutput(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockOutput.getDt()));
+	ASSERT(equal(0.5f, blockOutput.get()));
+	ASSERT(equal(-1.0f, blockOutput.getMin()));
+	ASSERT(equal(1.0f, blockOutput.getMax()));
+	// test update below min
+	blockOutput.update(-2.0f);
+	ASSERT(equal(-1.0f, blockOutput.get()));
+	// test update above max
+	blockOutput.update(2.0f);
+	ASSERT(equal(1.0f, blockOutput.get()));
+	// test trim
+	blockOutput.update(0.0f);
+	ASSERT(equal(0.5f, blockOutput.get()));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandUniformTest()
+{
+	srand(1234);
+	printf("Test BlockRandUniform\t\t: ");
+	BlockRandUniform blockRandUniform(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockRandUniform.getDt()));
+	ASSERT(equal(-1.0f, blockRandUniform.getMin()));
+	ASSERT(equal(1.0f, blockRandUniform.getMax()));
+	// test update
+	int n = 10000;
+	float mean = blockRandUniform.update();
+
+	// recursive mean algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandUniform.update();
+		mean += (val - mean) / i;
+		ASSERT(val <= blockRandUniform.getMax());
+		ASSERT(val >= blockRandUniform.getMin());
+	}
+
+	ASSERT(equal(mean, (blockRandUniform.getMin() +
+			    blockRandUniform.getMax()) / 2, 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+int blockRandGaussTest()
+{
+	srand(1234);
+	printf("Test BlockRandGauss\t\t: ");
+	BlockRandGauss blockRandGauss(NULL, "TEST");
+	// test initial state
+	ASSERT(equal(0.0f, blockRandGauss.getDt()));
+	ASSERT(equal(1.0f, blockRandGauss.getMean()));
+	ASSERT(equal(2.0f, blockRandGauss.getStdDev()));
+	// test update
+	int n = 10000;
+	float mean = blockRandGauss.update();
+	float sum = 0;
+
+	// recursive mean, stdev algorithm from Knuth
+	for (int i = 2; i < n + 1; i++) {
+		float val = blockRandGauss.update();
+		float newMean = mean + (val - mean) / i;
+		sum += (val - mean) * (val - newMean);
+		mean = newMean;
+	}
+
+	float stdDev = sqrt(sum / (n - 1));
+	ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1));
+	ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace control

apps/controllib/blocks.hpp

@@ -0,0 +1,494 @@
+/****************************************************************************
+ *
+ *   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 blocks.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <assert.h>
+#include <time.h>
+#include <stdlib.h>
+#include <mathlib/math/test/test.hpp>
+
+#include "block/Block.hpp"
+#include "block/BlockParam.hpp"
+
+namespace control
+{
+
+int __EXPORT basicBlocksTest();
+
+/**
+ * A limiter/ saturation.
+ * The output of update is the input, bounded
+ * by min/max.
+ */
+class __EXPORT BlockLimit : public Block
+{
+public:
+// methods
+	BlockLimit(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_min(this, "MIN"),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockLimit() {};
+	float update(float input);
+// accessors
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+protected:
+// attributes
+	BlockParam<float> _min;
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockLimitTest();
+
+/**
+ * A symmetric limiter/ saturation.
+ * Same as limiter but with only a max, is used for
+ * upper limit of +max, and lower limit of -max
+ */
+class __EXPORT BlockLimitSym : public Block
+{
+public:
+// methods
+	BlockLimitSym(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_max(this, "MAX")
+	{};
+	virtual ~BlockLimitSym() {};
+	float update(float input);
+// accessors
+	float getMax() { return _max.get(); }
+protected:
+// attributes
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockLimitSymTest();
+
+/**
+ * A low pass filter as described here:
+ * http://en.wikipedia.org/wiki/Low-pass_filter.
+ */
+class __EXPORT BlockLowPass : public Block
+{
+public:
+// methods
+	BlockLowPass(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_state(0),
+		_fCut(this, "") // only one parameter, no need to name
+	{};
+	virtual ~BlockLowPass() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCut() { return _fCut.get(); }
+	void setState(float state) { _state = state; }
+protected:
+// attributes
+	float _state;
+	BlockParam<float> _fCut;
+};
+
+int __EXPORT blockLowPassTest();
+
+/**
+ * A high pass filter as described here:
+ * http://en.wikipedia.org/wiki/High-pass_filter.
+ */
+class __EXPORT BlockHighPass : public Block
+{
+public:
+// methods
+	BlockHighPass(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_u(0),
+		_y(0),
+		_fCut(this, "") // only one parameter, no need to name
+	{};
+	virtual ~BlockHighPass() {};
+	float update(float input);
+// accessors
+	float getU() {return _u;}
+	float getY() {return _y;}
+	float getFCut() {return _fCut.get();}
+	void setU(float u) {_u = u;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _u; /**< previous input */
+	float _y; /**< previous output */
+	BlockParam<float> _fCut; /**< cut-off frequency, Hz */
+};
+
+int __EXPORT blockHighPassTest();
+
+/**
+ * A rectangular integrator.
+ * A limiter is built into the class to bound the
+ * integral's internal state. This is important
+ * for windup protection.
+ * @see Limit
+ */
+class __EXPORT BlockIntegral: public SuperBlock
+{
+public:
+// methods
+	BlockIntegral(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_y(0),
+		_limit(this, "") {};
+	virtual ~BlockIntegral() {};
+	float update(float input);
+// accessors
+	float getY() {return _y;}
+	float getMax() {return _limit.getMax();}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _y; /**< previous output */
+	BlockLimitSym _limit; /**< limiter */
+};
+
+int __EXPORT blockIntegralTest();
+
+/**
+ * A trapezoidal integrator.
+ * http://en.wikipedia.org/wiki/Trapezoidal_rule
+ * A limiter is built into the class to bound the
+ * integral's internal state. This is important
+ * for windup protection.
+ * @see Limit
+ */
+class __EXPORT BlockIntegralTrap : public SuperBlock
+{
+public:
+// methods
+	BlockIntegralTrap(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_u(0),
+		_y(0),
+		_limit(this, "") {};
+	virtual ~BlockIntegralTrap() {};
+	float update(float input);
+// accessors
+	float getU() {return _u;}
+	float getY() {return _y;}
+	float getMax() {return _limit.getMax();}
+	void setU(float u) {_u = u;}
+	void setY(float y) {_y = y;}
+protected:
+// attributes
+	float _u; /**< previous input */
+	float _y; /**< previous output */
+	BlockLimitSym _limit; /**< limiter */
+};
+
+int __EXPORT blockIntegralTrapTest();
+
+/**
+ * A simple derivative approximation.
+ * This uses the previous and current input.
+ * This has a built in low pass filter.
+ * @see LowPass
+ */
+class __EXPORT BlockDerivative : public SuperBlock
+{
+public:
+// methods
+	BlockDerivative(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_u(0),
+		_lowPass(this, "LP")
+	{};
+	virtual ~BlockDerivative() {};
+	float update(float input);
+// accessors
+	void setU(float u) {_u = u;}
+	float getU() {return _u;}
+	float getLP() {return _lowPass.getFCut();}
+protected:
+// attributes
+	float _u; /**< previous input */
+	BlockLowPass _lowPass; /**< low pass filter */
+};
+
+int __EXPORT blockDerivativeTest();
+
+/**
+ * A proportional controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockP: public Block
+{
+public:
+// methods
+	BlockP(SuperBlock *parent, const char *name) :
+		Block(parent, name),
+		_kP(this, "") // only one param, no need to name
+	{};
+	virtual ~BlockP() {};
+	float update(float input) {
+		return getKP() * input;
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+protected:
+	BlockParam<float> _kP;
+};
+
+int __EXPORT blockPTest();
+
+/**
+ * A proportional-integral controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPI: public SuperBlock
+{
+public:
+// methods
+	BlockPI(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_kP(this, "P"),
+		_kI(this, "I")
+	{};
+	virtual ~BlockPI() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKI() * getIntegral().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	BlockIntegral &getIntegral() { return _integral; }
+private:
+	BlockIntegral _integral;
+	BlockParam<float> _kP;
+	BlockParam<float> _kI;
+};
+
+int __EXPORT blockPITest();
+
+/**
+ * A proportional-derivative controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPD: public SuperBlock
+{
+public:
+// methods
+	BlockPD(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_derivative(this, "D"),
+		_kP(this, "P"),
+		_kD(this, "D")
+	{};
+	virtual ~BlockPD() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKD() * getDerivative().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKD() { return _kD.get(); }
+	BlockDerivative &getDerivative() { return _derivative; }
+private:
+	BlockDerivative _derivative;
+	BlockParam<float> _kP;
+	BlockParam<float> _kD;
+};
+
+int __EXPORT blockPDTest();
+
+/**
+ * A proportional-integral-derivative controller.
+ * @link http://en.wikipedia.org/wiki/PID_controller
+ */
+class __EXPORT BlockPID: public SuperBlock
+{
+public:
+// methods
+	BlockPID(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_integral(this, "I"),
+		_derivative(this, "D"),
+		_kP(this, "P"),
+		_kI(this, "I"),
+		_kD(this, "D")
+	{};
+	virtual ~BlockPID() {};
+	float update(float input) {
+		return getKP() * input +
+		       getKI() * getIntegral().update(input) +
+		       getKD() * getDerivative().update(input);
+	}
+// accessors
+	float getKP() { return _kP.get(); }
+	float getKI() { return _kI.get(); }
+	float getKD() { return _kD.get(); }
+	BlockIntegral &getIntegral() { return _integral; }
+	BlockDerivative &getDerivative() { return _derivative; }
+private:
+// attributes
+	BlockIntegral _integral;
+	BlockDerivative _derivative;
+	BlockParam<float> _kP;
+	BlockParam<float> _kI;
+	BlockParam<float> _kD;
+};
+
+int __EXPORT blockPIDTest();
+
+/**
+ * An output trim/ saturation block
+ */
+class __EXPORT BlockOutput: public SuperBlock
+{
+public:
+// methods
+	BlockOutput(SuperBlock *parent, const char *name) :
+		SuperBlock(parent, name),
+		_trim(this, "TRIM"),
+		_limit(this, ""),
+		_val(0) {
+		update(0);
+	};
+	virtual ~BlockOutput() {};
+	void update(float input) {
+		_val = _limit.update(input + getTrim());
+	}
+// accessors
+	float getMin() { return _limit.getMin(); }
+	float getMax() { return _limit.getMax(); }
+	float getTrim() { return _trim.get(); }
+	float get() { return _val; }
+private:
+// attributes
+	BlockParam<float> _trim;
+	BlockLimit _limit;
+	float _val;
+};
+
+int __EXPORT blockOutputTest();
+
+/**
+ * A uniform random number generator
+ */
+class __EXPORT BlockRandUniform: public Block
+{
+public:
+// methods
+	BlockRandUniform(SuperBlock *parent,
+			 const char *name) :
+		Block(parent, name),
+		_min(this, "MIN"),
+		_max(this, "MAX") {
+		// seed should be initialized somewhere
+		// in main program for all calls to rand
+		// XXX currently in nuttx if you seed to 0, rand breaks
+	};
+	virtual ~BlockRandUniform() {};
+	float update() {
+		static float rand_max = MAX_RAND;
+		float rand_val = rand();
+		float bounds = getMax() - getMin();
+		return getMin() + (rand_val * bounds) / rand_max;
+	}
+// accessors
+	float getMin() { return _min.get(); }
+	float getMax() { return _max.get(); }
+private:
+// attributes
+	BlockParam<float> _min;
+	BlockParam<float> _max;
+};
+
+int __EXPORT blockRandUniformTest();
+
+class __EXPORT BlockRandGauss: public Block
+{
+public:
+// methods
+	BlockRandGauss(SuperBlock *parent,
+		       const char *name) :
+		Block(parent, name),
+		_mean(this, "MEAN"),
+		_stdDev(this, "DEV") {
+		// seed should be initialized somewhere
+		// in main program for all calls to rand
+		// XXX currently in nuttx if you seed to 0, rand breaks
+	};
+	virtual ~BlockRandGauss() {};
+	float update() {
+		static float V1, V2, S;
+		static int phase = 0;
+		float X;
+
+		if (phase == 0) {
+			do {
+				float U1 = (float)rand() / MAX_RAND;
+				float U2 = (float)rand() / MAX_RAND;
+				V1 = 2 * U1 - 1;
+				V2 = 2 * U2 - 1;
+				S = V1 * V1 + V2 * V2;
+			} while (S >= 1 || fabsf(S) < 1e-8f);
+
+			X = V1 * float(sqrt(-2 * float(log(S)) / S));
+
+		} else
+			X = V2 * float(sqrt(-2 * float(log(S)) / S));
+
+		phase = 1 - phase;
+		return X * getStdDev() + getMean();
+	}
+// accessors
+	float getMean() { return _mean.get(); }
+	float getStdDev() { return _stdDev.get(); }
+private:
+// attributes
+	BlockParam<float> _mean;
+	BlockParam<float> _stdDev;
+};
+
+int __EXPORT blockRandGaussTest();
+
+} // namespace control

apps/controllib/fixedwing.cpp

@@ -0,0 +1,367 @@
+/****************************************************************************
+ *
+ *   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 fixedwing.cpp
+ *
+ * Controller library code
+ */
+
+#include "fixedwing.hpp"
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_rLowPass(this, "R_LP"),
+	_rWashout(this, "R_HP"),
+	_r2Rdr(this, "R2RDR"),
+	_rudder(0)
+{
+}
+
+BlockYawDamper::~BlockYawDamper() {};
+
+void BlockYawDamper::update(float rCmd, float r)
+{
+	_rudder = _r2Rdr.update(rCmd -
+				_rWashout.update(_rLowPass.update(r)));
+}
+
+BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_yawDamper(this, ""),
+	_pLowPass(this, "P_LP"),
+	_qLowPass(this, "Q_LP"),
+	_p2Ail(this, "P2AIL"),
+	_q2Elv(this, "Q2ELV"),
+	_aileron(0),
+	_elevator(0)
+{
+}
+
+BlockStabilization::~BlockStabilization() {};
+
+void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
+				float p, float q, float r)
+{
+	_aileron = _p2Ail.update(
+			   pCmd - _pLowPass.update(p));
+	_elevator = _q2Elv.update(
+			    qCmd - _qLowPass.update(q));
+	_yawDamper.update(rCmd, r);
+}
+
+BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_psi2Phi(this, "PSI2PHI"),
+	_phi2P(this, "PHI2P"),
+	_phiLimit(this, "PHI_LIM")
+{
+}
+
+BlockHeadingHold::~BlockHeadingHold() {};
+
+float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
+{
+	float psiError = _wrap_pi(psiCmd - psi);
+	float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+	return _phi2P.update(phiCmd - phi);
+}
+
+BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Theta(this, "V2THE"),
+	_theta2Q(this, "THE2Q"),
+	_theLimit(this, "THE"),
+	_vLimit(this, "V")
+{
+}
+
+BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
+
+float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
+{
+	// negative sign because nose over to increase speed
+	float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_v2Thr(this, "V2THR")
+{
+}
+
+BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
+
+float BlockVelocityHoldFrontside::update(float vCmd, float v)
+{
+	return _v2Thr.update(vCmd - v);
+}
+
+BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Thr(this, "H2THR"),
+	_throttle(0)
+{
+}
+
+BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
+
+void BlockAltitudeHoldBackside::update(float hCmd, float h)
+{
+	_throttle = _h2Thr.update(hCmd - h);
+}
+
+BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_h2Theta(this, "H2THE"),
+	_theta2Q(this, "THE2Q")
+{
+}
+
+BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
+
+float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
+{
+	float thetaCmd = _h2Theta.update(hCmd - h);
+	return _theta2Q.update(thetaCmd - theta);
+}
+
+BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
+		const char *name,
+		BlockStabilization *stabilization) :
+	SuperBlock(parent, name),
+	_stabilization(stabilization),
+	_headingHold(this, ""),
+	_velocityHold(this, ""),
+	_altitudeHold(this, ""),
+	_trimAil(this, "TRIM_AIL"),
+	_trimElv(this, "TRIM_ELV"),
+	_trimRdr(this, "TRIM_RDR"),
+	_trimThr(this, "TRIM_THR")
+{
+}
+
+BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
+
+void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
+				    float h, float v,
+				    float phi, float theta, float psi,
+				    float p, float q, float r)
+{
+	_altitudeHold.update(hCmd, h);
+	_stabilization->update(
+		_headingHold.update(psiCmd, phi, psi, p),
+		_velocityHold.update(vCmd, v, theta, q),
+		rCmd,
+		p, q, r);
+}
+
+BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	_xtYawLimit(this, "XT2YAW"),
+	_xt2Yaw(this, "XT2YAW"),
+	_psiCmd(0)
+{
+}
+
+BlockWaypointGuidance::~BlockWaypointGuidance() {};
+
+void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
+				   vehicle_attitude_s &att,
+				   vehicle_global_position_setpoint_s &posCmd,
+				   vehicle_global_position_setpoint_s &lastPosCmd)
+{
+
+	// heading to waypoint
+	float psiTrack = get_bearing_to_next_waypoint(
+				 (double)pos.lat / (double)1e7d,
+				 (double)pos.lon / (double)1e7d,
+				 (double)posCmd.lat / (double)1e7d,
+				 (double)posCmd.lon / (double)1e7d);
+
+	// cross track
+	struct crosstrack_error_s xtrackError;
+	get_distance_to_line(&xtrackError,
+			     (double)pos.lat / (double)1e7d,
+			     (double)pos.lon / (double)1e7d,
+			     (double)lastPosCmd.lat / (double)1e7d,
+			     (double)lastPosCmd.lon / (double)1e7d,
+			     (double)posCmd.lat / (double)1e7d,
+			     (double)posCmd.lon / (double)1e7d);
+
+	_psiCmd = _wrap_2pi(psiTrack -
+			    _xtYawLimit.update(_xt2Yaw.update(xtrackError.distance)));
+}
+
+BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
+	SuperBlock(parent, name),
+	// subscriptions
+	_att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
+	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
+	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
+	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
+	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
+	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
+	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+	// publications
+	_actuators(&getPublications(), ORB_ID(actuator_controls_0))
+{
+}
+
+BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
+
+BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
+	BlockUorbEnabledAutopilot(parent, name),
+	_stabilization(this, ""), // no name needed, already unique
+	_backsideAutopilot(this, "", &_stabilization),
+	_guide(this, ""),
+	_vCmd(this, "V_CMD"),
+	_attPoll(),
+	_lastPosCmd(),
+	_timeStamp(0)
+{
+	_attPoll.fd = _att.getHandle();
+	_attPoll.events = POLLIN;
+}
+
+void BlockMultiModeBacksideAutopilot::update()
+{
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (poll(&_attPoll, 1, 100) < 0) return; // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) return;
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// store old position command before update if new command sent
+	if (_posCmd.updated()) {
+		_lastPosCmd = _posCmd.getData();
+	}
+
+	// check for new updates
+	if (_param_update.updated()) updateParams();
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	// only update guidance in auto mode
+	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+	}
+
+	// XXX handle STABILIZED (loiter on spot) as well
+	// once the system switches from manual or auto to stabilized
+	// the setpoint should update to loitering around this position
+
+	// handle autopilot modes
+	if (_status.state_machine == SYSTEM_STATE_AUTO ||
+		_status.state_machine == SYSTEM_STATE_STABILIZED) {
+
+		// update guidance
+		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+
+		// calculate velocity, XXX should be airspeed, but using ground speed for now
+		float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
+
+		// commands
+		float rCmd = 0;
+
+		_backsideAutopilot.update(
+			_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
+			_pos.alt, v,
+			_att.roll, _att.pitch, _att.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed
+		);
+		_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
+		_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
+		_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
+		_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
+
+	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
+
+		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+
+			_actuators.control[CH_AIL] = _manual.roll;
+			_actuators.control[CH_ELV] = _manual.pitch;
+			_actuators.control[CH_RDR] = _manual.yaw;
+			_actuators.control[CH_THR] = _manual.throttle;
+
+		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+			_stabilization.update(
+			_ratesCmd.roll, _ratesCmd.pitch, _ratesCmd.yaw,
+			_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+			_actuators.control[CH_AIL] = _stabilization.getAileron();
+			_actuators.control[CH_ELV] = _stabilization.getElevator();
+			_actuators.control[CH_RDR] = _stabilization.getRudder();
+			_actuators.control[CH_THR] = _manual.throttle;
+		}
+	}
+
+	// update all publications
+	updatePublications();
+}
+
+BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot()
+{
+	// send one last publication when destroyed, setting
+	// all output to zero
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		_actuators.control[i] = 0.0f;
+
+	updatePublications();
+}
+
+} // namespace fixedwing
+
+} // namespace control
+

apps/controllib/fixedwing.hpp

@@ -0,0 +1,438 @@
+/****************************************************************************
+ *
+ *   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 fixedwing.h
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <drivers/drv_hrt.h>
+#include <poll.h>
+
+#include "blocks.hpp"
+#include "block/UOrbSubscription.hpp"
+#include "block/UOrbPublication.hpp"
+
+extern "C" {
+#include <systemlib/geo/geo.h>
+}
+
+namespace control
+{
+
+namespace fixedwing
+{
+
+/**
+ * BlockYawDamper
+ *
+ * This block has more explations to help new developers
+ * add their own blocks. It includes a limited explanation
+ * of some C++ basics.
+ *
+ * Block: The generic class describing a typical block as you
+ * would expect in Simulink or ScicosLab. A block can have
+ * parameters. It cannot have other blocks.
+ *
+ * SuperBlock: A superblock is a block that can have other
+ * blocks. It has methods that manage the blocks beneath it.
+ *
+ * BlockYawDamper inherits from SuperBlock publically, this
+ * means that any public function in SuperBlock are public within
+ * BlockYawDamper and may be called from outside the
+ * class methods. Any protected function within block
+ * are private to the class and may not be called from
+ * outside this class. Protected should be preferred
+ * where possible to public as it is important to
+ * limit access to the bare minimum to prevent
+ * accidental errors.
+ */
+class __EXPORT BlockYawDamper : public SuperBlock
+{
+private:
+	/**
+	 * Declaring other blocks used by this block
+	 *
+	 * In this section we declare all child blocks that
+	 * this block is composed of. They are private
+	 * members so only this block has direct access to
+	 * them.
+	 */
+	BlockLowPass _rLowPass;
+	BlockHighPass _rWashout;
+	BlockP _r2Rdr;
+
+	/**
+	 * Declaring output values and accessors
+	 *
+	 * If we have any output values for the block we
+	 * declare them here. Output can be directly returned
+	 * through the update function, but outputs should be
+	 * declared here if the information will likely be requested
+	 * again, or if there are multiple outputs.
+	 *
+	 * You should only be able to set outputs from this block,
+	 * so the outputs are declared in the private section.
+	 * A get accessor is provided
+	 * in the public section for other blocks to get the
+	 * value of the output.
+	 */
+	float _rudder;
+public:
+	/**
+	 * BlockYawDamper Constructor
+	 *
+	 * The job of the constructor is to initialize all
+	 * parameter in this block and initialize all child
+	 * blocks. Note also, that the output values must be
+	 * initialized here. The order of the members in the
+	 * member initialization list should follow the
+	 * order in which they are declared within the class.
+	 * See the private member declarations above.
+	 *
+	 * Block Construction
+	 *
+	 * All blocks are constructed with their parent block
+	 * and their name. This allows parameters within the
+	 * block to construct a fully qualified name from
+	 * concatenating the two. If the name provided to the
+	 * block is "", then the block will use the parent
+	 * name as it's name. This is useful in cases where
+	 * you have a block that has parameters "MIN", "MAX",
+	 * such as BlockLimit and you do not want an extra name
+	 * to qualify them since the parent block has no "MIN",
+	 * "MAX" parameters.
+	 *
+	 * Block Parameter Construction
+	 *
+	 * Block parameters are named constants, they are
+	 * constructed using:
+	 * BlockParam::BlockParam(Block * parent, const char * name)
+	 * This funciton takes both a parent block and a name.
+	 * The constructore then uses the parent name and the name of
+	 * the paramter to ask the px4 param library if it has any
+	 * parameters with this name. If it does, a handle to the
+	 * parameter is retrieved.
+	 *
+	 * Block/ BlockParam Naming
+	 *
+	 * When desigining new blocks, the naming of the parameters and
+	 * blocks determines the fully qualified name of the parameters
+	 * within the ground station, so it is important to choose
+	 * short, easily understandable names. Again, when a name of
+	 * "" is passed, the parent block name is used as the value to
+	 * prepend to paramters names.
+	 */
+	BlockYawDamper(SuperBlock *parent, const char *name);
+	/**
+	 * Block deconstructor
+	 *
+	 * It is always a good idea to declare a virtual
+	 * deconstructor so that upon calling delete from
+	 * a class derived from this, all of the
+	 * deconstructors all called, the derived class first, and
+	 * then the base class
+	 */
+	virtual ~BlockYawDamper();
+
+	/**
+	 * Block update function
+	 *
+	 * The job of the update function is to compute the output
+	 * values for the block. In a simple block with one output,
+	 * the output may be returned directly. If the output is
+	 * required frequenly by other processses, it might be a
+	 * good idea to declare a member to store the temporary
+	 * variable.
+	 */
+	void update(float rCmd, float r);
+
+	/**
+	 * Rudder output value accessor
+	 *
+	 * This is a public accessor function, which means that the
+	 * private value _rudder is returned to anyone calling
+	 * BlockYawDamper::getRudder(). Note thate a setRudder() is
+	 * not provided, this is because the updateParams() call
+	 * for a block provides the mechanism for updating the
+	 * paramter.
+	 */
+	float getRudder() { return _rudder; }
+};
+
+/**
+ * Stability augmentation system.
+ * Aircraft Control and Simulation, Stevens and Lewis, pg. 292, 299
+ */
+class __EXPORT BlockStabilization : public SuperBlock
+{
+private:
+	BlockYawDamper _yawDamper;
+	BlockLowPass _pLowPass;
+	BlockLowPass _qLowPass;
+	BlockP _p2Ail;
+	BlockP _q2Elv;
+	float _aileron;
+	float _elevator;
+public:
+	BlockStabilization(SuperBlock *parent, const char *name);
+	virtual ~BlockStabilization();
+	void update(float pCmd, float qCmd, float rCmd,
+		    float p, float q, float r);
+	float getAileron() { return _aileron; }
+	float getElevator() { return _elevator; }
+	float getRudder() { return _yawDamper.getRudder(); }
+};
+
+/**
+ * Heading hold autopilot block.
+ * Aircraft Control and Simulation, Stevens and Lewis
+ * Heading hold, pg. 348
+ */
+class __EXPORT BlockHeadingHold : public SuperBlock
+{
+private:
+	BlockP _psi2Phi;
+	BlockP _phi2P;
+	BlockLimitSym _phiLimit;
+public:
+	BlockHeadingHold(SuperBlock *parent, const char *name);
+	virtual ~BlockHeadingHold();
+	/**
+	 * returns pCmd
+	 */
+	float update(float psiCmd, float phi, float psi, float p);
+};
+
+/**
+ * Frontside/ Backside Control Systems
+ *
+ * Frontside :
+ *   velocity error -> throttle
+ *   altitude error -> elevator
+ *
+ * Backside :
+ *   velocity error -> elevator
+ *   altitude error -> throttle
+ *
+ * Backside control systems are more resilient at
+ * slow speeds on the back-side of the power
+ * required curve/ landing etc. Less performance
+ * than frontside at high speeds.
+ */
+
+/**
+ * Backside velocity hold autopilot block.
+ * v -> theta -> q -> elevator
+ */
+class __EXPORT BlockVelocityHoldBackside : public SuperBlock
+{
+private:
+	BlockPID _v2Theta;
+	BlockPID _theta2Q;
+	BlockLimit _theLimit;
+	BlockLimit _vLimit;
+public:
+	BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
+	virtual ~BlockVelocityHoldBackside();
+	/**
+	 * returns qCmd
+	 */
+	float update(float vCmd, float v, float theta, float q);
+};
+
+/**
+ * Frontside velocity hold autopilot block.
+ * v -> throttle
+ */
+class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
+{
+private:
+	BlockPID _v2Thr;
+public:
+	BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
+	virtual ~BlockVelocityHoldFrontside();
+	/**
+	 * returns throttle
+	 */
+	float update(float vCmd, float v);
+};
+
+/**
+ * Backside altitude hold autopilot block.
+ * h -> throttle
+ */
+class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
+{
+private:
+	BlockPID _h2Thr;
+	float _throttle;
+public:
+	BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
+	virtual ~BlockAltitudeHoldBackside();
+	void update(float hCmd, float h);
+	float getThrottle() { return _throttle; }
+};
+
+/**
+ * Frontside altitude hold autopilot block.
+ * h -> theta > q -> elevator
+ */
+class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
+{
+private:
+	BlockPID _h2Theta;
+	BlockPID _theta2Q;
+public:
+	BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
+	virtual ~BlockAltitudeHoldFrontside();
+	/**
+	 * return qCmd
+	 */
+	float update(float hCmd, float h, float theta, float q);
+};
+
+/**
+ * Backside autopilot
+ */
+class __EXPORT BlockBacksideAutopilot : public SuperBlock
+{
+private:
+	BlockStabilization *_stabilization;
+	BlockHeadingHold _headingHold;
+	BlockVelocityHoldBackside _velocityHold;
+	BlockAltitudeHoldBackside _altitudeHold;
+	BlockParam<float> _trimAil;
+	BlockParam<float> _trimElv;
+	BlockParam<float> _trimRdr;
+	BlockParam<float> _trimThr;
+public:
+	BlockBacksideAutopilot(SuperBlock *parent,
+			       const char *name,
+			       BlockStabilization *stabilization);
+	virtual ~BlockBacksideAutopilot();
+	void update(float hCmd, float vCmd, float rCmd, float psiCmd,
+		    float h, float v,
+		    float phi, float theta, float psi,
+		    float p, float q, float r);
+	float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
+	float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
+	float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
+	float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
+};
+
+/**
+ * Waypoint Guidance block
+ */
+class __EXPORT BlockWaypointGuidance : public SuperBlock
+{
+private:
+	BlockLimitSym _xtYawLimit;
+	BlockP _xt2Yaw;
+	float _psiCmd;
+public:
+	BlockWaypointGuidance(SuperBlock *parent, const char *name);
+	virtual ~BlockWaypointGuidance();
+	void update(vehicle_global_position_s &pos,
+		    vehicle_attitude_s &att,
+		    vehicle_global_position_setpoint_s &posCmd,
+		    vehicle_global_position_setpoint_s &lastPosCmd);
+	float getPsiCmd() { return _psiCmd; }
+};
+
+/**
+ * UorbEnabledAutopilot
+ */
+class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
+{
+protected:
+	// subscriptions
+	UOrbSubscription<vehicle_attitude_s> _att;
+	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
+	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
+	UOrbSubscription<vehicle_global_position_s> _pos;
+	UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
+	UOrbSubscription<manual_control_setpoint_s> _manual;
+	UOrbSubscription<vehicle_status_s> _status;
+	UOrbSubscription<parameter_update_s> _param_update;
+	// publications
+	UOrbPublication<actuator_controls_s> _actuators;
+public:
+	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
+	virtual ~BlockUorbEnabledAutopilot();
+};
+
+/**
+ * Multi-mode Autopilot
+ */
+class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
+{
+private:
+	BlockStabilization _stabilization;
+	BlockBacksideAutopilot _backsideAutopilot;
+	BlockWaypointGuidance _guide;
+	BlockParam<float> _vCmd;
+
+	struct pollfd _attPoll;
+	vehicle_global_position_setpoint_s _lastPosCmd;
+	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
+	uint64_t _timeStamp;
+public:
+	BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name);
+	void update();
+	virtual ~BlockMultiModeBacksideAutopilot();
+};
+
+
+} // namespace fixedwing
+
+} // namespace control
+

apps/controllib/test_params.c

@@ -0,0 +1,22 @@
+#include <systemlib/param/param.h>
+// WARNING:
+//  do not changes these unless
+//  you want to recompute the
+//  answers for all of the unit tests
+
+PARAM_DEFINE_FLOAT(TEST_MIN, -1.0f);
+PARAM_DEFINE_FLOAT(TEST_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(TEST_TRIM, 0.5f);
+PARAM_DEFINE_FLOAT(TEST_HP, 10.0f);
+PARAM_DEFINE_FLOAT(TEST_LP, 10.0f);
+
+PARAM_DEFINE_FLOAT(TEST_P, 0.2f);
+
+PARAM_DEFINE_FLOAT(TEST_I, 0.1f);
+PARAM_DEFINE_FLOAT(TEST_I_MAX, 1.0f);
+
+PARAM_DEFINE_FLOAT(TEST_D, 0.01f);
+PARAM_DEFINE_FLOAT(TEST_D_LP, 10.0f);
+
+PARAM_DEFINE_FLOAT(TEST_MEAN, 1.0f);
+PARAM_DEFINE_FLOAT(TEST_DEV, 2.0f);

apps/drivers/boards/px4fmu/px4fmu_init.c

@@ -95,8 +95,6 @@
  * Protected Functions
  ****************************************************************************/
 
-extern int adc_devinit(void);
-
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -150,9 +148,7 @@ __EXPORT int nsh_archinitialize(void)
 	int result;
 
 	/* configure the high-resolution time/callout interface */
-#ifdef CONFIG_HRT_TIMER
 	hrt_init();
-#endif
 
 	/* configure CPU load estimation */
 #ifdef CONFIG_SCHED_INSTRUMENTATION
@@ -160,27 +156,21 @@ __EXPORT int nsh_archinitialize(void)
 #endif
 
 	/* set up the serial DMA polling */
-#ifdef SERIAL_HAVE_DMA
+	static struct hrt_call serial_dma_call;
-	{
+	struct timespec ts;
-		static struct hrt_call serial_dma_call;
-		struct timespec ts;
 
-		/*
+	/*
-		 * Poll at 1ms intervals for received bytes that have not triggered
+	 * Poll at 1ms intervals for received bytes that have not triggered
-		 * a DMA event.
+	 * a DMA event.
-		 */
+	 */
-		ts.tv_sec = 0;
+	ts.tv_sec = 0;
-		ts.tv_nsec = 1000000;
+	ts.tv_nsec = 1000000;
 
-		hrt_call_every(&serial_dma_call,
+	hrt_call_every(&serial_dma_call,
-			       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
-			       ts_to_abstime(&ts),
+		       ts_to_abstime(&ts),
-			       (hrt_callout)stm32_serial_dma_poll,
+		       (hrt_callout)stm32_serial_dma_poll,
-			       NULL);
+		       NULL);
-	}
-#endif
-
-	message("\r\n");
 
 	// initial LED state
 	drv_led_start();
@@ -209,8 +199,7 @@ __EXPORT int nsh_archinitialize(void)
 
 	message("[boot] Successfully initialized SPI port 1\r\n");
 
-#if defined(CONFIG_STM32_SPI3)
+	/* Get the SPI port for the microSD slot */
-	/* Get the SPI port */
 
 	message("[boot] Initializing SPI port 3\n");
 	spi3 = up_spiinitialize(3);
@@ -233,23 +222,11 @@ __EXPORT int nsh_archinitialize(void)
 	}
 
 	message("[boot] Successfully bound SPI port 3 to the MMCSD driver\n");
-#endif /* SPI3 */
 
-#ifdef CONFIG_ADC
+	stm32_configgpio(GPIO_ADC1_IN10);
-	int adc_state = adc_devinit();
+	stm32_configgpio(GPIO_ADC1_IN11);
-
+	stm32_configgpio(GPIO_ADC1_IN12);
-	if (adc_state != OK) {
+	stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-		/* Try again */
-		adc_state = adc_devinit();
-
-		if (adc_state != OK) {
-			/* Give up */
-			message("[boot] FAILED adc_devinit: %d\n", adc_state);
-			return -ENODEV;
-		}
-	}
-
-#endif
 
 	return OK;
 }

apps/drivers/boards/px4io/px4io_init.c

@@ -92,4 +92,7 @@ __EXPORT void stm32_boardinitialize(void)
 	stm32_configgpio(GPIO_ACC_OC_DETECT);
 	stm32_configgpio(GPIO_SERVO_OC_DETECT);
 	stm32_configgpio(GPIO_BTN_SAFETY);
+
+	stm32_configgpio(GPIO_ADC_VBATT);
+	stm32_configgpio(GPIO_ADC_IN5);
 }

apps/drivers/boards/px4io/px4io_internal.h

@@ -61,28 +61,6 @@
 #define GPIO_LED3       (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|\
                          GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN10)
 
-/* R/C in/out channels **************************************************************/
-
-/* XXX just GPIOs for now - eventually timer pins */
-
-#define GPIO_CH1_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_CH2_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_CH3_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_CH4_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN9)
-#define GPIO_CH5_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_CH6_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN7)
-#define GPIO_CH7_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_CH8_IN     (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN1)
-
-#define GPIO_CH1_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0)
-#define GPIO_CH2_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1)
-#define GPIO_CH3_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8)
-#define GPIO_CH4_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN9)
-#define GPIO_CH5_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN6)
-#define GPIO_CH6_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN7)
-#define GPIO_CH7_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0)
-#define GPIO_CH8_OUT    (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1)
-
 /* Safety switch button *************************************************************/
 
 #define GPIO_BTN_SAFETY (GPIO_INPUT|GPIO_CNF_INFLOAT|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN5)
@@ -98,3 +76,8 @@
 
 #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
 #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+
+/* Analog inputs ********************************************************************/
+
+#define GPIO_ADC_VBATT	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN4)
+#define GPIO_ADC_IN5	(GPIO_INPUT|GPIO_CNF_ANALOGIN|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN5)

apps/drivers/drv_adc.h

@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file drv_adc.h
+ *
+ * ADC driver interface.
+ *
+ * This defines additional operations over and above the standard NuttX
+ * ADC API.
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#define ADC_DEVICE_PATH		"/dev/adc0"
+
+/*
+ * ioctl definitions
+ */

apps/drivers/drv_mixer.h

@@ -100,28 +100,13 @@ struct mixer_simple_s {
  */
 #define MIXERIOCADDSIMPLE	_MIXERIOC(2)
 
-/** multirotor output definition */
+/* _MIXERIOC(3) was deprecated */
-struct mixer_rotor_output_s {
+/* _MIXERIOC(4) was deprecated */
-	float			angle;		/**< rotor angle clockwise from forward in radians */
-	float			distance;	/**< motor distance from centre in arbitrary units */
-};
-
-/** multirotor mixer */
-struct mixer_multirotor_s {
-	uint8_t			rotor_count;
-	struct mixer_control_s	controls[4];	/**< controls are roll, pitch, yaw, thrust */
-	struct mixer_rotor_output_s rotors[0];	/**< actual size of the array is set by rotor_count */
-};
 
 /**
- * Add a multirotor mixer in (struct mixer_multirotor_s *)arg
+ * Add mixer(s) from the buffer in (const char *)arg
  */
-#define MIXERIOCADDMULTIROTOR	_MIXERIOC(3)
+#define MIXERIOCLOADBUF		_MIXERIOC(5)
-
-/**
- * Add mixers(s) from a the file in (const char *)arg
- */
-#define MIXERIOCLOADFILE	_MIXERIOC(4)
 
 /*
  * XXX Thoughts for additional operations:

apps/drivers/drv_pwm_output.h

@@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
+/** set update rate in Hz */
+#define PWM_SERVO_SET_UPDATE_RATE	_IOC(_PWM_SERVO_BASE, 2)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 

apps/drivers/hil/hil.cpp

@@ -68,11 +68,11 @@
 #include <drivers/device/device.h>
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
-// #include <drivers/boards/HIL/HIL_internal.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_mixer.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/mixer/mixer.h>
-#include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -382,7 +382,6 @@ HIL::task_main()
 		/* this would be bad... */
 		if (ret < 0) {
 			log("poll error %d", errno);
-			usleep(1000000);
 			continue;
 		}
 
@@ -396,16 +395,27 @@ HIL::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				_mixers->mix(&outputs.output[0], num_outputs);
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
-					/* scale for PWM output 900 - 2100us */
+					/* last resort: catch NaN, INF and out-of-band errors */
-					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					if (i < (unsigned)outputs.noutputs &&
-
+						isfinite(outputs.output[i]) &&
-					/* output to the servo */
+						outputs.output[i] >= -1.0f &&
-					// up_pwm_servo_set(i, outputs.output[i]);
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					} else {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = 900;
+					}
 				}
 
 				/* and publish for anyone that cares to see */
@@ -419,9 +429,6 @@ HIL::task_main()
 
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
-
-			/* update PWM servo armed status */
-			// up_pwm_servo_arm(aa.armed);
 		}
 	}
 
@@ -503,6 +510,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		// up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		g_hil->set_pwm_rate(arg);
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
@@ -577,26 +588,19 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
-	case MIXERIOCADDMULTIROTOR:
+	case MIXERIOCLOADBUF: {
-		/* XXX not yet supported */
+			const char *buf = (const char *)arg;
-		ret = -ENOTTY;
+			unsigned buflen = strnlen(buf, 1024);
-		break;
 
-	case MIXERIOCLOADFILE: {
+			if (_mixers == nullptr)
-			const char *path = (const char *)arg;
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
-			if (_mixers != nullptr) {
-				delete _mixers;
-				_mixers = nullptr;
-			}
-
-			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 			if (_mixers == nullptr) {
 				ret = -ENOMEM;
+
 			} else {
 
-				debug("loading mixers from %s", path);
+				ret = _mixers->load_from_buf(buf, buflen);
-				ret = _mixers->load_from_file(path);
 
 				if (ret != 0) {
 					debug("mixer load failed with %d", ret);
@@ -605,10 +609,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
-
 			break;
 		}
 
+
 	default:
 		ret = -ENOTTY;
 		break;

apps/drivers/px4fmu/fmu.cpp

@@ -58,6 +58,7 @@
 #include <drivers/drv_pwm_output.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/boards/px4fmu/px4fmu_internal.h>
+#include <drivers/drv_hrt.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -65,6 +66,7 @@
 #include <drivers/drv_mixer.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
 
 #include <systemlib/err.h>
@@ -97,6 +99,7 @@ private:
 	int		_t_actuators;
 	int		_t_armed;
 	orb_advert_t	_t_outputs;
+	orb_advert_t	_t_actuators_effective;
 	unsigned	_num_outputs;
 	bool		_primary_pwm_device;
 
@@ -162,6 +165,7 @@ PX4FMU::PX4FMU() :
 	_t_actuators(-1),
 	_t_armed(-1),
 	_t_outputs(0),
+	_t_actuators_effective(0),
 	_num_outputs(0),
 	_primary_pwm_device(false),
 	_task_should_exit(false),
@@ -319,6 +323,13 @@ PX4FMU::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
+	/* advertise the effective control inputs */
+	actuator_controls_effective_s controls_effective;
+	memset(&controls_effective, 0, sizeof(controls_effective));
+	/* advertise the effective control inputs */
+	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
+				   &controls_effective);
+
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
@@ -336,8 +347,16 @@ PX4FMU::task_main()
 		if (_current_update_rate != _update_rate) {
 			int update_rate_in_ms = int(1000 / _update_rate);
 
-			if (update_rate_in_ms < 2)
+			/* reject faster than 500 Hz updates */
+			if (update_rate_in_ms < 2) {
 				update_rate_in_ms = 2;
+				_update_rate = 500;
+			}
+			/* reject slower than 50 Hz updates */
+			if (update_rate_in_ms > 20) {
+				update_rate_in_ms = 20;
+				_update_rate = 50;
+			}
 
 			orb_set_interval(_t_actuators, update_rate_in_ms);
 			up_pwm_servo_set_rate(_update_rate);
@@ -364,20 +383,39 @@ PX4FMU::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				_mixers->mix(&outputs.output[0], num_outputs);
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs);
+				outputs.timestamp = hrt_absolute_time();
+
+				// XXX output actual limited values
+				memcpy(&controls_effective, &_controls, sizeof(controls_effective));
+
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1), _t_actuators_effective, &controls_effective);
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
-					/* scale for PWM output 900 - 2100us */
+					/* last resort: catch NaN, INF and out-of-band errors */
-					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					if (i < outputs.noutputs &&
+						isfinite(outputs.output[i]) &&
+						outputs.output[i] >= -1.0f &&
+						outputs.output[i] <= 1.0f) {
+						/* scale for PWM output 900 - 2100us */
+						outputs.output[i] = 1500 + (600 * outputs.output[i]);
+					} else {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = 900;
+					}
 
 					/* output to the servo */
 					up_pwm_servo_set(i, outputs.output[i]);
 				}
 
 				/* and publish for anyone that cares to see */
-				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
+				orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs);
 			}
 		}
 
@@ -394,6 +432,7 @@ PX4FMU::task_main()
 	}
 
 	::close(_t_actuators);
+	::close(_t_actuators_effective);
 	::close(_t_armed);
 
 	/* make sure servos are off */
@@ -470,6 +509,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		set_pwm_rate(arg);
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
@@ -500,7 +543,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		/* FALLTHROUGH */
 	case PWM_SERVO_GET(0):
 	case PWM_SERVO_GET(1): {
-			channel = cmd - PWM_SERVO_SET(0);
+			channel = cmd - PWM_SERVO_GET(0);
 			*(servo_position_t *)arg = up_pwm_servo_get(channel);
 			break;
 		}
@@ -544,28 +587,19 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 			break;
 		}
 
-	case MIXERIOCADDMULTIROTOR:
+	case MIXERIOCLOADBUF: {
-		/* XXX not yet supported */
+			const char *buf = (const char *)arg;
-		ret = -ENOTTY;
+			unsigned buflen = strnlen(buf, 1024);
-		break;
 
-	case MIXERIOCLOADFILE: {
+			if (_mixers == nullptr)
-			const char *path = (const char *)arg;
+				_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
-
-			if (_mixers != nullptr) {
-				delete _mixers;
-				_mixers = nullptr;
-			}
-
-			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
 			if (_mixers == nullptr) {
 				ret = -ENOMEM;
 
 			} else {
 
-				debug("loading mixers from %s", path);
+				ret = _mixers->load_from_buf(buf, buflen);
-				ret = _mixers->load_from_file(path);
 
 				if (ret != 0) {
 					debug("mixer load failed with %d", ret);
@@ -574,7 +608,6 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 					ret = -EINVAL;
 				}
 			}
-
 			break;
 		}
 

apps/drivers/px4io/uploader.cpp

@@ -51,6 +51,50 @@
 
 #include "uploader.h"
 
+static const uint32_t crctab[] =
+{
+	0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
+	0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
+	0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+	0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5,
+	0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
+	0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+	0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f,
+	0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d,
+	0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+	0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
+	0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457,
+	0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+	0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb,
+	0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9,
+	0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+	0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad,
+	0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683,
+	0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+	0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7,
+	0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
+	0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+	0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79,
+	0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f,
+	0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+	0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
+	0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21,
+	0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+	0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45,
+	0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db,
+	0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+	0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf,
+	0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
+};
+
+static uint32_t
+crc32(const uint8_t *src, unsigned len, unsigned state)
+{
+	for (unsigned i = 0; i < len; i++)
+		state = crctab[(state ^ src[i]) & 0xff] ^ (state >> 8);
+	return state;
+}
+
 PX4IO_Uploader::PX4IO_Uploader() :
 	_io_fd(-1),
 	_fw_fd(-1)
@@ -110,6 +154,17 @@ PX4IO_Uploader::upload(const char *filenames[])
 			}
 		}
 
+		ret = get_info(INFO_BL_REV, bl_rev);
+
+		if (ret == OK) {
+			if (bl_rev <= BL_REV) {
+				log("found bootloader revision: %d", bl_rev);
+			} else {
+				log("found unsupported bootloader revision %d, exiting", bl_rev);
+				return OK;
+			}
+		}
+
 		ret = erase();
 
 		if (ret != OK) {
@@ -124,7 +179,11 @@ PX4IO_Uploader::upload(const char *filenames[])
 			continue;
 		}
 
-		ret = verify();
+		if (bl_rev <= 2)
+			ret = verify_rev2();
+		else if(bl_rev == 3) {
+			ret = verify_rev3();
+		}
 
 		if (ret != OK) {
 			log("verify failed");
@@ -190,6 +249,7 @@ PX4IO_Uploader::drain()
 
 	do {
 		ret = recv(c, 250);
+
 		if (ret == OK) {
 			//log("discard 0x%02x", c);
 		}
@@ -319,7 +379,7 @@ PX4IO_Uploader::program()
 }
 
 int
-PX4IO_Uploader::verify()
+PX4IO_Uploader::verify_rev2()
 {
 	uint8_t	file_buf[PROG_MULTI_MAX];
 	ssize_t count;
@@ -379,6 +439,74 @@ PX4IO_Uploader::verify()
 	return OK;
 }
 
+int
+PX4IO_Uploader::verify_rev3()
+{
+	int ret;
+	uint8_t	file_buf[4];
+	ssize_t count;
+	uint32_t sum = 0;
+	uint32_t bytes_read = 0;
+	uint32_t fw_size = 0;
+	uint32_t crc = 0;
+	uint8_t fill_blank = 0xff;
+
+	log("verify...");
+	lseek(_fw_fd, 0, SEEK_SET);
+
+	ret = get_info(INFO_FLASH_SIZE, fw_size);
+	send(PROTO_EOC);
+
+	if (ret != OK) {
+		log("could not read firmware size");
+		return ret;
+	}
+
+	/* read through the firmware file again and calculate the checksum*/
+	while (true) {
+		lseek(_fw_fd, 0, SEEK_CUR);
+		count = read(_fw_fd, file_buf, sizeof(file_buf));
+
+		/* set the rest to ff */
+		if (count == 0) {
+			break;
+		}
+		/* stop if the file cannot be read */
+		if (count < 0)
+			return -errno;
+
+		/* calculate crc32 sum */
+		sum = crc32((uint8_t *)&file_buf, sizeof(file_buf), sum);
+
+		bytes_read += count;
+	}
+
+	/* fill the rest with 0xff */
+	while (bytes_read < fw_size) {
+		sum = crc32(&fill_blank, sizeof(fill_blank), sum);
+		bytes_read += sizeof(fill_blank);
+	}
+
+	/* request CRC from IO */
+	send(PROTO_GET_CRC);
+	send(PROTO_EOC);
+
+	ret = recv((uint8_t*)(&crc), sizeof(crc));
+
+	if (ret != OK) {
+		log("did not receive CRC checksum");
+		return ret;
+	}
+
+	/* compare the CRC sum from the IO with the one calculated */
+	if (sum != crc) {
+		log("CRC wrong: received: %d, expected: %d", crc, sum);
+		return -EINVAL;
+	}
+
+	return OK;
+}
+
 int
 PX4IO_Uploader::reboot()
 {

apps/drivers/px4io/uploader.h

@@ -64,21 +64,25 @@ private:
 		PROTO_CHIP_VERIFY	= 0x24,
 		PROTO_PROG_MULTI	= 0x27,
 		PROTO_READ_MULTI	= 0x28,
+		PROTO_GET_CRC		= 0x29,
 		PROTO_REBOOT		= 0x30,
 
 		INFO_BL_REV		= 1,		/**< bootloader protocol revision */
-		BL_REV			= 2,		/**< supported bootloader protocol  */
+		BL_REV			= 3,		/**< supported bootloader protocol  */
 		INFO_BOARD_ID		= 2,		/**< board type */
 		INFO_BOARD_REV		= 3,		/**< board revision */
 		INFO_FLASH_SIZE		= 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 */
+
 	};
 
 	int			_io_fd;
 	int			_fw_fd;
 
+	uint32_t	bl_rev; /**< bootloader revision */
+
 	void			log(const char *fmt, ...);
 
 	int			recv(uint8_t &c, unsigned timeout = 1000);
@@ -91,7 +95,8 @@ private:
 	int			get_info(int param, uint32_t &val);
 	int			erase();
 	int			program();
-	int			verify();
+	int			verify_rev2();
+	int			verify_rev3();
 	int			reboot();
 	int			compare(bool &identical);
 };

apps/drivers/stm32/adc/Makefile

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# STM32 ADC driver
+#
+
+APPNAME		 = adc
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk

apps/drivers/stm32/adc/adc.cpp

@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   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 adc.cpp
+ *
+ * Driver for the STM32 ADC.
+ *
+ * This is a low-rate driver, designed for sampling things like voltages
+ * and so forth. It avoids the gross complexity of the NuttX ADC driver.
+ */
+
+#include <nuttx/config.h>
+#include <drivers/device/device.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <stdio.h>
+#include <unistd.h>
+
+#include <arch/board/board.h>
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_adc.h>
+
+#include <arch/stm32/chip.h>
+#include <stm32_internal.h>
+#include <stm32_gpio.h>
+
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+
+/*
+ * Register accessors.
+ * For now, no reason not to just use ADC1.
+ */
+#define REG(_reg)	(*(volatile uint32_t *)(STM32_ADC1_BASE + _reg))
+
+#define rSR		REG(STM32_ADC_SR_OFFSET)
+#define rCR1		REG(STM32_ADC_CR1_OFFSET)
+#define rCR2		REG(STM32_ADC_CR2_OFFSET)
+#define rSMPR1		REG(STM32_ADC_SMPR1_OFFSET)
+#define rSMPR2		REG(STM32_ADC_SMPR2_OFFSET)
+#define rJOFR1		REG(STM32_ADC_JOFR1_OFFSET)
+#define rJOFR2		REG(STM32_ADC_JOFR2_OFFSET)
+#define rJOFR3		REG(STM32_ADC_JOFR3_OFFSET)
+#define rJOFR4		REG(STM32_ADC_JOFR4_OFFSET)
+#define rHTR		REG(STM32_ADC_HTR_OFFSET)
+#define rLTR		REG(STM32_ADC_LTR_OFFSET)
+#define rSQR1		REG(STM32_ADC_SQR1_OFFSET)
+#define rSQR2		REG(STM32_ADC_SQR2_OFFSET)
+#define rSQR3		REG(STM32_ADC_SQR3_OFFSET)
+#define rJSQR		REG(STM32_ADC_JSQR_OFFSET)
+#define rJDR1		REG(STM32_ADC_JDR1_OFFSET)
+#define rJDR2		REG(STM32_ADC_JDR2_OFFSET)
+#define rJDR3		REG(STM32_ADC_JDR3_OFFSET)
+#define rJDR4		REG(STM32_ADC_JDR4_OFFSET)
+#define rDR		REG(STM32_ADC_DR_OFFSET)
+
+#ifdef STM32_ADC_CCR
+# define rCCR		REG(STM32_ADC_CCR_OFFSET)
+#endif
+
+class ADC : public device::CDev
+{
+public:
+	ADC(uint32_t channels);
+	~ADC();
+
+	virtual int		init();
+
+	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+	virtual ssize_t		read(file *filp, char *buffer, size_t len);
+
+protected:
+	virtual int		open_first(struct file *filp);
+	virtual int		close_last(struct file *filp);
+
+private:
+	static const hrt_abstime _tickrate = 10000;	/**< 100Hz base rate */
+	
+	hrt_call		_call;
+	perf_counter_t		_sample_perf;
+
+	unsigned		_channel_count;
+	adc_msg_s		*_samples;		/**< sample buffer */
+
+	/** work trampoline */
+	static void		_tick_trampoline(void *arg);
+
+	/** worker function */
+	void			_tick();
+
+	/**
+	 * Sample a single channel and return the measured value.
+	 *
+	 * @param channel		The channel to sample.
+	 * @return			The sampled value, or 0xffff if
+	 *				sampling failed.
+	 */
+	uint16_t		_sample(unsigned channel);
+
+};
+
+ADC::ADC(uint32_t channels) :
+	CDev("adc", ADC_DEVICE_PATH),
+	_sample_perf(perf_alloc(PC_ELAPSED, "ADC samples")),
+	_channel_count(0),
+	_samples(nullptr)
+{
+	_debug_enabled = true;
+
+	/* always enable the temperature sensor */
+	channels |= 1 << 16;
+
+	/* allocate the sample array */
+	for (unsigned i = 0; i < 32; i++) {
+		if (channels & (1 << i)) {
+			_channel_count++;
+		}
+	}
+	_samples = new adc_msg_s[_channel_count];
+
+	/* prefill the channel numbers in the sample array */
+	if (_samples != nullptr) {
+		unsigned index = 0;
+		for (unsigned i = 0; i < 32; i++) {
+			if (channels & (1 << i)) {
+				_samples[index].am_channel = i;
+				_samples[index].am_data = 0;
+				index++;
+			}
+		}
+	}
+}
+
+ADC::~ADC()
+{
+	if (_samples != nullptr)
+		delete _samples;
+}
+
+int
+ADC::init()
+{
+	/* do calibration if supported */
+#ifdef ADC_CR2_CAL
+	rCR2 |= ADC_CR2_CAL;
+	usleep(100);
+	if (rCR2 & ADC_CR2_CAL)
+		return -1;
+#endif
+
+	/* arbitrarily configure all channels for 55 cycle sample time */
+	rSMPR1 = 0b00000011011011011011011011011011;
+	rSMPR2 = 0b00011011011011011011011011011011;
+
+	/* XXX for F2/4, might want to select 12-bit mode? */
+	rCR1 = 0;
+
+	/* enable the temperature sensor / Vrefint channel if supported*/
+	rCR2 = 
+#ifdef ADC_CR2_TSVREFE
+		/* enable the temperature sensor in CR2 */
+		ADC_CR2_TSVREFE |
+#endif
+		0;
+
+#ifdef ADC_CCR_TSVREFE
+	/* enable temperature sensor in CCR */
+	rCCR = ADC_CCR_TSVREFE;
+#endif
+
+	/* configure for a single-channel sequence */
+	rSQR1 = 0;
+	rSQR2 = 0;
+	rSQR3 = 0;	/* will be updated with the channel each tick */ 
+
+	/* power-cycle the ADC and turn it on */
+	rCR2 &= ~ADC_CR2_ADON;
+	usleep(10);
+	rCR2 |= ADC_CR2_ADON;
+	usleep(10);
+	rCR2 |= ADC_CR2_ADON;
+	usleep(10);
+
+	/* kick off a sample and wait for it to complete */
+	hrt_abstime now = hrt_absolute_time();
+	rCR2 |= ADC_CR2_SWSTART;
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* don't wait for more than 500us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 500) {
+			log("sample timeout");
+			return -1;
+			return 0xffff;
+		}
+	}
+
+
+	debug("init done");
+
+	/* create the device node */
+	return CDev::init();
+}
+
+int
+ADC::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	return -ENOTTY;
+}
+
+ssize_t
+ADC::read(file *filp, char *buffer, size_t len)
+{
+	const size_t maxsize = sizeof(adc_msg_s) * _channel_count;
+
+	if (len > maxsize)
+		len = maxsize;
+
+	/* block interrupts while copying samples to avoid racing with an update */
+	irqstate_t flags = irqsave();
+	memcpy(buffer, _samples, len);
+	irqrestore(flags);
+
+	return len;
+}
+
+int
+ADC::open_first(struct file *filp)
+{
+	/* get fresh data */
+	_tick();
+
+	/* and schedule regular updates */
+	hrt_call_every(&_call, _tickrate, _tickrate, _tick_trampoline, this);
+
+	return 0;
+}
+
+int
+ADC::close_last(struct file *filp)
+{
+	hrt_cancel(&_call);
+	return 0;
+}
+
+void
+ADC::_tick_trampoline(void *arg)
+{
+	((ADC *)arg)->_tick();
+}
+
+void
+ADC::_tick()
+{
+	/* scan the channel set and sample each */
+	for (unsigned i = 0; i < _channel_count; i++)
+		_samples[i].am_data = _sample(_samples[i].am_channel);
+}
+
+uint16_t
+ADC::_sample(unsigned channel)
+{
+	perf_begin(_sample_perf);
+
+	/* clear any previous EOC */
+	if (rSR & ADC_SR_EOC)
+		rSR &= ~ADC_SR_EOC;
+
+	/* run a single conversion right now - should take about 60 cycles (a few microseconds) max */
+	rSQR3 = channel;
+	rCR2 |= ADC_CR2_SWSTART;
+
+	/* wait for the conversion to complete */
+	hrt_abstime now = hrt_absolute_time();
+	while (!(rSR & ADC_SR_EOC)) {
+
+		/* don't wait for more than 50us, since that means something broke - should reset here if we see this */
+		if ((hrt_absolute_time() - now) > 50) {
+			log("sample timeout");
+			return 0xffff;
+		}
+	}
+
+	/* read the result and clear EOC */
+	uint16_t result = rDR;
+
+	perf_end(_sample_perf);
+	return result;
+}
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int adc_main(int argc, char *argv[]);
+
+namespace
+{
+ADC	*g_adc;
+
+void
+test(void)
+{
+
+	int fd = open(ADC_DEVICE_PATH, O_RDONLY);
+	if (fd < 0)
+		err(1, "can't open ADC device");
+
+	for (unsigned i = 0; i < 50; i++) {
+		adc_msg_s data[8];
+		ssize_t count = read(fd, data, sizeof(data));
+
+		if (count < 0)
+			errx(1, "read error");
+
+		unsigned channels = count / sizeof(data[0]);
+
+		for (unsigned j = 0; j < channels; j++) {
+			printf ("%d: %u  ", data[j].am_channel, data[j].am_data);
+		}
+
+		printf("\n");
+		usleep(500000);
+	}
+
+	exit(0);
+}
+}
+
+int
+adc_main(int argc, char *argv[])
+{
+	if (g_adc == nullptr) {
+		/* XXX this hardcodes the default channel set for PX4FMU - should be configurable */
+		g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13));
+
+		if (g_adc == nullptr)
+			errx(1, "couldn't allocate the ADC driver");
+
+		if (g_adc->init() != OK) {
+			delete g_adc;
+			errx(1, "ADC init failed");
+		}
+	}
+
+	if (argc > 1) {
+		if (!strcmp(argv[1], "test"))
+			test();
+	}
+
+	exit(0);
+}

apps/examples/Kconfig

@@ -3,206 +3,60 @@
 # see misc/tools/kconfig-language.txt.
 #
 
-menu "ADC Example"
 source "$APPSDIR/examples/adc/Kconfig"
-endmenu
-
-menu "Buttons Example"
 source "$APPSDIR/examples/buttons/Kconfig"
-endmenu
-
-menu "CAN Example"
 source "$APPSDIR/examples/can/Kconfig"
-endmenu
-
-menu "USB CDC/ACM Class Driver Example"
 source "$APPSDIR/examples/cdcacm/Kconfig"
-endmenu
-
-menu "USB composite Class Driver Example"
 source "$APPSDIR/examples/composite/Kconfig"
-endmenu
+source "$APPSDIR/examples/cxxtest/Kconfig"
-
-menu "DHCP Server Example"
 source "$APPSDIR/examples/dhcpd/Kconfig"
-endmenu
+source "$APPSDIR/examples/elf/Kconfig"
-
-menu "FTP Client Example"
 source "$APPSDIR/examples/ftpc/Kconfig"
-endmenu
-
-menu "FTP Server Example"
 source "$APPSDIR/examples/ftpd/Kconfig"
-endmenu
-
-menu "\"Hello, World!\" Example"
 source "$APPSDIR/examples/hello/Kconfig"
-endmenu
-
-menu "\"Hello, World!\" C++ Example"
 source "$APPSDIR/examples/helloxx/Kconfig"
-endmenu
+source "$APPSDIR/examples/json/Kconfig"
-
-menu "USB HID Keyboard Example"
 source "$APPSDIR/examples/hidkbd/Kconfig"
-endmenu
+source "$APPSDIR/examples/keypadtest/Kconfig"
-
-menu "IGMP Example"
 source "$APPSDIR/examples/igmp/Kconfig"
-endmenu
-
-menu "LCD Read/Write Example"
 source "$APPSDIR/examples/lcdrw/Kconfig"
-endmenu
-
-menu "Memory Management Example"
 source "$APPSDIR/examples/mm/Kconfig"
-endmenu
-
-menu "File System Mount Example"
 source "$APPSDIR/examples/mount/Kconfig"
-endmenu
-
-menu "FreeModBus Example"
 source "$APPSDIR/examples/modbus/Kconfig"
-endmenu
-
-menu "Network Test Example"
 source "$APPSDIR/examples/nettest/Kconfig"
-endmenu
-
-menu "NuttShell (NSH) Example"
 source "$APPSDIR/examples/nsh/Kconfig"
-endmenu
-
-menu "NULL Example"
 source "$APPSDIR/examples/null/Kconfig"
-endmenu
-
-menu "NX Graphics Example"
 source "$APPSDIR/examples/nx/Kconfig"
-endmenu
-
-menu "NxConsole Example"
 source "$APPSDIR/examples/nxconsole/Kconfig"
-endmenu
-
-menu "NXFFS File System Example"
 source "$APPSDIR/examples/nxffs/Kconfig"
-endmenu
-
-menu "NXFLAT Example"
 source "$APPSDIR/examples/nxflat/Kconfig"
-endmenu
-
-menu "NX Graphics \"Hello, World!\" Example"
 source "$APPSDIR/examples/nxhello/Kconfig"
-endmenu
-
-menu "NX Graphics image Example"
 source "$APPSDIR/examples/nximage/Kconfig"
-endmenu
-
-menu "NX Graphics lines Example"
 source "$APPSDIR/examples/nxlines/Kconfig"
-endmenu
-
-menu "NX Graphics Text Example"
 source "$APPSDIR/examples/nxtext/Kconfig"
-endmenu
-
-menu "OS Test Example"
 source "$APPSDIR/examples/ostest/Kconfig"
-endmenu
-
-menu "Pascal \"Hello, World!\"example"
 source "$APPSDIR/examples/pashello/Kconfig"
-endmenu
-
-menu "Pipe Example"
 source "$APPSDIR/examples/pipe/Kconfig"
-endmenu
-
-menu "Poll Example"
 source "$APPSDIR/examples/poll/Kconfig"
-endmenu
-
-menu "Pulse Width Modulation (PWM) Example"
 source "$APPSDIR/examples/pwm/Kconfig"
-endmenu
-
-menu "Quadrature Encoder Example"
 source "$APPSDIR/examples/qencoder/Kconfig"
-endmenu
+source "$APPSDIR/examples/relays/Kconfig"
-
-menu "RGMP Example"
 source "$APPSDIR/examples/rgmp/Kconfig"
-endmenu
-
-menu "ROMFS Example"
 source "$APPSDIR/examples/romfs/Kconfig"
-endmenu
-
-menu "sendmail Example"
 source "$APPSDIR/examples/sendmail/Kconfig"
-endmenu
-
-menu "Serial Loopback Example"
 source "$APPSDIR/examples/serloop/Kconfig"
-endmenu
-
-menu "Telnet Daemon Example"
 source "$APPSDIR/examples/telnetd/Kconfig"
-endmenu
-
-menu "THTTPD Web Server Example"
 source "$APPSDIR/examples/thttpd/Kconfig"
-endmenu
-
-menu "TIFF Generation Example"
 source "$APPSDIR/examples/tiff/Kconfig"
-endmenu
-
-menu "Touchscreen Example"
 source "$APPSDIR/examples/touchscreen/Kconfig"
-endmenu
-
-menu "UDP Example"
 source "$APPSDIR/examples/udp/Kconfig"
-endmenu
-
-menu "UDP Discovery Daemon Example"
 source "$APPSDIR/examples/discover/Kconfig"
-endmenu
-
-menu "uIP Web Server Example"
 source "$APPSDIR/examples/uip/Kconfig"
-endmenu
-
-menu "USB Serial Test Example"
 source "$APPSDIR/examples/usbserial/Kconfig"
-endmenu
-
-menu "USB Mass Storage Class Example"
 source "$APPSDIR/examples/usbstorage/Kconfig"
-endmenu
-
-menu "USB Serial Terminal Example"
 source "$APPSDIR/examples/usbterm/Kconfig"
-endmenu
-
-menu "Watchdog timer Example"
 source "$APPSDIR/examples/watchdog/Kconfig"
-endmenu
-
-menu "wget Example"
 source "$APPSDIR/examples/wget/Kconfig"
-endmenu
+source "$APPSDIR/examples/wgetjson/Kconfig"
-
-menu "WLAN Example"
 source "$APPSDIR/examples/wlan/Kconfig"
-endmenu
-
-menu "XML RPC Example"
 source "$APPSDIR/examples/xmlrpc/Kconfig"
-endmenu

apps/examples/Make.defs

@@ -54,6 +54,10 @@ ifeq ($(CONFIG_EXAMPLES_COMPOSITE),y)
 CONFIGURED_APPS += examples/composite
 endif
 
+ifeq ($(CONFIG_EXAMPLES_CXXTEST),y)
+CONFIGURED_APPS += examples/cxxtest
+endif
+
 ifeq ($(CONFIG_EXAMPLES_DHCPD),y)
 CONFIGURED_APPS += examples/dhcpd
 endif
@@ -62,6 +66,10 @@ ifeq ($(CONFIG_EXAMPLES_DISCOVER),y)
 CONFIGURED_APPS += examples/discover
 endif
 
+ifeq ($(CONFIG_EXAMPLES_ELF),y)
+CONFIGURED_APPS += examples/elf
+endif
+
 ifeq ($(CONFIG_EXAMPLES_FTPC),y)
 CONFIGURED_APPS += examples/ftpc
 endif
@@ -86,6 +94,14 @@ ifeq ($(CONFIG_EXAMPLES_IGMP),y)
 CONFIGURED_APPS += examples/igmp
 endif
 
+ifeq ($(CONFIG_EXAMPLES_JSON),y)
+CONFIGURED_APPS += examples/json
+endif
+
+ifeq ($(CONFIG_EXAMPLES_KEYPADTEST),y)
+CONFIGURED_APPS += examples/keypadtest
+endif
+
 ifeq ($(CONFIG_EXAMPLES_LCDRW),y)
 CONFIGURED_APPS += examples/lcdrw
 endif
@@ -94,6 +110,10 @@ ifeq ($(CONFIG_EXAMPLES_MM),y)
 CONFIGURED_APPS += examples/mm
 endif
 
+ifeq ($(CONFIG_EXAMPLES_MODBUS),y)
+CONFIGURED_APPS += examples/modbus
+endif
+
 ifeq ($(CONFIG_EXAMPLES_MOUNT),y)
 CONFIGURED_APPS += examples/mount
 endif
@@ -166,6 +186,10 @@ ifeq ($(CONFIG_EXAMPLES_QENCODER),y)
 CONFIGURED_APPS += examples/qencoder
 endif
 
+ifeq ($(CONFIG_EXAMPLES_RELAYS),y)
+CONFIGURED_APPS += examples/relays
+endif
+
 ifeq ($(CONFIG_EXAMPLES_RGMP),y)
 CONFIGURED_APPS += examples/rgmp
 endif
@@ -226,6 +250,10 @@ ifeq ($(CONFIG_EXAMPLES_WGET),y)
 CONFIGURED_APPS += examples/wget
 endif
 
+ifeq ($(CONFIG_EXAMPLES_WGETJSON),y)
+CONFIGURED_APPS += examples/wgetjson
+endif
+
 ifeq ($(CONFIG_EXAMPLES_WLAN),y)
 CONFIGURED_APPS += examples/wlan
 endif