Merged master

.cproject

@@ -77,7 +77,7 @@
 		</cconfiguration>
 	</storageModule>
 	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="PX4-Firmware.null.714401899" name="PX4-Firmware"/>
+		<project id="PX4Firmware.null.2007659608" name="PX4Firmware"/>
 	</storageModule>
 	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
 	<storageModule moduleId="refreshScope" versionNumber="2">

Makefile

@@ -118,6 +118,13 @@ ifeq ($(PX4_TARGET_OS),qurt)
 include $(PX4_BASE)makefiles/qurt/firmware_qurt.mk
 endif
 
+qurt_fixup:
+	makefiles/qurt/setup.sh $(PX4_BASE)
+
+restore:
+	cd src/lib/eigen && git checkout .
+	git submodule update
+	
 #
 # Versioning
 #
@@ -155,7 +162,7 @@ size:
 
 .PHONY: checksubmodules
 checksubmodules:
-	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh)
+	$(Q) ($(PX4_BASE)/Tools/check_submodules.sh $(PX4_TARGET_OS))
 
 .PHONY: updatesubmodules
 updatesubmodules:

ROMFS/px4fmu_common/init.d/rc.uavcan

@@ -3,16 +3,50 @@
 # UAVCAN initialization script.
 #
 
+#
+# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
+# TODO there should be a smarter way.
+#
+set UAVCAN_ENABLE 0
 if param compare UAVCAN_ENABLE 1
+then
+	set UAVCAN_ENABLE 1
+fi
+if param compare UAVCAN_ENABLE 2
+then
+	set UAVCAN_ENABLE 2
+fi
+
+echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"
+
+#
+# Starting stuff according to UAVCAN_ENABLE value
+#
+if [ $UAVCAN_ENABLE -ge 1 ]
 then
 	if uavcan start
 	then
-		# First sensor publisher to initialize takes lowest instance ID
-		# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
-		sleep 1
 		echo "[i] UAVCAN started"
 	else
 		echo "[i] ERROR: Could not start UAVCAN"
 		tone_alarm $TUNE_ERR
 	fi
 fi
+
+if [ $UAVCAN_ENABLE -ge 2 ]
+then
+	if uavcan start fw
+	then
+		echo "[i] UAVCAN servers started"
+	else
+		echo "[i] ERROR: Could not start UAVCAN servers"
+		tone_alarm $TUNE_ERR
+	fi
+fi
+
+if [ $UAVCAN_ENABLE -ge 1 ]
+then
+	# First sensor publisher to initialize takes lowest instance ID
+	# This delay ensures that UAVCAN-interfaced sensors will be allocated on lowest instance IDs
+	sleep 8
+fi

ROMFS/px4fmu_common/init.d/rc.usb

@@ -3,26 +3,7 @@
 # USB MAVLink start
 #
 
-mavlink start -r 800000 -d /dev/ttyACM0 -x
-# Enable a number of interesting streams we want via USB
-mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
-mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
-mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
-mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
-mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10
-mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
-mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100
-mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
-mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS -r 5
-mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
-mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_1 -r 20
-mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
-mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
-mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5
-mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100
-mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20
-mavlink stream -d /dev/ttyACM0 -s CAMERA_TRIGGER -r 500
-mavlink stream -d /dev/ttyACM0 -s VTOL_STATE -r 2
+mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
 
 # Exit shell to make it available to MAVLink
 exit

ROMFS/px4fmu_common/init.d/rcS

@@ -89,6 +89,70 @@ then
 		fi
 	fi
 
+	# Compare existing params and save defaults
+	# this only needs to be in for 1-2 releases
+	if param compare RC_MAP_THROTTLE 0
+	then
+		# So this is a default setup,
+		# now lets find out if channel 3
+		# is calibrated
+		if param compare RC3_MIN 1000
+		then
+			# This is default, do nothing
+		else
+			# Set old default
+			param set RC_MAP_THROTTLE 3
+		fi
+	fi
+
+	# Compare existing params and save defaults
+	# this only needs to be in for 1-2 releases
+	if param compare RC_MAP_ROLL 0
+	then
+		# So this is a default setup,
+		# now lets find out if channel 1
+		# is calibrated
+		if param compare RC1_MIN 1000
+		then
+			# This is default, do nothing
+		else
+			# Set old default
+			param set RC_MAP_ROLL 1
+		fi
+	fi
+
+	# Compare existing params and save defaults
+	# this only needs to be in for 1-2 releases
+	if param compare RC_MAP_PITCH 0
+	then
+		# So this is a default setup,
+		# now lets find out if channel 2
+		# is calibrated
+		if param compare RC2_MIN 1000
+		then
+			# This is default, do nothing
+		else
+			# Set old default
+			param set RC_MAP_PITCH 2
+		fi
+	fi
+
+	# Compare existing params and save defaults
+	# this only needs to be in for 1-2 releases
+	if param compare RC_MAP_YAW 0
+	then
+		# So this is a default setup,
+		# now lets find out if channel 4
+		# is calibrated
+		if param compare RC4_MIN 1000
+		then
+			# This is default, do nothing
+		else
+			# Set old default
+			param set RC_MAP_YAW 4
+		fi
+	fi
+
 	#
 	# Start system state indicator
 	#
@@ -287,6 +351,7 @@ then
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
+		set GPS no
 		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
@@ -300,6 +365,11 @@ then
 	then
 	fi
 
+	#
+	# UAVCAN
+	#
+	sh /etc/init.d/rc.uavcan
+
 	#
 	# Sensors System (start before Commander so Preflight checks are properly run)
 	#
@@ -315,9 +385,12 @@ then
 			gps start
 		fi
 	fi
+	unset GPS
 	unset GPS_FAKE
 
 	# Needs to be this early for in-air-restarts
+	# Wait 10 ms for sensors (workaround for airspeed, to be removed)
+	usleep 10000
 	commander start
 
 	#
@@ -505,10 +578,6 @@ then
 		fi
 	fi
 
-	# UAVCAN
-	#
-	sh /etc/init.d/rc.uavcan
-
 	#
 	# Logging
 	#

Tools/check_submodules.sh

@@ -25,8 +25,7 @@ if [ -d NuttX/nuttx ];
 		exit 1
 	fi
 else
-	git submodule init;
-	git submodule update;
+	git submodule update --init --recursive
 fi
 
 
@@ -47,8 +46,7 @@ if [ -d mavlink/include/mavlink/v1.0 ];
 		exit 1
 	fi
 else
-	git submodule init;
-	git submodule update;
+	git submodule update --init --recursive
 fi
 
 
@@ -70,30 +68,35 @@ then
 		exit 1
 	fi
 else
-	git submodule init;
-	git submodule update;
+	git submodule update --init --recursive
 fi
 
 if [ -d src/lib/eigen ]
 then
-	STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
-	if [ -z "$STATUSRETVAL" ]
+	echo "ARG = $1"
+	if [ $1 = "qurt" ]
 	then
-		echo "Checked Eigen submodule, correct version found"
+		# QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11
+		STATUSRETVAL=$(true)
 	else
-		echo ""
-		echo ""
-		echo "New commits required:"
-		echo "$(git submodule summary)"
-		echo ""
-		echo ""
-		echo "eigen sub repo not at correct version. Try 'git submodule update'"
-		echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
-		exit 1
+		STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<")
+		if [ -z "$STATUSRETVAL" ]
+		then
+			echo "Checked Eigen submodule, correct version found"
+		else
+			echo ""
+			echo ""
+			echo "New commits required:"
+			echo "$(git submodule summary)"
+			echo ""
+			echo ""
+			echo "eigen sub repo not at correct version. Try 'git submodule update'"
+			echo "or follow instructions on http://pixhawk.org/dev/git/submodules"
+			exit 1
+		fi
 	fi
 else
-	git submodule init;
-	git submodule update;
+	git submodule update --init --recursive
 fi
 
 if [ -d Tools/gencpp ]
@@ -114,8 +117,7 @@ then
 		exit 1
 	fi
 else
-	git submodule init;
-	git submodule update;
+	git submodule update --init --recursive
 fi
 
 if [ -d Tools/genmsg ]
@@ -136,8 +138,7 @@ then
 		exit 1
 	fi
 else
-	git submodule init;
-	git submodule update;
+	git submodule update --init --recursive
 fi
 
 exit 0

Tools/px4params/srcparser.py

@@ -1,5 +1,7 @@
 import sys
 import re
+global default_var
+default_var = {}
 
 class ParameterGroup(object):
     """
@@ -98,6 +100,7 @@ class SourceParser(object):
     re_comment_end = re.compile(r'(.*?)\s*\*\/')
     re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
     re_px4_parameter_definition = re.compile(r'PX4_PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*\)\s*;')
+    re_px4_param_default_definition = re.compile(r'#define\s*PARAM_([A-Z_][A-Z0-9_]*)\s*([^ ,\)]+)\s*')
     re_cut_type_specifier = re.compile(r'[a-z]+$')
     re_is_a_number = re.compile(r'^-?[0-9\.]')
     re_remove_dots = re.compile(r'\.+$')
@@ -114,13 +117,6 @@ class SourceParser(object):
     def __init__(self):
         self.param_groups = {}
 
-    def GetSupportedExtensions(self):
-        """
-        Returns list of supported file extensions that can be parsed by this
-        parser.
-        """
-        return [".cpp", ".c"]
-
     def Parse(self, contents):
         """
         Incrementally parse program contents and append all found parameters
@@ -193,6 +189,10 @@ class SourceParser(object):
                 name = None
                 defval = ""
                 # Non-empty line outside the comment
+                m = self.re_px4_param_default_definition.match(line)
+                if m:
+                    name_m, defval_m = m.group(1,2)
+                    default_var[name_m] = defval_m
                 m = self.re_parameter_definition.match(line)
                 if m:
                     tp, name, defval = m.group(1, 2, 3)
@@ -200,6 +200,8 @@ class SourceParser(object):
                     m = self.re_px4_parameter_definition.match(line)
                     if m:
                         tp, name = m.group(1, 2)
+                        if default_var.has_key(name+'_DEFAULT'):
+                            defval = default_var[name+'_DEFAULT']
                 if tp is not None:
                     # Remove trailing type specifier from numbers: 0.1f => 0.1
                     if defval != "" and self.re_is_a_number.match(defval):

Tools/px4params/srcscanner.py

@@ -13,10 +13,16 @@ class SourceScanner(object):
         Scans provided path and passes all found contents to the parser using
         parser.Parse method.
         """
-        extensions = tuple(parser.GetSupportedExtensions())
+        extensions1 = tuple([".h"])
+        extensions2 = tuple([".cpp", ".c"])
         for dirname, dirnames, filenames in os.walk(srcdir):
             for filename in filenames:
-                if filename.endswith(extensions):
+                if filename.endswith(extensions1):
+                    path = os.path.join(dirname, filename)
+                    if not self.ScanFile(path, parser):
+                        return False
+            for filename in filenames:
+                if filename.endswith(extensions2):
                     path = os.path.join(dirname, filename)
                     if not self.ScanFile(path, parser):
                         return False

Tools/sitl_run.sh

@@ -2,4 +2,4 @@
 
 mkdir -p Build/posix_sitl.build/rootfs/fs/microsd
 mkdir -p Build/posix_sitl.build/rootfs/eeprom
-cd Build/posix_sitl.build && ./mainapp ../$1
+cd Build/posix_sitl.build && ./mainapp ../../$1

makefiles/firmware.mk

@@ -181,6 +181,12 @@ EXTRA_CLEANS		 =
 #
 INCLUDE_DIRS		+= $(PX4_MODULE_SRC)drivers/boards/$(BOARD)
 
+################################################################################
+# External library includes
+################################################################################
+
+INCLUDE_DIRS		+= $(PX4_BASE)src/lib/eigen/
+
 ################################################################################
 # OS specific libraries and paths
 ################################################################################

makefiles/nuttx/config_px4fmu-v2_default.mk

@@ -143,19 +143,19 @@ MODULES		+= examples/rover_steering_control
 #
 #MODULES		+= examples/math_demo
 # Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/hello_sky
+# https://px4.io/dev/px4_simple_app
 #MODULES		+= examples/px4_simple_app
 
 # Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/daemon
+# https://px4.io/dev/daemon
 #MODULES		+= examples/px4_daemon_app
 
 # Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/debug_values
+# https://px4.io/dev/debug_values
 #MODULES		+= examples/px4_mavlink_debug
 
 # Tutorial code from
-# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+# https://px4.io/dev/example_fixedwing_control
 #MODULES			+= examples/fixedwing_control
 
 # Hardware test

makefiles/posix/config_posix_sitl.mk

@@ -49,6 +49,10 @@ MODULES		+= modules/mc_att_control
 MODULES 	+= modules/mc_pos_control_multiplatform
 MODULES		+= modules/mc_att_control_multiplatform
 MODULES		+= modules/land_detector
+MODULES		+= modules/fw_att_control
+MODULES		+= modules/fw_pos_control_l1
+
+
 
 #
 # Library modules
@@ -67,9 +71,13 @@ MODULES 	+= modules/controllib
 #
 MODULES		+= lib/mathlib
 MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
+MODULES		+= lib/launchdetection
+
 
 #
 # POSIX port

makefiles/qurt/config_qurt_adsp.mk

@@ -1,3 +1,9 @@
+#Added configuration specific flags here. 
+
+# For Actual flight we need to link against the driver dynamic libraries
+LDFLAGS	+= -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50
+LDFLAGS	+= -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc
+
 #
 # Makefile for the EAGLE QuRT *default* configuration
 #

makefiles/qurt/config_qurt_hil.mk

@@ -57,6 +57,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
+MODULES 	+= modules/controllib
 
 #
 # QuRT port

makefiles/qurt/setup.sh

@@ -0,0 +1,48 @@
+############################################################################
+#
+# Copyright (c) 2015 Mark Charlebois. 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.
+#
+# ###########################################################################
+
+#
+# This file is required to modify the PX4 project so it can be build with
+# the Hexagon toolchain.
+#
+# 1. Downgrade Eigen to 3.2 because the C++11 features of latest version
+#    are not supported
+# 2. Patch Eigen because the Hexagon toolchain complex.h defines "I"
+#
+
+cd $1
+cd src/lib/eigen/
+git checkout .
+git checkout e7850ed81f9c469e02df496ef09ae32ec0379b71
+patch -p1 < ../../../makefiles/qurt/qurt_eigen.patch 
+

makefiles/qurt/toolchain_hexagon.mk

@@ -246,10 +246,6 @@ LDFLAGS			+=  -g -mv5 -mG0lib -G0 -fpic -shared \
 			   $(EXTRALDFLAGS) \
 			   $(addprefix -L,$(LIB_DIRS))
 
-# driver dynamic libraries
-LDFLAGS	+= -L${DSPAL_ROOT}/mpu_spi/hexagon_Debug_dynamic_toolv64/ship -lmpu9x50
-LDFLAGS	+= -L${DSPAL_ROOT}/uart_esc/hexagon_Debug_dynamic_toolv64/ship -luart_esc
-
 # Compiler support library
 #
 LIBGCC			:= $(shell $(CC) $(ARCHCPUFLAGS) -print-libgcc-file-name)

msg/input_rc.msg

@@ -0,0 +1,21 @@
+uint8 RC_INPUT_SOURCE_UNKNOWN = 0
+uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1
+uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2
+uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3
+uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4
+uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5
+uint8 RC_INPUT_SOURCE_MAVLINK = 6
+
+uint8 RC_INPUT_MAX_CHANNELS = 18 	# Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
+
+uint64 timestamp_publication		# publication time
+uint64 timestamp_last_signal		# last valid reception time
+uint32 channel_count			# number of channels actually being seen
+int32 rssi				# receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception
+bool rc_failsafe			# explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly.
+bool rc_lost				# RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
+uint16 rc_lost_frame_count		# Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality.
+uint16 rc_total_frame_count		# Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike funtionality.
+uint16 rc_ppm_frame_length		# Length of a single PPM frame. Zero for non-PPM systems
+uint8 input_source			# Input source
+uint16[18] values			# measured pulse widths for each of the supported channels

msg/mission.msg

@@ -0,0 +1,3 @@
+int32 dataman_id	# default 0, there are two offboard storage places in the dataman: 0 or 1
+uint32 count		# count of the missions stored in the dataman
+int32 current_seq	# default -1, start at the one changed latest

msg/output_pwm.msg

@@ -0,0 +1,3 @@
+uint8 PWM_OUTPUT_MAX_CHANNELS = 16
+uint16[16] values
+uint32 channel_count

msg/rc_parameter_map.msg

@@ -0,0 +1,11 @@
+uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
+uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
+
+uint64 timestamp	# time at which the map was updated
+bool[3] valid		#true for RC-Param channels which are mapped to a param
+int32[3] param_index	# corresponding param index, this field is ignored if set to -1, in this case param_id will be used
+char[51] param_id	# MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated
+float32[3] scale		# scale to map the RC input [-1, 1] to a parameter value
+float32[3] value0		# inital value around which the parameter value is changed
+float32[3] value_min	# minimal parameter value
+float32[3] value_max	# minimal parameter value

msg/sensor_accel.msg

@@ -0,0 +1,13 @@
+uint64 timestamp
+uint64 error_count
+float32 x		# acceleration in the NED X board axis in m/s^2
+float32 y		# acceleration in the NED Y board axis in m/s^2
+float32 z		# acceleration in the NED Z board axis in m/s^2
+float32 temperature	# temperature in degrees celsius
+float32 range_m_s2	# range in m/s^2 (+- this value)
+float32 scaling
+
+int16 x_raw
+int16 y_raw
+int16 z_raw
+int16 temperature_raw

msg/sensor_baro.msg

@@ -0,0 +1,5 @@
+float32 pressure
+float32 altitude
+float32 temperature
+uint64 timestamp
+uint64 error_count

msg/sensor_gyro.msg

@@ -0,0 +1,13 @@
+uint64 timestamp
+uint64 error_count
+float32 x		# angular velocity in the NED X board axis in rad/s
+float32 y		# angular velocity in the NED Y board axis in rad/s
+float32 z		# angular velocity in the NED Z board axis in rad/s
+float32 temperature	# temperature in degrees celcius
+float32 range_rad_s
+float32 scaling
+
+int16 x_raw
+int16 y_raw
+int16 z_raw
+int16 temperature_raw

msg/sensor_mag.msg

@@ -0,0 +1,12 @@
+uint64 timestamp
+uint64 error_count
+float32 x
+float32 y
+float32 z
+float32 range_ga
+float32 scaling
+float32 temperature
+
+int16 x_raw
+int16 y_raw
+int16 z_raw

msg/tecs_status.msg

@@ -18,9 +18,12 @@ float32 airspeed_filtered
 float32 airspeedDerivativeSp
 float32 airspeedDerivative
 
-float32 totalEnergyRateSp
-float32 totalEnergyRate
-float32 energyDistributionRateSp
-float32 energyDistributionRate
+float32 totalEnergyError
+float32 energyDistributionError
+float32 totalEnergyRateError
+float32 energyDistributionRateError
+
+float32 throttle_integ
+float32 pitch_integ
 
 uint8 mode

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -564,7 +564,7 @@ CONFIG_USART1_2STOP=0
 # USART2 Configuration
 #
 CONFIG_USART2_RXBUFSIZE=600
-CONFIG_USART2_TXBUFSIZE=1860
+CONFIG_USART2_TXBUFSIZE=1100
 CONFIG_USART2_BAUD=57600
 CONFIG_USART2_BITS=8
 CONFIG_USART2_PARITY=0
@@ -575,8 +575,8 @@ CONFIG_USART2_OFLOWCONTROL=y
 #
 # USART3 Configuration
 #
-CONFIG_USART3_RXBUFSIZE=400
-CONFIG_USART3_TXBUFSIZE=400
+CONFIG_USART3_RXBUFSIZE=300
+CONFIG_USART3_TXBUFSIZE=300
 CONFIG_USART3_BAUD=57600
 CONFIG_USART3_BITS=8
 CONFIG_USART3_PARITY=0
@@ -587,8 +587,8 @@ CONFIG_USART3_OFLOWCONTROL=y
 #
 # UART4 Configuration
 #
-CONFIG_UART4_RXBUFSIZE=400
-CONFIG_UART4_TXBUFSIZE=400
+CONFIG_UART4_RXBUFSIZE=300
+CONFIG_UART4_TXBUFSIZE=300
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_BITS=8
 CONFIG_UART4_PARITY=0
@@ -599,8 +599,8 @@ CONFIG_UART4_2STOP=0
 #
 # USART6 Configuration
 #
-CONFIG_USART6_RXBUFSIZE=400
-CONFIG_USART6_TXBUFSIZE=400
+CONFIG_USART6_RXBUFSIZE=300
+CONFIG_USART6_TXBUFSIZE=300
 CONFIG_USART6_BAUD=57600
 CONFIG_USART6_BITS=8
 CONFIG_USART6_PARITY=0
@@ -611,8 +611,8 @@ CONFIG_USART6_2STOP=0
 #
 # UART7 Configuration
 #
-CONFIG_UART7_RXBUFSIZE=400
-CONFIG_UART7_TXBUFSIZE=400
+CONFIG_UART7_RXBUFSIZE=300
+CONFIG_UART7_TXBUFSIZE=300
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_BITS=8
 CONFIG_UART7_PARITY=0
@@ -623,8 +623,8 @@ CONFIG_UART7_2STOP=0
 #
 # UART8 Configuration
 #
-CONFIG_UART8_RXBUFSIZE=400
-CONFIG_UART8_TXBUFSIZE=400
+CONFIG_UART8_RXBUFSIZE=300
+CONFIG_UART8_TXBUFSIZE=300
 CONFIG_UART8_BAUD=57600
 CONFIG_UART8_BITS=8
 CONFIG_UART8_PARITY=0
@@ -667,7 +667,7 @@ CONFIG_CDCACM_NWRREQS=4
 CONFIG_CDCACM_NRDREQS=4
 CONFIG_CDCACM_BULKIN_REQLEN=96
 CONFIG_CDCACM_RXBUFSIZE=600
-CONFIG_CDCACM_TXBUFSIZE=5000
+CONFIG_CDCACM_TXBUFSIZE=4000
 CONFIG_CDCACM_VENDORID=0x26ac
 CONFIG_CDCACM_PRODUCTID=0x0011
 CONFIG_CDCACM_VENDORSTR="3D Robotics"

posix-configs/SITL/init/rc.fixed_wing

@@ -29,7 +29,7 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
-hil mode_pwm
+pwm_out_sim mode_pwm
 commander start
 sensors start
 land_detector start fixedwing

posix-configs/SITL/init/rcS

@@ -34,7 +34,7 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
-hil mode_pwm
+pwm_out_sim mode_pwm
 commander start
 sensors start
 land_detector start multicopter

posix-configs/SITL/init/rc_iris_ros

@@ -48,7 +48,7 @@ accelsim start
 barosim start
 adcsim start
 gpssim start
-hil mode_pwm
+pwm_out_sim mode_pwm
 commander start
 sensors start
 land_detector start multicopter

src/drivers/airspeed/airspeed.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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,8 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file ets_airspeed.cpp
- * @author Simon Wilks
+ * @file airspeed.cpp
+ * @author Simon Wilks <simon@px4.io>
+ * @author Lorenz Meier <lorenz@px4.io>
  *
  * Driver for the Eagle Tree Airspeed V3 connected via I2C.
  */
@@ -76,7 +77,7 @@
 
 #include <drivers/airspeed/airspeed.h>
 
-Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) :
+Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
 	I2C("Airspeed", path, bus, address, 100000),
 	_reports(nullptr),
 	_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
@@ -105,12 +106,14 @@ Airspeed::~Airspeed()
 	/* make sure we are truly inactive */
 	stop();
 
-	if (_class_instance != -1)
+	if (_class_instance != -1) {
 		unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
+	}
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 
 	// free perf counters
 	perf_free(_sample_perf);
@@ -124,13 +127,15 @@ Airspeed::init()
 	int ret = ERROR;
 
 	/* do I2C init (and probe) first */
-	if (I2C::init() != OK)
+	if (I2C::init() != OK) {
 		goto out;
+	}
 
 	/* allocate basic report buffers */
 	_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	/* register alternate interfaces if we have to */
 	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
@@ -146,8 +151,9 @@ Airspeed::init()
 		/* measurement will have generated a report, publish */
 		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
 
-		if (_airspeed_pub == nullptr)
+		if (_airspeed_pub == nullptr) {
 			warnx("uORB started?");
+		}
 	}
 
 	ret = OK;
@@ -166,7 +172,7 @@ Airspeed::probe()
 	_retries = 4;
 	int ret = measure();
 
-        // drop back to 2 retries once initialised
+	// drop back to 2 retries once initialised
 	_retries = 2;
 	return ret;
 }
@@ -179,20 +185,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCSPOLLRATE: {
 			switch (arg) {
 
-				/* switching to manual polling */
+			/* switching to manual polling */
 			case SENSOR_POLLRATE_MANUAL:
 				stop();
 				_measure_ticks = 0;
 				return OK;
 
-				/* external signalling (DRDY) not supported */
+			/* external signalling (DRDY) not supported */
 			case SENSOR_POLLRATE_EXTERNAL:
 
-				/* zero would be bad */
+			/* zero would be bad */
 			case 0:
 				return -EINVAL;
 
-				/* set default/max polling rate */
+			/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT: {
 					/* do we need to start internal polling? */
@@ -202,13 +208,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(_conversion_interval);
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
 
-				/* adjust to a legal polling interval in Hz */
+			/* adjust to a legal polling interval in Hz */
 			default: {
 					/* do we need to start internal polling? */
 					bool want_start = (_measure_ticks == 0);
@@ -217,15 +224,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(_conversion_interval))
+					if (ticks < USEC2TICK(_conversion_interval)) {
 						return -EINVAL;
+					}
 
 					/* update interval for next measurement */
 					_measure_ticks = ticks;
 
 					/* if we need to start the poll state machine, do it */
-					if (want_start)
+					if (want_start) {
 						start();
+					}
 
 					return OK;
 				}
@@ -233,21 +242,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 		}
 
 	case SENSORIOCGPOLLRATE:
-		if (_measure_ticks == 0)
+		if (_measure_ticks == 0) {
 			return SENSOR_POLLRATE_MANUAL;
+		}
 
 		return (1000 / _measure_ticks);
 
 	case SENSORIOCSQUEUEDEPTH: {
 			/* lower bound is mandatory, upper bound is a sanity check */
-			if ((arg < 1) || (arg > 100))
+			if ((arg < 1) || (arg > 100)) {
 				return -EINVAL;
+			}
 
 			irqstate_t flags = irqsave();
+
 			if (!_reports->resize(arg)) {
 				irqrestore(flags);
 				return -ENOMEM;
 			}
+
 			irqrestore(flags);
 
 			return OK;
@@ -261,16 +274,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return -EINVAL;
 
 	case AIRSPEEDIOCSSCALE: {
-		struct airspeed_scale *s = (struct airspeed_scale*)arg;
-		_diff_pres_offset = s->offset_pa;
-		return OK;
+			struct airspeed_scale *s = (struct airspeed_scale *)arg;
+			_diff_pres_offset = s->offset_pa;
+			return OK;
 		}
 
 	case AIRSPEEDIOCGSCALE: {
-		struct airspeed_scale *s = (struct airspeed_scale*)arg;
-		s->offset_pa = _diff_pres_offset;
-		s->scale = 1.0f;
-		return OK;
+			struct airspeed_scale *s = (struct airspeed_scale *)arg;
+			s->offset_pa = _diff_pres_offset;
+			s->scale = 1.0f;
+			return OK;
 		}
 
 	default:
@@ -287,8 +300,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
 	int ret = 0;
 
 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}
 
 	/* if automatic measurement is enabled */
 	if (_measure_ticks > 0) {
@@ -369,6 +383,7 @@ Airspeed::update_status()
 
 		if (_subsys_pub != nullptr) {
 			orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
+
 		} else {
 			_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
 		}
@@ -402,6 +417,7 @@ Airspeed::print_info()
 void
 Airspeed::new_report(const differential_pressure_s &report)
 {
-	if (!_reports->force(&report))
+	if (!_reports->force(&report)) {
 		perf_count(_buffer_overflows);
+	}
 }

src/drivers/device/vdev.cpp

@@ -367,6 +367,8 @@ VDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup)
 			/* yes? post the notification */
 			if (fds->revents != 0)
 				sem_post(fds->sem);
+		} else {
+			PX4_WARN("Store Poll Waiter error.");
 		}
 
 	} else {
@@ -524,5 +526,4 @@ const char *VDev::devList(unsigned int *next)
 	return NULL;
 }
 
-} // namespace device
-
+} // namespace device

src/drivers/device/vdev.h

@@ -324,6 +324,13 @@ public:
 	static const char *devList(unsigned int *next);
 	static const char *topicList(unsigned int *next);
 
+	/**
+	 * Get the device name.
+	 *
+	 * @return the file system string of the device handle
+	 */
+	const char*	get_devname() { return _devname; }
+
 protected:
 
 	int register_driver(const char *name, void *data);
@@ -406,13 +413,6 @@ protected:
 	 */
 	virtual int unregister_class_devname(const char *class_devname, unsigned class_instance);
 
-	/**
-	 * Get the device name.
-	 *
-	 * @return the file system string of the device handle
-	 */
-	const char*	get_devname() { return _devname; }
-
 	bool		_pub_blocked;		/**< true if publishing should be blocked */
 
 private:

src/drivers/drv_accel.h

@@ -51,29 +51,8 @@
 #define ACCEL1_DEVICE_PATH	"/dev/accel1"
 #define ACCEL2_DEVICE_PATH	"/dev/accel2"
 
-/**
- * accel report structure.  Reads from the device must be in multiples of this
- * structure.
- */
-struct accel_report {
-	uint64_t timestamp;
-	uint64_t integral_dt;	/**< integration time */
-	uint64_t error_count;
-	float x;		/**< acceleration in the NED X board axis in m/s^2 */
-	float y;		/**< acceleration in the NED Y board axis in m/s^2 */
-	float z;		/**< acceleration in the NED Z board axis in m/s^2 */
-	float x_integral;	/**< velocity in the NED X board axis in m/s over the integration time frame */
-	float y_integral;	/**< velocity in the NED Y board axis in m/s over the integration time frame */
-	float z_integral;	/**< velocity in the NED Z board axis in m/s over the integration time frame */
-	float temperature;	/**< temperature in degrees celsius */
-	float range_m_s2;	/**< range in m/s^2 (+- this value) */
-	float scaling;
-
-	int16_t x_raw;
-	int16_t y_raw;
-	int16_t z_raw;
-	int16_t temperature_raw;
-};
+#include <uORB/topics/sensor_accel.h>
+#define accel_report sensor_accel_s
 
 /** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
 struct accel_scale {
@@ -85,11 +64,6 @@ struct accel_scale {
 	float	z_scale;
 };
 
-/*
- * ObjDev tag for raw accelerometer data.
- */
-ORB_DECLARE(sensor_accel);
-
 /*
  * ioctl() definitions
  *

src/drivers/drv_baro.h

@@ -50,22 +50,8 @@
 #define BARO_BASE_DEVICE_PATH	"/dev/baro"
 #define BARO0_DEVICE_PATH	"/dev/baro0"
 
-/**
- * baro report structure.  Reads from the device must be in multiples of this
- * structure.
- */
-struct baro_report {
-	float pressure;
-	float altitude;
-	float temperature;
-	uint64_t timestamp;
-	uint64_t error_count;
-};
-
-/*
- * ObjDev tag for raw barometer data.
- */
-ORB_DECLARE(sensor_baro);
+#include <uORB/topics/sensor_baro.h>
+#define baro_report sensor_baro_s
 
 /*
  * ioctl() definitions

src/drivers/drv_gyro.h

@@ -51,29 +51,8 @@
 #define GYRO1_DEVICE_PATH	"/dev/gyro1"
 #define GYRO2_DEVICE_PATH	"/dev/gyro2"
 
-/**
- * gyro report structure.  Reads from the device must be in multiples of this
- * structure.
- */
-struct gyro_report {
-	uint64_t timestamp;
-	uint64_t integral_dt;	/**< integration time */
-	uint64_t error_count;
-	float x;		/**< angular velocity in the NED X board axis in rad/s */
-	float y;		/**< angular velocity in the NED Y board axis in rad/s */
-	float z;		/**< angular velocity in the NED Z board axis in rad/s */
-	float x_integral;	/**< delta angle in the NED X board axis in rad/s in the integration time frame */
-	float y_integral;	/**< delta angle in the NED Y board axis in rad/s in the integration time frame */
-	float z_integral;	/**< delta angle in the NED Z board axis in rad/s in the integration time frame */
-	float temperature;	/**< temperature in degrees celcius */
-	float range_rad_s;
-	float scaling;
-
-	int16_t x_raw;
-	int16_t y_raw;
-	int16_t z_raw;
-	int16_t temperature_raw;
-};
+#include <uORB/topics/sensor_gyro.h>
+#define gyro_report sensor_gyro_s
 
 /** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
 struct gyro_scale {
@@ -85,11 +64,6 @@ struct gyro_scale {
 	float	z_scale;
 };
 
-/*
- * ObjDev tag for raw gyro data.
- */
-ORB_DECLARE(sensor_gyro);
-
 /*
  * ioctl() definitions
  */

src/drivers/drv_mag.h

@@ -49,26 +49,8 @@
 #define MAG1_DEVICE_PATH	"/dev/mag1"
 #define MAG2_DEVICE_PATH	"/dev/mag2"
 
-/**
- * mag report structure.  Reads from the device must be in multiples of this
- * structure.
- *
- * Output values are in gauss.
- */
-struct mag_report {
-	uint64_t timestamp;
-	uint64_t error_count;
-	float x;
-	float y;
-	float z;
-	float range_ga;
-	float scaling;
-	float temperature;
-
-	int16_t x_raw;
-	int16_t y_raw;
-	int16_t z_raw;
-};
+#include <uORB/topics/sensor_mag.h>
+#define mag_report sensor_mag_s
 
 /** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
 struct mag_scale {
@@ -80,12 +62,6 @@ struct mag_scale {
 	float	z_scale;
 };
 
-/*
- * ObjDev tag for raw magnetometer data.
- */
-ORB_DECLARE(sensor_mag);
-
-
 /*
  * ioctl() definitions
  */

src/drivers/drv_pwm_output.h

@@ -61,10 +61,17 @@ __BEGIN_DECLS
 #define PWM_OUTPUT_BASE_DEVICE_PATH "/dev/pwm_output"
 #define PWM_OUTPUT0_DEVICE_PATH	"/dev/pwm_output0"
 
+#include <uORB/topics/output_pwm.h>
+#define pwm_output_values output_pwm_s
+
+#ifndef PWM_OUTPUT_MAX_CHANNELS
+ #define PWM_OUTPUT_MAX_CHANNELS output_pwm_s::PWM_OUTPUT_MAX_CHANNELS
+#endif
+
 /**
  * Maximum number of PWM output channels supported by the device.
  */
-#define PWM_OUTPUT_MAX_CHANNELS	16
+//#define PWM_OUTPUT_MAX_CHANNELS	16
 
 /**
  * Lowest minimum PWM in us
@@ -107,19 +114,6 @@ __BEGIN_DECLS
  */
 typedef uint16_t	servo_position_t;
 
-/**
- * Servo output status structure.
- *
- * May be published to output_pwm, or written to a PWM output
- * device.
- */
-struct pwm_output_values {
-	/** desired pulse widths for each of the supported channels */
-	servo_position_t	values[PWM_OUTPUT_MAX_CHANNELS];
-	unsigned			channel_count;
-};
-
-
 /**
  * RC config values for a channel
  *
@@ -136,11 +130,6 @@ struct pwm_output_rc_config {
 	bool     rc_reverse;
 };
 
-/*
- * ORB tag for PWM outputs.
- */
-ORB_DECLARE(output_pwm);
-
 /*
  * ioctl() definitions
  *

src/drivers/drv_px4flow.h

@@ -46,11 +46,6 @@
 
 #define PX4FLOW0_DEVICE_PATH	"/dev/px4flow0"
 
-/*
- * ObjDev tag for px4flow data.
- */
-ORB_DECLARE(optical_flow);
-
 /*
  * ioctl() definitions
  *

src/drivers/drv_range_finder.h

@@ -48,11 +48,6 @@
 #define RANGE_FINDER0_DEVICE_PATH	"/dev/range_finder0"
 #define MB12XX_MAX_RANGEFINDERS	12	// Maximum number of Maxbotix sensors on bus
 
-/*
- * ObjDev tag for distance sensor data.
- */
-ORB_DECLARE(distance_sensor);
-
 /*
  * ioctl() definitions
  *

src/drivers/drv_rc_input.h

@@ -57,11 +57,6 @@
  */
 #define RC_INPUT0_DEVICE_PATH	"/dev/input_rc0"
 
-/**
- * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels.
- */
-#define RC_INPUT_MAX_CHANNELS	18
-
 /**
  * Maximum RSSI value
  */
@@ -82,10 +77,9 @@
  */
 #define RC_INPUT_MAX_DEADZONE_US	500
 
-/**
- * @addtogroup topics
- * @{
- */
+#include <uORB/topics/input_rc.h>
+#define pwm_output_values output_pwm_s
+#define rc_input_values input_rc_s
 
 /**
  * Input signal type, value is a control position from zero to 100
@@ -93,85 +87,6 @@
  */
 typedef uint16_t		rc_input_t;
 
-enum RC_INPUT_SOURCE {
-	RC_INPUT_SOURCE_UNKNOWN = 0,
-	RC_INPUT_SOURCE_PX4FMU_PPM,
-	RC_INPUT_SOURCE_PX4IO_PPM,
-	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
-	RC_INPUT_SOURCE_PX4IO_SBUS,
-	RC_INPUT_SOURCE_PX4IO_ST24,
-	RC_INPUT_SOURCE_MAVLINK
-};
-
-/**
- * R/C input status structure.
- *
- * Published to input_rc, may also be published to other names depending
- * on the board involved.
- */
-struct rc_input_values {
-	/** publication time */
-	uint64_t		timestamp_publication;
-
-	/** last valid reception time */
-	uint64_t		timestamp_last_signal;
-
-	/** number of channels actually being seen */
-	uint32_t		channel_count;
-
-	/** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception */
-	int32_t			rssi;
-
-	/**
-	 * explicit failsafe flag: true on TX failure or TX out of range , false otherwise.
-	 * Only the true state is reliable, as there are some (PPM) receivers on the market going
-	 * into failsafe without telling us explicitly.
-	 * */
-	bool			rc_failsafe;
-
-	/**
-	 * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise.
-	 * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems.
-	 * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss.
-	 * */
-	bool			rc_lost;
-
-	/**
-	 * Number of lost RC frames.
-	 * Note: intended purpose: observe the radio link quality if RSSI is not available
-	 * This value must not be used to trigger any failsafe-alike funtionality.
-	 * */
-	uint16_t		rc_lost_frame_count;
-
-	/**
-	 * Number of total RC frames.
-	 * Note: intended purpose: observe the radio link quality if RSSI is not available
-	 * This value must not be used to trigger any failsafe-alike funtionality.
-	 * */
-	uint16_t		rc_total_frame_count;
-
-	/**
-	 * Length of a single PPM frame.
-	 * Zero for non-PPM systems
-	 */
-	uint16_t		rc_ppm_frame_length;
-
-	/** Input source */
-	enum RC_INPUT_SOURCE 	input_source;
-
-	/** measured pulse widths for each of the supported channels */
-	rc_input_t		values[RC_INPUT_MAX_CHANNELS];
-};
-
-/**
- * @}
- */
-
-/*
- * ObjDev tag for R/C inputs.
- */
-ORB_DECLARE(input_rc);
-
 #define _RC_INPUT_BASE		0x2b00
 
 /** Fetch R/C input values into (rc_input_values *)arg */

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -94,7 +94,7 @@
 class ETSAirspeed : public Airspeed
 {
 public:
-	ETSAirspeed(int bus, int address = I2C_ADDRESS, const char* path = ETS_PATH);
+	ETSAirspeed(int bus, int address = I2C_ADDRESS, const char *path = ETS_PATH);
 
 protected:
 
@@ -113,8 +113,8 @@ protected:
  */
 extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]);
 
-ETSAirspeed::ETSAirspeed(int bus, int address, const char* path) : Airspeed(bus, address,
-	CONVERSION_INTERVAL, path)
+ETSAirspeed::ETSAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
+			CONVERSION_INTERVAL, path)
 {
 
 }
@@ -155,7 +155,8 @@ ETSAirspeed::collect()
 	}
 
 	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
-        if (diff_pres_pa_raw == 0) {
+
+	if (diff_pres_pa_raw == 0) {
 		// a zero value means the pressure sensor cannot give us a
 		// value. We need to return, and not report a value or the
 		// caller could end up using this value as part of an
@@ -163,7 +164,7 @@ ETSAirspeed::collect()
 		perf_count(_comms_errors);
 		DEVICE_LOG("zero value from sensor"); 
 		return -1;
-        }
+	}
 
 	// The raw value still should be compensated for the known offset
 	diff_pres_pa_raw -= _diff_pres_offset;
@@ -175,7 +176,7 @@ ETSAirspeed::collect()
 
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 
 	// XXX we may want to smooth out the readings to remove noise.
 	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
@@ -210,6 +211,7 @@ ETSAirspeed::cycle()
 
 		/* perform collection */
 		ret = collect();
+
 		if (OK != ret) {
 			perf_count(_comms_errors);
 			/* restart the measurement state machine */
@@ -239,6 +241,7 @@ ETSAirspeed::cycle()
 
 	/* measurement phase */
 	ret = measure();
+
 	if (OK != ret) {
 		DEVICE_DEBUG("measure error");
 	}
@@ -287,26 +290,31 @@ start(int i2c_bus)
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new ETSAirspeed(i2c_bus);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->Airspeed::init())
+	if (OK != g_dev->Airspeed::init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(AIRSPEED0_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		goto fail;
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		goto fail;
+	}
 
 	exit(0);
 
@@ -351,21 +359,24 @@ test()
 
 	int fd = open(ETS_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'ets_airspeed start' if the driver is not running", ETS_PATH);
+	}
 
 	/* do a simple demand read */
 	sz = read(fd, &report, sizeof(report));
 
-	if (sz != sizeof(report))
+	if (sz != sizeof(report)) {
 		err(1, "immediate read failed");
+	}
 
 	warnx("single read");
 	warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 
 	/* start the sensor polling at 2Hz */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
+	}
 
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
@@ -376,22 +387,25 @@ test()
 		fds.events = POLLIN;
 		ret = poll(&fds, 1, 2000);
 
-		if (ret != 1)
+		if (ret != 1) {
 			errx(1, "timed out waiting for sensor data");
+		}
 
 		/* now go get it */
 		sz = read(fd, &report, sizeof(report));
 
-		if (sz != sizeof(report))
+		if (sz != sizeof(report)) {
 			err(1, "periodic read failed");
+		}
 
 		warnx("periodic read %u", i);
 		warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa);
 	}
 
 	/* reset the sensor polling to its default rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT))
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
 		errx(1, "failed to set default rate");
+	}
 
 	errx(0, "PASS");
 }
@@ -404,14 +418,17 @@ reset()
 {
 	int fd = open(ETS_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "failed ");
+	}
 
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
 		err(1, "driver reset failed");
+	}
 
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
 		err(1, "driver poll restart failed");
+	}
 
 	exit(0);
 }
@@ -422,8 +439,9 @@ reset()
 void
 info()
 {
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		errx(1, "driver not running");
+	}
 
 	printf("state @ %p\n", g_dev);
 	g_dev->print_info();
@@ -462,32 +480,37 @@ ets_airspeed_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		ets_airspeed::start(i2c_bus);
+	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		ets_airspeed::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		ets_airspeed::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		ets_airspeed::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		ets_airspeed::info();
+	}
 
 	ets_airspeed_usage();
 	exit(0);

src/drivers/irlock/irlock.cpp

@@ -110,7 +110,7 @@ private:
 	int 		read_device_block(struct irlock_s *block);
 
 	/** internal variables **/
-	RingBuffer *_reports;
+	ringbuffer::RingBuffer *_reports;
 	bool _sensor_ok;
 	work_s _work;
 	uint32_t _read_failures;
@@ -158,7 +158,7 @@ int IRLOCK::init()
 	}
 
 	/** allocate buffer storing values read from sensor **/
-	_reports = new RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s));
+	_reports = new ringbuffer::RingBuffer(IRLOCK_OBJECTS_MAX, sizeof(struct irlock_s));
 
 	if (_reports == nullptr) {
 		return ENOTTY;

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -138,7 +138,7 @@ protected:
 extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]);
 
 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
-	CONVERSION_INTERVAL, path),
+			CONVERSION_INTERVAL, path),
 	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
 	_t_system_power(-1),
 	system_power{}
@@ -189,9 +189,11 @@ MEASAirspeed::collect()
 		break;
 
 	case 1:
-		/* fallthrough */
+
+	/* fallthrough */
 	case 2:
-		/* fallthrough */
+
+	/* fallthrough */
 	case 3:
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
@@ -219,11 +221,11 @@ MEASAirspeed::collect()
 	  are generated when the bottom port is used as the static
 	  port on the pitot and top port is used as the dynamic port
 	 */
-	float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min);
+	float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
 	float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
 
-        // correct for 5V rail voltage if possible
-        voltage_correction(diff_press_pa_raw, temperature);
+	// correct for 5V rail voltage if possible
+	voltage_correction(diff_press_pa_raw, temperature);
 
 	// the raw value still should be compensated for the known offset
 	diff_press_pa_raw -= _diff_pres_offset;
@@ -276,6 +278,7 @@ MEASAirspeed::cycle()
 
 		/* perform collection */
 		ret = collect();
+
 		if (OK != ret) {
 			/* restart the measurement state machine */
 			start();
@@ -304,6 +307,7 @@ MEASAirspeed::cycle()
 
 	/* measurement phase */
 	ret = measure();
+
 	if (OK != ret) {
 		DEVICE_DEBUG("measure error");
 	}
@@ -332,18 +336,23 @@ void
 MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 {
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+
 	if (_t_system_power == -1) {
 		_t_system_power = orb_subscribe(ORB_ID(system_power));
 	}
+
 	if (_t_system_power == -1) {
 		// not available
 		return;
 	}
+
 	bool updated = false;
 	orb_check(_t_system_power, &updated);
+
 	if (updated) {
 		orb_copy(ORB_ID(system_power), _t_system_power, &system_power);
 	}
+
 	if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) {
 		// not valid, skip correction
 		return;
@@ -354,12 +363,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	  apply a piecewise linear correction, flattening at 0.5V from 5V
 	 */
 	float voltage_diff = system_power.voltage5V_v - 5.0f;
+
 	if (voltage_diff > 0.5f) {
 		voltage_diff = 0.5f;
 	}
+
 	if (voltage_diff < -0.5f) {
 		voltage_diff = -0.5f;
 	}
+
 	diff_press_pa -= voltage_diff * slope;
 
 	/*
@@ -367,12 +379,15 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
 	 */
 	const float temp_slope = 0.887f;
 	voltage_diff = system_power.voltage5V_v - 5.0f;
+
 	if (voltage_diff > 0.5f) {
 		voltage_diff = 0.5f;
 	}
+
 	if (voltage_diff < -1.0f) {
 		voltage_diff = -1.0f;
 	}
+
 	temperature -= voltage_diff * temp_slope;
 #endif // CONFIG_ARCH_BOARD_PX4FMU_V2
 }

src/drivers/pwm_out_sim/module.mk

@@ -38,6 +38,7 @@
 MODULE_COMMAND	= pwm_out_sim
 
 SRCS		= pwm_out_sim.cpp
+
 MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE	= 1200

src/drivers/pwm_out_sim/pwm_out_sim.cpp

@@ -234,7 +234,7 @@ PWMSim::init()
 	_task = px4_task_spawn_cmd("pwm_out_sim",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
-			   1200,
+			   1000,
 			   (px4_main_t)&PWMSim::task_main_trampoline,
 			   nullptr);
 
@@ -406,7 +406,7 @@ PWMSim::task_main()
 				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
 
 			/* can we mix? */
-			if (_mixers != nullptr) {
+			if (_armed && _mixers != nullptr) {
 				/* do mixing */
 				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL);
 				outputs.timestamp = hrt_absolute_time();

src/drivers/px4fmu/fmu.cpp

@@ -354,7 +354,7 @@ PX4FMU::init()
 	_task = px4_task_spawn_cmd("fmuservo",
 			       SCHED_DEFAULT,
 			       SCHED_PRIORITY_DEFAULT,
-			       1600,
+			       1200,
 			       (main_t)&PX4FMU::task_main_trampoline,
 			       nullptr);
 
@@ -607,7 +607,7 @@ PX4FMU::task_main()
 	orb_advert_t to_input_rc = 0;
 
 	memset(&rc_in, 0, sizeof(rc_in));
-	rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
+	rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM;
 #endif
 
 	/* initialize PWM limit lib */
@@ -772,8 +772,8 @@ PX4FMU::task_main()
 			// we have a new PPM frame. Publish it.
 			rc_in.channel_count = ppm_decoded_channels;
 
-			if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
-				rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
+			if (rc_in.channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+				rc_in.channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
 			}
 
 			for (uint8_t i = 0; i < rc_in.channel_count; i++) {

src/drivers/px4io/px4io.cpp

@@ -672,8 +672,8 @@ PX4IO::init()
 		return -1;
 	}
 
-	if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
-		_max_rc_input = RC_INPUT_MAX_CHANNELS;
+	if (_max_rc_input > input_rc_s::RC_INPUT_MAX_CHANNELS)
+		_max_rc_input = input_rc_s::RC_INPUT_MAX_CHANNELS;
 
 	param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);
 	param_get(param_find("RC_RSSI_PWM_MAX"), &_rssi_pwm_max);
@@ -1675,10 +1675,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	int	ret;
 
 	/* we don't have the status bits, so input_source has to be set elsewhere */
-	input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
+	input_rc.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
 
 	const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT);
-	uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog];
+	uint16_t regs[input_rc_s::RC_INPUT_MAX_CHANNELS + prolog];
 
 	/*
 	 * Read the channel count and the first 9 channels.
@@ -1697,8 +1697,8 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	channel_count = regs[PX4IO_P_RAW_RC_COUNT];
 
 	/* limit the channel count */
-	if (channel_count > RC_INPUT_MAX_CHANNELS) {
-		channel_count = RC_INPUT_MAX_CHANNELS;
+	if (channel_count > input_rc_s::RC_INPUT_MAX_CHANNELS) {
+		channel_count = input_rc_s::RC_INPUT_MAX_CHANNELS;
 	}
 
 	_rc_chan_count = channel_count;
@@ -1736,7 +1736,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
 	}
 
 	/* get RSSI from input channel */
-	if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
+	if (_rssi_pwm_chan > 0 && _rssi_pwm_chan <= input_rc_s::RC_INPUT_MAX_CHANNELS && _rssi_pwm_max - _rssi_pwm_min != 0) {
 		int rssi = (input_rc.values[_rssi_pwm_chan - 1] - _rssi_pwm_min) /
 			((_rssi_pwm_max - _rssi_pwm_min) / 100);
 		rssi = rssi > 100 ? 100 : rssi;
@@ -1764,19 +1764,19 @@ PX4IO::io_publish_raw_rc()
 
 	/* sort out the source of the values */
 	if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
-		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+		rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
 
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
-		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+		rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
 
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
-		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+		rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
 
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
-		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24;
+		rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
 
 	} else {
-		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
+		rc_val.input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
 
 		/* only keep publishing RC input if we ever got a valid input */
 		if (_rc_last_valid == 0) {
@@ -2660,19 +2660,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 			/* sort out the source of the values */
 			if (status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
-				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_PPM;
+				rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_PPM;
 
 			} else if (status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
-				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
+				rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
 
 			} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
-				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
+				rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_SBUS;
 
 			} else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
-				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24;
+				rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_PX4IO_ST24;
 
 			} else {
-				rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
+				rc_val->input_source = input_rc_s::RC_INPUT_SOURCE_UNKNOWN;
 			}
 
 			/* read raw R/C input values */
@@ -2757,7 +2757,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		   on param_get()
 		*/
 		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
-		if (config->channel >= RC_INPUT_MAX_CHANNELS) {
+		if (config->channel >= input_rc_s::RC_INPUT_MAX_CHANNELS) {
 			/* fail with error */
 			return -E2BIG;
 		}

src/examples/px4_daemon_app/px4_daemon_app.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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

src/examples/px4_mavlink_debug/px4_mavlink_debug.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
+ *   Copyright (c) 2012-2015 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
@@ -35,6 +34,8 @@
 /**
  * @file px4_mavlink_debug.c
  * Debug application example for PX4 autopilot
+ *
+ * @author Example User <mail@example.com>
  */
 
 #include <px4_config.h>

src/examples/px4_simple_app/px4_simple_app.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
+ *   Copyright (c) 2012-2015 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
@@ -35,6 +34,8 @@
 /**
  * @file px4_simple_app.c
  * Minimal application example for PX4 autopilot
+ *
+ * @author Example User <mail@example.com>
  */
 
 #include <px4_config.h>

src/lib/ecl/attitude_fw/ecl_controller.cpp

@@ -130,7 +130,7 @@ float ECL_Controller::get_desired_bodyrate()
 
 float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
 	float airspeed_result = airspeed;
-	if (!isfinite(airspeed)) {
+	if (!PX4_ISFINITE(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed_result = 0.5f * (minspeed + maxspeed);
 	} else if (airspeed < minspeed) {

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -60,62 +60,24 @@ ECL_PitchController::~ECL_PitchController()
 
 float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
-	float roll = ctl_data.roll;
 
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(ctl_data.pitch_setpoint) &&
-	      isfinite(roll) &&
-	      isfinite(ctl_data.pitch) &&
-	      isfinite(ctl_data.airspeed))) {
+	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
+	      PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.airspeed))) {
 		perf_count(_nonfinite_input_perf);
 		warnx("not controlling pitch");
 		return _rate_setpoint;
 	}
 
-	/* flying inverted (wings upside down) ? */
-	bool inverted = false;
-
-	/* roll is used as feedforward term and inverted flight needs to be considered */
-	if (fabsf(roll) < math::radians(90.0f)) {
-		/* not inverted, but numerically still potentially close to infinity */
-		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
-
-	} else {
-		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
-
-		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
-		if (roll > 0.0f) {
-			/* right hemisphere */
-			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
-
-		} else {
-			/* left hemisphere */
-			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
-		}
-	}
-
-	/* input conditioning */
-	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
-	/* calculate the offset in the rate resulting from rolling  */
-	//xxx needs explanation and conversion to body angular rates or should be removed
-	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
-				  tanf(roll) * sinf(roll)) * _roll_ff;
-
-	if (inverted) {
-		turn_offset = -turn_offset;
-	}
-
 	/* Calculate the error */
 	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
 
 	/*  Apply P controller: rate setpoint from current error and time constant */
 	_rate_setpoint =  pitch_error / _tc;
 
-	/* add turn offset */
-	_rate_setpoint += turn_offset;
-
-	/* limit the rate */ //XXX: move to body angluar rates
-
+	/* limit the rate */
 	if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
 		if (_rate_setpoint > 0.0f) {
 			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
@@ -132,14 +94,14 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
 float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(ctl_data.roll) &&
-	      isfinite(ctl_data.pitch) &&
-	      isfinite(ctl_data.pitch_rate) &&
-	      isfinite(ctl_data.yaw_rate) &&
-	      isfinite(ctl_data.yaw_rate_setpoint) &&
-	      isfinite(ctl_data.airspeed_min) &&
-	      isfinite(ctl_data.airspeed_max) &&
-	      isfinite(ctl_data.scaler))) {
+	if (!(PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.pitch_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) &&
+	      PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
 		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
@@ -160,11 +122,48 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
 	_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
 			     cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
 
-	/* Transform estimation to body angular rates (jacobian) */
-	float pitch_bodyrate = cosf(ctl_data.roll) * ctl_data.pitch_rate +
-			       cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate;
+	/* apply turning offset to desired bodyrate setpoint*/
+	/* flying inverted (wings upside down)*/
+	bool inverted = false;
+	float constrained_roll;
+	/* roll is used as feedforward term and inverted flight needs to be considered */
+	if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
+		/* not inverted, but numerically still potentially close to infinity */
+		constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
 
-	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
+	} else {
+		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
+		inverted = true;
+		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
+		if (ctl_data.roll > 0.0f) {
+			/* right hemisphere */
+			constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
+
+		} else {
+			/* left hemisphere */
+			constrained_roll = math::constrain(ctl_data.roll, math::radians(-100.0f), math::radians(-180.0f));
+		}
+	}
+
+	/* input conditioning */
+	float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
+
+	/* Calculate desired body fixed y-axis angular rate needed to compensate for roll angle.
+	   For reference see Automatic Control of Aircraft and Missiles by John H. Blakelock, pg. 175
+	   Availible on google books 8/11/2015: 
+	   https://books.google.com/books?id=ubcczZUDCsMC&pg=PA175#v=onepage&q&f=false*/
+	float body_fixed_turn_offset = (fabsf((CONSTANTS_ONE_G / airspeed) *
+				  		tanf(constrained_roll) * sinf(constrained_roll)));
+
+	if (inverted) {
+		body_fixed_turn_offset = -body_fixed_turn_offset;
+	}
+
+	/* Finally add the turn offset to your bodyrate setpoint*/
+	_bodyrate_setpoint += body_fixed_turn_offset;
+
+
+	_rate_error = _bodyrate_setpoint - ctl_data.pitch_rate;
 
 	if (!lock_integrator && _k_i > 0.0f) {
 

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -59,7 +59,7 @@ ECL_RollController::~ECL_RollController()
 float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(ctl_data.roll_setpoint) && isfinite(ctl_data.roll))) {
+	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) && PX4_ISFINITE(ctl_data.roll))) {
 		perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
@@ -83,13 +83,13 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
 float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(ctl_data.pitch) &&
-	      isfinite(ctl_data.roll_rate) &&
-	      isfinite(ctl_data.yaw_rate) &&
-	      isfinite(ctl_data.yaw_rate_setpoint) &&
-	      isfinite(ctl_data.airspeed_min) &&
-	      isfinite(ctl_data.airspeed_max) &&
-	      isfinite(ctl_data.scaler))) {
+	if (!(PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.roll_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) &&
+	      PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
 		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
@@ -109,11 +109,8 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
 	/* Transform setpoint to body angular rates (jacobian) */
 	_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
 
-	/* Transform estimation to body angular rates (jacobian) */
-	float roll_bodyrate = ctl_data.roll_rate - sinf(ctl_data.pitch) * ctl_data.yaw_rate;
-
 	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
+	_rate_error = _bodyrate_setpoint - ctl_data.roll_rate; //body angular rate error
 
 	if (!lock_integrator && _k_i > 0.0f) {
 

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -101,13 +101,13 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
 float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(ctl_data.roll) &&
-	      isfinite(ctl_data.pitch) &&
-	      isfinite(ctl_data.speed_body_u) &&
-	      isfinite(ctl_data.speed_body_v) &&
-	      isfinite(ctl_data.speed_body_w) &&
-	      isfinite(ctl_data.roll_rate_setpoint) &&
-	      isfinite(ctl_data.pitch_rate_setpoint))) {
+	if (!(PX4_ISFINITE(ctl_data.roll) &&
+	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(ctl_data.speed_body_u) &&
+	      PX4_ISFINITE(ctl_data.speed_body_v) &&
+	      PX4_ISFINITE(ctl_data.speed_body_w) &&
+	      PX4_ISFINITE(ctl_data.roll_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.pitch_rate_setpoint))) {
 		perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
@@ -145,7 +145,7 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 
 //	counter++;
 
-	if (!isfinite(_rate_setpoint)) {
+	if (!PX4_ISFINITE(_rate_setpoint)) {
 		warnx("yaw rate sepoint not finite");
 		_rate_setpoint = 0.0f;
 	}
@@ -156,10 +156,10 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
 float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
-	      isfinite(ctl_data.yaw_rate) && isfinite(ctl_data.pitch_rate_setpoint) &&
-	      isfinite(ctl_data.airspeed_min) && isfinite(ctl_data.airspeed_max) &&
-	      isfinite(ctl_data.scaler))) {
+	if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
+	      PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
+	      PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
+	      PX4_ISFINITE(ctl_data.scaler))) {
 		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}
@@ -179,7 +179,7 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
 	/* input conditioning */
 	float airspeed = ctl_data.airspeed;
 
-	if (!isfinite(airspeed)) {
+	if (!PX4_ISFINITE(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
 
@@ -198,12 +198,8 @@ float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl
 		_bodyrate_setpoint -= (ctl_data.acc_body_y / (airspeed * cosf(ctl_data.pitch)));
 	}
 
-	/* Transform estimation to body angular rates (jacobian) */
-	float yaw_bodyrate = -sinf(ctl_data.roll) * ctl_data.pitch_rate +
-			     cosf(ctl_data.roll) * cosf(ctl_data.pitch) * ctl_data.yaw_rate;
-
 	/* Calculate body angular rate error */
-	_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
+	_rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
 
 	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
 

src/lib/external_lgpl/tecs/tecs.cpp

@@ -62,6 +62,10 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
 		_integ3_state = baro_altitude;
 		_integ2_state = 0.0f;
 		_integ1_state = 0.0f;
+
+		// Reset the filter state as we just switched from non-altitude control
+		// to altitude control mode
+		_states_initalized = false;
 	}
 
 	_update_50hz_last_usec = now;
@@ -94,7 +98,7 @@ void TECS::update_state(float baro_altitude, float airspeed, const math::Matrix<
 	// Only required if airspeed is being measured and controlled
 	float temp = 0;
 
-	if (isfinite(airspeed) && airspeed_sensor_enabled()) {
+	if (PX4_ISFINITE(airspeed) && airspeed_sensor_enabled()) {
 		// Get DCM
 		// Calculate speed rate of change
 		// XXX check
@@ -138,7 +142,7 @@ void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
 
 	// Get airspeed or default to halfway between min and max if
 	// airspeed is not being used and set speed rate to zero
-	if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
+	if (!PX4_ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) {
 		// If no airspeed available use average of min and max
 		_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
 
@@ -224,12 +228,12 @@ void TECS::_update_speed_demand(void)
 void TECS::_update_height_demand(float demand, float state)
 {
 	// Handle initialization
-	if (isfinite(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
+	if (PX4_ISFINITE(demand) && fabsf(_hgt_dem_in_old) < 0.1f) {
 		_hgt_dem_in_old = demand;
 	}
 	// Apply 2 point moving average to demanded height
 	// This is required because height demand is updated in steps
-	if (isfinite(demand)) {
+	if (PX4_ISFINITE(demand)) {
 		_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
 	} else {
 		_hgt_dem = _hgt_dem_in_old;
@@ -309,12 +313,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 	// Calculate total energy values
 	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
 	float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
-	float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
+	_STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
 
 	// Apply 0.5 second first order filter to STEdot_error
 	// This is required to remove accelerometer noise from the  measurement
-	STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
-	_STEdotErrLast = STEdot_error;
+	_STEdot_error = 0.2f * _STEdot_error + 0.8f * _STEdotErrLast;
+	_STEdotErrLast = _STEdot_error;
 
 	// Calculate throttle demand
 	// If underspeed condition is set, then demand full throttle
@@ -342,7 +346,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 		}
 
 		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
-		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = (_STE_error + _STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
 		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// Rate limit PD + FF throttle
@@ -438,11 +442,11 @@ void TECS::_update_pitch(void)
 	// Calculate Specific Energy Balance demand, and error
 	float SEB_dem      = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
 	float SEBdot_dem   = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
-	float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
-	float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
+	_SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
+	_SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
 
 	// Calculate integrator state, constraining input if pitch limits are exceeded
-	float integ7_input = SEB_error * _integGain;
+	float integ7_input = _SEB_error * _integGain;
 
 	if (_pitch_dem_unc > _PITCHmaxf) {
 		integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
@@ -460,7 +464,7 @@ void TECS::_update_pitch(void)
 	// demand equal to the minimum value (which is )set by the mission plan during this mode). Otherwise the
 	// integrator has to catch up before the nose can be raised to reduce speed during climbout.
 	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
-	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	float temp = _SEB_error + _SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
 	if (_climbOutDem)
 	{
 		temp += _PITCHminf * gainInv;
@@ -598,18 +602,26 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 		_tecs_state.mode = ECL_TECS_MODE_NORMAL;
 	}
 
-	_tecs_state.hgt_dem  = _hgt_dem_adj;
-	_tecs_state.hgt      = _integ3_state;
-	_tecs_state.dhgt_dem = _hgt_rate_dem;
-	_tecs_state.dhgt     = _integ2_state;
-	_tecs_state.spd_dem  = _TAS_dem_adj;
-	_tecs_state.spd      = _integ5_state;
-	_tecs_state.dspd     = _vel_dot;
-	_tecs_state.ithr     = _integ6_state;
-	_tecs_state.iptch    = _integ7_state;
-	_tecs_state.thr      = _throttle_dem;
-	_tecs_state.ptch     = _pitch_dem;
-	_tecs_state.dspd_dem = _TAS_rate_dem;
+	_tecs_state.altitude_sp = _hgt_dem_adj;
+	_tecs_state.altitude_filtered = _integ3_state;
+	_tecs_state.altitude_rate_sp = _hgt_rate_dem;
+	_tecs_state.altitude_rate = _integ2_state;
+
+	_tecs_state.airspeed_sp = _TAS_dem_adj;
+	_tecs_state.airspeed_rate_sp = _TAS_rate_dem;
+	_tecs_state.airspeed_filtered = _integ5_state;
+	_tecs_state.airspeed_rate = _vel_dot;
+
+	_tecs_state.total_energy_error = _STE_error;
+	_tecs_state.energy_distribution_error = _SEB_error;
+	_tecs_state.total_energy_rate_error = _STEdot_error;
+	_tecs_state.energy_distribution_error = _SEBdot_error;
+
+	_tecs_state.energy_error_integ = _integ6_state;
+	_tecs_state.energy_distribution_error_integ = _integ7_state;
+
+	_tecs_state.throttle_integ 	= _integ6_state;
+	_tecs_state.pitch_integ 	= _integ7_state;
 
 	_update_pitch_throttle_last_usec = now;
 

src/lib/external_lgpl/tecs/tecs.h

@@ -79,6 +79,10 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
+		_STE_error(0.0f),
+		_STEdot_error(0.0f),
+		_SEB_error(0.0f),
+		_SEBdot_error(0.0f),
 		_airspeed_enabled(false),
 		_states_initalized(false),
 		_in_air(false),
@@ -137,18 +141,22 @@ public:
 
 	struct tecs_state {
 		uint64_t timestamp;
-		float hgt;
-		float dhgt;
-		float hgt_dem;
-		float dhgt_dem;
-		float spd_dem;
-		float spd;
-		float dspd;
-		float ithr;
-		float iptch;
-		float thr;
-		float ptch;
-		float dspd_dem;
+		float altitude_filtered;
+		float altitude_sp;
+		float altitude_rate;
+		float altitude_rate_sp;
+		float airspeed_filtered;
+		float airspeed_sp;
+		float airspeed_rate;
+		float airspeed_rate_sp;
+		float energy_error_integ;
+		float energy_distribution_error_integ;
+		float total_energy_error;
+		float total_energy_rate_error;
+		float energy_distribution_error;
+		float energy_distribution_rate_error;
+		float throttle_integ;
+		float pitch_integ;
 		enum ECL_TECS_MODE mode;
 	};
 
@@ -376,6 +384,15 @@ private:
 	// Specific energy error quantities
 	float _STE_error;
 
+	// Energy error rate
+	float _STEdot_error;
+
+	// Specific energy balance error
+	float _SEB_error;
+
+	// Specific energy balance error rate
+	float _SEBdot_error;
+
 	// Time since last update of main TECS loop (seconds)
 	float _DT;
 

src/lib/mathlib/math/Matrix.hpp

@@ -285,7 +285,7 @@ public:
 
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				res[i][j] = data[i][j] / num;
+				res.data[i][j] = data[i][j] / num;
 
 		return res;
 	}
@@ -375,7 +375,7 @@ public:
 			printf("[ ");
 
 			for (unsigned int j = 0; j < N; j++)
-				printf("%.3f\t", data[i][j]);
+				printf("%.3f\t", (double)data[i][j]);
 
 			printf(" ]\n");
 		}

src/lib/px4_eigen.h

@@ -46,8 +46,6 @@
 #pragma GCC diagnostic ignored "-Wfloat-equal"
 #define _GLIBCXX_USE_C99_FP_MACROS_DYNAMIC 1
 
-#define EIGEN_MAX_STATIC_ALIGN_BYTES 16
-
 #include <eigen/Eigen/Core>
 #include <eigen/Eigen/Geometry>
 #pragma GCC diagnostic pop

src/modules/commander/commander.cpp

@@ -108,6 +108,7 @@
 #include <geo/geo.h>
 #include <systemlib/state_table.h>
 #include <dataman/dataman.h>
+#include <navigator/navigation.h>
 
 #include "px4_custom_mode.h"
 #include "commander_helper.h"
@@ -238,6 +239,8 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct ma
 
 void set_control_mode();
 
+bool stabilization_required();
+
 void print_reject_mode(struct vehicle_status_s *current_status, const char *msg);
 
 void print_reject_arm(const char *msg);
@@ -294,7 +297,7 @@ int commander_main(int argc, char *argv[])
 					     SCHED_PRIORITY_MAX - 40,
 					     3400,
 					     commander_thread_main,
-					     (argv) ? (char * const *)&argv[2] : (char * const *)NULL);
+					     (char * const *)&argv[0]);
 
 		unsigned constexpr max_wait_us = 1000000;
 		unsigned constexpr max_wait_steps = 2000;
@@ -359,7 +362,7 @@ int commander_main(int argc, char *argv[])
 
 			if (calib_ret) {
 				warnx("calibration failed, exiting.");
-				return 0;
+				return 1;
 			} else {
 				return 0;
 			}
@@ -388,7 +391,7 @@ int commander_main(int argc, char *argv[])
 		int mavlink_fd_local = px4_open(MAVLINK_LOG_DEVICE, 0);
 		arm_disarm(false, mavlink_fd_local, "command line");
 		px4_close(mavlink_fd_local);
-                return 0;
+		return 0;
 	}
 
 	usage("unrecognized command");
@@ -874,6 +877,23 @@ int commander_thread_main(int argc, char *argv[])
 	bool arm_tune_played = false;
 	bool was_armed = false;
 
+	bool startup_in_hil = false;
+
+#ifdef __PX4_NUTTX
+	/* NuttX indicates 3 arguments when only 2 are present */
+	argc -= 1;
+#endif
+
+	if (argc > 2) {
+		if (!strcmp(argv[2],"-hil")) {
+			startup_in_hil = true;
+		} else {
+			PX4_ERR("Argument %s not supported.", argv[2]);
+			PX4_ERR("COMMANDER NOT STARTED");
+			thread_should_exit = true;
+		}
+	}
+
 	/* set parameters */
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
@@ -955,7 +975,12 @@ int commander_thread_main(int argc, char *argv[])
 	status.main_state =vehicle_status_s::MAIN_STATE_MANUAL;
 	status.nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 	status.arming_state = vehicle_status_s::ARMING_STATE_INIT;
-	status.hil_state = vehicle_status_s::HIL_STATE_OFF;
+
+	if(startup_in_hil) {
+		status.hil_state = vehicle_status_s::HIL_STATE_ON;
+	} else {
+		status.hil_state = vehicle_status_s::HIL_STATE_OFF;
+	}
 	status.failsafe = false;
 
 	/* neither manual nor offboard control commands have been received */
@@ -2219,7 +2244,6 @@ int commander_thread_main(int argc, char *argv[])
 			arm_tune_played = false;
 		}
 
-		//fflush(stdout);
 		counter++;
 
 		int blink_state = blink_msg_state();
@@ -2537,8 +2561,8 @@ set_control_mode()
 	case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
-		control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab);
+		control_mode.flag_control_rates_enabled = stabilization_required();
+		control_mode.flag_control_attitude_enabled = stabilization_required();
 		control_mode.flag_control_altitude_enabled = false;
 		control_mode.flag_control_climb_rate_enabled = false;
 		control_mode.flag_control_position_enabled = false;
@@ -2579,8 +2603,8 @@ set_control_mode()
 		control_mode.flag_control_attitude_enabled = true;
 		control_mode.flag_control_altitude_enabled = true;
 		control_mode.flag_control_climb_rate_enabled = true;
-		control_mode.flag_control_position_enabled = true;
-		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_position_enabled = !status.in_transition_mode;
+		control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
@@ -2689,12 +2713,12 @@ set_control_mode()
 			!offboard_control_mode.ignore_acceleration_force;
 
 		control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
-			!offboard_control_mode.ignore_position) && !vtol_status.vtol_in_trans_mode;
+			!offboard_control_mode.ignore_position) && !status.in_transition_mode;
 
 		control_mode.flag_control_climb_rate_enabled = !offboard_control_mode.ignore_velocity ||
 			!offboard_control_mode.ignore_position;
 
-		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !vtol_status.vtol_in_trans_mode;
+		control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !status.in_transition_mode;
 
 		control_mode.flag_control_altitude_enabled = !offboard_control_mode.ignore_velocity ||
 			!offboard_control_mode.ignore_position;
@@ -2706,6 +2730,15 @@ set_control_mode()
 	}
 }
 
+bool
+stabilization_required()
+{
+	return (status.is_rotary_wing ||		// is a rotary wing, or
+		status.vtol_fw_permanent_stab || 	// is a VTOL in fixed wing mode and stabilisation is on, or
+		(vtol_status.vtol_in_trans_mode && 	// is currently a VTOL transitioning AND
+			!status.is_rotary_wing));	// is a fixed wing, ie: transitioning back to rotary wing mode
+}
+
 void
 print_reject_mode(struct vehicle_status_s *status_local, const char *msg)
 {

src/modules/commander/state_machine_helper_posix.cpp

@@ -1,634 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2015 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 state_machine_helper.cpp
- * State machine helper functions implementations
- *
- * @author Thomas Gubler <thomas@px4.io>
- * @author Julian Oes <julian@oes.ch>
- */
-
-#include <px4_posix.h>
-#include <stdio.h>
-#include <unistd.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <string.h>
-#include <math.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/differential_pressure.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_device.h>
-#include <mavlink/mavlink_log.h>
-
-#include "state_machine_helper.h"
-#include "commander_helper.h"
-#include "PreflightCheck.h"
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
-// This array defines the arming state transitions. The rows are the new state, and the columns
-// are the current state. Using new state and current  state you can index into the array which
-// will be true for a valid transition or false for a invalid transition. In some cases even
-// though the transition is marked as true additional checks must be made. See arming_state_transition
-// code for those checks.
-static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
-	//                                                    INIT,  STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
-	{ /* vehicle_status_s::ARMING_STATE_INIT */           true,  true,    false, false,       true,          false,  false },
-	{ /* vehicle_status_s::ARMING_STATE_STANDBY */        true,  true,    true,  true,        false,         false,  false },
-	{ /* vehicle_status_s::ARMING_STATE_ARMED */          false, true,    true,  false,       false,         false,  true },
-	{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */    false, false,   true,  true,        false,         false,  false },
-	{ /* vehicle_status_s::ARMING_STATE_STANDBY_ERROR */  true,  true,    true,  true,        true,          false,  false },
-	{ /* vehicle_status_s::ARMING_STATE_REBOOT */         true,  true,    false, false,       true,          true,   true },
-	{ /* vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE */ false, false,   false, false,       false,         false,  false }, // NYI
-};
-
-// You can index into the array with an arming_state_t in order to get it's textual representation
-static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
-	"ARMING_STATE_INIT",
-	"ARMING_STATE_STANDBY",
-	"ARMING_STATE_ARMED",
-	"ARMING_STATE_ARMED_ERROR",
-	"ARMING_STATE_STANDBY_ERROR",
-	"ARMING_STATE_REBOOT",
-	"ARMING_STATE_IN_AIR_RESTORE",
-};
-
-transition_result_t
-arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
-			const struct safety_s   *safety,		///< current safety settings
-			arming_state_t          new_arming_state,	///< arming state requested
-			struct actuator_armed_s *armed,			///< current armed status
-			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
-			const int               mavlink_fd)		///< mavlink fd for error reporting, 0 for none
-{
-	// Double check that our static arrays are still valid
-	ASSERT(vehicle_status_s::ARMING_STATE_INIT == 0);
-	ASSERT(vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE == vehicle_status_s::ARMING_STATE_MAX - 1);
-
-	transition_result_t ret = TRANSITION_DENIED;
-	arming_state_t current_arming_state = status->arming_state;
-	bool feedback_provided = false;
-
-	/* only check transition if the new state is actually different from the current one */
-	if (new_arming_state == current_arming_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-
-		/*
-		 * Get sensing state if necessary
-		 */
-		int prearm_ret = OK;
-
-		/* only perform the check if we have to */
-		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-			prearm_ret = prearm_check(status, mavlink_fd);
-		}
-
-		/*
-		 * Perform an atomic state update
-		 */
-		//irqstate_t flags = irqsave();
-
-		/* enforce lockdown in HIL */
-		if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
-			armed->lockdown = true;
-
-		} else {
-			armed->lockdown = false;
-		}
-
-		// Check that we have a valid state transition
-		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
-
-		if (valid_transition) {
-			// We have a good transition. Now perform any secondary validation.
-			if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-
-				//      Do not perform pre-arm checks if coming from in air restore
-				//      Allow if vehicle_status_s::HIL_STATE_ON
-				if (status->arming_state != vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE &&
-					status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
-
-					// Fail transition if pre-arm check fails
-					if (prearm_ret) {
-						/* the prearm check already prints the reject reason */
-						feedback_provided = true;
-						valid_transition = false;
-
-					// Fail transition if we need safety switch press
-					} else if (safety->safety_switch_available && !safety->safety_off) {
-
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
-						feedback_provided = true;
-						valid_transition = false;
-					}
-
-					// Perform power checks only if circuit breaker is not
-					// engaged for these checks
-					if (!status->circuit_breaker_engaged_power_check) {
-						// Fail transition if power is not good
-						if (!status->condition_power_input_valid) {
-
-							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
-							feedback_provided = true;
-							valid_transition = false;
-						}
-
-						// Fail transition if power levels on the avionics rail
-						// are measured but are insufficient
-						if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) {
-							// Check avionics rail voltages
-							if (status->avionics_power_rail_voltage < 4.75f) {
-								mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
-								feedback_provided = true;
-								valid_transition = false;
-							} else if (status->avionics_power_rail_voltage < 4.9f) {
-								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage);
-								feedback_provided = true;
-							} else if (status->avionics_power_rail_voltage > 5.4f) {
-								mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage);
-								feedback_provided = true;
-							}
-						}
-					}
-
-				}
-
-			} else if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) {
-				new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
-			}
-		}
-
-		// HIL can always go to standby
-		if (status->hil_state == vehicle_status_s::HIL_STATE_ON && new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
-			warnx("HIL STATE ON -> STANDBY");
-			valid_transition = true;
-		}
-
-		/* Sensors need to be initialized for STANDBY state */
-		if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
-			warnx("NOT ARMING: Sensors not operational.");
-			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
-			feedback_provided = true;
-			valid_transition = false;
-			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
-		}
-
-		/* Check if we are trying to arm, checks look good but we are in STANDBY_ERROR */
-		if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
-			new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
-			if (status->condition_system_sensors_initialized) {
-				mavlink_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming");
-			} else {
-				mavlink_log_critical(mavlink_fd, "Preflight check failed, refusing to arm");
-			}
-			feedback_provided = true;
-		}
-
-		// Finish up the state transition
-		if (valid_transition) {
-			armed->armed = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR;
-			armed->ready_to_arm = new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY;
-			ret = TRANSITION_CHANGED;
-			status->arming_state = new_arming_state;
-		}
-
-		/* end of atomic state update */
-		//irqrestore(flags);
-	}
-
-	if (ret == TRANSITION_DENIED) {
-		const char * str = "INVAL: %s - %s";
-		/* only print to console here by default as this is too technical to be useful during operation */
-		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
-
-		/* print to MAVLink if we didn't provide any feedback yet */
-		if (!feedback_provided) {
-			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
-		}
-	}
-
-	return ret;
-}
-
-bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed)
-{
-	// System is safe if:
-	// 1) Not armed
-	// 2) Armed, but in software lockdown (HIL)
-	// 3) Safety switch is present AND engaged -> actuators locked
-	if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) {
-		return true;
-
-	} else {
-		return false;
-	}
-}
-
-transition_result_t
-main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state)
-{
-	transition_result_t ret = TRANSITION_DENIED;
-
-	/* transition may be denied even if the same state is requested because conditions may have changed */
-	switch (new_main_state) {
-	case vehicle_status_s::MAIN_STATE_MANUAL:
-	case vehicle_status_s::MAIN_STATE_ACRO:
-		ret = TRANSITION_CHANGED;
-		break;
-
-	case vehicle_status_s::MAIN_STATE_ALTCTL:
-		/* need at minimum altitude estimate */
-		/* TODO: add this for fixedwing as well */
-		if (!status->is_rotary_wing ||
-		    (status->condition_local_altitude_valid ||
-		     status->condition_global_position_valid)) {
-			ret = TRANSITION_CHANGED;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_POSCTL:
-		/* need at minimum local position estimate */
-		if (status->condition_local_position_valid ||
-		    status->condition_global_position_valid) {
-			ret = TRANSITION_CHANGED;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
-		/* need global position estimate */
-		if (status->condition_global_position_valid) {
-			ret = TRANSITION_CHANGED;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
-	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
-		/* need global position and home position */
-		if (status->condition_global_position_valid && status->condition_home_position_valid) {
-			ret = TRANSITION_CHANGED;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_OFFBOARD:
-
-		/* need offboard signal */
-		if (!status->offboard_control_signal_lost) {
-			ret = TRANSITION_CHANGED;
-		}
-
-		break;
-
-	case vehicle_status_s::MAIN_STATE_MAX:
-	default:
-		break;
-	}
-	if (ret == TRANSITION_CHANGED) {
-		if (status->main_state != new_main_state) {
-			status->main_state = new_main_state;
-		} else {
-			ret = TRANSITION_NOT_CHANGED;
-		}
-	}
-
-	return ret;
-}
-
-/**
- * Transition from one hil state to another
- */
-transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
-{
-	transition_result_t ret = TRANSITION_DENIED;
-
-	if (current_status->hil_state == new_state) {
-		ret = TRANSITION_NOT_CHANGED;
-
-	} else {
-		switch (new_state) {
-		case vehicle_status_s::HIL_STATE_OFF:
-			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
-			ret = TRANSITION_DENIED;
-			break;
-
-		case vehicle_status_s::HIL_STATE_ON:
-			if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
-			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
-			    || current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
-
-				const char *devname;
-				unsigned int handle = 0;
-				for(;;) {
-					devname = px4_get_device_names(&handle);
-					if (devname == NULL)
-						break;
-
-					/* skip mavlink */
-					if (!strcmp(MAVLINK_LOG_DEVICE, devname)) {
-						continue;
-					}
-
-
-					int sensfd = px4_open(devname, 0);
-
-					if (sensfd < 0) {
-						warn("failed opening device %s", devname);
-						continue;
-					}
-
-					int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
-					px4_close(sensfd);
-
-						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
-				}
-				ret = TRANSITION_CHANGED;
-				mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
-			} else {
-				mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
-				ret = TRANSITION_DENIED;
-			}
-			break;
-
-		default:
-			warnx("Unknown HIL state");
-			break;
-		}
-	}
-
-	if (ret == TRANSITION_CHANGED) {
-		current_status->hil_state = new_state;
-		current_status->timestamp = hrt_absolute_time();
-		// XXX also set lockdown here
-		orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
-	}
-	return ret;
-}
-
-/**
- * Check failsafe and main status and set navigation status for navigator accordingly
- */
-bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
-		   const bool stay_in_failsafe)
-{
-	navigation_state_t nav_state_old = status->nav_state;
-
-	bool armed = (status->arming_state == vehicle_status_s::ARMING_STATE_ARMED || status->arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR);
-	status->failsafe = false;
-
-	/* evaluate main state to decide in normal (non-failsafe) mode */
-	switch (status->main_state) {
-	case vehicle_status_s::MAIN_STATE_ACRO:
-	case vehicle_status_s::MAIN_STATE_MANUAL:
-	case vehicle_status_s::MAIN_STATE_ALTCTL:
-	case vehicle_status_s::MAIN_STATE_POSCTL:
-		/* require RC for all manual modes */
-		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-
-		} else {
-			switch (status->main_state) {
-			case vehicle_status_s::MAIN_STATE_ACRO:
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ACRO;
-				break;
-
-			case vehicle_status_s::MAIN_STATE_MANUAL:
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
-				break;
-
-			case vehicle_status_s::MAIN_STATE_ALTCTL:
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
-				break;
-
-			case vehicle_status_s::MAIN_STATE_POSCTL:
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
-				break;
-
-			default:
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
-				break;
-			}
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_AUTO_MISSION:
-
-		/* go into failsafe
-		 * - if commanded to do so
-		 * - if we have an engine failure
-		 * - depending on datalink, RC and if the mission is finished */
-
-		/* first look at the commands */
-		if (status->engine_failure_cmd) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if (status->data_link_lost_cmd) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-		} else if (status->gps_failure_cmd) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
-		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
-
-		/* finished handling commands which have priority, now handle failures */
-		} else if (status->gps_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL;
-		} else if (status->engine_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-
-		/* datalink loss enabled:
-		 * check for datalink lost: this should always trigger RTGS */
-		} else if (data_link_loss_enabled && status->data_link_lost) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-
-		/* datalink loss disabled:
-		 * check if both, RC and datalink are lost during the mission
-		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
-		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
-						       (status->rc_signal_lost && mission_finished))) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-
-		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
-		} else if (!stay_in_failsafe){
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_AUTO_LOITER:
-		/* go into failsafe on a engine failure */
-		if (status->engine_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		/* also go into failsafe if just datalink is lost */
-		} else if (status->data_link_lost && data_link_loss_enabled) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-
-		/* go into failsafe if RC is lost and datalink loss is not set up */
-		} else if (status->rc_signal_lost && !data_link_loss_enabled) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-
-		/* don't bother if RC is lost if datalink is connected */
-		} else if (status->rc_signal_lost) {
-
-			/* this mode is ok, we don't need RC for loitering */
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
-		} else {
-			/* everything is perfect */
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_AUTO_RTL:
-		/* require global position and home, also go into failsafe on an engine failure */
-
-		if (status->engine_failure) {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if ((!status->condition_global_position_valid ||
-					!status->condition_home_position_valid)) {
-			status->failsafe = true;
-
-			if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-		} else {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
-		}
-		break;
-
-	case vehicle_status_s::MAIN_STATE_OFFBOARD:
-		/* require offboard control, otherwise stay where you are */
-		if (status->offboard_control_signal_lost && !status->rc_signal_lost) {
-			status->failsafe = true;
-
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_POSCTL;
-		} else if (status->offboard_control_signal_lost && status->rc_signal_lost) {
-			status->failsafe = true;
-
-			if (status->condition_local_position_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
-			}
-		} else {
-			status->nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
-		}
-	default:
-		break;
-	}
-
-	return status->nav_state != nav_state_old;
-}
-
-int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
-{
-	/* at this point this equals the preflight check, but might add additional
-	 * quantities later.
-	 */
-	bool checkAirspeed = false;
-	/* Perform airspeed check only if circuit breaker is not
-	 * engaged and it's not a rotary wing */
-	if (!status->circuit_breaker_engaged_airspd_check && !status->is_rotary_wing) {
-		checkAirspeed = true;
-	}
-
-	return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true);
-}

src/modules/dataman/dataman.h

@@ -39,6 +39,7 @@
 #ifndef _DATAMANAGER_H
 #define _DATAMANAGER_H
 
+#include <navigator/navigation.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -947,9 +947,14 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 
 	const float dtLastGoodGPS = static_cast<float>(hrt_absolute_time() - _previousGPSTimestamp) / 1e6f;
 
-	if (_gps.timestamp_position == 0 || (dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
+	if (!_local_pos.xy_global ||
+	    !_local_pos.v_xy_valid ||
+		_gps.timestamp_position == 0 ||
+		(dtLastGoodGPS >= POS_RESET_THRESHOLD)) {
+
 		_global_pos.eph = EPH_LARGE_VALUE;
 		_global_pos.epv = EPV_LARGE_VALUE;
+
 	} else {
 		_global_pos.eph = _gps.eph;
 		_global_pos.epv = _gps.epv;
@@ -1129,8 +1134,8 @@ int AttitudePositionEstimatorEKF::start()
 	/* start the task */
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 20,
-					 7500,
+					 SCHED_PRIORITY_MAX - 40,
+					 4800,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 

src/modules/ekf_att_pos_estimator/module.mk

@@ -42,5 +42,5 @@ SRCS		= ekf_att_pos_estimator_main.cpp \
 		  estimator_22states.cpp \
 		  estimator_utilities.cpp
 
-EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=6100
+EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=3400
 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -42,6 +42,9 @@
  */
 
 #include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h> 
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -168,7 +171,6 @@ private:
 		float p_rmax_pos;
 		float p_rmax_neg;
 		float p_integrator_max;
-		float p_roll_feedforward;
 		float r_p;
 		float r_i;
 		float r_ff;
@@ -178,7 +180,6 @@ private:
 		float y_i;
 		float y_d;
 		float y_ff;
-		float y_roll_feedforward;
 		float y_integrator_max;
 		float y_coordinated_min_speed;
 		int32_t y_coordinated_method;
@@ -211,7 +212,6 @@ private:
 		param_t p_rmax_pos;
 		param_t p_rmax_neg;
 		param_t p_integrator_max;
-		param_t p_roll_feedforward;
 		param_t r_p;
 		param_t r_i;
 		param_t r_ff;
@@ -221,7 +221,6 @@ private:
 		param_t y_i;
 		param_t y_d;
 		param_t y_ff;
-		param_t y_roll_feedforward;
 		param_t y_integrator_max;
 		param_t y_coordinated_min_speed;
 		param_t y_coordinated_method;
@@ -374,7 +373,6 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
 	_parameter_handles.p_integrator_max = param_find("FW_PR_IMAX");
-	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
 	_parameter_handles.r_p = param_find("FW_RR_P");
 	_parameter_handles.r_i = param_find("FW_RR_I");
@@ -426,7 +424,7 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_control_task);
+				px4_task_delete(_control_task);
 				break;
 			}
 		} while (_control_task != -1);
@@ -450,7 +448,6 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.p_rmax_pos, &(_parameters.p_rmax_pos));
 	param_get(_parameter_handles.p_rmax_neg, &(_parameters.p_rmax_neg));
 	param_get(_parameter_handles.p_integrator_max, &(_parameters.p_integrator_max));
-	param_get(_parameter_handles.p_roll_feedforward, &(_parameters.p_roll_feedforward));
 
 	param_get(_parameter_handles.r_p, &(_parameters.r_p));
 	param_get(_parameter_handles.r_i, &(_parameters.r_i));
@@ -493,7 +490,6 @@ FixedwingAttitudeControl::parameters_update()
 	_pitch_ctrl.set_integrator_max(_parameters.p_integrator_max);
 	_pitch_ctrl.set_max_rate_pos(math::radians(_parameters.p_rmax_pos));
 	_pitch_ctrl.set_max_rate_neg(math::radians(_parameters.p_rmax_neg));
-	_pitch_ctrl.set_roll_ff(_parameters.p_roll_feedforward);
 
 	/* roll control parameters */
 	_roll_ctrl.set_time_constant(_parameters.tconst);
@@ -671,8 +667,9 @@ FixedwingAttitudeControl::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -807,7 +804,7 @@ FixedwingAttitudeControl::task_main()
 			float flaps_control = 0.0f;
 
 			/* map flaps by default to manual if valid */
-			if (isfinite(_manual.flaps)) {
+			if (PX4_ISFINITE(_manual.flaps)) {
 				flaps_control = _manual.flaps;
 			}
 
@@ -817,7 +814,7 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */
-				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
+				if (bool nonfinite = !PX4_ISFINITE(_airspeed.true_airspeed_m_s) ||
 				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 					if (nonfinite) {
@@ -997,7 +994,7 @@ FixedwingAttitudeControl::task_main()
 				control_input.lock_integrator = lock_integrator;
 
 				/* Run attitude controllers */
-				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+				if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
 					_roll_ctrl.control_attitude(control_input);
 					_pitch_ctrl.control_attitude(control_input);
 					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
@@ -1009,8 +1006,8 @@ FixedwingAttitudeControl::task_main()
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(control_input);
-					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
-					if (!isfinite(roll_u)) {
+					_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
+					if (!PX4_ISFINITE(roll_u)) {
 						_roll_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 
@@ -1020,8 +1017,8 @@ FixedwingAttitudeControl::task_main()
 					}
 
 					float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
-					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
-					if (!isfinite(pitch_u)) {
+					_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
+					if (!PX4_ISFINITE(pitch_u)) {
 						_pitch_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
@@ -1041,11 +1038,11 @@ FixedwingAttitudeControl::task_main()
 					}
 
 					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
-					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+					_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 
 					/* add in manual rudder control */
 					_actuators.control[2] += yaw_manual;
-					if (!isfinite(yaw_u)) {
+					if (!PX4_ISFINITE(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
@@ -1055,11 +1052,11 @@ FixedwingAttitudeControl::task_main()
 
 					/* throttle passed through if it is finite and if no engine failure was
 					 * detected */
-					_actuators.control[3] = (isfinite(throttle_sp) &&
+					_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
 							!(_vehicle_status.engine_failure ||
 								_vehicle_status.engine_failure_cmd)) ?
 						throttle_sp : 0.0f;
-					if (!isfinite(throttle_sp)) {
+					if (!PX4_ISFINITE(throttle_sp)) {
 						if (_debug && loop_counter % 10 == 0) {
 							warnx("throttle_sp %.4f", (double)throttle_sp);
 						}
@@ -1139,7 +1136,6 @@ FixedwingAttitudeControl::task_main()
 
 	_control_task = -1;
 	_task_running = false;
-	_exit(0);
 }
 
 int
@@ -1152,7 +1148,7 @@ FixedwingAttitudeControl::start()
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1600,
-				       (main_t)&FixedwingAttitudeControl::task_main_trampoline,
+				       (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
 				       nullptr);
 
 	if (_control_task < 0) {
@@ -1166,23 +1162,29 @@ FixedwingAttitudeControl::start()
 int fw_att_control_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: fw_att_control {start|stop|status}");
+		warnx("usage: fw_att_control {start|stop|status}");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (att_control::g_control != nullptr)
-			errx(1, "already running");
+		if (att_control::g_control != nullptr) {
+			warnx("already running");
+			return 1;
+		}
 
 		att_control::g_control = new FixedwingAttitudeControl;
 
-		if (att_control::g_control == nullptr)
-			errx(1, "alloc failed");
+		if (att_control::g_control == nullptr) {
+			warnx("alloc failed");
+			return 1;
+		}
 
 		if (OK != att_control::g_control->start()) {
 			delete att_control::g_control;
 			att_control::g_control = nullptr;
-			err(1, "start failed");
+			warn("start failed");
+			return 1;
 		}
 
 		/* check if the waiting is necessary at all */
@@ -1196,24 +1198,28 @@ int fw_att_control_main(int argc, char *argv[])
 			}
 			printf("\n");
 		}
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (att_control::g_control == nullptr)
-			errx(1, "not running");
+		if (att_control::g_control == nullptr){
+			warnx("not running");
+			return 1;
+		}
 
 		delete att_control::g_control;
 		att_control::g_control = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (att_control::g_control) {
-			errx(0, "running");
+			warnx("running");
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}
 

src/modules/fw_att_control/fw_att_control_params.c

@@ -128,17 +128,6 @@ PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 60.0f);
  */
 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.4f);
 
-/**
- * Roll to Pitch feedforward gain.
- *
- * This compensates during turns and ensures the nose stays level.
- *
- * @min 0.0
- * @max 2.0
- * @group FW Attitude Control
- */
-PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f);
-
 /**
  * Roll rate proportional Gain
  *

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -52,6 +52,9 @@
  */
 
 #include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -604,7 +607,7 @@ FixedwingPositionControl::~FixedwingPositionControl()
 
 			/* if we have given up, kill it */
 			if (++i > 50) {
-				task_delete(_control_task);
+				px4_task_delete(_control_task);
 				break;
 			}
 		} while (_control_task != -1);
@@ -953,7 +956,7 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa
 
 float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
-	if (!isfinite(global_pos.terrain_alt)) {
+	if (!PX4_ISFINITE(global_pos.terrain_alt)) {
 		return land_setpoint_alt;
 	}
 
@@ -1024,7 +1027,11 @@ bool FixedwingPositionControl::update_desired_altitude(float dt)
 		_althold_epv = _global_pos.epv;
 		_was_in_deadband = true;
 	}
-
+	if (_vehicle_status.is_vtol) {
+		if (_vehicle_status.is_rotary_wing || _vehicle_status.in_transition_mode) {
+			_hold_alt = _global_pos.alt;
+		}
+	}
 	return climbout_mode;
 }
 
@@ -1728,8 +1735,9 @@ FixedwingPositionControl::task_main()
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1787,11 +1795,11 @@ FixedwingPositionControl::task_main()
 				_att_sp.timestamp = hrt_absolute_time();
 
 				/* lazily publish the setpoint only once available */
-				if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing) {
+				if (_attitude_sp_pub != nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
 					/* publish the attitude setpoint */
 					orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &_att_sp);
 
-				} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing) {
+				} else if (_attitude_sp_pub == nullptr && !_vehicle_status.is_rotary_wing && !_vehicle_status.in_transition_mode) {
 					/* advertise and publish */
 					_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
 				}
@@ -1821,7 +1829,6 @@ FixedwingPositionControl::task_main()
 	warnx("exiting.\n");
 
 	_control_task = -1;
-	_exit(0);
 }
 
 void FixedwingPositionControl::reset_takeoff_state()
@@ -1920,22 +1927,25 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 				break;
 		}
 
-		t.altitudeSp			= s.hgt_dem;
-		t.altitude_filtered		= s.hgt;
-		t.airspeedSp			= s.spd_dem;
-		t.airspeed_filtered		= s.spd;
+		t.altitudeSp 		= s.altitude_sp;
+		t.altitude_filtered = s.altitude_filtered;
+		t.airspeedSp 		= s.airspeed_sp;
+		t.airspeed_filtered = s.airspeed_filtered;
 
-		t.flightPathAngleSp		= s.dhgt_dem;
-		t.flightPathAngle		= s.dhgt;
-		t.flightPathAngleFiltered	= s.dhgt;
+		t.flightPathAngleSp 		= s.altitude_rate_sp;
+		t.flightPathAngle 			= s.altitude_rate;
+		t.flightPathAngleFiltered 	= s.altitude_rate;
 
-		t.airspeedDerivativeSp		= s.dspd_dem;
-		t.airspeedDerivative		= s.dspd;
+		t.airspeedDerivativeSp 	= s.airspeed_rate_sp;
+		t.airspeedDerivative 	= s.airspeed_rate;
 
-		t.totalEnergyRateSp		= s.thr;
-		t.totalEnergyRate		= s.ithr;
-		t.energyDistributionRateSp	= s.ptch;
-		t.energyDistributionRate	= s.iptch;
+		t.totalEnergyError 				= s.total_energy_error;
+		t.totalEnergyRateError 			= s.total_energy_rate_error;
+		t.energyDistributionError 		= s.energy_distribution_error;
+		t.energyDistributionRateError 	= s.energy_distribution_rate_error;
+		
+		t.throttle_integ 	= s.throttle_integ;
+		t.pitch_integ 		= s.pitch_integ;
 
 		if (_tecs_status_pub != nullptr) {
 			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
@@ -1955,7 +1965,7 @@ FixedwingPositionControl::start()
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       1600,
-				       (main_t)&FixedwingPositionControl::task_main_trampoline,
+				       (px4_main_t)&FixedwingPositionControl::task_main_trampoline,
 				       nullptr);
 
 	if (_control_task < 0) {
@@ -1969,16 +1979,20 @@ FixedwingPositionControl::start()
 int fw_pos_control_l1_main(int argc, char *argv[])
 {
 	if (argc < 2) {
-		errx(1, "usage: fw_pos_control_l1 {start|stop|status}");
+		warnx("usage: fw_pos_control_l1 {start|stop|status}");
+		return 1;
 	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (l1_control::g_control != nullptr)
-			errx(1, "already running");
+		if (l1_control::g_control != nullptr) {
+			warnx("already running");
+			return 1;
+		}
 
 		if (OK != FixedwingPositionControl::start()) {
-			err(1, "start failed");
+			warn("start failed");
+			return 1;
 		}
 
 		/* avoid memory fragmentation by not exiting start handler until the task has fully started */
@@ -1989,24 +2003,28 @@ int fw_pos_control_l1_main(int argc, char *argv[])
 		}
 		printf("\n");
 
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "stop")) {
-		if (l1_control::g_control == nullptr)
-			errx(1, "not running");
+		if (l1_control::g_control == nullptr){
+			warnx("not running");
+			return 1;
+		}
 
 		delete l1_control::g_control;
 		l1_control::g_control = nullptr;
-		exit(0);
+		return 0;
 	}
 
 	if (!strcmp(argv[1], "status")) {
 		if (l1_control::g_control) {
-			errx(0, "running");
+			warnx("running");
+			return 0;
 
 		} else {
-			errx(1, "not running");
+			warnx("not running");
+			return 1;
 		}
 	}
 

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -226,17 +226,17 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
-	/* Write part of the status message */
-	_status.flightPathAngleSp = flightPathAngleSp;
-	_status.flightPathAngle = flightPathAngle;
-	_status.flightPathAngleFiltered = flightPathAngleFiltered;
-	_status.airspeedDerivativeSp = airspeedDerivativeSp;
-	_status.airspeedDerivative = airspeedDerivative;
-	_status.totalEnergyRateSp = totalEnergyRateSp;
-	_status.totalEnergyRate = totalEnergyRate;
-	_status.energyDistributionRateSp = energyDistributionRateSp;
-	_status.energyDistributionRate = energyDistributionRate;
-	_status.mode = mode;
+	// /* Write part of the status message */
+	// _status.flightPathAngleSp = flightPathAngleSp;
+	// _status.flightPathAngle = flightPathAngle;
+	// _status.flightPathAngleFiltered = flightPathAngleFiltered;
+	// _status.airspeedDerivativeSp = airspeedDerivativeSp;
+	// _status.airspeedDerivative = airspeedDerivative;
+	// _status.totalEnergyRateSp = totalEnergyRateSp;
+	// _status.totalEnergyRate = totalEnergyRate;
+	// _status.energyDistributionRateSp = energyDistributionRateSp;
+	// _status.energyDistributionRate = energyDistributionRate;
+	// _status.mode = mode;
 
 	/** update control blocks **/
 	/* update total energy rate control block */

src/modules/mavlink/mavlink_main.cpp

@@ -1411,7 +1411,7 @@ Mavlink::task_main(int argc, char *argv[])
 	_datarate = 0;
 	_mode = MAVLINK_MODE_NORMAL;
 
-#ifndef __PX4_POSIX
+#ifdef __PX4_NUTTX
 	/* the NuttX optarg handler does not
 	 * ignore argv[0] like the POSIX handler
 	 * does, nor does it deal with non-flag
@@ -1484,6 +1484,8 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_ONBOARD;
 			} else if (strcmp(optarg, "osd") == 0) {
 				_mode = MAVLINK_MODE_OSD;
+			} else if (strcmp(optarg, "config") == 0) {
+				_mode = MAVLINK_MODE_CONFIG;
 			}
 
 			break;
@@ -1600,8 +1602,6 @@ Mavlink::task_main(int argc, char *argv[])
 	/* start the MAVLink receiver */
 	_receive_thread = MavlinkReceiver::receive_start(this);
 
-	_task_running = true;
-
 	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
 	uint64_t param_time = 0;
 	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
@@ -1693,6 +1693,27 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("VTOL_STATE", 0.5f);
 		break;
 
+	case MAVLINK_MODE_CONFIG:
+		// Enable a number of interesting streams we want via USB
+		configure_stream("PARAM_VALUE", 300.0f);
+		configure_stream("MISSION_ITEM", 50.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("VFR_HUD", 20.0f);
+		configure_stream("ATTITUDE", 100.0f);
+		configure_stream("ACTUATOR_CONTROL_TARGET0", 30.0f);
+		configure_stream("RC_CHANNELS_RAW", 5.0f);
+		configure_stream("SERVO_OUTPUT_RAW_0", 20.0f);
+		configure_stream("SERVO_OUTPUT_RAW_1", 20.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("MANUAL_CONTROL", 5.0f);
+		configure_stream("HIGHRES_IMU", 100.0f);
+		configure_stream("GPS_RAW_INT", 20.0f);
+		configure_stream("CAMERA_TRIGGER", 500.0f);
+		configure_stream("VTOL_STATE", 2.0f);
+
 	default:
 		break;
 	}
@@ -1877,6 +1898,9 @@ Mavlink::task_main(int argc, char *argv[])
 		}
 
 		perf_end(_loop_perf);
+
+		/* confirm task running only once fully initialized */
+		_task_running = true;
 	}
 
 	delete _subscribe_to_stream;

src/modules/mavlink/mavlink_main.h

@@ -147,7 +147,8 @@ public:
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
 		MAVLINK_MODE_ONBOARD,
-		MAVLINK_MODE_OSD
+		MAVLINK_MODE_OSD,
+		MAVLINK_MODE_CONFIG
 	};
 
 	void			set_mode(enum MAVLINK_MODE);

src/modules/mavlink/mavlink_mission.cpp

@@ -49,6 +49,7 @@
 #include <drivers/drv_hrt.h>
 
 #include <dataman/dataman.h>
+#include <navigator/navigation.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 

src/modules/mavlink/mavlink_parameters.cpp

@@ -159,9 +159,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
 				size_t i = map_rc.parameter_rc_channel_index;
 				_rc_param_map.param_index[i] = map_rc.param_index;
-				strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+				strncpy(&(_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
 				/* enforce null termination */
-				_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
+				_rc_param_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1) + rc_parameter_map_s::PARAM_ID_LEN] = '\0';
 				_rc_param_map.scale[i] = map_rc.scale;
 				_rc_param_map.value0[i] = map_rc.param_value0;
 				_rc_param_map.value_min[i] = map_rc.param_value_min;

src/modules/mavlink/mavlink_receiver.cpp

@@ -1050,7 +1050,7 @@ MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
 	rc.rc_lost_frame_count = 0;
 	rc.rc_total_frame_count = 1;
 	rc.rc_ppm_frame_length = 0;
-	rc.input_source = RC_INPUT_SOURCE_MAVLINK;
+	rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
 	rc.rssi = RC_INPUT_RSSI_MAX;
 
 	/* channels */
@@ -1094,7 +1094,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		rc.rc_lost_frame_count = 0;
 		rc.rc_total_frame_count = 1;
 		rc.rc_ppm_frame_length = 0;
-		rc.input_source = RC_INPUT_SOURCE_MAVLINK;
+		rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
 		rc.rssi = RC_INPUT_RSSI_MAX;
 
 		/* roll */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -993,9 +993,12 @@ MulticopterPositionControl::task_main()
 			reset_yaw_sp = true;
 		}
 
-		// XXX Temporary: for vtol use we need to reset the yaw setpoint when we are doing a transition
-		if (_vehicle_status.in_transition_mode) {
-			reset_yaw_sp = true;
+		/* reset yaw and altitude setpoint for VTOL which are in fw mode */
+		if (_vehicle_status.is_vtol) {
+			if (!_vehicle_status.is_rotary_wing) {
+				reset_yaw_sp = true;
+				_reset_alt_sp = true;
+			}
 		}
 
 		//Update previous arming state

src/modules/navigator/datalinkloss.cpp

@@ -45,6 +45,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
+#include <navigator/navigation.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>

src/modules/navigator/enginefailure.cpp

@@ -44,6 +44,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
+#include <navigator/navigation.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>

src/modules/navigator/gpsfailure.cpp

@@ -45,6 +45,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
+#include <navigator/navigation.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>

src/modules/navigator/mission.cpp

@@ -55,6 +55,7 @@
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <lib/mathlib/mathlib.h>
+#include <navigator/navigation.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -624,26 +625,29 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 {
 	/* select onboard/offboard mission */
 	int *mission_index_ptr;
-	struct mission_s *mission;
 	dm_item_t dm_item;
-	int mission_index_next;
+
+	struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
+	int mission_index_next = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
+
+	/* do not work on empty missions */
+	if (mission->count == 0) {
+		return false;
+	}
+
+	/* move to next item if there is one */
+	if (mission_index_next < ((int)mission->count - 1)) {
+		mission_index_next++;
+	}
 
 	if (onboard) {
 		/* onboard mission */
-		mission_index_next = _current_onboard_mission_index + 1;
 		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
-
-		mission = &_onboard_mission;
-
 		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
 
 	} else {
 		/* offboard mission */
-		mission_index_next = _current_offboard_mission_index + 1;
 		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
-
-		mission = &_offboard_mission;
-
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
@@ -653,7 +657,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
 			/* mission item index out of bounds */
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
+			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "[wpm] err: index: %d, max: %d", *mission_index_ptr, (int)mission->count);
 			return false;
 		}
 
@@ -665,7 +669,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		/* read mission item from datamanager */
 		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			mavlink_log_critical(_navigator->get_mavlink_fd(),
+			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(),
 			                     "ERROR waypoint could not be read");
 			return false;
 		}

src/modules/navigator/mission_block.h

@@ -43,6 +43,8 @@
 
 #include <drivers/drv_hrt.h>
 
+#include <navigator/navigation.h>
+
 #include <uORB/topics/mission.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>

src/modules/navigator/navigation.h

@@ -32,19 +32,18 @@
  ****************************************************************************/
 
 /**
- * @file mission.h
+ * @file navigation.h
  * Definition of a mission consisting of mission items.
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#ifndef TOPIC_MISSION_H_
-#define TOPIC_MISSION_H_
+#ifndef NAVIGATION_H_
+#define NAVIGATION_H_
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
 
 #define NUM_MISSIONS_SUPPORTED 256
 
@@ -103,15 +102,7 @@ struct mission_item_s {
 	int actuator_value;             /**< new value for selected actuator in ms 900...2000         */
 };
 
-/**
- * This topic used to notify navigator about mission changes, mission itself and new mission state
- * must be stored in dataman before publication.
- */
-struct mission_s {
-	int dataman_id;			/**< default 0, there are two offboard storage places in the dataman: 0 or 1 */
-	unsigned count;			/**< count of the missions stored in the dataman */
-	int current_seq;				/**< default -1, start at the one changed latest */
-};
+#include <uORB/topics/mission.h>
 
 /**
  * @}

src/modules/navigator/navigator.h

@@ -45,6 +45,7 @@
 
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
+#include <navigator/navigation.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>

src/modules/navigator/navigator_main.cpp

@@ -524,7 +524,7 @@ Navigator::start()
 	/* start the task */
 	_navigator_task = px4_task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX -5,
+					 SCHED_PRIORITY_DEFAULT - 5,
 					 1500,
 					 (px4_main_t)&Navigator::task_main_trampoline,
 					 nullptr);

src/modules/navigator/rcloss.cpp

@@ -47,7 +47,7 @@
 #include <geo/geo.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/mission.h>
+#include <navigator/navigation.h>
 #include <uORB/topics/home_position.h>
 
 #include "navigator.h"

src/modules/navigator/rtl.cpp

@@ -47,7 +47,7 @@
 #include <geo/geo.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/mission.h>
+#include <navigator/navigation.h>
 #include <uORB/topics/home_position.h>
 
 #include "navigator.h"

src/modules/navigator/rtl.h

@@ -44,8 +44,7 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission.h>
+#include <navigator/navigation.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -116,10 +116,10 @@ static inline int max(int val1, int val2)
 static void usage(const char *reason)
 {
 	if (reason) {
-		fprintf(stderr, "%s\n", reason);
+		PX4_INFO("%s\n", reason);
 	}
 
-	fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n");
+	PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n");
 	return;
 }
 
@@ -404,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						if (isfinite(sensor.baro_alt_meter)) {
+						if (PX4_ISFINITE(sensor.baro_alt_meter)) {
 							baro_offset += sensor.baro_alt_meter;
 							baro_init_cnt++;
 						}

src/modules/sdlog2/sdlog2.c

@@ -106,6 +106,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/git_version.h>
+#include <systemlib/printload.h>
 #include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
@@ -200,7 +201,8 @@ static unsigned long log_msgs_written = 0;
 static unsigned long log_msgs_skipped = 0;
 
 /* GPS time, used for log files naming */
-static uint64_t gps_time = 0;
+static uint64_t gps_time_sec = 0;
+static bool has_gps_3d_fix = false;
 
 /* current state of logging */
 static bool logging_enabled = false;
@@ -412,13 +414,13 @@ bool get_log_time_utc_tt(struct tm *tt, bool boot_time) {
 	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
 	time_t utc_time_sec;
 
-	if (_gpstime_only) {
-		utc_time_sec = gps_time / 1e6;
+	if (_gpstime_only && has_gps_3d_fix) {
+		utc_time_sec = gps_time_sec;
 	} else {
 		utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
 	}
-	if (utc_time_sec > PX4_EPOCH_SECS) {
 
+	if (utc_time_sec > PX4_EPOCH_SECS) {
 		/* strip the time elapsed since boot */
 		if (boot_time) {
 			utc_time_sec -= hrt_absolute_time() / 1e6;
@@ -737,9 +739,16 @@ void sdlog2_start_log()
 	}
 
 	/* write all performance counters */
+	hrt_abstime curr_time = hrt_absolute_time();
+	struct print_load_s load;
 	int perf_fd = open_perf_file("preflight");
+	init_print_load_s(curr_time, &load);
+	print_load(curr_time, perf_fd, &load);
 	dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
 	perf_print_all(perf_fd);
+	dprintf(perf_fd, "\nLOAD PRE-FLIGHT\n\n");
+	usleep(500 * 1000);
+	print_load(hrt_absolute_time(), perf_fd, &load);
 	close(perf_fd);
 
 	/* reset performance counters to get in-flight min and max values in post flight log */
@@ -775,8 +784,15 @@ void sdlog2_stop_log()
 
 	/* write all performance counters */
 	int perf_fd = open_perf_file("postflight");
+	hrt_abstime curr_time = hrt_absolute_time();
 	dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
 	perf_print_all(perf_fd);
+	struct print_load_s load;
+	dprintf(perf_fd, "\nLOAD POST-FLIGHT\n\n");
+	init_print_load_s(curr_time, &load);
+	print_load(curr_time, perf_fd, &load);
+	sleep(1);
+	print_load(hrt_absolute_time(), perf_fd, &load);
 	close(perf_fd);
 
 	/* free log writer performance counter */
@@ -989,7 +1005,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		sdlog2_usage(NULL);
 	}
 
-	gps_time = 0;
+	gps_time_sec = 0;
 
 	/* interpret logging params */
 
@@ -1289,7 +1305,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* check GPS topic to get GPS time */
 		if (log_name_timestamp) {
 			if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) {
-				gps_time = buf_gps_pos.time_utc_usec / 1e6;
+				gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
 			}
 		}
 
@@ -1320,7 +1336,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 		bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), &subs.gps_pos_sub, &buf_gps_pos);
 
 		if (gps_pos_updated && log_name_timestamp) {
-			gps_time = buf_gps_pos.time_utc_usec / 1e6;
+			gps_time_sec = buf_gps_pos.time_utc_usec / 1e6;
+			has_gps_3d_fix = buf_gps_pos.fix_type == 3;
 		}
 
 		if (!logging_enabled) {
@@ -1900,15 +1917,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
-			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
 			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
-			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
-			log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate;
-			log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp;
-			log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate;
+			log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
+			log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
+			log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
+			log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
+			log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ;
+			log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ;
 			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
 			LOGBUFFER_WRITE_AND_COUNT(TECS);
 		}
@@ -1976,7 +1994,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 void sdlog2_status()
 {
 	warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
-	warnx("time: gps: %u seconds", (unsigned)gps_time);
+	warnx("time: gps: %u seconds", (unsigned)gps_time_sec);
 	if (!logging_enabled) {
 		warnx("not logging");
 	} else {

src/modules/sdlog2/sdlog2_messages.h

@@ -363,16 +363,16 @@ struct log_TECS_s {
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
-	float flightPathAngleFiltered;
 	float airspeedSp;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
-
-	float totalEnergyRateSp;
-	float totalEnergyRate;
-	float energyDistributionRateSp;
-	float energyDistributionRate;
+	float totalEnergyError;
+	float totalEnergyRateError;
+	float energyDistributionError;
+	float energyDistributionRateError;
+	float pitch_integ;
+	float throttle_integ;
 
 	uint8_t mode;
 };
@@ -545,7 +545,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 	LOG_FORMAT(ENCD, "qfqf",	"cnt0,vel0,cnt1,vel1"),
 	LOG_FORMAT(TSYN, "Q", 		"TimeOffset"),

src/modules/sensors/sensor_params.c

@@ -1887,7 +1887,7 @@ PARAM_DEFINE_INT32(RC_TH_USER, 1);
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
+PARAM_DEFINE_INT32(RC_MAP_ROLL, 0);
 
 /**
  * Pitch control channel mapping.
@@ -1900,7 +1900,7 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
+PARAM_DEFINE_INT32(RC_MAP_PITCH, 0);
 
 /**
  * Failsafe channel mapping.
@@ -1927,7 +1927,7 @@ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0);  //Default to throttle function
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
+PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 0);
 
 /**
  * Yaw control channel mapping.
@@ -1940,7 +1940,7 @@ PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3);
  * @max 18
  * @group Radio Calibration
  */
-PARAM_DEFINE_INT32(RC_MAP_YAW, 4);
+PARAM_DEFINE_INT32(RC_MAP_YAW, 0);
 
 /**
  * Mode switch channel mapping.

src/modules/sensors/sensors.cpp

@@ -165,7 +165,7 @@ public:
 	int		start();
 
 private:
-	static const unsigned _rc_max_chan_count = RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
+	static const unsigned _rc_max_chan_count = input_rc_s::RC_INPUT_MAX_CHANNELS;	/**< maximum number of r/c channels we handle */
 
 	/**
 	 * Get and limit value for specified RC function. Returns NAN if not mapped.
@@ -239,7 +239,6 @@ private:
 	int32_t	_mag2_prio;			/**< mag2 sensor priority */
 	int32_t	_baro_prio;			/**< baro0 sensor priority */
 	int32_t	_baro1_prio;			/**< baro1 sensor priority */
-	int32_t	_diff_pres_prio;		/**< baro1 sensor priority */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -249,7 +248,7 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 	struct rc_parameter_map_s _rc_parameter_map;
-	float _param_rc_values[RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
+	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
 
 	math::Matrix<3, 3>	_board_rotation;	/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_mag_rotation[3];		/**< rotation matrix for the orientation that the external mag0 is mounted */
@@ -294,7 +293,7 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;
 
-		int rc_map_param[RC_PARAM_MAP_NCHAN];
+		int rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
 
 		int32_t rc_fails_thr;
 		float rc_assist_th;
@@ -350,8 +349,8 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;
 
-		param_t rc_map_param[RC_PARAM_MAP_NCHAN];
-		param_t rc_param[RC_PARAM_MAP_NCHAN];	/**< param handles for the paramters which are bound
+		param_t rc_map_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];
+		param_t rc_param[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];	/**< param handles for the paramters which are bound
 							  to a RC channel, equivalent float values in the
 							  _parameters struct are not existing
 							  because these parameters are never read. */
@@ -603,9 +602,9 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
 
 	/* RC to parameter mapping for changing parameters with RC */
-	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
-		char name[PARAM_ID_LEN];
-		snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
+	for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
+		char name[rc_parameter_map_s::PARAM_ID_LEN];
+		snprintf(name, rc_parameter_map_s::PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1
 		_parameter_handles.rc_map_param[i] = param_find(name);
 	}
 
@@ -783,7 +782,7 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
 
-	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+	for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 		param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i]));
 	}
 
@@ -831,7 +830,7 @@ Sensors::parameters_update()
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
 	_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
 
-	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+	for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 		_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
 
@@ -1544,6 +1543,12 @@ Sensors::parameter_update_poll(bool forced)
 		/* run through all mag sensors */
 		for (unsigned s = 0; s < 3; s++) {
 
+			/* set a valid default rotation (same as board).
+			 * if the mag is configured, this might be replaced
+			 * in the section below.
+			 */
+			_mag_rotation[s] = _board_rotation;
+
 			res = ERROR;
 			(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
 
@@ -1554,12 +1559,6 @@ Sensors::parameter_update_poll(bool forced)
 				continue;
 			}
 
-			/* set a valid default rotation (same as board).
-			 * if the mag is configured, this might be replaced
-			 * in the section below.
-			 */
-			_mag_rotation[s] = _board_rotation;
-
 			bool config_ok = false;
 
 			/* run through all stored calibrations */ 
@@ -1696,7 +1695,7 @@ Sensors::rc_parameter_map_poll(bool forced)
 		orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
 
 		/* update paramter handles to which the RC channels are mapped */
-		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 			if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 				/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 				 * or no request to map this channel to a param has been sent via mavlink
@@ -1709,17 +1708,17 @@ Sensors::rc_parameter_map_poll(bool forced)
 				_parameter_handles.rc_param[i] = param_for_used_index((unsigned)_rc_parameter_map.param_index[i]);
 
 			} else {
-				_parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]);
+				_parameter_handles.rc_param[i] = param_find(&_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)]);
 			}
 
 		}
 
 		warnx("rc to parameter map updated");
 
-		for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+		for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 			warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f",
 			      i,
-			      _rc_parameter_map.param_id[i],
+			      &_rc_parameter_map.param_id[i * (rc_parameter_map_s::PARAM_ID_LEN + 1)],
 			      (double)_rc_parameter_map.scale[i],
 			      (double)_rc_parameter_map.value0[i],
 			      (double)_rc_parameter_map.value_min[i],
@@ -1911,7 +1910,7 @@ Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 void
 Sensors::set_params_from_rc()
 {
-	for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
+	for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
 		if (_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
 			/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
 			 * or no request to map this channel to a param has been sent via mavlink
@@ -2142,7 +2141,7 @@ Sensors::task_main()
 		warnx("Sensor initialization failed");
 		_sensors_task = -1;
 		if (_fd_adc >=0) {
-			close(_fd_adc);
+			px4_close(_fd_adc);
 			_fd_adc = -1;
 		}
 		return;

src/modules/systemlib/circuit_breaker_params.c

@@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
 /**
  * Circuit breaker for IO safety
  *
- * Setting this parameter to 894281 will disable IO safety.
+ * Setting this parameter to 22027 will disable IO safety.
  * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
  *
  * @min 0

src/modules/systemlib/module.mk

@@ -55,11 +55,14 @@ SRCS		 = \
 
 ifeq ($(PX4_TARGET_OS),nuttx)
 SRCS		+= err.c \
+		   printload.c \
 		   up_cxxinitialize.c 
+else
+SRCS		+= print_load_posix.c
 endif
 
 ifneq ($(PX4_TARGET_OS),qurt)
-SRCS 		+= hx_stream.c 
+SRCS 		+= hx_stream.c
 endif
 
 MAXOPTIMIZATION	 = -Os

src/modules/systemlib/perf_counter.c

@@ -377,7 +377,7 @@ perf_reset(perf_counter_t handle)
 void
 perf_print_counter(perf_counter_t handle)
 {
-	perf_print_counter_fd(0, handle);
+	perf_print_counter_fd(1, handle);
 }
 
 void

src/modules/systemlib/print_load_posix.c

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 print_load_posix.c
+ *
+ * Print the current system load.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include <string.h>
+#include <stdio.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/printload.h>
+#include <drivers/drv_hrt.h>
+
+extern struct system_load_s system_load;
+
+#define CL "\033[K" // clear line
+
+void init_print_load_s(uint64_t t, struct print_load_s *s)
+{
+
+	s->total_user_time = 0;
+
+	s->running_count = 0;
+	s->blocked_count = 0;
+
+	s->new_time = t;
+	s->interval_start_time = t;
+
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		s->last_times[i] = 0;
+	}
+
+	s->interval_time_ms_inv = 0.f;
+}
+
+void print_load(uint64_t t, int fd, struct print_load_s *print_state)
+{
+}
+

src/modules/systemlib/printload.c

@@ -0,0 +1,278 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 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 printload.c
+ *
+ * Print the current system load.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ */
+
+#include <string.h>
+#include <stdio.h>
+
+#include <systemlib/cpuload.h>
+#include <systemlib/printload.h>
+#include <drivers/drv_hrt.h>
+
+extern struct system_load_s system_load;
+
+#define CL "\033[K" // clear line
+
+void init_print_load_s(uint64_t t, struct print_load_s *s)
+{
+
+	s->total_user_time = 0;
+
+	s->running_count = 0;
+	s->blocked_count = 0;
+
+	s->new_time = t;
+	s->interval_start_time = t;
+
+	for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
+		s->last_times[i] = 0;
+	}
+
+	s->interval_time_ms_inv = 0.f;
+}
+
+static const char *
+tstate_name(const tstate_t s)
+{
+	switch (s) {
+	case TSTATE_TASK_INVALID:
+		return "init";
+
+	case TSTATE_TASK_PENDING:
+		return "PEND";
+
+	case TSTATE_TASK_READYTORUN:
+		return "READY";
+
+	case TSTATE_TASK_RUNNING:
+		return "RUN";
+
+	case TSTATE_TASK_INACTIVE:
+		return "inact";
+
+	case TSTATE_WAIT_SEM:
+		return "w:sem";
+#ifndef CONFIG_DISABLE_SIGNALS
+
+	case TSTATE_WAIT_SIG:
+		return "w:sig";
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+
+	case TSTATE_WAIT_MQNOTEMPTY:
+		return "w:mqe";
+
+	case TSTATE_WAIT_MQNOTFULL:
+		return "w:mqf";
+#endif
+#ifdef CONFIG_PAGING
+
+	case TSTATE_WAIT_PAGEFILL:
+		return "w:pgf";
+#endif
+
+	default:
+		return "ERROR";
+	}
+}
+
+void print_load(uint64_t t, int fd, struct print_load_s *print_state)
+{
+	print_state->new_time = t;
+
+	int   i;
+	uint64_t curr_time_us;
+	uint64_t idle_time_us;
+	char *clear_line = "";
+
+	/* print system information */
+	if (fd == 1) {
+		dprintf(fd, "\033[H"); /* move cursor home and clear screen */
+		clear_line = CL;
+	}
+
+	curr_time_us = t;
+	idle_time_us = system_load.tasks[0].total_runtime;
+
+	if (print_state->new_time > print_state->interval_start_time) {
+		print_state->interval_time_ms_inv = 1.f / ((float)((print_state->new_time - print_state->interval_start_time) / 1000));
+	}
+
+	print_state->running_count = 0;
+	print_state->blocked_count = 0;
+	print_state->total_user_time = 0;
+
+	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+		uint64_t interval_runtime;
+
+		if (system_load.tasks[i].valid) {
+			switch (system_load.tasks[i].tcb->task_state) {
+			case TSTATE_TASK_PENDING:
+			case TSTATE_TASK_READYTORUN:
+			case TSTATE_TASK_RUNNING:
+				print_state->running_count++;
+				break;
+
+			case TSTATE_TASK_INVALID:
+			case TSTATE_TASK_INACTIVE:
+			case TSTATE_WAIT_SEM:
+#ifndef CONFIG_DISABLE_SIGNALS
+			case TSTATE_WAIT_SIG:
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+			case TSTATE_WAIT_MQNOTEMPTY:
+			case TSTATE_WAIT_MQNOTFULL:
+#endif
+#ifdef CONFIG_PAGING
+			case TSTATE_WAIT_PAGEFILL:
+#endif
+				print_state->blocked_count++;
+				break;
+			}
+		}
+
+		interval_runtime = (system_load.tasks[i].valid && print_state->last_times[i] > 0 &&
+				    system_load.tasks[i].total_runtime > print_state->last_times[i])
+				   ? (system_load.tasks[i].total_runtime - print_state->last_times[i]) / 1000
+				   : 0;
+
+		print_state->last_times[i] = system_load.tasks[i].total_runtime;
+
+		if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
+			print_state->curr_loads[i] = interval_runtime * print_state->interval_time_ms_inv;
+
+			if (i > 0) {
+				print_state->total_user_time += interval_runtime;
+			}
+
+		} else {
+			print_state->curr_loads[i] = 0;
+		}
+	}
+
+	for (i = 0; i < CONFIG_MAX_TASKS; i++) {
+		if (system_load.tasks[i].valid && (print_state->new_time > print_state->interval_start_time)) {
+			if (system_load.tasks[i].tcb->pid == 0) {
+				float idle;
+				float task_load;
+				float sched_load;
+
+				idle = print_state->curr_loads[0];
+				task_load = (float)(print_state->total_user_time) * print_state->interval_time_ms_inv;
+
+				/* this can happen if one tasks total runtime was not computed
+				   correctly by the scheduler instrumentation TODO */
+				if (task_load > (1.f - idle)) {
+					task_load = (1.f - idle);
+				}
+
+				sched_load = 1.f - idle - task_load;
+
+				dprintf(fd, "%sProcesses: %d total, %d running, %d sleeping\n",
+					clear_line,
+					system_load.total_count,
+					print_state->running_count,
+					print_state->blocked_count);
+				dprintf(fd, "%sCPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
+					clear_line,
+					(double)(task_load * 100.f),
+					(double)(sched_load * 100.f),
+					(double)(idle * 100.f));
+				dprintf(fd, "%sUptime: %.3fs total, %.3fs idle\n%s\n",
+					clear_line,
+					(double)curr_time_us / 1000000.d,
+					(double)idle_time_us / 1000000.d,
+					clear_line);
+
+				/* header for task list */
+				dprintf(fd, "%s%4s %*-s %8s %6s %11s %10s %-6s\n",
+					clear_line,
+					"PID",
+					CONFIG_TASK_NAME_SIZE, "COMMAND",
+					"CPU(ms)",
+					"CPU(%)",
+					"USED/STACK",
+					"PRIO(BASE)",
+#if CONFIG_RR_INTERVAL > 0
+					"TSLICE"
+#else
+					"STATE"
+#endif
+				       );
+			}
+
+			unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+					      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+			unsigned stack_free = 0;
+			uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+
+			while (stack_free < stack_size) {
+				if (*stack_sweeper++ != 0xff) {
+					break;
+				}
+
+				stack_free++;
+			}
+
+			dprintf(fd, "%s%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
+				clear_line,
+				system_load.tasks[i].tcb->pid,
+				CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
+				(system_load.tasks[i].total_runtime / 1000),
+				(int)(print_state->curr_loads[i] * 100.0f),
+				(int)((print_state->curr_loads[i] * 100.0f - (int)(print_state->curr_loads[i] * 100.0f)) * 1000),
+				stack_size - stack_free,
+				stack_size,
+				system_load.tasks[i].tcb->sched_priority,
+				system_load.tasks[i].tcb->base_priority);
+
+#if CONFIG_RR_INTERVAL > 0
+			/* print scheduling info with RR time slice */
+			dprintf(fd, " %6d\n", system_load.tasks[i].tcb->timeslice);
+#else
+			// print task state instead
+			dprintf(fd, " %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
+#endif
+		}
+	}
+
+	print_state->interval_start_time = print_state->new_time;
+}
+

src/modules/systemlib/printload.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2015 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
@@ -20,7 +20,7 @@
  * "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 ,
+ * 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
@@ -32,45 +32,39 @@
  ****************************************************************************/
 
 /**
- * @file rc_parameter_map.h
- * Maps RC channels to parameters
+ * @file printload.h
  *
- * @author Thomas Gubler <thomasgubler@gmail.com>
+ * Print the current system load.
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
-#ifndef TOPIC_RC_PARAMETER_MAP_H
-#define TOPIC_RC_PARAMETER_MAP_H
+#pragma once
 
+__BEGIN_DECLS
+
+#include <px4_config.h>
 #include <stdint.h>
-#include "../uORB.h"
 
-#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h
-#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN
+#ifndef CONFIG_MAX_TASKS
+#define CONFIG_MAX_TASKS 64
+#endif
 
-/**
- * @addtogroup topics
- * @{
- */
+struct print_load_s {
+	uint64_t total_user_time;
 
-struct rc_parameter_map_s {
-	uint64_t timestamp;			/**< time at which the map was updated */
+	int running_count;
+	int blocked_count;
 
-	bool valid[RC_PARAM_MAP_NCHAN];		/**< true for RC-Param channels which are mapped to a param */
-
-	int param_index[RC_PARAM_MAP_NCHAN];	/**< corresponding param index, this
-						  this field is ignored if set to -1, in this case param_id will
-						  be used*/
-	char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1];	/**< corresponding param id, null terminated */
-	float scale[RC_PARAM_MAP_NCHAN];	/** scale to map the RC input [-1, 1] to a parameter value */
-	float value0[RC_PARAM_MAP_NCHAN];	/** inital value around which the parameter value is changed */
-	float value_min[RC_PARAM_MAP_NCHAN];	/** minimal parameter value */
-	float value_max[RC_PARAM_MAP_NCHAN];	/** minimal parameter value */
+	uint64_t new_time;
+	uint64_t interval_start_time;
+	uint64_t last_times[CONFIG_MAX_TASKS];
+	float curr_loads[CONFIG_MAX_TASKS];
+	float interval_time_ms_inv;
 };
 
-/**
- * @}
- */
+__EXPORT void init_print_load_s(uint64_t t, struct print_load_s *s);
 
-ORB_DECLARE(rc_parameter_map);
+__EXPORT void print_load(uint64_t t, int fd, struct print_load_s *print_state);
 
-#endif
+__END_DECLS