Merge branch 'master' into gpio_led

Makefile

@@ -159,11 +159,11 @@ $(NUTTX_ARCHIVES): $(ARCHIVE_DIR)%.export: $(NUTTX_SRC) $(NUTTX_APPS)
 .PHONY:	clean
 clean:
 	$(Q) $(RMDIR) $(BUILD_DIR)*.build
-	$(Q) $(REMOVE) -f $(IMAGE_DIR)*.px4
+	$(Q) $(REMOVE) $(IMAGE_DIR)*.px4
 
 .PHONY:	distclean
 distclean: clean
-	$(Q) $(REMOVE) -f $(ARCHIVE_DIR)*.export
+	$(Q) $(REMOVE) $(ARCHIVE_DIR)*.export
 	$(Q) make -C $(NUTTX_SRC) -r $(MQUIET) distclean
 
 #
@@ -196,6 +196,11 @@ help:
 	@echo "  distclean"
 	@echo "    Remove all compilation products, including NuttX RTOS archives."
 	@echo ""
+	@echo "  upload"
+	@echo "    When exactly one config is being built, add this target to upload the"
+	@echo "    firmware to the board when the build is complete. Not supported for"
+	@echo "    all configurations."
+	@echo ""
 	@echo "  Common options:"
 	@echo "  ---------------"
 	@echo ""

ROMFS/px4fmu_common/init.d/rc.FMU_quad_x

@@ -20,10 +20,10 @@ uorb start
 # Load microSD params
 #
 echo "[init] loading microSD params"
-param select /fs/microsd/parameters
+param select /fs/microsd/params
-if [ -f /fs/microsd/parameters ]
+if [ -f /fs/microsd/params ]
 then
-	param load /fs/microsd/parameters
+	param load /fs/microsd/params
 fi
  
 #

ROMFS/px4fmu_common/init.d/rc.IO_QUAD

@@ -13,10 +13,10 @@ uorb start
 # Load microSD params
 #
 echo "[init] loading microSD params"
-param select /fs/microsd/parameters
+param select /fs/microsd/params
-if [ -f /fs/microsd/parameters ]
+if [ -f /fs/microsd/params ]
 then
-	param load /fs/microsd/parameters
+	param load /fs/microsd/params
 fi
  
 #

ROMFS/px4fmu_common/init.d/rc.PX4IO

@@ -13,10 +13,10 @@ uorb start
 # Load microSD params
 #
 echo "[init] loading microSD params"
-param select /fs/microsd/parameters
+param select /fs/microsd/params
-if [ -f /fs/microsd/parameters ]
+if [ -f /fs/microsd/params ]
 then
-	param load /fs/microsd/parameters
+	param load /fs/microsd/params
 fi
  
 #

ROMFS/px4fmu_common/init.d/rc.PX4IOAR

@@ -17,13 +17,13 @@ echo "[init] doing PX4IOAR startup..."
 uorb start
  
 #
-# Init the parameter storage
+# Load microSD params
 #
 echo "[init] loading microSD params"
-param select /fs/microsd/parameters
+param select /fs/microsd/params
-if [ -f /fs/microsd/parameters ]
+if [ -f /fs/microsd/params ]
 then
-	param load /fs/microsd/parameters
+	param load /fs/microsd/params
 fi
 
 #

ROMFS/px4fmu_common/init.d/rc.hil

@@ -17,10 +17,10 @@ hil mode_pwm
 # Load microSD params
 #
 echo "[init] loading microSD params"
-param select /fs/microsd/parameters
+param select /fs/microsd/params
-if [ -f /fs/microsd/parameters ]
+if [ -f /fs/microsd/params ]
 then
-	param load /fs/microsd/parameters
+	param load /fs/microsd/params
 fi
 
 #

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -7,6 +7,14 @@
 # Start sensor drivers here.
 #
 
+#
+# Check for UORB
+#
+if uorb start
+then
+	echo "uORB started"
+fi
+
 ms5611 start
 adc start
 

makefiles/README.txt

@@ -0,0 +1,71 @@
+PX4 Build System
+================
+
+The files in this directory implement the PX4 runtime firmware build system
+and configuration for the standard PX4 boards and software, in conjunction
+with Makefile in the parent directory.
+
+../Makefile
+
+	Top-level makefile for the PX4 build system. This makefile supports
+	building NuttX archives, as well as supervising the building of all
+	of the defined PX4 firmware configurations.
+
+	Try 'make help' in the parent directory for documentation.
+
+firmware.mk
+
+	Manages the build for one specific firmware configuration. 
+	See the comments at the top of this file for detailed documentation.
+
+	Builds modules, builtin command lists and the ROMFS (if configured).
+
+	This is the makefile directly used by external build systems; it can
+	be configured to compile modules both inside and outside the PX4
+	source tree. When used in this mode, at least BOARD, MODULES and 
+	CONFIG_FILE must be set.
+
+module.mk
+
+	Called by firmware.mk to build individual modules.
+	See the comments at the top of this file for detailed documentation.
+
+	Not normally used other than by firmware.mk.
+
+nuttx.mk
+
+	Called by ../Makefile to build or download the NuttX archives.
+
+upload.mk
+
+	Called by ../Makefile to upload files to a target board. Can be used
+	by external build systems as well.
+
+setup.mk
+
+	Provides common path and tool definitions. Implements host system-specific
+	compatibility hacks.
+
+board_<boardname>.mk
+
+	Board-specific configuration for <boardname>. Typically sets CONFIG_ARCH
+	and then includes the toolchain definition for the board.
+
+config_<boardname>_<configname>.mk
+
+	Parameters for a specific configuration on a specific board.
+	The board name is derived from the filename.  Sets MODULES to select
+	source modules to be included in the configuration, may also set
+	ROMFS_ROOT to build a ROMFS and BUILTIN_COMMANDS to include non-module
+	commands (e.g. from NuttX)
+
+toolchain_<toolchainname>.mk
+
+	Provides macros used to compile and link source files.
+	Accepts EXTRADEFINES to add additional pre-processor symbol definitions,
+	EXTRACFLAGS, EXTRACXXFLAGS, EXTRAAFLAGS and EXTRALDFLAGS to pass
+	additional flags to the C compiler, C++ compiler, assembler and linker
+	respectively.
+
+	Defines the COMPILE, COMPILEXX, ASSEMBLE, PRELINK, ARCHIVE and LINK
+	macros that are used elsewhere in the build system.

makefiles/config_px4fmu_default.mk

@@ -106,6 +106,10 @@ MODULES		+= modules/uORB
 # https://pixhawk.ethz.ch/px4/dev/debug_values
 #MODULES		+= examples/px4_mavlink_debug
 
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+MODULES			+= examples/fixedwing_control
+
 #
 # Transitional support - add commands from the NuttX export archive.
 #

makefiles/firmware.mk

@@ -201,9 +201,9 @@ MODULES			:= $(sort $(MODULES))
 # locate the first instance of a module by full path or by looking on the
 # module search path
 define MODULE_SEARCH
-	$(abspath $(firstword $(wildcard $(1)/module.mk) \
+	$(firstword $(abspath $(wildcard $(1)/module.mk)) \
-		$(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk)) \
+		$(abspath $(foreach search_dir,$(MODULE_SEARCH_DIRS),$(wildcard $(search_dir)/$(1)/module.mk))) \
-		MISSING_$1))
+		MISSING_$1)
 endef
 
 # make a list of module makefiles and check that we found them all
@@ -223,12 +223,15 @@ MODULE_OBJS		:= $(foreach path,$(dir $(MODULE_MKFILES)),$(WORK_DIR)$(path)module
 .PHONY: $(MODULE_OBJS)
 $(MODULE_OBJS):		relpath = $(patsubst $(WORK_DIR)%,%,$@)
 $(MODULE_OBJS):		mkfile = $(patsubst %module.pre.o,%module.mk,$(relpath))
+$(MODULE_OBJS):		workdir = $(@D)
 $(MODULE_OBJS):		$(GLOBAL_DEPS) $(NUTTX_CONFIG_HEADER)
+	$(Q) $(MKDIR) -p $(workdir)
 	$(Q) $(MAKE) -r -f $(PX4_MK_DIR)module.mk \
-		MODULE_WORK_DIR=$(dir $@) \
+		-C $(workdir) \
+		MODULE_WORK_DIR=$(workdir) \
 		MODULE_OBJ=$@ \
 		MODULE_MK=$(mkfile) \
-		MODULE_NAME=$(lastword $(subst /, ,$(@D))) \
+		MODULE_NAME=$(lastword $(subst /, ,$(workdir))) \
 		module
 
 # make a list of phony clean targets for modules
@@ -266,14 +269,18 @@ endif
 #
 
 # Add dependencies on anything in the ROMFS root
-ROMFS_DEPS		+= $(wildcard \
+ROMFS_FILES		+= $(wildcard \
-			     (ROMFS_ROOT)/* \
+			     $(ROMFS_ROOT)/* \
-			     (ROMFS_ROOT)/*/* \
+			     $(ROMFS_ROOT)/*/* \
-			     (ROMFS_ROOT)/*/*/* \
+			     $(ROMFS_ROOT)/*/*/* \
-			     (ROMFS_ROOT)/*/*/*/* \
+			     $(ROMFS_ROOT)/*/*/*/* \
-			     (ROMFS_ROOT)/*/*/*/*/* \
+			     $(ROMFS_ROOT)/*/*/*/*/* \
-			     (ROMFS_ROOT)/*/*/*/*/*/*)
+			     $(ROMFS_ROOT)/*/*/*/*/*/*)
-ROMFS_IMG		 = $(WORK_DIR)romfs.img
+ifeq ($(ROMFS_FILES),)
+$(error ROMFS_ROOT $(ROMFS_ROOT) specifies a directory containing no files)
+endif
+ROMFS_DEPS		+= $(ROMFS_FILES)
+ROMFS_IMG		 = romfs.img
 ROMFS_CSRC		 = $(ROMFS_IMG:.img=.c)
 ROMFS_OBJ		 = $(ROMFS_CSRC:.c=.o)
 LIBS			+= $(ROMFS_OBJ)

makefiles/module.mk

@@ -39,6 +39,10 @@
 # symbols, as the global namespace is shared between all modules. Normally an 
 # module will just export one or more <command>_main functions.
 #
+# IMPORTANT NOTE:
+#
+# This makefile assumes it is being invoked in the module's output directory.
+#
 
 #
 # Variables that can be set by the module's module.mk:
@@ -179,26 +183,30 @@ CXXFLAGS	+= -fvisibility=$(DEFAULT_VISIBILITY) -include $(PX4_INCLUDE_DIR)visibi
 #
 module:			$(MODULE_OBJ) $(MODULE_COMMAND_FILES)
 
+##
+## Locate sources (allows relative source paths in module.mk)
+##
+#define SRC_SEARCH
+#	$(abspath $(firstword $(wildcard $1) $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
+#endef
 #
-# Locate sources (allows relative source paths in module.mk)
+#ABS_SRCS		?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
+#MISSING_SRCS		:= $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
+#ifneq ($(MISSING_SRCS),)
+#$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
+#endif
+#ifeq ($(ABS_SRCS),)
+#$(error $(MODULE_MK): nothing to compile in SRCS)
+#endif
 #
-define SRC_SEARCH
+##
-	$(abspath $(firstword $(wildcard $1) $(wildcard $(MODULE_SRC)/$1) MISSING_$1))
+## Object files we will generate from sources
-endef
+##
+#OBJS			:= $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
 
-ABS_SRCS		?= $(foreach src,$(SRCS),$(call SRC_SEARCH,$(src)))
+OBJS		 = $(addsuffix .o,$(SRCS))
-MISSING_SRCS		:= $(subst MISSING_,,$(filter MISSING_%,$(ABS_SRCS)))
+$(info SRCS $(SRCS))
-ifneq ($(MISSING_SRCS),)
+$(info OBJS $(OBJS))
-$(error $(MODULE_MK): missing in SRCS: $(MISSING_SRCS))
-endif
-ifeq ($(ABS_SRCS),)
-$(error $(MODULE_MK): nothing to compile in SRCS)
-endif
-
-#
-# Object files we will generate from sources
-#
-OBJS			:= $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
 
 #
 # SRCS -> OBJS rules
@@ -206,13 +214,16 @@ OBJS			:= $(foreach src,$(ABS_SRCS),$(MODULE_WORK_DIR)$(src).o)
 
 $(OBJS):		$(GLOBAL_DEPS)
 
-$(filter %.c.o,$(OBJS)): $(MODULE_WORK_DIR)%.c.o: %.c $(GLOBAL_DEPS)
+vpath %.c $(MODULE_SRC)
+$(filter %.c.o,$(OBJS)): %.c.o: %.c $(GLOBAL_DEPS)
 	$(call COMPILE,$<,$@)
 
-$(filter %.cpp.o,$(OBJS)): $(MODULE_WORK_DIR)%.cpp.o: %.cpp $(GLOBAL_DEPS)
+vpath %.cpp $(MODULE_SRC)
+$(filter %.cpp.o,$(OBJS)): %.cpp.o: %.cpp $(GLOBAL_DEPS)
 	$(call COMPILEXX,$<,$@)
 
-$(filter %.S.o,$(OBJS)): $(MODULE_WORK_DIR)%.S.o: %.S $(GLOBAL_DEPS)
+vpath %.S $(MODULE_SRC)
+$(filter %.S.o,$(OBJS)): %.S.o: %.S $(GLOBAL_DEPS)
 	$(call ASSEMBLE,$<,$@)
 
 #

makefiles/toolchain_gnu-arm-eabi.mk

@@ -254,6 +254,20 @@ endef
 # - relink the object and insert the binary file
 # - edit symbol names to suit
 #
+# NOTE: exercise caution using this with absolute pathnames; it looks
+#       like the MinGW tools insert an extra _ in the binary symbol name; e.g.
+#	the path:
+#
+#	/d/px4/firmware/Build/px4fmu_default.build/romfs.img
+#
+#	is assigned symbols like:
+#
+#	_binary_d__px4_firmware_Build_px4fmu_default_build_romfs_img_size
+#
+#	when we would expect
+#
+#	_binary__d_px4_firmware_Build_px4fmu_default_build_romfs_img_size
+#
 define BIN_SYM_PREFIX
 	_binary_$(subst /,_,$(subst .,_,$1))
 endef
@@ -267,4 +281,5 @@ define BIN_TO_OBJ
 		--redefine-sym $(call BIN_SYM_PREFIX,$1)_start=$3 \
 		--redefine-sym $(call BIN_SYM_PREFIX,$1)_size=$3_len \
 		--strip-symbol $(call BIN_SYM_PREFIX,$1)_end
+	$(Q) $(REMOVE) $2.c $2.c.o
 endef

src/drivers/ardrone_interface/ardrone_motor_control.c

@@ -482,10 +482,10 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
 
 	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
+	motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
-	motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
+	motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
-	motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
+	motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
-	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
+	motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
 
 	/* send motors via UART */
 	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);

src/drivers/drv_pwm_output.h

@@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm);
 /** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
 #define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
 
+/** set the 'ARM ok' bit, which activates the safety switch */
+#define PWM_SERVO_SET_ARM_OK	_IOC(_PWM_SERVO_BASE, 5)
+
+/** clear the 'ARM ok' bit, which deactivates the safety switch */
+#define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 

src/drivers/px4fmu/fmu.cpp

@@ -606,6 +606,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(true);
 		break;
 
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+		// these are no-ops, as no safety switch
+		break;
+
 	case PWM_SERVO_DISARM:
 		up_pwm_servo_arm(false);
 		break;

src/drivers/px4io/px4io.cpp

@@ -416,7 +416,7 @@ PX4IO::init()
 	 * already armed, and has been configured for in-air restart
 	 */
 	if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
-	    (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+	    (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 
 	    	mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
 
@@ -500,10 +500,9 @@ PX4IO::init()
 
 		/* dis-arm IO before touching anything */
 		io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 
-			PX4IO_P_SETUP_ARMING_ARM_OK |
+			PX4IO_P_SETUP_ARMING_FMU_ARMED |
 			PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
-			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+			PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
-			PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
 
 		/* publish RC config to IO */
 		ret = io_set_rc_config();
@@ -702,16 +701,18 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (armed.armed) {
+	if (armed.armed && !armed.lockdown) {
-		set |= PX4IO_P_SETUP_ARMING_ARM_OK;
+		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
-		clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
+		clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	}
-	if (vstatus.flag_vector_flight_mode_ok) {
+
-		set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
+	if (armed.ready_to_arm) {
+		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	} else {
-		clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;					
+		clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 	}
+
 	if (vstatus.flag_external_manual_override_ok) {
 		set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 	} else {
@@ -1277,9 +1278,9 @@ PX4IO::print_status()
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT),
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE));
 	printf("amp_per_volt %.3f amp_offset %.3f mAhDischarged %.3f\n",
-	       _battery_amp_per_volt,
+	       (double)_battery_amp_per_volt,
-	       _battery_amp_bias,
+	       (double)_battery_amp_bias,
-	       _battery_mamphour_total);
+	       (double)_battery_mamphour_total);
 	printf("actuators");
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
@@ -1311,9 +1312,9 @@ PX4IO::print_status()
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_ARM_OK)             ? " ARM_OK" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
+		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK)   ? " VECTOR_FLIGHT_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""));
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 		io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -1354,12 +1355,22 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 	switch (cmd) {
 	case PWM_SERVO_ARM:
 		/* set the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FMU_ARMED);
+		break;
+
+	case PWM_SERVO_SET_ARM_OK:
+		/* set the 'OK to arm' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_IO_ARM_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_ARM_OK:
+		/* clear the 'OK to arm' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_IO_ARM_OK, 0);
 		break;
 
 	case PWM_SERVO_DISARM:
 		/* clear the 'armed' bit */
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED, 0);
 		break;
 
 	case PWM_SERVO_SET_UPDATE_RATE:

src/examples/fixedwing_control/main.c

@@ -0,0 +1,474 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file main.c
+ * Implementation of a fixed wing attitude controller. This file is a complete
+ * fixed wing controller flying manual attitude control or auto waypoint control.
+ * There is no need to touch any other system components to extend / modify the
+ * complete control architecture.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/pid/pid.h>
+#include <systemlib/geo/geo.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+
+/* process-specific header files */
+#include "params.h"
+
+/* Prototypes */
+/**
+ * Daemon management function.
+ */
+__EXPORT int ex_fixedwing_control_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int fixedwing_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+	float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, 
+	struct actuator_controls_s *actuators);
+
+void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);
+
+/* Variables */
+static bool thread_should_exit = false;		/**< Daemon exit flag */
+static bool thread_running = false;		/**< Daemon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static struct params p;
+static struct param_handles ph;
+
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+	float speed_body[], float gyro[], struct vehicle_rates_setpoint_s *rates_sp, 
+	struct actuator_controls_s *actuators)
+{
+
+	/* 
+	 * The PX4 architecture provides a mixer outside of the controller.
+	 * The mixer is fed with a default vector of actuator controls, representing
+	 * moments applied to the vehicle frame. This vector
+	 * is structured as:
+	 *
+	 * Control Group 0 (attitude):
+	 * 
+	 *    0  -  roll   (-1..+1)
+	 *    1  -  pitch  (-1..+1)
+	 *    2  -  yaw    (-1..+1)
+	 *    3  -  thrust ( 0..+1)
+	 *    4  -  flaps  (-1..+1)
+	 *    ...
+	 *
+	 * Control Group 1 (payloads / special):
+	 *
+	 *    ...
+	 */
+
+	/*
+	 * Calculate roll error and apply P gain
+	 */
+	float roll_err = att->roll - att_sp->roll_body;
+	actuators->control[0] = roll_err * p.roll_p;
+
+	/*
+	 * Calculate pitch error and apply P gain
+	 */
+	float pitch_err = att->pitch - att_sp->pitch_body;
+	actuators->control[1] = pitch_err * p.pitch_p;
+}
+
+void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
+{
+
+	/*
+	 * Calculate heading error of current position to desired position
+	 */
+
+	/* PX4 uses 1e7 scaled integers to represent global coordinates for max resolution */
+	float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d);
+
+	/* calculate heading error */
+	float yaw_err = att->yaw - bearing;
+	/* apply control gain */
+	att_sp->roll_body = yaw_err * p.hdng_p;
+}
+
+/* Main Thread */
+int fixedwing_control_thread_main(int argc, char *argv[])
+{
+	/* read arguments */
+	bool verbose = false;
+
+	for (int i = 1; i < argc; i++) {
+		if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
+			verbose = true;
+		}
+	}
+
+	/* welcome user (warnx prints a line, including an appended\n, with variable arguments */
+	warnx("[example fixedwing control] started");
+
+	/* initialize parameters, first the handles, then the values */
+	parameters_init(&ph);
+	parameters_update(&ph, &p);
+
+	/* declare and safely initialize all structs to zero */
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_attitude_setpoint_s att_sp;
+	memset(&att_sp, 0, sizeof(att_sp));
+	struct vehicle_rates_setpoint_s rates_sp;
+	memset(&rates_sp, 0, sizeof(rates_sp));
+	struct vehicle_global_position_s global_pos;
+	memset(&global_pos, 0, sizeof(global_pos));
+	struct manual_control_setpoint_s manual_sp;
+	memset(&manual_sp, 0, sizeof(manual_sp));
+	struct vehicle_status_s vstatus;
+	memset(&vstatus, 0, sizeof(vstatus));
+	struct vehicle_global_position_setpoint_s global_sp;
+	memset(&global_sp, 0, sizeof(global_sp));
+
+	/* output structs */
+	struct actuator_controls_s actuators;
+	memset(&actuators, 0, sizeof(actuators));
+
+
+	/* publish actuator controls */
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) {
+		actuators.control[i] = 0.0f;
+	}
+
+	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	orb_advert_t rates_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+
+	/* subscribe */
+	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	int param_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	/* Setup of loop */
+	float gyro[3] = {0.0f, 0.0f, 0.0f};
+	float speed_body[3] = {0.0f, 0.0f, 0.0f};
+	struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
+				{ .fd = att_sub, .events = POLLIN }};
+
+	while (!thread_should_exit) {
+
+		/*
+		 * Wait for a sensor or param update, check for exit condition every 500 ms.
+		 * This means that the execution will block here without consuming any resources,
+		 * but will continue to execute the very moment a new attitude measurement or
+		 * a param update is published. So no latency in contrast to the polling
+		 * design pattern (do not confuse the poll() system call with polling).
+		 *
+		 * This design pattern makes the controller also agnostic of the attitude
+		 * update speed - it runs as fast as the attitude updates with minimal latency.
+		 */
+		int ret = poll(fds, 2, 500);
+
+		if (ret < 0) {
+			/* poll error, this will not really happen in practice */
+			warnx("poll error");
+
+		} else if (ret == 0) {
+			/* no return value = nothing changed for 500 ms, ignore */
+		} else {
+
+			/* only update parameters if they changed */
+			if (fds[0].revents & POLLIN) {
+				/* read from param to clear updated flag (uORB API requirement) */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), param_sub, &update);
+
+				/* if a param update occured, re-read our parameters */
+				parameters_update(&ph, &p);
+			}
+
+			/* only run controller if attitude changed */
+			if (fds[1].revents & POLLIN) {
+
+
+				/* Check if there is a new position measurement or position setpoint */
+				bool pos_updated;
+				orb_check(global_pos_sub, &pos_updated);
+				bool global_sp_updated;
+				orb_check(global_sp_sub, &global_sp_updated);
+
+				/* get a local copy of attitude */
+				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
+
+				if (global_sp_updated)
+					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
+
+				if (pos_updated) {
+					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
+
+					if (att.R_valid) {
+						speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
+						speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
+						speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
+
+					} else {
+						speed_body[0] = 0;
+						speed_body[1] = 0;
+						speed_body[2] = 0;
+
+						warnx("Did not get a valid R\n");
+					}
+				}
+
+				orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
+				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);
+
+				gyro[0] = att.rollspeed;
+				gyro[1] = att.pitchspeed;
+				gyro[2] = att.yawspeed;
+
+				/* control */
+
+				if (vstatus.state_machine == SYSTEM_STATE_AUTO ||
+				    vstatus.state_machine == SYSTEM_STATE_STABILIZED) {
+
+				    	/* simple heading control */
+				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
+
+				    	/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
+				    	actuators.control[1] = 0.0f;
+				    	actuators.control[2] = 0.0f;
+					
+					/* simple attitude control */
+					control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+
+					/* pass through throttle */
+					actuators.control[3] = att_sp.thrust;
+
+					/* set flaps to zero */
+					actuators.control[4] = 0.0f;
+
+				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
+					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+
+						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
+						if (vstatus.rc_signal_lost) {
+
+							/* put plane into loiter */
+							att_sp.roll_body = 0.3f;
+							att_sp.pitch_body = 0.0f;
+
+							/* limit throttle to 60 % of last value if sane */
+							if (isfinite(manual_sp.throttle) &&
+							    (manual_sp.throttle >= 0.0f) &&
+							    (manual_sp.throttle <= 1.0f)) {
+								att_sp.thrust = 0.6f * manual_sp.throttle;
+
+							} else {
+								att_sp.thrust = 0.0f;
+							}
+
+							att_sp.yaw_body = 0;
+
+							// XXX disable yaw control, loiter
+
+						} else {
+
+							att_sp.roll_body = manual_sp.roll;
+							att_sp.pitch_body = manual_sp.pitch;
+							att_sp.yaw_body = 0;
+							att_sp.thrust = manual_sp.throttle;
+						}
+
+						att_sp.timestamp = hrt_absolute_time();
+
+						/* attitude control */
+						control_attitude(&att_sp, &att, speed_body, gyro, &rates_sp, &actuators);
+
+						/* pass through throttle */
+						actuators.control[3] = att_sp.thrust;
+
+						/* pass through flaps */
+						if (isfinite(manual_sp.flaps)) {
+							actuators.control[4] = manual_sp.flaps;
+
+						} else {
+							actuators.control[4] = 0.0f;
+						}
+
+					} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+						/* directly pass through values */
+						actuators.control[0] = manual_sp.roll;
+						/* positive pitch means negative actuator -> pull up */
+						actuators.control[1] = manual_sp.pitch;
+						actuators.control[2] = manual_sp.yaw;
+						actuators.control[3] = manual_sp.throttle;
+
+						if (isfinite(manual_sp.flaps)) {
+							actuators.control[4] = manual_sp.flaps;
+
+						} else {
+							actuators.control[4] = 0.0f;
+						}
+					}
+				}
+
+				/* publish rates */
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);
+
+				/* sanity check and publish actuator outputs */
+				if (isfinite(actuators.control[0]) &&
+				    isfinite(actuators.control[1]) &&
+				    isfinite(actuators.control[2]) &&
+				    isfinite(actuators.control[3])) {
+					orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+				}
+			}
+		}
+	}
+
+	printf("[ex_fixedwing_control] exiting, stopping all motors.\n");
+	thread_running = false;
+
+	/* kill all outputs */
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++)
+		actuators.control[i] = 0.0f;
+
+	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+	fflush(stdout);
+
+	return 0;
+}
+
+/* Startup Functions */
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: ex_fixedwing_control {start|stop|status}\n\n");
+	exit(1);
+}
+
+/**
+ * The daemon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int ex_fixedwing_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("ex_fixedwing_control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("ex_fixedwing_control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 20,
+					 2048,
+					 fixedwing_control_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		thread_running = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tex_fixedwing_control is running\n");
+
+		} else {
+			printf("\tex_fixedwing_control not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+
+

src/examples/fixedwing_control/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Fixedwing Attitude Control Demo / Example Application
+#
+
+MODULE_COMMAND	= ex_fixedwing_control
+
+SRCS		= main.c \
+		  params.c

src/examples/fixedwing_control/params.c

@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file params.c
+ * 
+ * Parameters for fixedwing demo
+ */
+
+#include "params.h"
+
+/* controller parameters, use max. 15 characters for param name! */
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_HDNG_P, 0.2f);
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_ROLL_P, 0.2f);
+
+/**
+ *
+ */
+PARAM_DEFINE_FLOAT(EXFW_PITCH_P, 0.2f);
+
+int parameters_init(struct param_handles *h)
+{
+	/* PID parameters */
+	h->hdng_p 	=	param_find("EXFW_HDNG_P");
+	h->roll_p 	=	param_find("EXFW_ROLL_P");
+	h->pitch_p 	=	param_find("EXFW_PITCH_P");
+
+	return OK;
+}
+
+int parameters_update(const struct param_handles *h, struct params *p)
+{
+	param_get(h->hdng_p, &(p->hdng_p));
+	param_get(h->roll_p, &(p->roll_p));
+	param_get(h->pitch_p, &(p->pitch_p));
+
+	return OK;
+}

src/examples/fixedwing_control/params.h

@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file params.h
+ * 
+ * Definition of parameters for fixedwing example
+ */
+
+#include <systemlib/param/param.h>
+
+struct params {
+	float hdng_p;
+	float roll_p;
+	float pitch_p;
+};
+
+struct param_handles {
+	param_t hdng_p;
+	param_t roll_p;
+	param_t pitch_p;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct param_handles *h, struct params *p);

src/examples/px4_daemon_app/module.mk

@@ -37,4 +37,4 @@
 
 MODULE_COMMAND		= px4_daemon_app
 
-SRCS				= px4_daemon_app.c
+SRCS			= px4_daemon_app.c

src/examples/px4_mavlink_debug/module.mk

@@ -37,4 +37,4 @@
 
 MODULE_COMMAND		= px4_mavlink_debug
 
-SRCS				= px4_mavlink_debug.c
+SRCS			= px4_mavlink_debug.c

src/examples/px4_simple_app/module.mk

@@ -37,4 +37,4 @@
 
 MODULE_COMMAND		= px4_simple_app
 
-SRCS				= px4_simple_app.c
+SRCS			= px4_simple_app.c

src/modules/commander/commander.c

@@ -1503,21 +1503,39 @@ int commander_thread_main(int argc, char *argv[])
 			if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY ||
 			     current_status.state_machine == SYSTEM_STATE_AUTO  ||
 			     current_status.state_machine == SYSTEM_STATE_MANUAL)) {
-				/* armed */
+				/* armed, solid */
-				led_toggle(LED_BLUE);
+				led_on(LED_AMBER);
 
 			} else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
 				/* not armed */
-				led_toggle(LED_BLUE);
+				led_toggle(LED_AMBER);
 			}
 
-			/* toggle error led at 5 Hz in HIL mode */
+			if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) {
+
+				/* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */
+				if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000)
+					&& (gps_position.fix_type == GPS_FIX_TYPE_3D)) {
+					/* GPS lock */
+					led_on(LED_BLUE);
+
+				} else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
+					/* no GPS lock, but GPS module is aquiring lock */
+					led_toggle(LED_BLUE);
+				}
+
+			} else {
+				/* no GPS info, don't light the blue led */
+				led_off(LED_BLUE);
+			}
+
+			/* toggle GPS led at 5 Hz in HIL mode */
 			if (current_status.flag_hil_enabled) {
 				/* hil enabled */
-				led_toggle(LED_AMBER);
+				led_toggle(LED_BLUE);
 
 			} else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) {
-				/* toggle error (red) at 5 Hz on low battery or error */
+				/* toggle arming (red) at 5 Hz on low battery or error */
 				led_toggle(LED_AMBER);
 
 			} else {

src/modules/commander/state_machine_helper.c

@@ -249,6 +249,11 @@ void publish_armed_status(const struct vehicle_status_s *current_status)
 {
 	struct actuator_armed_s armed;
 	armed.armed = current_status->flag_system_armed;
+
+	/* XXX allow arming by external components on multicopters only if not yet armed by RC */
+	/* XXX allow arming only if core sensors are ok */
+	armed.ready_to_arm = true;
+
 	/* lock down actuators if required, only in HIL */
 	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
 	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);

src/modules/mathlib/CMSIS/module.mk

@@ -38,8 +38,9 @@
 #
 # Find sources
 #
-DSPLIB_SRCDIR		:= $(PX4_MODULE_SRC)/modules/mathlib/CMSIS
+DSPLIB_SRCDIR		:= $(dir $(lastword $(MAKEFILE_LIST)))
-ABS_SRCS		:= $(wildcard $(DSPLIB_SRCDIR)/DSP_Lib/Source/*/*.c)
+SRCLIST			:= $(wildcard $(DSPLIB_SRCDIR)DSP_Lib/Source/*/*.c)
+SRCS			:= $(patsubst $(DSPLIB_SRCDIR)%,%,$(SRCLIST))
 
 INCLUDE_DIRS		+= $(DSPLIB_SRCDIR)/Include \
 			   $(DSPLIB_SRCDIR)/Device/ARM/ARMCM4/Include \

src/modules/px4iofirmware/mixer.cpp

@@ -174,7 +174,7 @@ mixer_tick(void)
 	 * here.
 	 */
 	bool should_arm = (
-	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	    /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	 	/* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
 		/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
 		/* IO initialised without error */  (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) &&
@@ -246,7 +246,7 @@ void
 mixer_handle_text(const void *buffer, size_t length)
 {
 	/* do not allow a mixer change while fully armed */
-	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+	if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 	    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 		return;
 	}

src/modules/px4iofirmware/protocol.h

@@ -145,9 +145,9 @@
 #define PX4IO_P_SETUP_FEATURES			0
 
 #define PX4IO_P_SETUP_ARMING			1	 /* arming controls */
-#define PX4IO_P_SETUP_ARMING_ARM_OK		(1 << 0) /* OK to arm */
+#define PX4IO_P_SETUP_ARMING_IO_ARM_OK		(1 << 0) /* OK to arm the IO side */
+#define PX4IO_P_SETUP_ARMING_FMU_ARMED		(1 << 1) /* FMU is already armed */
 #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK	(1 << 2) /* OK to switch to manual override via override RC channel */
-#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK	(1 << 3) /* OK to perform position / vector control (= position lock) */
 #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK	(1 << 4) /* OK to try in-air restart */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */

src/modules/px4iofirmware/registers.c

@@ -142,9 +142,10 @@ volatile uint16_t	r_page_setup[] =
 };
 
 #define PX4IO_P_SETUP_FEATURES_VALID	(0)
-#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_ARM_OK | \
+#define PX4IO_P_SETUP_ARMING_VALID	(PX4IO_P_SETUP_ARMING_FMU_ARMED | \
 					 PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
-					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
+					 PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \
+					 PX4IO_P_SETUP_ARMING_IO_ARM_OK)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -311,7 +312,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			 * so that an in-air reset of FMU can not lead to a
 			 * lockup of the IO arming state.
 			 */
-			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+			if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
 				r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
 			}
 
@@ -362,7 +363,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 	case PX4IO_PAGE_RC_CONFIG: {
 
 		/* do not allow a RC config change while fully armed */
-		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+		if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) &&
 		    /* IO is armed */  (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 			break;
 		}

src/modules/px4iofirmware/safety.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,7 +32,8 @@
  ****************************************************************************/
 
 /**
- * @file Safety button logic.
+ * @file safety.c
+ * Safety button logic.
  */
 
 #include <nuttx/config.h>
@@ -56,11 +57,11 @@ static unsigned counter = 0;
 /*
  * Define the various LED flash sequences for each system state.
  */
-#define LED_PATTERN_SAFE 			0xffff		/**< always on 				*/
+#define LED_PATTERN_FMU_OK_TO_ARM 		0x0003		/**< slow blinking			*/
-#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 	0xFFFE		/**< always on with short break 	*/
+#define LED_PATTERN_FMU_REFUSE_TO_ARM 		0x5555		/**< fast blinking			*/
-#define LED_PATTERN_FMU_ARMED 			0x4444		/**< slow blinking			*/
+#define LED_PATTERN_IO_ARMED 			0x5050		/**< long off, then double blink 	*/
-#define LED_PATTERN_IO_ARMED 			0x5555		/**< fast blinking 			*/
+#define LED_PATTERN_FMU_ARMED 			0x5500		/**< long off, then quad blink 		*/
-#define LED_PATTERN_IO_FMU_ARMED 		0x5050		/**< long off then double blink 	*/
+#define LED_PATTERN_IO_FMU_ARMED 		0xffff		/**< constantly on			*/
 
 static unsigned blink_counter = 0;
 
@@ -109,7 +110,8 @@ safety_check_button(void *arg)
 	 * state machine, keep ARM_COUNTER_THRESHOLD the same
 	 * length in all cases of the if/else struct below.
 	 */
-	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
+	if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+		(r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
 			counter++;
@@ -120,8 +122,6 @@ safety_check_button(void *arg)
 			counter++;
 		}
 
-		/* Disarm quickly */
-
 	} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
 
 		if (counter < ARM_COUNTER_THRESHOLD) {
@@ -138,21 +138,21 @@ safety_check_button(void *arg)
 	}
 
 	/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
-	uint16_t pattern = LED_PATTERN_SAFE;
+	uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
 
 	if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
-		if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+		if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 			pattern = LED_PATTERN_IO_FMU_ARMED;
 
 		} else {
 			pattern = LED_PATTERN_IO_ARMED;
 		}
 
-	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) {
 		pattern = LED_PATTERN_FMU_ARMED;
+	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
+		pattern = LED_PATTERN_FMU_OK_TO_ARM;
 
-	} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
-		pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
 	}
 
 	/* Turn the LED on if we have a 1 at the current bit position */

src/modules/uORB/topics/actuator_controls.h

@@ -69,6 +69,7 @@ ORB_DECLARE(actuator_controls_3);
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
 	bool	armed;		/**< Set to true if system is armed */
+	bool	ready_to_arm;	/**< Set to true if system is ready to be armed */
 	bool	lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
 };