Merge branch 'master' into offboard2

.gitignore

@@ -35,6 +35,7 @@ mavlink/include/mavlink/v0.9/
 /Documentation/doxygen*objdb*tmp
 .tags
 .tags_sorted_by_file
+.pydevproject
 *.creator
 *.includes
 *.files

.ycm_extra_conf.py

@@ -0,0 +1,173 @@
+# This file is NOT licensed under the GPLv3, which is the license for the rest
+# of YouCompleteMe.
+#
+# Here's the license text for this file:
+#
+# This is free and unencumbered software released into the public domain.
+#
+# Anyone is free to copy, modify, publish, use, compile, sell, or
+# distribute this software, either in source code form or as a compiled
+# binary, for any purpose, commercial or non-commercial, and by any
+# means.
+#
+# In jurisdictions that recognize copyright laws, the author or authors
+# of this software dedicate any and all copyright interest in the
+# software to the public domain. We make this dedication for the benefit
+# of the public at large and to the detriment of our heirs and
+# successors. We intend this dedication to be an overt act of
+# relinquishment in perpetuity of all present and future rights to this
+# software under copyright law.
+#
+# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
+# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+# OTHER DEALINGS IN THE SOFTWARE.
+#
+# For more information, please refer to <http://unlicense.org/>
+
+import os
+import ycm_core
+
+# These are the compilation flags that will be used in case there's no
+# compilation database set (by default, one is not set).
+# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
+flags = [
+'-Wall',
+'-Wextra',
+'-Werror',
+#'-Wc++98-compat',
+'-Wno-long-long',
+'-Wno-variadic-macros',
+'-fexceptions',
+'-DNDEBUG',
+# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM
+# source code needs it.
+#'-DUSE_CLANG_COMPLETER',
+# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
+# language to use when compiling headers. So it will guess. Badly. So C++
+# headers will be compiled as C headers. You don't want that so ALWAYS specify
+# a "-std=<something>".
+# For a C project, you would set this to something like 'c99' instead of
+# 'c++11'.
+'-std=c++11',
+# ...and the same thing goes for the magic -x option which specifies the
+# language that the files to be compiled are written in. This is mostly
+# relevant for c++ headers.
+# For a C project, you would set this to 'c' instead of 'c++'.
+'-x',
+'c++',
+'-undef', # get rid of standard definitions to allow us to include arm math header
+'-I', os.path.join(os.path.expanduser("~"),'gcc-arm-none-eabi-4_7-2013q3/arm-none-eabi/include'),
+'-I', 'Build/px4io-v1_default.build/nuttx-export/include/',
+'-I', 'Build/px4io-v2_default.build/nuttx-export/include/',
+'-I', './NuttX/nuttx/arch/arm/include',
+'-include', './src/include/visibility.h',
+'-I', './src',
+'-I', './src/modules',
+'-I', './src/include',
+'-I', './src/lib',
+'-I', './NuttX',
+]
+
+
+# Set this to the absolute path to the folder (NOT the file!) containing the
+# compile_commands.json file to use that instead of 'flags'. See here for
+# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
+#
+# Most projects will NOT need to set this to anything; you can just change the
+# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
+compilation_database_folder = ''
+
+if os.path.exists( compilation_database_folder ):
+  database = ycm_core.CompilationDatabase( compilation_database_folder )
+else:
+  database = None
+
+SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
+
+def DirectoryOfThisScript():
+  return os.path.dirname( os.path.abspath( __file__ ) )
+
+
+def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
+  if not working_directory:
+    return list( flags )
+  new_flags = []
+  make_next_absolute = False
+  path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
+  for flag in flags:
+    new_flag = flag
+
+    if make_next_absolute:
+      make_next_absolute = False
+      if not flag.startswith( '/' ):
+        new_flag = os.path.join( working_directory, flag )
+
+    for path_flag in path_flags:
+      if flag == path_flag:
+        make_next_absolute = True
+        break
+
+      if flag.startswith( path_flag ):
+        path = flag[ len( path_flag ): ]
+        new_flag = path_flag + os.path.join( working_directory, path )
+        break
+
+    if new_flag:
+      new_flags.append( new_flag )
+  return new_flags
+
+
+def IsHeaderFile( filename ):
+  extension = os.path.splitext( filename )[ 1 ]
+  return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
+
+
+def GetCompilationInfoForFile( filename ):
+  # The compilation_commands.json file generated by CMake does not have entries
+  # for header files. So we do our best by asking the db for flags for a
+  # corresponding source file, if any. If one exists, the flags for that file
+  # should be good enough.
+  if IsHeaderFile( filename ):
+    basename = os.path.splitext( filename )[ 0 ]
+    for extension in SOURCE_EXTENSIONS:
+      replacement_file = basename + extension
+      if os.path.exists( replacement_file ):
+        compilation_info = database.GetCompilationInfoForFile(
+          replacement_file )
+        if compilation_info.compiler_flags_:
+          return compilation_info
+    return None
+  return database.GetCompilationInfoForFile( filename )
+
+
+def FlagsForFile( filename, **kwargs ):
+  if database:
+    # Bear in mind that compilation_info.compiler_flags_ does NOT return a
+    # python list, but a "list-like" StringVec object
+    compilation_info = GetCompilationInfoForFile( filename )
+    if not compilation_info:
+      return None
+
+    final_flags = MakeRelativePathsInFlagsAbsolute(
+      compilation_info.compiler_flags_,
+      compilation_info.compiler_working_dir_ )
+
+    # NOTE: This is just for YouCompleteMe; it's highly likely that your project
+    # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR
+    # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT.
+    #try:
+    #  final_flags.remove( '-stdlib=libc++' )
+    #except ValueError:
+    #  pass
+  else:
+    relative_to = DirectoryOfThisScript()
+    final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
+
+  return {
+    'flags': final_flags,
+    'do_cache': True
+  }

ROMFS/px4fmu_common/init.d/10016_3dr_iris

@@ -10,11 +10,11 @@ sh /etc/init.d/rc.mc_defaults
 if [ $DO_AUTOCONFIG == yes ]
 then
 	# TODO tune roll/pitch separately
-	param set MC_ROLL_P 9.0
+	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.13
 	param set MC_ROLLRATE_I 0.0
 	param set MC_ROLLRATE_D 0.004
-	param set MC_PITCH_P 9.0
+	param set MC_PITCH_P 7.0
 	param set MC_PITCHRATE_P 0.13
 	param set MC_PITCHRATE_I 0.0
 	param set MC_PITCHRATE_D 0.004

ROMFS/px4fmu_common/init.d/11001_hexa_cox

@@ -2,7 +2,7 @@
 #
 # UNTESTED UNTESTED!
 #
-# Generic 10” Hexa coaxial geometry
+# Generic 10" Hexa coaxial geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
 #

ROMFS/px4fmu_common/init.d/12001_octo_cox

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Octo coaxial geometry
+# Generic 10" Octo coaxial geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
 #

ROMFS/px4fmu_common/init.d/2103_skyhunter_1800

@@ -0,0 +1,5 @@
+#!nsh
+
+sh /etc/init.d/rc.fw_defaults
+
+set MIXER FMU_AET

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -2,7 +2,38 @@
 #
 # Phantom FPV Flying Wing
 #
-# Simon Wilks <sjwilks@gmail.com>
+# Simon Wilks <sjwilks@gmail.com>, Thomas Gubler <thomasgubler@gmail.com>
 #
 
+sh /etc/init.d/rc.fw_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set FW_AIRSPD_MIN 13
+	param set FW_AIRSPD_TRIM 18
+	param set FW_AIRSPD_MAX 40
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.75
+	param set FW_L1_PERIOD 15
+	param set FW_PR_FF 0.2
+	param set FW_PR_I 0.005
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.03
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 0
+	param set FW_RR_FF 0.5
+	param set FW_RR_I 0.02
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.08
+	param set FW_R_LIM 70
+	param set FW_R_RMAX 0
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
+fi
+
 set MIXER FMU_Q

ROMFS/px4fmu_common/init.d/3033_wingwing

@@ -7,4 +7,29 @@
 
 sh /etc/init.d/rc.fw_defaults
 
+if [ $DO_AUTOCONFIG == yes ]
+then
+	param set BAT_N_CELLS 2
+	param set FW_AIRSPD_MAX 15
+	param set FW_AIRSPD_MIN 7
+	param set FW_AIRSPD_TRIM 11
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 12
+	param set FW_LND_ANG 15
+	param set FW_LND_FLALT 5
+	param set FW_LND_HHDIST 15
+	param set FW_LND_HVIRT 13
+	param set FW_LND_TLALT 5
+	param set FW_THR_LND_MAX 0
+	param set FW_P_ROLLFF 2
+	param set FW_PR_FF 0.6
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.06
+	param set FW_RR_FF 0.6
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.09
+	param set FW_THR_CRUISE 0.65
+fi
+
 set MIXER FMU_Q

ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha

@@ -0,0 +1,42 @@
+#!nsh
+#
+# TBS Caipirinha Flying Wing
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+#
+
+sh /etc/init.d/rc.fw_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	
+	# TODO: these are the X5 default parameters, update them to the caipi
+
+	param set FW_AIRSPD_MIN 15
+	param set FW_AIRSPD_TRIM 20
+	param set FW_AIRSPD_MAX 40
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 15
+	param set FW_PR_FF 0.3
+	param set FW_PR_I 0
+	param set FW_PR_IMAX 0.2
+	param set FW_PR_P 0.03
+	param set FW_P_LIM_MAX 45
+	param set FW_P_LIM_MIN -45
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 0
+	param set FW_RR_FF 0.3
+	param set FW_RR_I 0
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.03
+	param set FW_R_LIM 60
+	param set FW_R_RMAX 0
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
+fi
+
+set MIXER FMU_Q

ROMFS/px4fmu_common/init.d/4001_quad_x

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Quad X geometry
+# Generic 10" Quad X geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
 #

ROMFS/px4fmu_common/init.d/4008_ardrone

@@ -0,0 +1,37 @@
+#!nsh
+#
+# ARDrone
+#
+
+echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
+
+# Just use the default multicopter settings.
+sh /etc/init.d/rc.mc_defaults
+
+#
+# Load default params for this platform
+#
+if [ $DO_AUTOCONFIG == yes ]
+then
+    # Set all params here, then disable autoconfig
+    param set MC_ROLL_P 5.0
+    param set MC_ROLLRATE_P 0.13
+    param set MC_ROLLRATE_I 0.0
+    param set MC_ROLLRATE_D 0.0
+    param set MC_PITCH_P 5.0
+    param set MC_PITCHRATE_P 0.13
+    param set MC_PITCHRATE_I 0.0
+    param set MC_PITCHRATE_D 0.0
+    param set MC_YAW_P 1.0
+    param set MC_YAWRATE_P 0.15
+    param set MC_YAWRATE_I 0.0
+    param set MC_YAWRATE_D 0.0
+    param set MC_YAW_FF 0.15
+    param set BAT_V_SCALING 0.00838095238
+fi
+
+set OUTPUT_MODE ardrone
+set USE_IO no
+set MIXER skip
+# set MAV_TYPE because no specific mixer is set
+set MAV_TYPE 2

ROMFS/px4fmu_common/init.d/5001_quad_+

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Quad + geometry
+# Generic 10" Quad + geometry
 #
 # Anton Babushkin <anton.babushkin@me.com>
 #

ROMFS/px4fmu_common/init.d/6001_hexa_x

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Hexa X geometry
+# Generic 10" Hexa X geometry
 #
 # Anton Babushkin <anton.babushkin@me.com>
 #

ROMFS/px4fmu_common/init.d/7001_hexa_+

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Hexa + geometry
+# Generic 10" Hexa + geometry
 #
 # Anton Babushkin <anton.babushkin@me.com>
 #

ROMFS/px4fmu_common/init.d/8001_octo_x

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Octo X geometry
+# Generic 10" Octo X geometry
 #
 # Anton Babushkin <anton.babushkin@me.com>
 #

ROMFS/px4fmu_common/init.d/9001_octo_+

@@ -1,6 +1,6 @@
 #!nsh
 #
-# Generic 10” Octo + geometry
+# Generic 10" Octo + geometry
 #
 # Anton Babushkin <anton.babushkin@me.com>
 #

ROMFS/px4fmu_common/init.d/rc.autostart

@@ -68,6 +68,12 @@ then
 	set MODE custom
 fi
 
+if param compare SYS_AUTOSTART 2103 103
+then
+        sh /etc/init.d/2103_skyhunter_1800
+        set MODE custom
+fi
+
 #
 # Flying wing
 #
@@ -97,6 +103,11 @@ then
 	sh /etc/init.d/3034_fx79
 fi
 
+if param compare SYS_AUTOSTART 3100
+then
+	sh /etc/init.d/3100_tbs_caipirinha
+fi
+
 #
 # Quad X
 #
@@ -106,6 +117,15 @@ then
 	sh /etc/init.d/4001_quad_x
 fi
 
+#
+# ARDrone
+#
+
+if param compare SYS_AUTOSTART 4008 8
+then
+    sh /etc/init.d/4008_ardrone
+fi
+
 if param compare SYS_AUTOSTART 4010 10
 then
 	sh /etc/init.d/4010_dji_f330

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -11,4 +11,6 @@ then
 	param set NAV_RTL_ALT 100
 	param set NAV_RTL_LAND_T -1
 	param set NAV_ACCEPT_RAD 50
+	param set RC_SCALE_ROLL 1
+	param set RC_SCALE_PITCH 1
 fi

ROMFS/px4fmu_common/init.d/rc.interface

@@ -3,7 +3,7 @@
 # Script to configure control interface
 #
 
-if [ $MIXER != none ]
+if [ $MIXER != none -a $MIXER != skip ]
 then
 	#
 	# Load mixer
@@ -33,8 +33,11 @@ then
 		tone_alarm $TUNE_OUT_ERROR
 	fi
 else
-	echo "[init] Mixer not defined"
-	tone_alarm $TUNE_OUT_ERROR
+	if [ $MIXER != skip ]
+	then
+		echo "[init] Mixer not defined"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
 fi
 
 if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]

ROMFS/px4fmu_common/init.d/rc.usb

@@ -5,38 +5,12 @@
 
 echo "Starting MAVLink on this USB console"
 
-# Stop tone alarm
-tone_alarm stop
-
-#
-# Check for UORB
-#
-if uorb start
-then
-	echo "uORB started"
-fi
-
-# Tell MAVLink that this link is "fast"
-if mavlink stop
-then
-	echo "stopped other MAVLink instance"
-fi
-mavlink start -b 230400 -d /dev/ttyACM0
-
-# Stop commander
-if commander stop
-then
-	echo "Commander stopped"
-fi
-sleep 1
-
-# Start the commander
-if commander start
-then
-	echo "Commander started"
-fi
-
-echo "MAVLink started, exiting shell.."
+mavlink start -r 10000 -d /dev/ttyACM0
+# Enable a number of interesting streams we want via USB
+mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
+mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
+mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10
 
 # Exit shell to make it available to MAVLink
-exit 
+exit

ROMFS/px4fmu_common/init.d/rcS

@@ -108,7 +108,6 @@ then
 	set HIL no
 	set VEHICLE_TYPE none
 	set MIXER none
-	set USE_IO yes
 	set OUTPUT_MODE none
 	set PWM_OUTPUTS none
 	set PWM_RATE none
@@ -117,7 +116,10 @@ then
 	set PWM_MAX none
 	set MKBLCTRL_MODE none
 	set FMU_MODE pwm
+	set MAVLINK_FLAGS default
+	set EXIT_ON_END no
 	set MAV_TYPE none
+	set LOAD_DEFAULT_APPS yes
 	
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
@@ -128,6 +130,16 @@ then
 	else
 		set DO_AUTOCONFIG no
 	fi
+	
+	#
+	# Set USE_IO flag
+	#
+	if param compare SYS_USE_IO 1
+	then
+		set USE_IO yes
+	else
+		set USE_IO no
+	fi
 		
 	#
 	# Set parameters and env variables for selected AUTOSTART
@@ -240,6 +252,11 @@ then
 		fi
 	fi
 
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		set FMU_MODE gpio_serial
+	fi
+
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
@@ -277,9 +294,9 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		if [ $OUTPUT_MODE == fmu ]
+		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
-			echo "[init] Use FMU PWM as primary output"
+			echo "[init] Use FMU as primary output"
 			if fmu mode_$FMU_MODE
 			then
 				echo "[init] FMU mode_$FMU_MODE started"
@@ -294,7 +311,7 @@ then
 				then
 					set TTYS1_BUSY yes
 				fi
-				if [ $FMU_MODE == pwm_gpio ]
+				if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 				then
 					set TTYS1_BUSY yes
 				fi
@@ -351,7 +368,7 @@ then
 				fi
 			fi
 		else
-			if [ $OUTPUT_MODE != fmu ]
+			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
 			then
 				if fmu mode_$FMU_MODE
 				then
@@ -367,7 +384,7 @@ then
 					then
 						set TTYS1_BUSY yes
 					fi
-					if [ $FMU_MODE == pwm_gpio ]
+					if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 					then
 						set TTYS1_BUSY yes
 					fi
@@ -379,28 +396,34 @@ then
 	#
 	# MAVLink
 	#
-	set EXIT_ON_END no
 	
-	if [ $HIL == yes ]
+	if [ $MAVLINK_FLAGS == default ]
 	then
-		sleep 1
-		mavlink start -b 230400 -d /dev/ttyACM0 
-		usleep 5000
-	else
-		if [ $TTYS1_BUSY == yes ]
+		if [ $HIL == yes ]
 		then
-			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-			mavlink start -d /dev/ttyS0
+			sleep 1
+			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
 			usleep 5000
-			
-			# Exit from nsh to free port for mavlink
-			set EXIT_ON_END yes
 		else
-			# Start MAVLink on default port: ttyS1
-			mavlink start
-			usleep 5000
+			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
+			if [ $TTYS1_BUSY == yes ]
+			then
+				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+				usleep 5000
+				
+				# Exit from nsh to free port for mavlink
+				set EXIT_ON_END yes
+			else
+				# Start MAVLink on default port: ttyS1
+				set MAVLINK_FLAGS "-r 1000"
+				usleep 5000
+			fi
 		fi
 	fi
+
+	mavlink start $MAVLINK_FLAGS
+	usleep 5000
 	
 	#
 	# Start the datamanager
@@ -427,6 +450,14 @@ then
 		gps start
 	fi
 	
+	#
+	# Start up ARDrone Motor interface
+	#
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		ardrone_interface start -d /dev/ttyS1
+	fi
+
 	#
 	# Fixed wing setup
 	#
@@ -452,7 +483,10 @@ then
 		sh /etc/init.d/rc.interface
 		
 		# Start standard fixedwing apps
-		sh /etc/init.d/rc.fw_apps
+		if [ $LOAD_DEFAULT_APPS == yes ]
+		then
+			sh /etc/init.d/rc.fw_apps
+		fi
 	fi
 
 	#
@@ -508,7 +542,10 @@ then
 		sh /etc/init.d/rc.interface
 		
 		# Start standard multicopter apps
-		sh /etc/init.d/rc.mc_apps
+		if [ $LOAD_DEFAULT_APPS == yes ]
+		then
+			sh /etc/init.d/rc.mc_apps
+		fi
 	fi
 
 	#
@@ -531,6 +568,7 @@ then
 
 	if [ $EXIT_ON_END == yes ]
 	then
+		echo "[init] Exit from nsh"
 		exit
 	fi
 

Tools/.gitignore

@@ -0,0 +1,2 @@
+parameters.wiki
+parameters.xml

Tools/fetch_log.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Log fetcher
+#
+# Print list of logs:
+#     python fetch_log.py
+#
+# Fetch log:
+#     python fetch_log.py sess001/log001.bin
+#
+
+import serial, time, sys, os
+
+def wait_for_string(ser, s, timeout=1.0, debug=False):
+    t0 = time.time()
+    buf = []
+    res = []
+    n = 0
+    while (True):
+        c = ser.read()
+        if debug:
+            sys.stderr.write(c)
+        buf.append(c)
+        if len(buf) > len(s):
+            res.append(buf.pop(0))
+            n += 1
+            if n % 10000 == 0:
+                sys.stderr.write(str(n) + "\n")
+        if "".join(buf) == s:
+            break
+        if timeout > 0.0 and time.time() - t0 > timeout:
+            raise Exception("Timeout while waiting for: " + s)
+    return "".join(res)
+
+def exec_cmd(ser, cmd, timeout):
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout)
+    return wait_for_string(ser, "nsh> \x1b[K", timeout)
+
+def ls_dir(ser, dir, timeout=1.0):
+    res = []
+    for line in exec_cmd(ser, "ls -l " + dir, timeout).splitlines()[1:]:
+        res.append((line[20:], int(line[11:19].strip()), line[1] == "d"))
+    return res
+
+def list_logs(ser):
+    logs_dir = "/fs/microsd/log"
+    res = []
+    for d in ls_dir(ser, logs_dir):
+        if d[2]:
+            sess_dir = d[0][:-1]
+            for f in ls_dir(ser, logs_dir + "/" + sess_dir):
+                log_file = f[0]
+                log_size = f[1]
+                res.append(sess_dir + "/" + log_file + "\t" + str(log_size))
+    return "\n".join(res)
+
+def fetch_log(ser, fn, timeout):
+    cmd = "dumpfile " + fn
+    ser.write(cmd + "\n")
+    ser.flush()
+    wait_for_string(ser, cmd + "\r\n", timeout, True)
+    res = wait_for_string(ser, "\n", timeout, True)
+    data = []
+    if res.startswith("OK"):
+        size = int(res.split()[1])
+        n = 0
+        print "Reading data:"
+        while (n < size):
+            buf = ser.read(min(size - n, 8192))
+            data.append(buf)
+            n += len(buf)
+            sys.stdout.write(".")
+            sys.stdout.flush()
+        print
+    else:
+        raise Exception("Error reading log")
+    wait_for_string(ser, "nsh> \x1b[K", timeout)
+    return "".join(data)
+
+def main():
+    dev = "/dev/tty.usbmodem1"
+    ser = serial.Serial(dev, "115200", timeout=0.2)
+    if len(sys.argv) < 2:
+        print list_logs(ser)
+    else:
+        log_file = sys.argv[1]
+        data = fetch_log(ser, "/fs/microsd/log/" + log_file, 1.0)
+        try:
+            os.mkdir(log_file.split("/")[0])
+        except:
+            pass
+        fout = open(log_file, "wb")
+        fout.write(data)
+        fout.close()
+    ser.close()
+
+if __name__ == "__main__":
+    main()

Tools/fix_code_style.sh

@@ -16,4 +16,5 @@ astyle \
     --ignore-exclude-errors-x	\
     --lineend=linux		\
     --exclude=EASTL		\
+    --add-brackets \
     $*

Tools/fix_code_style_ubuntu.sh

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

Tools/px4params/.gitignore

@@ -1,4 +0,0 @@
-parameters.wiki
-parameters.xml
-parameters.wikirpc.xml
-cookies.txt

Tools/px4params/README.md

@@ -1,9 +1 @@
-h1. PX4 Parameters Processor
-
-It's designed to scan PX4 source codes, find declarations of tunable parameters,
-and generate the list in various formats.
-
-Currently supported formats are:
-
-* XML for the parametric UI generator
-* Human-readable description in DokuWiki format
+This folder contains a python library used by px_process_params.py

Tools/px4params/__init__.py

@@ -0,0 +1 @@
+__all__ = ["srcscanner", "srcparser", "xmlout", "dokuwikiout", "dokuwikirpc"]

Tools/px4params/dokuwikiout.py

@@ -0,0 +1,44 @@
+from xml.sax.saxutils import escape
+import codecs
+
+class DokuWikiTablesOutput():
+    def __init__(self, groups):
+        result = ("====== Parameter Reference ======\n"
+                  "<note>**This list is auto-generated from the source code** and contains the most recent parameter documentation.</note>\n"
+                  "\n")
+        for group in groups:
+            result += "==== %s ====\n\n" % group.GetName()
+            result += "|< 100% 25% 45% 10% 10% 10% >|\n"
+            result += "^ Name ^ Description ^  Min ^  Max ^  Default ^\n"
+            result += "^ :::  ^ Comment ^^^^\n"
+            for param in group.GetParams():
+                code = param.GetFieldValue("code")
+                name = param.GetFieldValue("short_desc")
+                min_val = param.GetFieldValue("min")
+                max_val = param.GetFieldValue("max")
+                def_val = param.GetFieldValue("default")
+                long_desc = param.GetFieldValue("long_desc")
+
+                if name == code:
+                    name = ""
+                else:
+                    name = name.replace("\n", " ")
+                    name = name.replace("|", "%%|%%")
+                    name = name.replace("^", "%%^%%")
+
+                result += "| **%s** |" % code
+                result += " %s |" % name
+                result += "  %s |" % (min_val or "")
+                result += "  %s |" % (max_val or "")
+                result += "  %s |" % (def_val or "")
+                result += "\n"
+
+                if long_desc is not None:
+                    result += "| ::: | <div>%s</div> ||||\n" % long_desc
+
+            result += "\n"
+        self.output = result;
+
+    def Save(self, filename):
+        with codecs.open(filename, 'w', 'utf-8') as f:
+            f.write(self.output)

Tools/px4params/dokuwikirpc.py

@@ -0,0 +1,16 @@
+try:
+    import xmlrpclib
+except ImportError:
+    import xmlrpc.client as xmlrpclib 
+
+# See https://www.dokuwiki.org/devel:xmlrpc for a list of available functions!
+# Usage example:
+#     xmlrpc = dokuwikirpc.get_xmlrpc(url, username, password)
+#     print(xmlrpc.dokuwiki.getVersion())
+
+def get_xmlrpc(url, username, password):
+    #proto, url = url.split("://")
+    #url = proto + "://" + username + ":" + password + "@" + url + "/lib/exe/xmlrpc.php"
+    url += "/lib/exe/xmlrpc.php?u=" + username + "&p=" + password
+    
+    return xmlrpclib.ServerProxy(url)

Tools/px4params/output_dokuwiki_listings.py

@@ -1,31 +0,0 @@
-import codecs
-
-class DokuWikiListingsOutput():
-    def __init__(self, groups):
-        result = ""
-        for group in groups:
-            result += "==== %s ====\n\n" % group.GetName()
-            for param in group.GetParams():
-                code = param.GetFieldValue("code")
-                name = param.GetFieldValue("short_desc")
-                if code != name:
-                    name = "%s (%s)" % (name, code)
-                result += "=== %s ===\n\n" % name
-                long_desc = param.GetFieldValue("long_desc")
-                if long_desc is not None:
-                    result += "%s\n\n" % long_desc
-                min_val = param.GetFieldValue("min")
-                if min_val is not None:
-                    result += "* Minimal value: %s\n" % min_val
-                max_val = param.GetFieldValue("max")
-                if max_val is not None:
-                    result += "* Maximal value: %s\n" % max_val
-                def_val = param.GetFieldValue("default")
-                if def_val is not None:
-                    result += "* Default value: %s\n" % def_val
-                result += "\n"
-        self.output = result
-
-    def Save(self, filename):
-        with codecs.open(filename, 'w', 'utf-8') as f:
-            f.write(self.output)

Tools/px4params/output_dokuwiki_tables.py

@@ -1,76 +0,0 @@
-from xml.sax.saxutils import escape
-import codecs
-
-class DokuWikiTablesOutput():
-    def __init__(self, groups):
-        result = "====== Parameter Reference ======\nThis list is auto-generated every few minutes and contains the most recent parameter names and default values.\n\n"
-        for group in groups:
-            result += "==== %s ====\n\n" % group.GetName()
-            result += "|< 100% 20% 20% 10% 10% 10% 30%>|\n"
-            result += "^ Name ^ Description ^  Min ^  Max ^  Default ^ Comment ^\n"
-            for param in group.GetParams():
-                code = param.GetFieldValue("code")
-                name = param.GetFieldValue("short_desc")
-                min_val = param.GetFieldValue("min")
-                max_val = param.GetFieldValue("max")
-                def_val = param.GetFieldValue("default")
-                long_desc = param.GetFieldValue("long_desc")
-
-                name = name.replace("\n", " ")
-                result += "| %s | %s |" % (code, name)
-
-                if min_val is not None:
-                    result += "  %s |" % min_val
-                else:
-                    result += " |"
-
-                if max_val is not None:
-                    result += "  %s |" % max_val
-                else:
-                    result += " |"
-
-                if def_val is not None:
-                    result += "  %s |" % def_val
-                else:
-                    result += " |"
-
-                if long_desc is not None:
-                    long_desc = long_desc.replace("\n", " ")
-                    result += " %s |" % long_desc
-                else:
-                    result += " |"
-
-                result += "\n"
-            result += "\n"
-        self.output = result;
-
-    def Save(self, filename):
-        with codecs.open(filename, 'w', 'utf-8') as f:
-            f.write(self.output)
-
-    def SaveRpc(self, filename):
-        with codecs.open(filename, 'w', 'utf-8') as f:
-            f.write("""<?xml version='1.0'?>
-<methodCall>
-  <methodName>wiki.putPage</methodName>
-      <params>
-          <param> 
-            <value>
-              <string>:firmware:parameters</string>
-            </value>
-          </param>
-          <param> 
-            <value>
-              <string>""")
-            f.write(escape(self.output))
-            f.write("""</string>
-            </value>
-        </param>
-        <param> 
-            <value>
-                <name>sum</name>
-                <string>Updated parameters automagically from code.</string>
-            </value>
-        </param>
-    </params>
-</methodCall>""")

Tools/px4params/srcparser.py

@@ -44,6 +44,7 @@ class Parameter(object):
         "default": 6,
         "min": 5,
         "max": 4,
+        "unit": 3,
         # all others == 0 (sorted alphabetically)
     }
 
@@ -71,7 +72,7 @@ class Parameter(object):
         """
         return self.fields.get(code)
 
-class Parser(object):
+class SourceParser(object):
     """
     Parses provided data and stores all found parameters internally.
     """
@@ -86,7 +87,7 @@ class Parser(object):
     re_is_a_number = re.compile(r'^-?[0-9\.]')
     re_remove_dots = re.compile(r'\.+$')
 
-    valid_tags = set(["min", "max", "group"])
+    valid_tags = set(["group", "min", "max", "unit"])
 
     # Order of parameter groups
     priority = {

Tools/px4params/srcscanner.py

@@ -2,26 +2,21 @@ import os
 import re
 import codecs
 
-class Scanner(object):
+class SourceScanner(object):
     """
     Traverses directory tree, reads all source files, and passes their contents
     to the Parser.
     """
 
-    re_file_extension = re.compile(r'\.([^\.]+)$')
-
     def ScanDir(self, srcdir, parser):
         """
         Scans provided path and passes all found contents to the parser using
         parser.Parse method.
         """
-        extensions = set(parser.GetSupportedExtensions())
+        extensions = tuple(parser.GetSupportedExtensions())
         for dirname, dirnames, filenames in os.walk(srcdir):
             for filename in filenames:
-                m = self.re_file_extension.search(filename)
-                if m:
-                    ext = m.group(1)
-                    if ext in extensions:
+                if filename.endswith(extensions):
                         path = os.path.join(dirname, filename)
                         self.ScanFile(path, parser)
 

Tools/px4params/xmlrpc.sh

@@ -1,5 +0,0 @@
-python px_process_params.py
-
-rm cookies.txt
-curl --cookie cookies.txt --cookie-jar cookies.txt --user-agent Mozilla/4.0 --data "u=$XMLRPCUSER&p=$XMLRPCPASS" https://pixhawk.org/start?do=login
-curl -k --cookie cookies.txt -H "Content-Type: application/xml" -X POST --data-binary @parameters.wikirpc.xml "https://pixhawk.org/lib/exe/xmlrpc.php"

Tools/px_process_params.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2013-2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# PX4 paramers processor (main executable file)
+#
+# This tool scans the PX4 source code for declarations of tunable parameters
+# and outputs the list in various formats.
+#
+# Currently supported formats are:
+#   * XML for the parametric UI generator
+#   * Human-readable description in DokuWiki page format
+#
+# This tool also allows to automatically upload the human-readable version
+# to the DokuWiki installation via XML-RPC.
+#
+
+from __future__ import print_function
+import sys
+import os
+import argparse
+from px4params import srcscanner, srcparser, xmlout, dokuwikiout, dokuwikirpc
+
+def main():
+    # Parse command line arguments
+    parser = argparse.ArgumentParser(description="Process parameter documentation.")
+    parser.add_argument("-s", "--src-path",
+                        default="../src",
+                        metavar="PATH",
+                        help="path to source files to scan for parameters")
+    parser.add_argument("-x", "--xml",
+                        nargs='?',
+                        const="parameters.xml",
+                        metavar="FILENAME",
+                        help="Create XML file"
+                             " (default FILENAME: parameters.xml)")
+    parser.add_argument("-w", "--wiki",
+                        nargs='?',
+                        const="parameters.wiki",
+                        metavar="FILENAME",
+                        help="Create DokuWiki file"
+                             " (default FILENAME: parameters.wiki)")
+    parser.add_argument("-u", "--wiki-update",
+                        nargs='?',
+                        const="firmware:parameters",
+                        metavar="PAGENAME",
+                        help="Update DokuWiki page"
+                             " (default PAGENAME: firmware:parameters)")
+    parser.add_argument("--wiki-url",
+                        default="https://pixhawk.org",
+                        metavar="URL",
+                        help="DokuWiki URL"
+                             " (default: https://pixhawk.org)")
+    parser.add_argument("--wiki-user",
+                        default=os.environ.get('XMLRPCUSER', None),
+                        metavar="USERNAME",
+                        help="DokuWiki XML-RPC user name"
+                             " (default: $XMLRPCUSER environment variable)")
+    parser.add_argument("--wiki-pass",
+                        default=os.environ.get('XMLRPCPASS', None),
+                        metavar="PASSWORD",
+                        help="DokuWiki XML-RPC user password"
+                             " (default: $XMLRPCUSER environment variable)")
+    parser.add_argument("--wiki-summary",
+                        metavar="SUMMARY",
+                        default="Automagically updated parameter documentation from code.",
+                        help="DokuWiki page edit summary")
+    args = parser.parse_args()
+
+    # Check for valid command
+    if not (args.xml or args.wiki or args.wiki_update):
+        print("Error: You need to specify at least one output method!\n")
+        parser.print_usage()
+        sys.exit(1)
+
+    # Initialize source scanner and parser
+    scanner = srcscanner.SourceScanner()
+    parser = srcparser.SourceParser()
+
+    # Scan directories, and parse the files
+    print("Scanning source path " + args.src_path)
+    scanner.ScanDir(args.src_path, parser)
+    param_groups = parser.GetParamGroups()
+
+    # Output to XML file
+    if args.xml:
+        print("Creating XML file " + args.xml)
+        out = xmlout.XMLOutput(param_groups)
+        out.Save(args.xml)
+
+    # Output to DokuWiki tables
+    if args.wiki or args.wiki_update:
+        out = dokuwikiout.DokuWikiTablesOutput(param_groups)
+        if args.wiki:
+            print("Creating wiki file " + args.wiki)
+            out.Save(args.wiki)
+        if args.wiki_update:
+            if args.wiki_user and args.wiki_pass:
+                print("Updating wiki page " + args.wiki_update)
+                xmlrpc = dokuwikirpc.get_xmlrpc(args.wiki_url, args.wiki_user, args.wiki_pass)
+                xmlrpc.wiki.putPage(args.wiki_update, out.output, {'sum': args.wiki_summary})
+            else:
+                print("Error: You need to specify DokuWiki XML-RPC username and password!")
+
+    print("All done!")
+
+
+if __name__ == "__main__":
+    main()

Tools/px_romfs_pruner.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   Copyright (C) 2014 PX4 Development Team. All rights reserved.
+#   Author: Julian Oes <joes@student.ethz.ch>
+
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+
+"""
+px_romfs_pruner.py:
+Delete all comments and newlines before ROMFS is converted to an image
+"""
+
+from __future__ import print_function
+import argparse
+import os
+
+def main():
+    
+        # Parse commandline arguments
+        parser = argparse.ArgumentParser(description="ROMFS pruner.")
+        parser.add_argument('--folder', action="store", help="ROMFS scratch folder.")
+        args = parser.parse_args()
+    
+        print("Pruning ROMFS files.")
+        
+        # go through 
+        for (root, dirs, files) in os.walk(args.folder):
+                for file in files:
+                        # only prune text files
+                        if ".zip" in file or ".bin" in file:
+                                continue
+                        
+                        file_path = os.path.join(root, file)                        
+                        
+                        # read file line by line
+                        pruned_content = ""
+                        with open(file_path, "r") as f:
+                                for line in  f:
+                                        
+                                        # handle mixer files differently than startup files
+                                        if file_path.endswith(".mix"):
+                                                if line.startswith(("Z:", "M:", "R: ", "O:", "S:")):
+                                                        pruned_content += line
+                                        else:
+                                                if not line.isspace() and not line.strip().startswith("#"):
+                                                        pruned_content += line
+                        
+                        # overwrite old scratch file
+                        with open(file_path, "w") as f:
+                                f.write(pruned_content)
+                        
+    
+if __name__ == '__main__':
+        main()

Tools/px_update_wiki.sh

@@ -0,0 +1,2 @@
+# Remember to set the XMLRPCUSER and XMLRPCPASS environment variables
+python px_process_params.py --wiki-update

makefiles/config_px4fmu-v1_default.mk

@@ -56,6 +56,7 @@ MODULES		+= systemcmds/tests
 MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
 
 #
 # General system control

makefiles/config_px4fmu-v2_default.mk

@@ -27,6 +27,7 @@ MODULES		+= drivers/l3gd20
 MODULES		+= drivers/hmc5883
 MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
+MODULES		+= drivers/sf0x
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
@@ -63,6 +64,7 @@ MODULES		+= systemcmds/config
 MODULES		+= systemcmds/nshterm
 MODULES		+= systemcmds/mtd
 MODULES		+= systemcmds/hw_ver
+MODULES		+= systemcmds/dumpfile
 
 #
 # General system control
@@ -70,7 +72,7 @@ MODULES		+= systemcmds/hw_ver
 MODULES		+= modules/commander
 MODULES		+= modules/navigator
 MODULES		+= modules/mavlink
-MODULES		+= modules/mavlink_onboard
+MODULES		+= modules/gpio_led
 
 #
 # Estimation modules (EKF/ SO3 / other filters)

makefiles/firmware.mk

@@ -355,6 +355,9 @@ ROMFS_OBJ		 = $(ROMFS_CSRC:.c=.o)
 LIBS			+= $(ROMFS_OBJ)
 LINK_DEPS		+= $(ROMFS_OBJ)
 
+# Remove all comments from startup and mixer files
+ROMFS_PRUNER	 = $(PX4_BASE)/Tools/px_romfs_pruner.py
+
 # Turn the ROMFS image into an object file
 $(ROMFS_OBJ): $(ROMFS_IMG) $(GLOBAL_DEPS)
 	$(call BIN_TO_OBJ,$<,$@,romfs_img)
@@ -372,6 +375,7 @@ ifneq ($(ROMFS_EXTRA_FILES),)
 	$(Q) $(MKDIR) -p $(ROMFS_SCRATCH)/extras
 	$(Q) $(COPY) $(ROMFS_EXTRA_FILES) $(ROMFS_SCRATCH)/extras
 endif
+	$(Q) $(PYTHON) -u $(ROMFS_PRUNER) --folder $(ROMFS_SCRATCH)
 
 EXTRA_CLEANS		+= $(ROMGS_OBJ) $(ROMFS_IMG)
 

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_DESCRIPTORS=36
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
@@ -417,7 +417,7 @@ CONFIG_PREALLOC_TIMERS=50
 #
 # Stack and heap information
 #
-CONFIG_IDLETHREAD_STACKSIZE=6000
+CONFIG_IDLETHREAD_STACKSIZE=4096
 CONFIG_USERMAIN_STACKSIZE=4096
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
@@ -720,11 +720,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2048
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2048
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_UART7_RS485 is not set
-# CONFIG_UART7_RXDMA is not set
+CONFIG_UART7_RXDMA=y
 # CONFIG_UART8_RS485 is not set
 CONFIG_UART8_RXDMA=y
 CONFIG_SERIAL_DISABLE_REORDERING=y
@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=32
+CONFIG_NFILE_DESCRIPTORS=36
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4
@@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
 # CONFIG_USART3_SERIAL_CONSOLE is not set
 # CONFIG_UART4_SERIAL_CONSOLE is not set
 # CONFIG_USART6_SERIAL_CONSOLE is not set
-# CONFIG_UART7_SERIAL_CONSOLE is not set
-CONFIG_UART8_SERIAL_CONSOLE=y
+CONFIG_UART7_SERIAL_CONSOLE=y
+# CONFIG_UART8_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
 #
@@ -796,11 +796,11 @@ CONFIG_SCHED_WORKQUEUE=y
 CONFIG_SCHED_HPWORK=y
 CONFIG_SCHED_WORKPRIORITY=192
 CONFIG_SCHED_WORKPERIOD=5000
-CONFIG_SCHED_WORKSTACKSIZE=4000
+CONFIG_SCHED_WORKSTACKSIZE=2000
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKPERIOD=50000
-CONFIG_SCHED_LPWORKSTACKSIZE=4000
+CONFIG_SCHED_LPWORKSTACKSIZE=2000
 # CONFIG_LIB_KBDCODEC is not set
 # CONFIG_LIB_SLCDCODEC is not set
 

src/drivers/device/cdev.cpp

@@ -124,7 +124,7 @@ CDev::register_class_devname(const char *class_devname)
 			if (ret == OK) break;
 		} else {
 			char name[32];
-			snprintf(name, sizeof(name), "%s%u", class_devname, class_instance);
+			snprintf(name, sizeof(name), "%s%d", class_devname, class_instance);
 			ret = register_driver(name, &fops, 0666, (void *)this);
 			if (ret == OK) break;
 		}

src/drivers/drv_pwm_output.h

@@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm);
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
-#define DSMX8_BIND_PULSES 10	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
+#define DSMX8_BIND_PULSES 10 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)

src/drivers/drv_px4flow.h

@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Rangefinder driver interface.
+ */
+
+#ifndef _DRV_PX4FLOW_H
+#define _DRV_PX4FLOW_H
+
+#include <stdint.h>
+#include <sys/ioctl.h>
+
+#include "drv_sensor.h"
+#include "drv_orb_dev.h"
+
+#define PX4FLOW_DEVICE_PATH	"/dev/px4flow"
+
+/**
+ * Optical flow in NED body frame in SI units.
+ *
+ * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ */
+struct px4flow_report {
+
+	uint64_t timestamp;		/**< in microseconds since system start          */
+
+	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
+	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
+	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
+	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
+	float ground_distance_m;	/**< Altitude / distance to ground in meters */
+	uint8_t	quality;		/**< Quality of the measurement, 0: bad quality, 255: maximum quality */
+	uint8_t sensor_id;		/**< id of the sensor emitting the flow value */
+
+};
+
+/*
+ * ObjDev tag for px4flow data.
+ */
+ORB_DECLARE(optical_flow);
+
+/*
+ * ioctl() definitions
+ *
+ * px4flow drivers also implement the generic sensor driver
+ * interfaces from drv_sensor.h
+ */
+
+#define _PX4FLOWIOCBASE			(0x7700)
+#define __PX4FLOWIOC(_n)		(_IOC(_PX4FLOWIOCBASE, _n))
+
+
+#endif /* _DRV_PX4FLOW_H */

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -132,7 +132,6 @@ ETSAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
 	}
 
 	return ret;
@@ -308,7 +307,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no ETS airspeed sensor connected");
 }
 
 /**

src/drivers/gps/gps.cpp

@@ -232,6 +232,11 @@ GPS::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case SENSORIOCRESET:
 		cmd_reset();
 		break;
+
+	default:
+		/* give it to parent if no one wants it */
+		ret = CDev::ioctl(filp, cmd, arg);
+		break;
 	}
 
 	unlock();

src/drivers/mb12xx/mb12xx.cpp

@@ -37,7 +37,7 @@
  *
  * Driver for the Maxbotix sonar range finders connected via I2C.
  */
-	 
+
 #include <nuttx/config.h>
 
 #include <drivers/device/i2c.h>
@@ -84,7 +84,7 @@
 /* Device limits */
 #define MB12XX_MIN_DISTANCE (0.20f)
 #define MB12XX_MAX_DISTANCE (7.65f)
-	 
+
 #define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
 
 /* oddly, ERROR is not defined for c++ */
@@ -102,17 +102,17 @@ class MB12XX : public device::I2C
 public:
 	MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
 	virtual ~MB12XX();
-	
+
 	virtual int 		init();
-	
+
 	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
 	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
-	
+
 	/**
 	* Diagnostics - print some basic information about the driver.
 	*/
 	void				print_info();
-	
+
 protected:
 	virtual int			probe();
 
@@ -124,13 +124,13 @@ private:
 	bool				_sensor_ok;
 	int					_measure_ticks;
 	bool				_collect_phase;
-	
+
 	orb_advert_t		_range_finder_topic;
 
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
-	
+
 	/**
 	* Test whether the device supported by the driver is present at a
 	* specific address.
@@ -139,7 +139,7 @@ private:
 	* @return		True if the device is present.
 	*/
 	int					probe_address(uint8_t address);
-	
+
 	/**
 	* Initialise the automatic measurement state machine and start it.
 	*
@@ -147,12 +147,12 @@ private:
 	*       to make it more aggressive about resetting the bus in case of errors.
 	*/
 	void				start();
-	
+
 	/**
 	* Stop the automatic measurement state machine.
 	*/
 	void				stop();
-	
+
 	/**
 	* Set the min and max distance thresholds if you want the end points of the sensors
 	* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
@@ -162,7 +162,7 @@ private:
 	void				set_maximum_distance(float max);
 	float				get_minimum_distance();
 	float				get_maximum_distance();
-	
+
 	/**
 	* Perform a poll cycle; collect from the previous measurement
 	* and start a new one.
@@ -177,8 +177,8 @@ private:
 	* @param arg		Instance pointer for the driver that is polling.
 	*/
 	static void		cycle_trampoline(void *arg);
-	
-	
+
+
 };
 
 /*
@@ -200,8 +200,8 @@ MB12XX::MB12XX(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
-	
+	_debug_enabled = false;
+
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
 }
@@ -212,8 +212,9 @@ MB12XX::~MB12XX()
 	stop();
 
 	/* free any existing reports */
-	if (_reports != nullptr)
+	if (_reports != nullptr) {
 		delete _reports;
+	}
 }
 
 int
@@ -222,22 +223,25 @@ MB12XX::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(2, sizeof(range_finder_report));
 
-	if (_reports == nullptr)
+	if (_reports == nullptr) {
 		goto out;
+	}
 
 	/* get a publish handle on the range finder topic */
 	struct range_finder_report zero_report;
 	memset(&zero_report, 0, sizeof(zero_report));
 	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
 
-	if (_range_finder_topic < 0)
+	if (_range_finder_topic < 0) {
 		debug("failed to create sensor_range_finder object. Did you start uOrb?");
+	}
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
@@ -256,13 +260,13 @@ void
 MB12XX::set_minimum_distance(float min)
 {
 	_min_distance = min;
-}	
+}
 
 void
 MB12XX::set_maximum_distance(float max)
 {
 	_max_distance = max;
-}	
+}
 
 float
 MB12XX::get_minimum_distance()
@@ -284,20 +288,20 @@ MB12XX::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? */
@@ -307,13 +311,14 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					_measure_ticks = USEC2TICK(MB12XX_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);
@@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
 					unsigned ticks = USEC2TICK(1000000 / arg);
 
 					/* check against maximum rate */
-					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
+					if (ticks < USEC2TICK(MB12XX_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;
 				}
@@ -338,45 +345,49 @@ MB12XX::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))
-			return -EINVAL;
-		
-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
 			irqrestore(flags);
-			return -ENOMEM;
+
+			return OK;
 		}
-		irqrestore(flags);
-		
-		return OK;
-	}
 
 	case SENSORIOCGQUEUEDEPTH:
 		return _reports->size();
-		
+
 	case SENSORIOCRESET:
 		/* XXX implement this */
 		return -EINVAL;
-	
-	case RANGEFINDERIOCSETMINIUMDISTANCE:
-	{
-		set_minimum_distance(*(float *)arg);
-		return 0;
-	}
-	break;
-	case RANGEFINDERIOCSETMAXIUMDISTANCE:
-	{
-		set_maximum_distance(*(float *)arg);
-		return 0;
-	}
-	break;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
 	default:
 		/* give it to the superclass */
 		return I2C::ioctl(filp, cmd, arg);
@@ -391,8 +402,9 @@ MB12XX::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) {
@@ -453,14 +465,14 @@ MB12XX::measure()
 	uint8_t cmd = MB12XX_TAKE_RANGE_REG;
 	ret = transfer(&cmd, 1, nullptr, 0);
 
-	if (OK != ret)
-	{
+	if (OK != ret) {
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
 		return ret;
 	}
+
 	ret = OK;
-	
+
 	return ret;
 }
 
@@ -468,32 +480,31 @@ int
 MB12XX::collect()
 {
 	int	ret = -EIO;
-	
+
 	/* read from the sensor */
 	uint8_t val[2] = {0, 0};
-	
+
 	perf_begin(_sample_perf);
-	
+
 	ret = transfer(nullptr, 0, &val[0], 2);
-	
-	if (ret < 0)
-	{
+
+	if (ret < 0) {
 		log("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
 	}
-	
+
 	uint16_t distance = val[0] << 8 | val[1];
-	float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
+	float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
 	struct range_finder_report report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
-	
+
 	/* publish it */
 	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
 
@@ -519,17 +530,19 @@ MB12XX::start()
 
 	/* schedule a cycle to start things */
 	work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
-	
+
 	/* notify about state change */
 	struct subsystem_info_s info = {
 		true,
 		true,
 		true,
-		SUBSYSTEM_TYPE_RANGEFINDER};
+		SUBSYSTEM_TYPE_RANGEFINDER
+	};
 	static orb_advert_t pub = -1;
 
 	if (pub > 0) {
 		orb_publish(ORB_ID(subsystem_info), pub, &info);
+
 	} else {
 		pub = orb_advertise(ORB_ID(subsystem_info), &info);
 	}
@@ -583,8 +596,9 @@ MB12XX::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -635,33 +649,37 @@ start()
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver */
 	g_dev = new MB12XX(MB12XX_BUS);
 
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
-	if (OK != g_dev->init())
+	if (OK != g_dev->init()) {
 		goto fail;
+	}
 
 	/* set the poll rate to default, starts automatic data collection */
 	fd = open(RANGE_FINDER_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);
 
 fail:
 
-	if (g_dev != nullptr) 
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
 	}
@@ -674,15 +692,14 @@ fail:
  */
 void stop()
 {
-	if (g_dev != nullptr)
-	{
+	if (g_dev != nullptr) {
 		delete g_dev;
 		g_dev = nullptr;
-	}
-	else
-	{
+
+	} else {
 		errx(1, "driver not running");
 	}
+
 	exit(0);
 }
 
@@ -700,22 +717,25 @@ test()
 
 	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_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("measurement: %0.2f m", (double)report.distance);
 	warnx("time:        %lld", report.timestamp);
 
 	/* 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++) {
@@ -726,20 +746,27 @@ 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("measurement: %0.3f", (double)report.distance);
 		warnx("time:        %lld", report.timestamp);
 	}
 
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
 	errx(0, "PASS");
 }
 
@@ -751,14 +778,17 @@ reset()
 {
 	int fd = open(RANGE_FINDER_DEVICE_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);
 }
@@ -769,8 +799,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();
@@ -786,32 +817,37 @@ mb12xx_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		mb12xx::start();
-	
-	 /*
-	  * Stop the driver
-	  */
-	 if (!strcmp(argv[1], "stop"))
-		 mb12xx::stop();
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		mb12xx::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		mb12xx::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		mb12xx::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		mb12xx::info();
+	}
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }

src/drivers/md25/md25.cpp

@@ -52,7 +52,7 @@
 #include <arch/board/board.h>
 #include <mavlink/mavlink_log.h>
 
-#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 
@@ -587,7 +587,7 @@ int md25Sine(const char *deviceName, uint8_t bus, uint8_t address, float amplitu
 	float prev_revolution = md25.getRevolutions1();
 
 	// debug publication
-	control::UOrbPublication<debug_key_value_s> debug_msg(NULL,
+	uORB::Publication<debug_key_value_s> debug_msg(NULL,
 			ORB_ID(debug_key_value));
 
 	// sine wave for motor 1

src/drivers/md25/md25.hpp

@@ -46,7 +46,7 @@
 
 #include <poll.h>
 #include <stdio.h>
-#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -270,7 +270,7 @@ private:
 	struct pollfd _controlPoll;
 
 	/** actuator controls subscription */
-	control::UOrbSubscription<actuator_controls_s> _actuators;
+	uORB::Subscription<actuator_controls_s> _actuators;
 
 	// local copy of data from i2c device
 	uint8_t _version;

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -79,6 +79,8 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
 
@@ -99,12 +101,14 @@
 #define ADDR_READ_MR			0x00	/* write to this address to start conversion */
 
 /* Measurement rate is 100Hz */
+#define MEAS_RATE 100.0f
+#define MEAS_DRIVER_FILTER_FREQ 3.0f
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
 
 class MEASAirspeed : public Airspeed
 {
 public:
-	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525);
+	MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
 
 protected:
 
@@ -116,6 +120,7 @@ protected:
 	virtual int	measure();
 	virtual int	collect();
 
+	math::LowPassFilter2p	_filter;
 };
 
 /*
@@ -123,8 +128,9 @@ 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)
+MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address,
+			CONVERSION_INTERVAL, path),
+			_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ)
 {
 
 }
@@ -161,23 +167,25 @@ MEASAirspeed::collect()
 	ret = transfer(nullptr, 0, &val[0], 4);
 
 	if (ret < 0) {
-                perf_count(_comms_errors);
-                perf_end(_sample_perf);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
 		return ret;
 	}
 
-	uint8_t status = val[0] & 0xC0;
+	uint8_t status = (val[0] & 0xC0) >> 6;
 
-	if (status == 2) {
-		log("err: stale data");
-                perf_count(_comms_errors);
-                perf_end(_sample_perf);
-		return ret;
-	} else if (status == 3) {
-		log("err: fault");
-                perf_count(_comms_errors);
-                perf_end(_sample_perf);
-		return ret;                
+	switch (status) {
+	case 0:
+		break;
+
+	case 1:
+		/* fallthrough */
+	case 2:
+		/* fallthrough */
+	case 3:
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return -EAGAIN;
 	}
 
 	int16_t dp_raw = 0, dT_raw = 0;
@@ -193,21 +201,24 @@ MEASAirspeed::collect()
 	*/
 	const float P_min = -1.0f;
 	const float P_max = 1.0f;
-	float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
-        if (diff_press_pa < 0.0f)
-            diff_press_pa = 0.0f;
+	float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset;
+
+	if (diff_press_pa < 0.0f) {
+		diff_press_pa = 0.0f;
+	}
 
 	struct differential_pressure_s report;
 
 	/* track maximum differential pressure measured (so we can work out top speed). */
 	if (diff_press_pa > _max_differential_pressure_pa) {
-	    _max_differential_pressure_pa = diff_press_pa;
+		_max_differential_pressure_pa = diff_press_pa;
 	}
 
 	report.timestamp = hrt_absolute_time();
-        report.error_count = perf_event_count(_comms_errors);
+	report.error_count = perf_event_count(_comms_errors);
 	report.temperature = temperature;
 	report.differential_pressure_pa = diff_press_pa;
+	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa);
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 
@@ -261,8 +272,9 @@ MEASAirspeed::cycle()
 	}
 
 	/* measurement phase */
-	if (OK != measure())
+	if (OK != measure()) {
 		log("measure error");
+	}
 
 	/* next phase is collection */
 	_collect_phase = true;
@@ -303,15 +315,17 @@ start(int i2c_bus)
 {
 	int fd;
 
-	if (g_dev != nullptr)
+	if (g_dev != nullptr) {
 		errx(1, "already started");
+	}
 
 	/* create the driver, try the MS4525DO first */
 	g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
 
 	/* check if the MS4525DO was instantiated */
-	if (g_dev == nullptr)
+	if (g_dev == nullptr) {
 		goto fail;
+	}
 
 	/* try the MS5525DSO next if init fails */
 	if (OK != g_dev->Airspeed::init()) {
@@ -319,22 +333,26 @@ start(int i2c_bus)
 		g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
 
 		/* check if the MS5525DSO was instantiated */
-		if (g_dev == nullptr)
+		if (g_dev == nullptr) {
 			goto fail;
+		}
 
 		/* both versions failed if the init for the MS5525DSO fails, give up */
-		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(AIRSPEED_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);
 
@@ -345,7 +363,7 @@ fail:
 		g_dev = nullptr;
 	}
 
-	errx(1, "driver start failed");
+	errx(1, "no MS4525 airspeed sensor connected");
 }
 
 /**
@@ -379,21 +397,24 @@ test()
 
 	int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY);
 
-	if (fd < 0)
+	if (fd < 0) {
 		err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_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: %8.4f pa", (double)report.differential_pressure_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++) {
@@ -404,14 +425,16 @@ 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: %8.4f pa", (double)report.differential_pressure_pa);
@@ -419,8 +442,9 @@ test()
 	}
 
 	/* 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");
 }
@@ -433,14 +457,17 @@ reset()
 {
 	int fd = open(AIRSPEED_DEVICE_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);
 }
@@ -451,8 +478,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();
@@ -491,32 +519,37 @@ meas_airspeed_main(int argc, char *argv[])
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start"))
+	if (!strcmp(argv[1], "start")) {
 		meas_airspeed::start(i2c_bus);
+	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop"))
+	if (!strcmp(argv[1], "stop")) {
 		meas_airspeed::stop();
+	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(argv[1], "test")) {
 		meas_airspeed::test();
+	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(argv[1], "reset")) {
 		meas_airspeed::reset();
+	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
 		meas_airspeed::info();
+	}
 
 	meas_airspeed_usage();
 	exit(0);

src/drivers/px4flow/module.mk

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -32,11 +32,9 @@
 ############################################################################
 
 #
-# MAVLink protocol to uORB interface process (XXX hack for onboard use)
+# Makefile to build the PX4FLOW driver.
 #
 
-MODULE_COMMAND	 = mavlink_onboard
-SRCS		 = mavlink.c \
-		   mavlink_receiver.c
+MODULE_COMMAND	= px4flow
 
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+SRCS		= px4flow.cpp

src/drivers/px4flow/px4flow.cpp

@@ -0,0 +1,806 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4flow.cpp
+ * @author Dominik Honegger
+ *
+ * Driver for the PX4FLOW module connected via I2C.
+ */
+	 
+#include <nuttx/config.h>
+
+#include <drivers/device/i2c.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_px4flow.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+//#include <uORB/topics/optical_flow.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+#define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
+#define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
+//range 0x42 - 0x49
+
+/* PX4FLOW Registers addresses */
+#define PX4FLOW_REG	0x00		/* Measure Register */
+
+#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+//struct i2c_frame
+//{
+//    uint16_t frame_count;
+//    int16_t pixel_flow_x_sum;
+//    int16_t pixel_flow_y_sum;
+//    int16_t flow_comp_m_x;
+//    int16_t flow_comp_m_y;
+//    int16_t qual;
+//    int16_t gyro_x_rate;
+//    int16_t gyro_y_rate;
+//    int16_t gyro_z_rate;
+//    uint8_t gyro_range;
+//    uint8_t sonar_timestamp;
+//    int16_t ground_distance;
+//};
+//
+//struct i2c_frame f;
+
+class PX4FLOW : public device::I2C
+{
+public:
+	PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS);
+	virtual ~PX4FLOW();
+	
+	virtual int 		init();
+	
+	virtual ssize_t		read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+	
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+	
+protected:
+	virtual int			probe();
+
+private:
+
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int					_measure_ticks;
+	bool				_collect_phase;
+	
+	orb_advert_t		_px4flow_topic;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_comms_errors;
+	perf_counter_t		_buffer_overflows;
+	
+	/**
+	* Test whether the device supported by the driver is present at a
+	* specific address.
+	*
+	* @param address	The I2C bus address to probe.
+	* @return		True if the device is present.
+	*/
+	int					probe_address(uint8_t address);
+	
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+	
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+	
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int					measure();
+	int					collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void		cycle_trampoline(void *arg);
+	
+	
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
+
+PX4FLOW::PX4FLOW(int bus, int address) :
+	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000),//400khz
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_px4flow_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
+{
+	// enable debug() calls
+	_debug_enabled = true;
+	
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+PX4FLOW::~PX4FLOW()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr)
+		delete _reports;
+}
+
+int
+PX4FLOW::init()
+{
+	int ret = ERROR;
+
+	/* do I2C init (and probe) first */
+	if (I2C::init() != OK)
+		goto out;
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(px4flow_report));
+
+	if (_reports == nullptr)
+		goto out;
+
+	/* get a publish handle on the px4flow topic */
+	struct px4flow_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
+
+	if (_px4flow_topic < 0)
+		debug("failed to create px4flow object. Did you start uOrb?");
+
+	ret = OK;
+	/* sensor is ok, but we don't really know if it is within range */
+	_sensor_ok = true;
+out:
+	return ret;
+}
+
+int
+PX4FLOW::probe()
+{
+	return measure();
+}
+
+int
+PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+				/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+				/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+				/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+				/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(PX4FLOW_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start)
+						start();
+
+					return OK;
+				}
+
+				/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(PX4FLOW_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)
+						start();
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		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))
+			return -EINVAL;
+		
+		irqstate_t flags = irqsave();
+		if (!_reports->resize(arg)) {
+			irqrestore(flags);
+			return -ENOMEM;
+		}
+		irqrestore(flags);
+		
+		return OK;
+	}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+		
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+	
+	default:
+		/* give it to the superclass */
+		return I2C::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+PX4FLOW::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct px4flow_report);
+	struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1)
+		return -ENOSPC;
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(PX4FLOW_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+PX4FLOW::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	uint8_t cmd = PX4FLOW_REG;
+	ret = transfer(&cmd, 1, nullptr, 0);
+
+	if (OK != ret)
+	{
+		perf_count(_comms_errors);
+		log("i2c::transfer returned %d", ret);
+		 printf("i2c::transfer flow returned %d");
+		return ret;
+	}
+	ret = OK;
+	
+	return ret;
+}
+
+int
+PX4FLOW::collect()
+{
+	int	ret = -EIO;
+	
+	/* read from the sensor */
+	uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0};
+	
+	perf_begin(_sample_perf);
+	
+	ret = transfer(nullptr, 0, &val[0], 22);
+	
+	if (ret < 0)
+	{
+		log("error reading from sensor: %d", ret);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+		return ret;
+	}
+	
+//	f.frame_count = val[1] << 8 | val[0];
+//	f.pixel_flow_x_sum= val[3] << 8 | val[2];
+//	f.pixel_flow_y_sum= val[5] << 8 | val[4];
+//	f.flow_comp_m_x= val[7] << 8 | val[6];
+//	f.flow_comp_m_y= val[9] << 8 | val[8];
+//	f.qual= val[11] << 8 | val[10];
+//	f.gyro_x_rate= val[13] << 8 | val[12];
+//	f.gyro_y_rate= val[15] << 8 | val[14];
+//	f.gyro_z_rate= val[17] << 8 | val[16];
+//	f.gyro_range= val[18];
+//	f.sonar_timestamp= val[19];
+//	f.ground_distance= val[21] << 8 | val[20];
+
+	int16_t flowcx = val[7] << 8 | val[6];
+	int16_t flowcy = val[9] << 8 | val[8];
+	int16_t gdist = val[21] << 8 | val[20];
+
+	struct px4flow_report report;
+	report.flow_comp_x_m = float(flowcx)/1000.0f;
+	report.flow_comp_y_m = float(flowcy)/1000.0f;
+	report.flow_raw_x= val[3] << 8 | val[2];
+	report.flow_raw_y= val[5] << 8 | val[4];
+	report.ground_distance_m =float(gdist)/1000.0f;
+	report.quality=  val[10];
+	report.sensor_id = 0;
+	report.timestamp = hrt_absolute_time();
+
+
+	/* publish it */
+	orb_publish(ORB_ID(optical_flow), _px4flow_topic, &report);
+
+	/* post a report to the ring */
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+PX4FLOW::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1);
+	
+	/* notify about state change */
+	struct subsystem_info_s info = {
+		true,
+		true,
+		true,
+		SUBSYSTEM_TYPE_OPTICALFLOW};
+	static orb_advert_t pub = -1;
+
+	if (pub > 0) {
+		orb_publish(ORB_ID(subsystem_info), pub, &info);
+	} else {
+		pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	}
+}
+
+void
+PX4FLOW::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+PX4FLOW::cycle_trampoline(void *arg)
+{
+	PX4FLOW *dev = (PX4FLOW *)arg;
+
+	dev->cycle();
+}
+
+void
+PX4FLOW::cycle()
+{
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		if (OK != collect()) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(PX4FLOW_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&PX4FLOW::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure())
+		log("measure error");
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&PX4FLOW::cycle_trampoline,
+		   this,
+		   USEC2TICK(PX4FLOW_CONVERSION_INTERVAL));
+}
+
+void
+PX4FLOW::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %u ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace px4flow
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+PX4FLOW	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start()
+{
+	int fd;
+
+	if (g_dev != nullptr)
+		errx(1, "already started");
+
+	/* create the driver */
+	g_dev = new PX4FLOW(PX4FLOW_BUS);
+
+	if (g_dev == nullptr)
+		goto fail;
+
+	if (OK != g_dev->init())
+		goto fail;
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		goto fail;
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX) < 0)
+		goto fail;
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) 
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr)
+	{
+		delete g_dev;
+		g_dev = nullptr;
+	}
+	else
+	{
+		errx(1, "driver not running");
+	}
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct px4flow_report report;
+	ssize_t sz;
+	int ret;
+
+	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "%s open failed (try 'px4flow start' if the driver is not running", PX4FLOW_DEVICE_PATH);
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report))
+	//	err(1, "immediate read failed");
+
+	warnx("single read");
+	warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
+	warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
+	warnx("time:        %lld", report.timestamp);
+
+
+	/* start the sensor polling at 2Hz */
+	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++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = poll(&fds, 1, 2000);
+
+		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))
+			err(1, "periodic read failed");
+
+		warnx("periodic read %u", i);
+		warnx("flowx: %0.2f m/s", (double)report.flow_comp_x_m);
+		warnx("flowy: %0.2f m/s", (double)report.flow_comp_y_m);
+		warnx("time:        %lld", report.timestamp);
+
+
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(PX4FLOW_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0)
+		err(1, "failed ");
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0)
+		err(1, "driver reset failed");
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
+		err(1, "driver poll restart failed");
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr)
+		errx(1, "driver not running");
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+px4flow_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start"))
+		px4flow::start();
+	
+	 /*
+	  * Stop the driver
+	  */
+	 if (!strcmp(argv[1], "stop"))
+		 px4flow::stop();
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test"))
+		px4flow::test();
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset"))
+		px4flow::reset();
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
+		px4flow::info();
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}

src/drivers/px4fmu/fmu.cpp

@@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[])
 	}
 
 
-	fprintf(stderr, "FMU: unrecognised command, try:\n");
+	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)

src/drivers/px4io/px4io.cpp

@@ -244,8 +244,7 @@ private:
 	volatile int		_task;			///< worker task id
 	volatile bool		_task_should_exit;	///< worker terminate flag
 
-	int			_mavlink_fd;		///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
-	int			_thread_mavlink_fd;	///< mavlink file descriptor for thread.
+	int			_mavlink_fd;		///< mavlink file descriptor.
 
 	perf_counter_t		_perf_update;		///<local performance counter for status updates
 	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
@@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
-	_thread_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
 	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
 	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
@@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
 
-	/* open MAVLink text channel */
-	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
-
 	_debug_enabled = false;
 	_servorail_status.rssi_v = 0;
 }
@@ -785,7 +780,7 @@ PX4IO::task_main()
 	hrt_abstime poll_last = 0;
 	hrt_abstime orb_check_last = 0;
 
-	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -880,6 +875,10 @@ PX4IO::task_main()
 			/* run at 5Hz */
 			orb_check_last = now;
 
+			/* try to claim the MAVLink log FD */
+			if (_mavlink_fd < 0)
+				_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+
 			/* check updates on uORB topics and handle it */
 			bool updated = false;
 
@@ -1275,16 +1274,14 @@ void
 PX4IO::dsm_bind_ioctl(int dsmMode)
 {
 	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
-		/* 0: dsm2, 1:dsmx */
-		if ((dsmMode == 0) || (dsmMode == 1)) {
-			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8"));
-			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
-		} else {
-			mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
-		}
+		mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
+		int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
+
+		if (ret)
+			mavlink_log_critical(_mavlink_fd, "binding failed.");
 
 	} else {
-		mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
+		mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
 	}
 }
 
@@ -1335,12 +1332,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
 	battery_status.discharged_mah = _battery_mamphour_total;
 	_battery_last_timestamp = battery_status.timestamp;
 
-	/* lazily publish the battery voltage */
-	if (_to_battery > 0) {
-		orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
+	/* the announced battery status would conflict with the simulated battery status in HIL */
+	if (!(_pub_blocked)) {
+		/* lazily publish the battery voltage */
+		if (_to_battery > 0) {
+			orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
 
-	} else {
-		_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		} else {
+			_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
+		}
 	}
 }
 
@@ -1924,12 +1924,14 @@ PX4IO::print_status()
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE));
 #endif
 	printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
-	printf("controls");
+	for (unsigned group = 0; group < 4; group++) {
+		printf("controls %u:", group);
 
-	for (unsigned i = 0; i < _max_controls; i++)
-		printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
+		for (unsigned i = 0; i < _max_controls; i++)
+			printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i));
 
-	printf("\n");
+		printf("\n");
+	}
 
 	for (unsigned i = 0; i < _max_rc_input; i++) {
 		unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
@@ -1960,8 +1962,7 @@ PX4IO::print_status()
 }
 
 int
-PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
-/* Make it obvious that file * isn't used here */
+PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 {
 	int ret = OK;
 
@@ -2115,14 +2116,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	case DSM_BIND_START:
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
-		usleep(500000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
-		usleep(72000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
-		usleep(50000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+
+		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
+		if (arg == DSM2_BIND_PULSES ||
+		    arg == DSMX_BIND_PULSES ||
+		    arg == DSMX8_BIND_PULSES) {
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+			usleep(500000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+			usleep(72000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+			usleep(50000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+
+			ret = OK;
+		} else {
+			ret = -EINVAL;
+		}
 		break;
 
 	case DSM_BIND_POWER_UP:
@@ -2363,8 +2374,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	default:
-		/* not a recognized value */
-		ret = -ENOTTY;
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filep, cmd, arg);
+		break;
 	}
 
 	return ret;
@@ -2615,7 +2627,7 @@ bind(int argc, char *argv[])
 #endif
 
 	if (argc < 3)
-		errx(0, "needs argument, use dsm2 or dsmx");
+		errx(0, "needs argument, use dsm2, dsmx or dsmx8");
 
 	if (!strcmp(argv[2], "dsm2"))
 		pulses = DSM2_BIND_PULSES;
@@ -2624,7 +2636,7 @@ bind(int argc, char *argv[])
 	else if (!strcmp(argv[2], "dsmx8"))
 		pulses = DSMX8_BIND_PULSES;
 	else 
-		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+		errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
 	// Test for custom pulse parameter
 	if (argc > 3)
 		pulses = atoi(argv[3]);

src/drivers/rgbled/rgbled.cpp

@@ -242,6 +242,8 @@ RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return OK;
 
 	default:
+		/* see if the parent class can make any use of it */
+		ret = CDev::ioctl(filp, cmd, arg);
 		break;
 	}
 

src/drivers/roboclaw/RoboClaw.cpp

@@ -53,7 +53,7 @@
 #include <arch/board/board.h>
 #include <mavlink/mavlink_log.h>
 
-#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/Publication.hpp>
 #include <uORB/topics/debug_key_value.h>
 #include <drivers/drv_hrt.h>
 

src/drivers/roboclaw/RoboClaw.hpp

@@ -45,7 +45,7 @@
 
 #include <poll.h>
 #include <stdio.h>
-#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <drivers/device/i2c.h>
 
@@ -169,7 +169,7 @@ private:
 	struct pollfd _controlPoll;
 
 	/** actuator controls subscription */
-	control::UOrbSubscription<actuator_controls_s> _actuators;
+	uORB::Subscription<actuator_controls_s> _actuators;
 
 	// private data
 	float _motor1Position;

src/drivers/sf0x/module.mk

@@ -1,7 +1,6 @@
-#!/usr/bin/env python
 ############################################################################
 #
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2014 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
@@ -33,35 +32,9 @@
 ############################################################################
 
 #
-# PX4 paramers processor (main executable file)
-#
-# It scans src/ subdirectory of the project, collects all parameters
-# definitions, and outputs list of parameters in XML and DokuWiki formats.
+# Makefile to build the Lightware laser range finder driver.
 #
 
-import scanner
-import srcparser
-import output_xml
-import output_dokuwiki_tables
-import output_dokuwiki_listings
+MODULE_COMMAND	= sf0x
 
-# Initialize parser
-prs = srcparser.Parser()
-
-# Scan directories, and parse the files
-sc = scanner.Scanner()
-sc.ScanDir("../../src", prs)
-groups = prs.GetParamGroups()
-
-# Output into XML
-out = output_xml.XMLOutput(groups)
-out.Save("parameters.xml")
-
-# Output to DokuWiki listings
-#out = output_dokuwiki_listings.DokuWikiListingsOutput(groups)
-#out.Save("parameters.wiki")
-
-# Output to DokuWiki tables
-out = output_dokuwiki_tables.DokuWikiTablesOutput(groups)
-out.Save("parameters.wiki")
-out.SaveRpc("parameters.wikirpc.xml")
+SRCS		= sf0x.cpp

src/drivers/sf0x/sf0x.cpp

@@ -0,0 +1,977 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 sf0x.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Greg Hulands
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <semaphore.h>
+#include <string.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+#include <termios.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_range_finder.h>
+#include <drivers/device/device.h>
+#include <drivers/device/ringbuffer.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/subsystem_info.h>
+
+#include <board_config.h>
+
+/* Configuration Constants */
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+#ifndef CONFIG_SCHED_WORKQUEUE
+# error This requires CONFIG_SCHED_WORKQUEUE.
+#endif
+
+#define SF0X_CONVERSION_INTERVAL	83334
+#define SF0X_TAKE_RANGE_REG		'd'
+#define SF02F_MIN_DISTANCE		0.0f
+#define SF02F_MAX_DISTANCE		40.0f
+#define SF0X_DEFAULT_PORT		"/dev/ttyS2"
+
+class SF0X : public device::CDev
+{
+public:
+	SF0X(const char *port = SF0X_DEFAULT_PORT);
+	virtual ~SF0X();
+
+	virtual int 			init();
+
+	virtual ssize_t			read(struct file *filp, char *buffer, size_t buflen);
+	virtual int			ioctl(struct file *filp, int cmd, unsigned long arg);
+
+	/**
+	* Diagnostics - print some basic information about the driver.
+	*/
+	void				print_info();
+
+protected:
+	virtual int			probe();
+
+private:
+	float				_min_distance;
+	float				_max_distance;
+	work_s				_work;
+	RingBuffer			*_reports;
+	bool				_sensor_ok;
+	int				_measure_ticks;
+	bool				_collect_phase;
+	int				_fd;
+	char				_linebuf[10];
+	unsigned			_linebuf_index;
+	hrt_abstime			_last_read;
+
+	orb_advert_t			_range_finder_topic;
+
+	perf_counter_t			_sample_perf;
+	perf_counter_t			_comms_errors;
+	perf_counter_t			_buffer_overflows;
+
+	/**
+	* Initialise the automatic measurement state machine and start it.
+	*
+	* @note This function is called at open and error time.  It might make sense
+	*       to make it more aggressive about resetting the bus in case of errors.
+	*/
+	void				start();
+
+	/**
+	* Stop the automatic measurement state machine.
+	*/
+	void				stop();
+
+	/**
+	* Set the min and max distance thresholds if you want the end points of the sensors
+	* range to be brought in at all, otherwise it will use the defaults SF0X_MIN_DISTANCE
+	* and SF0X_MAX_DISTANCE
+	*/
+	void				set_minimum_distance(float min);
+	void				set_maximum_distance(float max);
+	float				get_minimum_distance();
+	float				get_maximum_distance();
+
+	/**
+	* Perform a poll cycle; collect from the previous measurement
+	* and start a new one.
+	*/
+	void				cycle();
+	int				measure();
+	int				collect();
+	/**
+	* Static trampoline from the workq context; because we don't have a
+	* generic workq wrapper yet.
+	*
+	* @param arg		Instance pointer for the driver that is polling.
+	*/
+	static void			cycle_trampoline(void *arg);
+
+
+};
+
+/*
+ * Driver 'main' command.
+ */
+extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
+
+SF0X::SF0X(const char *port) :
+	CDev("SF0X", RANGE_FINDER_DEVICE_PATH),
+	_min_distance(SF02F_MIN_DISTANCE),
+	_max_distance(SF02F_MAX_DISTANCE),
+	_reports(nullptr),
+	_sensor_ok(false),
+	_measure_ticks(0),
+	_collect_phase(false),
+	_fd(-1),
+	_linebuf_index(0),
+	_last_read(0),
+	_range_finder_topic(-1),
+	_sample_perf(perf_alloc(PC_ELAPSED, "sf0x_read")),
+	_comms_errors(perf_alloc(PC_COUNT, "sf0x_comms_errors")),
+	_buffer_overflows(perf_alloc(PC_COUNT, "sf0x_buffer_overflows"))
+{
+	/* open fd */
+	_fd = ::open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
+
+	if (_fd < 0) {
+		warnx("FAIL: laser fd");
+	}
+
+	/* tell it to stop auto-triggering */
+	char stop_auto = ' ';
+	(void)::write(_fd, &stop_auto, 1);
+	usleep(100);
+	(void)::write(_fd, &stop_auto, 1);
+
+	struct termios uart_config;
+
+	int termios_state;
+
+	/* fill the struct for the new configuration */
+	tcgetattr(_fd, &uart_config);
+
+	/* clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+	/* no parity, one stop bit */
+	uart_config.c_cflag &= ~(CSTOPB | PARENB);
+
+	unsigned speed = B9600;
+
+	/* set baud rate */
+	if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d ISPD", termios_state);
+	}
+
+	if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
+		warnx("ERR CFG: %d OSPD\n", termios_state);
+	}
+
+	if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
+		warnx("ERR baud %d ATTR", termios_state);
+	}
+
+	// disable debug() calls
+	_debug_enabled = false;
+
+	// work_cancel in the dtor will explode if we don't do this...
+	memset(&_work, 0, sizeof(_work));
+}
+
+SF0X::~SF0X()
+{
+	/* make sure we are truly inactive */
+	stop();
+
+	/* free any existing reports */
+	if (_reports != nullptr) {
+		delete _reports;
+	}
+}
+
+int
+SF0X::init()
+{
+	int ret = ERROR;
+	unsigned i = 0;
+
+	/* do regular cdev init */
+	if (CDev::init() != OK) {
+		goto out;
+	}
+
+	/* allocate basic report buffers */
+	_reports = new RingBuffer(2, sizeof(range_finder_report));
+
+	if (_reports == nullptr) {
+		warnx("mem err");
+		goto out;
+	}
+
+	/* get a publish handle on the range finder topic */
+	struct range_finder_report zero_report;
+	memset(&zero_report, 0, sizeof(zero_report));
+	_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
+
+	if (_range_finder_topic < 0) {
+		warnx("advert err");
+	}
+
+	/* attempt to get a measurement 5 times */
+	while (ret != OK && i < 5) {
+
+		if (measure()) {
+			ret = ERROR;
+			_sensor_ok = false;
+		}
+
+		usleep(100000);
+
+		if (collect()) {
+			ret = ERROR;
+			_sensor_ok = false;
+
+		} else {
+			ret = OK;
+			/* sensor is ok, but we don't really know if it is within range */
+			_sensor_ok = true;
+		}
+
+		i++;
+	}
+
+	/* close the fd */
+	::close(_fd);
+	_fd = -1;
+out:
+	return ret;
+}
+
+int
+SF0X::probe()
+{
+	return measure();
+}
+
+void
+SF0X::set_minimum_distance(float min)
+{
+	_min_distance = min;
+}
+
+void
+SF0X::set_maximum_distance(float max)
+{
+	_max_distance = max;
+}
+
+float
+SF0X::get_minimum_distance()
+{
+	return _min_distance;
+}
+
+float
+SF0X::get_maximum_distance()
+{
+	return _max_distance;
+}
+
+int
+SF0X::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	switch (cmd) {
+
+	case SENSORIOCSPOLLRATE: {
+			switch (arg) {
+
+			/* switching to manual polling */
+			case SENSOR_POLLRATE_MANUAL:
+				stop();
+				_measure_ticks = 0;
+				return OK;
+
+			/* external signalling (DRDY) not supported */
+			case SENSOR_POLLRATE_EXTERNAL:
+
+			/* zero would be bad */
+			case 0:
+				return -EINVAL;
+
+			/* set default/max polling rate */
+			case SENSOR_POLLRATE_MAX:
+			case SENSOR_POLLRATE_DEFAULT: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* set interval for next measurement to minimum legal value */
+					_measure_ticks = USEC2TICK(SF0X_CONVERSION_INTERVAL);
+
+					/* if we need to start the poll state machine, do it */
+					if (want_start) {
+						start();
+					}
+
+					return OK;
+				}
+
+			/* adjust to a legal polling interval in Hz */
+			default: {
+					/* do we need to start internal polling? */
+					bool want_start = (_measure_ticks == 0);
+
+					/* convert hz to tick interval via microseconds */
+					unsigned ticks = USEC2TICK(1000000 / arg);
+
+					/* check against maximum rate */
+					if (ticks < USEC2TICK(SF0X_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) {
+						start();
+					}
+
+					return OK;
+				}
+			}
+		}
+
+	case SENSORIOCGPOLLRATE:
+		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)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}
+
+			irqrestore(flags);
+
+			return OK;
+		}
+
+	case SENSORIOCGQUEUEDEPTH:
+		return _reports->size();
+
+	case SENSORIOCRESET:
+		/* XXX implement this */
+		return -EINVAL;
+
+	case RANGEFINDERIOCSETMINIUMDISTANCE: {
+			set_minimum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
+			set_maximum_distance(*(float *)arg);
+			return 0;
+		}
+		break;
+
+	default:
+		/* give it to the superclass */
+		return CDev::ioctl(filp, cmd, arg);
+	}
+}
+
+ssize_t
+SF0X::read(struct file *filp, char *buffer, size_t buflen)
+{
+	unsigned count = buflen / sizeof(struct range_finder_report);
+	struct range_finder_report *rbuf = reinterpret_cast<struct range_finder_report *>(buffer);
+	int ret = 0;
+
+	/* buffer must be large enough */
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	/* if automatic measurement is enabled */
+	if (_measure_ticks > 0) {
+
+		/*
+		 * While there is space in the caller's buffer, and reports, copy them.
+		 * Note that we may be pre-empted by the workq thread while we are doing this;
+		 * we are careful to avoid racing with them.
+		 */
+		while (count--) {
+			if (_reports->get(rbuf)) {
+				ret += sizeof(*rbuf);
+				rbuf++;
+			}
+		}
+
+		/* if there was no data, warn the caller */
+		return ret ? ret : -EAGAIN;
+	}
+
+	/* manual measurement - run one conversion */
+	do {
+		_reports->flush();
+
+		/* trigger a measurement */
+		if (OK != measure()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* wait for it to complete */
+		usleep(SF0X_CONVERSION_INTERVAL);
+
+		/* run the collection phase */
+		if (OK != collect()) {
+			ret = -EIO;
+			break;
+		}
+
+		/* state machine will have generated a report, copy it out */
+		if (_reports->get(rbuf)) {
+			ret = sizeof(*rbuf);
+		}
+
+	} while (0);
+
+	return ret;
+}
+
+int
+SF0X::measure()
+{
+	int ret;
+
+	/*
+	 * Send the command to begin a measurement.
+	 */
+	char cmd = SF0X_TAKE_RANGE_REG;
+	ret = ::write(_fd, &cmd, 1);
+
+	if (ret != sizeof(cmd)) {
+		perf_count(_comms_errors);
+		log("write fail %d", ret);
+		return ret;
+	}
+
+	ret = OK;
+
+	return ret;
+}
+
+int
+SF0X::collect()
+{
+	int	ret;
+
+	perf_begin(_sample_perf);
+
+	/* clear buffer if last read was too long ago */
+	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
+
+	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+		_linebuf_index = 0;
+	}
+
+	/* read from the sensor (uart buffer) */
+	ret = ::read(_fd, &_linebuf[_linebuf_index], sizeof(_linebuf) - _linebuf_index);
+
+	if (ret < 0) {
+		_linebuf[sizeof(_linebuf) - 1] = '\0';
+		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
+		perf_count(_comms_errors);
+		perf_end(_sample_perf);
+
+		/* only throw an error if we time out */
+		if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
+			return ret;
+
+		} else {
+			return -EAGAIN;
+		}
+	}
+
+	_linebuf_index += ret;
+
+	if (_linebuf_index >= sizeof(_linebuf)) {
+		_linebuf_index = 0;
+	}
+
+	_last_read = hrt_absolute_time();
+
+	if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
+		/* incomplete read, reschedule ourselves */
+		return -EAGAIN;
+	}
+
+	char *end;
+	float si_units;
+	bool valid;
+
+	/* enforce line ending */
+	_linebuf[sizeof(_linebuf) - 1] = '\0';
+
+	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
+		si_units = -1.0f;
+		valid = false;
+
+	} else {
+		si_units = strtod(_linebuf, &end);
+		valid = true;
+	}
+
+	debug("val (float): %8.4f, raw: %s\n", si_units, _linebuf);
+
+	/* done with this chunk, resetting */
+	_linebuf_index = 0;
+
+	struct range_finder_report report;
+
+	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
+	report.timestamp = hrt_absolute_time();
+	report.error_count = perf_event_count(_comms_errors);
+	report.distance = si_units;
+	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);
+
+	/* publish it */
+	orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
+
+	if (_reports->force(&report)) {
+		perf_count(_buffer_overflows);
+	}
+
+	/* notify anyone waiting for data */
+	poll_notify(POLLIN);
+
+	ret = OK;
+
+	perf_end(_sample_perf);
+	return ret;
+}
+
+void
+SF0X::start()
+{
+	/* reset the report ring and state machine */
+	_collect_phase = false;
+	_reports->flush();
+
+	/* schedule a cycle to start things */
+	work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, 1);
+
+	// /* notify about state change */
+	// struct subsystem_info_s info = {
+	// 	true,
+	// 	true,
+	// 	true,
+	// 	SUBSYSTEM_TYPE_RANGEFINDER
+	// };
+	// static orb_advert_t pub = -1;
+
+	// if (pub > 0) {
+	// 	orb_publish(ORB_ID(subsystem_info), pub, &info);
+
+	// } else {
+	// 	pub = orb_advertise(ORB_ID(subsystem_info), &info);
+	// }
+}
+
+void
+SF0X::stop()
+{
+	work_cancel(HPWORK, &_work);
+}
+
+void
+SF0X::cycle_trampoline(void *arg)
+{
+	SF0X *dev = static_cast<SF0X *>(arg);
+
+	dev->cycle();
+}
+
+void
+SF0X::cycle()
+{
+	/* fds initialized? */
+	if (_fd < 0) {
+		/* open fd */
+		_fd = ::open(SF0X_DEFAULT_PORT, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	}
+
+	/* collection phase? */
+	if (_collect_phase) {
+
+		/* perform collection */
+		int collect_ret = collect();
+
+		if (collect_ret == -EAGAIN) {
+			/* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   USEC2TICK(1100));
+			return;
+		}
+
+		if (OK != collect_ret) {
+			log("collection error");
+			/* restart the measurement state machine */
+			start();
+			return;
+		}
+
+		/* next phase is measurement */
+		_collect_phase = false;
+
+		/*
+		 * Is there a collect->measure gap?
+		 */
+		if (_measure_ticks > USEC2TICK(SF0X_CONVERSION_INTERVAL)) {
+
+			/* schedule a fresh cycle call when we are ready to measure again */
+			work_queue(HPWORK,
+				   &_work,
+				   (worker_t)&SF0X::cycle_trampoline,
+				   this,
+				   _measure_ticks - USEC2TICK(SF0X_CONVERSION_INTERVAL));
+
+			return;
+		}
+	}
+
+	/* measurement phase */
+	if (OK != measure()) {
+		log("measure error");
+	}
+
+	/* next phase is collection */
+	_collect_phase = true;
+
+	/* schedule a fresh cycle call when the measurement is done */
+	work_queue(HPWORK,
+		   &_work,
+		   (worker_t)&SF0X::cycle_trampoline,
+		   this,
+		   USEC2TICK(SF0X_CONVERSION_INTERVAL));
+}
+
+void
+SF0X::print_info()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_buffer_overflows);
+	printf("poll interval:  %d ticks\n", _measure_ticks);
+	_reports->print_info("report queue");
+}
+
+/**
+ * Local functions in support of the shell command.
+ */
+namespace sf0x
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+SF0X	*g_dev;
+
+void	start();
+void	stop();
+void	test();
+void	reset();
+void	info();
+
+/**
+ * Start the driver.
+ */
+void
+start(const char *port)
+{
+	int fd;
+
+	if (g_dev != nullptr) {
+		errx(1, "already started");
+	}
+
+	/* create the driver */
+	g_dev = new SF0X(port);
+
+	if (g_dev == nullptr) {
+		goto fail;
+	}
+
+	if (OK != g_dev->init()) {
+		goto fail;
+	}
+
+	/* set the poll rate to default, starts automatic data collection */
+	fd = open(RANGE_FINDER_DEVICE_PATH, 0);
+
+	if (fd < 0) {
+		warnx("device open fail");
+		goto fail;
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		goto fail;
+	}
+
+	exit(0);
+
+fail:
+
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+	}
+
+	errx(1, "driver start failed");
+}
+
+/**
+ * Stop the driver
+ */
+void stop()
+{
+	if (g_dev != nullptr) {
+		delete g_dev;
+		g_dev = nullptr;
+
+	} else {
+		errx(1, "driver not running");
+	}
+
+	exit(0);
+}
+
+/**
+ * Perform some basic functional tests on the driver;
+ * make sure we can collect data from the sensor in polled
+ * and automatic modes.
+ */
+void
+test()
+{
+	struct range_finder_report report;
+	ssize_t sz;
+
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "%s open failed (try 'sf0x start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
+	}
+
+	/* do a simple demand read */
+	sz = read(fd, &report, sizeof(report));
+
+	if (sz != sizeof(report)) {
+		err(1, "immediate read failed");
+	}
+
+	warnx("single read");
+	warnx("measurement: %0.2f m", (double)report.distance);
+	warnx("time:        %lld", report.timestamp);
+
+	/* start the sensor polling at 2Hz */
+	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++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		int ret = poll(&fds, 1, 2000);
+
+		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)) {
+			err(1, "periodic read failed");
+		}
+
+		warnx("periodic read %u", i);
+		warnx("measurement: %0.3f", (double)report.distance);
+		warnx("time:        %lld", report.timestamp);
+	}
+
+	/* reset the sensor polling to default rate */
+	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
+		errx(1, "failed to set default poll rate");
+	}
+
+	errx(0, "PASS");
+}
+
+/**
+ * Reset the driver.
+ */
+void
+reset()
+{
+	int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
+
+	if (fd < 0) {
+		err(1, "failed ");
+	}
+
+	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
+		err(1, "driver reset failed");
+	}
+
+	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
+		err(1, "driver poll restart failed");
+	}
+
+	exit(0);
+}
+
+/**
+ * Print a little info about the driver.
+ */
+void
+info()
+{
+	if (g_dev == nullptr) {
+		errx(1, "driver not running");
+	}
+
+	printf("state @ %p\n", g_dev);
+	g_dev->print_info();
+
+	exit(0);
+}
+
+} // namespace
+
+int
+sf0x_main(int argc, char *argv[])
+{
+	/*
+	 * Start/load the driver.
+	 */
+	if (!strcmp(argv[1], "start")) {
+		if (argc > 2) {
+			sf0x::start(argv[2]);
+
+		} else {
+			sf0x::start(SF0X_DEFAULT_PORT);
+		}
+	}
+
+	/*
+	 * Stop the driver
+	 */
+	if (!strcmp(argv[1], "stop")) {
+		sf0x::stop();
+	}
+
+	/*
+	 * Test the driver/device.
+	 */
+	if (!strcmp(argv[1], "test")) {
+		sf0x::test();
+	}
+
+	/*
+	 * Reset the driver.
+	 */
+	if (!strcmp(argv[1], "reset")) {
+		sf0x::reset();
+	}
+
+	/*
+	 * Print driver information.
+	 */
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+		sf0x::info();
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+}

src/include/containers/List.hpp

@@ -32,9 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Node.h
+ * @file List.hpp
  *
- * A node of a linked list.
+ * A linked list.
  */
 
 #pragma once
@@ -43,7 +43,7 @@ template<class T>
 class __EXPORT ListNode
 {
 public:
-	ListNode() : _sibling(NULL) {
+	ListNode() : _sibling(nullptr) {
 	}
 	void setSibling(T sibling) { _sibling = sibling; }
 	T getSibling() { return _sibling; }

src/include/mavlink/mavlink_log.h

@@ -100,6 +100,7 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...);
  */
 #define mavlink_log_info(_fd, _text, ...)		mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__);
 
+
 struct mavlink_logmessage {
 	char text[MAVLINK_LOG_MAXLEN + 1];
 	unsigned char severity;
@@ -112,6 +113,7 @@ struct mavlink_logbuffer {
 	struct mavlink_logmessage *elems;
 };
 
+__BEGIN_DECLS
 void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size);
 
 void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb);
@@ -125,6 +127,7 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_
 int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem);
 
 void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...);
+__END_DECLS
 
 #endif
 

src/lib/ecl/ecl.h

@@ -38,7 +38,6 @@
  */
 
 #include <drivers/drv_hrt.h>
-#include <geo/geo.h>
 
 #define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time
+#define ecl_elapsed_time hrt_elapsed_time

src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@@ -38,6 +38,8 @@
  *
  */
 
+#include <float.h>
+
 #include "ecl_l1_pos_controller.h"
 
 float ECL_L1_Pos_Controller::nav_roll()
@@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
 	/* calculate the vector from waypoint A to current position */
 	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
-	/* store the normalized vector from waypoint A to current position */
-	math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+	math::Vector<2> vector_A_to_airplane_unit;
+
+	/* prevent NaN when normalizing */
+	if (vector_A_to_airplane.length() > FLT_EPSILON) {
+		/* store the normalized vector from waypoint A to current position */
+		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+	} else {
+		vector_A_to_airplane_unit = vector_A_to_airplane;
+	}
 
 	/* calculate eta angle towards the loiter center */
 

src/lib/external_lgpl/tecs/tecs.cpp

@@ -3,13 +3,10 @@
 #include "tecs.h"
 #include <ecl/ecl.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 using namespace math;
 
-#ifndef CONSTANTS_ONE_G
-#define CONSTANTS_ONE_G GRAVITY
-#endif
-
 /**
  * @file tecs.cpp
  *

src/lib/external_lgpl/tecs/tecs.h

@@ -28,16 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
-
-		_airspeed_enabled(false),
-		_throttle_slewrate(0.0f),
-		_climbOutDem(false),
-		_hgt_dem_prev(0.0f),
-		_hgt_dem_adj_last(0.0f),
-		_hgt_dem_in_old(0.0f),
-		_TAS_dem_last(0.0f),
-		_TAS_dem_adj(0.0f),
-		_TAS_dem(0.0f),
+		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
 		_integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
 		_integ5_state(0.0f),
 		_integ6_state(0.0f),
 		_integ7_state(0.0f),
-		_pitch_dem(0.0f),
 		_last_pitch_dem(0.0f),
+		_vel_dot(0.0f),
+		_TAS_dem(0.0f),
+		_TAS_dem_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_prev(0.0f),
+		_TAS_dem_adj(0.0f),
+		_STEdotErrLast(0.0f),
+		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
 		_SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
-		_vel_dot(0.0f),
-		_STEdotErrLast(0.0f) {
-
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f)
+	{
 	}
 
 	bool airspeed_sensor_enabled() {

src/lib/geo/geo.c

@@ -441,14 +441,14 @@ __EXPORT float _wrap_pi(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > M_PI_F) {
+	while (bearing >= M_PI_F) {
 		bearing -= M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= -M_PI_F) {
+	while (bearing < -M_PI_F) {
 		bearing += M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
@@ -465,14 +465,14 @@ __EXPORT float _wrap_2pi(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > M_TWOPI_F) {
+	while (bearing >= M_TWOPI_F) {
 		bearing -= M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= 0.0f) {
+	while (bearing < 0.0f) {
 		bearing += M_TWOPI_F;
 		if (c++ > 3)
 			return NAN;
@@ -489,14 +489,14 @@ __EXPORT float _wrap_180(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > 180.0f) {
+	while (bearing >= 180.0f) {
 		bearing -= 360.0f;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= -180.0f) {
+	while (bearing < -180.0f) {
 		bearing += 360.0f;
 		if (c++ > 3)
 			return NAN;
@@ -513,14 +513,14 @@ __EXPORT float _wrap_360(float bearing)
 	}
 
 	int c = 0;
-	while (bearing > 360.0f) {
+	while (bearing >= 360.0f) {
 		bearing -= 360.0f;
 		if (c++ > 3)
 			return NAN;
 	}
 
 	c = 0;
-	while (bearing <= 0.0f) {
+	while (bearing < 0.0f) {
 		bearing += 360.0f;
 		if (c++ > 3)
 			return NAN;

src/lib/launchdetection/CatapultLaunchMethod.cpp

@@ -41,12 +41,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
-CatapultLaunchMethod::CatapultLaunchMethod() :
-	last_timestamp(0),
+namespace launchdetection
+{
+
+CatapultLaunchMethod::CatapultLaunchMethod(SuperBlock *parent) :
+	SuperBlock(parent, "CAT"),
+	last_timestamp(hrt_absolute_time()),
 	integrator(0.0f),
 	launchDetected(false),
-	threshold_accel(NULL, "LAUN_CAT_A", false),
-	threshold_time(NULL, "LAUN_CAT_T", false)
+	threshold_accel(this, "A"),
+	threshold_time(this, "T")
 {
 
 }
@@ -83,8 +87,11 @@ bool CatapultLaunchMethod::getLaunchDetected()
 	return launchDetected;
 }
 
-void CatapultLaunchMethod::updateParams()
+
+void CatapultLaunchMethod::reset()
 {
-	threshold_accel.update();
-	threshold_time.update();
+	integrator = 0.0f;
+	launchDetected = false;
+}
+
 }

src/lib/launchdetection/CatapultLaunchMethod.h

@@ -44,22 +44,24 @@
 #include "LaunchMethod.h"
 
 #include <drivers/drv_hrt.h>
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-class CatapultLaunchMethod : public LaunchMethod
+namespace launchdetection
+{
+
+class CatapultLaunchMethod : public LaunchMethod, public control::SuperBlock
 {
 public:
-	CatapultLaunchMethod();
+	CatapultLaunchMethod(SuperBlock *parent);
 	~CatapultLaunchMethod();
 
 	void update(float accel_x);
 	bool getLaunchDetected();
-	void updateParams();
+	void reset();
 
 private:
 	hrt_abstime last_timestamp;
-//	float threshold_accel_raw;
-//	float threshold_time;
 	float integrator;
 	bool launchDetected;
 
@@ -69,3 +71,5 @@ private:
 };
 
 #endif /* CATAPULTLAUNCHMETHOD_H_ */
+
+}

src/lib/launchdetection/LaunchDetector.cpp

@@ -42,12 +42,16 @@
 #include "CatapultLaunchMethod.h"
 #include <systemlib/err.h>
 
+namespace launchdetection
+{
+
 LaunchDetector::LaunchDetector() :
-	launchdetection_on(NULL, "LAUN_ALL_ON", false),
-	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
+	SuperBlock(NULL, "LAUN"),
+	launchdetection_on(this, "ALL_ON"),
+	throttlePreTakeoff(this, "THR_PRE")
 {
 	/* init all detectors */
-	launchMethods[0] = new CatapultLaunchMethod();
+	launchMethods[0] = new CatapultLaunchMethod(this);
 
 
 	/* update all parameters of all detectors */
@@ -59,6 +63,12 @@ LaunchDetector::~LaunchDetector()
 
 }
 
+void LaunchDetector::reset()
+{
+	/* Reset all detectors */
+	launchMethods[0]->reset();
+}
+
 void LaunchDetector::update(float accel_x)
 {
 	if (launchdetection_on.get() == 1) {
@@ -81,12 +91,4 @@ bool LaunchDetector::getLaunchDetected()
 	return false;
 }
 
-void LaunchDetector::updateParams() {
-
-	launchdetection_on.update();
-	throttlePreTakeoff.update();
-
-	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
-		launchMethods[i]->updateParams();
-	}
 }

src/lib/launchdetection/LaunchDetector.h

@@ -45,18 +45,21 @@
 #include <stdint.h>
 
 #include "LaunchMethod.h"
-
+#include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-class __EXPORT LaunchDetector
+namespace launchdetection
+{
+
+class __EXPORT LaunchDetector : public control::SuperBlock
 {
 public:
 	LaunchDetector();
 	~LaunchDetector();
+	void reset();
 
 	void update(float accel_x);
 	bool getLaunchDetected();
-	void updateParams();
 	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
 
 	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
@@ -71,5 +74,6 @@ private:
 
 };
 
+}
 
 #endif // LAUNCHDETECTOR_H

src/lib/launchdetection/LaunchMethod.h

@@ -41,14 +41,20 @@
 #ifndef LAUNCHMETHOD_H_
 #define LAUNCHMETHOD_H_
 
+namespace launchdetection
+{
+
 class LaunchMethod
 {
 public:
 	virtual void update(float accel_x) = 0;
 	virtual bool getLaunchDetected() = 0;
-	virtual void updateParams() = 0;
+	virtual void reset() = 0;
+
 protected:
 private:
 };
 
+}
+
 #endif /* LAUNCHMETHOD_H_ */

src/modules/att_pos_estimator_ekf/KalmanNav.hpp

@@ -47,8 +47,8 @@
 #include <mathlib/mathlib.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
-#include <controllib/uorb/UOrbSubscription.hpp>
-#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
@@ -138,13 +138,13 @@ protected:
 	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
 	math::Quaternion q;         /**< quaternion from body to nav frame */
 	// subscriptions
-	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
-	control::UOrbSubscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
-	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
+	uORB::Subscription<sensor_combined_s> _sensors;          /**< sensors sub. */
+	uORB::Subscription<vehicle_gps_position_s> _gps;         /**< gps sub. */
+	uORB::Subscription<parameter_update_s> _param_update;    /**< parameter update sub. */
 	// publications
-	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
-	control::UOrbPublication<vehicle_local_position_s> _localPos;   /**< local position pub. */
-	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
+	uORB::Publication<vehicle_global_position_s> _pos;       /**< position pub. */
+	uORB::Publication<vehicle_local_position_s> _localPos;   /**< local position pub. */
+	uORB::Publication<vehicle_attitude_s> _att;              /**< attitude pub. */
 	// time stamps
 	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
 	uint64_t _predictTimeStamp; /**< prediction time stamp */

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -277,7 +277,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_ekf_params ekf_params;
@@ -380,9 +379,8 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
 						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
@@ -392,11 +390,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					hrt_abstime vel_t = 0;
@@ -445,11 +442,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					z_k[6] = raw.magnetometer_ga[0];
@@ -477,6 +473,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
 
+						/* update mag declination rotation matrix */
+						R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
+
 						x_aposteriori_k[0] = z_k[0];
 						x_aposteriori_k[1] = z_k[1];
 						x_aposteriori_k[2] = z_k[2];

src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp

@@ -445,7 +445,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 	// XXX write this out to perf regs
 
 	/* keep track of sensor updates */
-	uint32_t sensor_last_count[3] = {0, 0, 0};
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
 	struct attitude_estimator_so3_params so3_comp_params;
@@ -526,9 +525,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					uint8_t update_vect[3] = {0, 0, 0};
 
 					/* Fill in gyro measurements */
-					if (sensor_last_count[0] != raw.gyro_counter) {
+					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_last_count[0] = raw.gyro_counter;
 						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
@@ -538,11 +536,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
-					if (sensor_last_count[1] != raw.accelerometer_counter) {
+					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_last_count[1] = raw.accelerometer_counter;
 						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
-						sensor_last_timestamp[1] = raw.timestamp;
+						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
 					acc[0] = raw.accelerometer_m_s2[0];
@@ -550,11 +547,10 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[])
 					acc[2] = raw.accelerometer_m_s2[2];
 
 					/* update magnetometer measurements */
-					if (sensor_last_count[2] != raw.magnetometer_counter) {
+					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_last_count[2] = raw.magnetometer_counter;
 						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
-						sensor_last_timestamp[2] = raw.timestamp;
+						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
 					mag[0] = raw.magnetometer_ga[0];

src/modules/commander/accelerometer_calibration.cpp

@@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 				 (double)accel_ref[orient][2]);
 
 		data_collected[orient] = true;
-		tune_neutral();
+		tune_neutral(true);
 	}
 
 	close(sensor_combined_sub);

src/modules/commander/airspeed_calibration.cpp

@@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
 
 		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-		tune_neutral();
+		tune_neutral(true);
 		close(diff_pres_sub);
 		return OK;
 

src/modules/commander/commander.cpp

@@ -119,6 +119,7 @@ extern struct system_load_s system_load;
 
 #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */
 #define RC_TIMEOUT 100000
+#define RC_TIMEOUT_HIL 500000
 #define OFFBOARD_TIMEOUT 200000
 #define DIFFPRESS_TIMEOUT 2000000
 
@@ -616,7 +617,6 @@ int commander_thread_main(int argc, char *argv[])
 	/* not yet initialized */
 	commander_initialized = false;
 
-	bool battery_tune_played = false;
 	bool arm_tune_played = false;
 
 	/* set parameters */
@@ -909,7 +909,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
 
 			/* disarm if safety is now on and still armed */
-			if (safety.safety_switch_available && !safety.safety_off && armed.armed) {
+			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
@@ -969,7 +969,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
@@ -1032,14 +1032,12 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
-			battery_tune_played = false;
 
 		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
-			battery_tune_played = false;
 
 			if (armed.armed) {
 				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
@@ -1113,12 +1111,20 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
-				tune_positive();
+				tune_positive(true);
 			}
 		}
 
+
+		/*
+		 * XXX workaround:
+		 * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz)
+		 * which can trigger RC loss if the computer/simulator lags.
+		 */
+		uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT;
+
 		/* start RC input check */
-		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) {
+		if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) {
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
@@ -1208,8 +1214,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* evaluate the main state machine according to mode switches */
 			res = set_main_state_rc(&status);
 
+			/* play tune on mode change only if armed, blink LED always */
 			if (res == TRANSITION_CHANGED) {
-				tune_positive();
+				tune_positive(armed.armed);
 
 			} else if (res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
@@ -1309,7 +1316,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* flight termination in manual mode if assisted switch is on easy position */
 		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive();
+				tune_positive(armed.armed);
 			}
 		}
 
@@ -1364,26 +1371,23 @@ int commander_thread_main(int argc, char *argv[])
 		/* play arming and battery warning tunes */
 		if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
 			/* play tune when armed */
-			if (tune_arm() == OK)
-				arm_tune_played = true;
-
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-			/* play tune on battery warning */
-			if (tune_low_bat() == OK)
-				battery_tune_played = true;
+			set_tune(TONE_ARMING_WARNING_TUNE);
+			arm_tune_played = true;
 
 		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
-			if (tune_critical_bat() == OK)
-				battery_tune_played = true;
+			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (battery_tune_played) {
-			tune_stop();
-			battery_tune_played = false;
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
+			/* play tune on battery warning or failsafe */
+			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
+
+		} else {
+			set_tune(TONE_STOP_TUNE);
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
+		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 			arm_tune_played = false;
 		}
 
@@ -1478,11 +1482,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
-				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-					rgbled_set_color(RGBLED_COLOR_AMBER);
-				}
-
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
+				rgbled_set_color(RGBLED_COLOR_AMBER);
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
@@ -1802,15 +1803,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg)
 		sprintf(s, "#audio: REJECT %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 
-		// only buzz if armed, because else we're driving people nuts indoors
-		// they really need to look at the leds as well.
-		if (status->arming_state == ARMING_STATE_ARMED) {
-			tune_negative();
-		} else {
-
-			// Always show the led indication
-			led_negative();
-		}
+		/* only buzz if armed, because else we're driving people nuts indoors
+		they really need to look at the leds as well. */
+		tune_negative(armed.armed);
 	}
 }
 
@@ -1824,7 +1819,7 @@ print_reject_arm(const char *msg)
 		char s[80];
 		sprintf(s, "#audio: %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+		tune_negative(true);
 	}
 }
 
@@ -1832,27 +1827,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-			tune_positive();
+			tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
 		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
 		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
 		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
 		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	default:
@@ -1992,9 +1987,9 @@ void *commander_low_prio_loop(void *arg)
 				}
 
 				if (calib_ret == OK)
-					tune_positive();
+					tune_positive(true);
 				else
-					tune_negative();
+					tune_negative(true);
 
 				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 

src/modules/commander/commander_helper.cpp

@@ -45,6 +45,7 @@
 #include <stdbool.h>
 #include <fcntl.h>
 #include <math.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -81,11 +82,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
 	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
-static int buzzer;
-static hrt_abstime blink_msg_end;
+static int buzzer = -1;
+static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
+static hrt_abstime tune_end = 0;		// end time of currently played tune, 0 for repeating tunes or silence
+static int tune_current = TONE_STOP_TUNE;		// currently playing tune, can be interrupted after tune_end
+static unsigned int tune_durations[TONE_NUMBER_OF_TUNES];
 
 int buzzer_init()
 {
+	tune_end = 0;
+	tune_current = 0;
+	memset(tune_durations, 0, sizeof(tune_durations));
+	tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
+	tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
+	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
+	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
+
 	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
@@ -101,58 +113,60 @@ void buzzer_deinit()
 	close(buzzer);
 }
 
-void tune_error()
-{
-	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+void set_tune(int tune) {
+	unsigned int new_tune_duration = tune_durations[tune];
+	/* don't interrupt currently playing non-repeating tune by repeating */
+	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
+		/* allow interrupting current non-repeating tune by the same tune */
+		if (tune != tune_current || new_tune_duration != 0) {
+			ioctl(buzzer, TONE_SET_ALARM, tune);
+		}
+		tune_current = tune;
+		if (new_tune_duration != 0) {
+			tune_end = hrt_absolute_time() + new_tune_duration;
+		} else {
+			tune_end = 0;
+		}
+	}
 }
 
-void tune_positive()
+/**
+ * Blink green LED and play positive tune (if use_buzzer == true).
+ */
+void tune_positive(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_GREEN);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+	}
 }
 
-void tune_neutral()
+/**
+ * Blink white LED and play neutral tune (if use_buzzer == true).
+ */
+void tune_neutral(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_WHITE);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
+	}
 }
 
-void tune_negative()
-{
-	led_negative();
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
-}
-
-void led_negative()
+/**
+ * Blink red LED and play negative tune (if use_buzzer == true).
+ */
+void tune_negative(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-}
-
-int tune_arm()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
-}
-
-int tune_low_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
-}
-
-int tune_critical_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
-}
-
-void tune_stop()
-{
-	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
+	}
 }
 
 int blink_msg_state()
@@ -161,6 +175,7 @@ int blink_msg_state()
 		return 0;
 
 	} else if (hrt_absolute_time() > blink_msg_end) {
+		blink_msg_end = 0;
 		return 2;
 
 	} else {
@@ -168,8 +183,8 @@ int blink_msg_state()
 	}
 }
 
-static int leds;
-static int rgbleds;
+static int leds = -1;
+static int rgbleds = -1;
 
 int led_init()
 {

src/modules/commander/commander_helper.h

@@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
 int buzzer_init(void);
 void buzzer_deinit(void);
 
-void tune_error(void);
-void tune_positive(void);
-void tune_neutral(void);
-void tune_negative(void);
-int tune_arm(void);
-int tune_low_bat(void);
-int tune_critical_bat(void);
-void tune_stop(void);
-
-void led_negative();
+void set_tune(int tune);
+void tune_positive(bool use_buzzer);
+void tune_neutral(bool use_buzzer);
+void tune_negative(bool use_buzzer);
 
 int blink_msg_state();
 

src/modules/commander/px4_custom_mode.h

@@ -8,6 +8,8 @@
 #ifndef PX4_CUSTOM_MODE_H_
 #define PX4_CUSTOM_MODE_H_
 
+#include <stdint.h>
+
 enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
 	PX4_CUSTOM_MAIN_MODE_SEATBELT,

src/modules/commander/state_machine_helper.cpp

@@ -44,6 +44,7 @@
 #include <stdbool.h>
 #include <dirent.h>
 #include <fcntl.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -321,10 +322,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 	bool valid_transition = false;
 	int ret = ERROR;
 
-	warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
-
 	if (current_status->hil_state == new_state) {
-		warnx("Hil state not changed");
 		valid_transition = true;
 
 	} else {
@@ -352,23 +350,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 
 				/* list directory */
 				DIR		*d;
-				struct dirent	*direntry;
 				d = opendir("/dev");
 				if (d) {
 
+					struct dirent	*direntry;
+					char devname[24];
+
 					while ((direntry = readdir(d)) != NULL) {
 
-						int sensfd = ::open(direntry->d_name, 0);
-						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0);
+						/* skip serial ports */
+						if (!strncmp("tty", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip mtd devices */
+						if (!strncmp("mtd", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip ram devices */
+						if (!strncmp("ram", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip MMC devices */
+						if (!strncmp("mmc", direntry->d_name, 3)) {
+							continue;
+						}
+						/* skip mavlink */
+						if (!strcmp("mavlink", direntry->d_name)) {
+							continue;
+						}
+						/* skip console */
+						if (!strcmp("console", direntry->d_name)) {
+							continue;
+						}
+						/* skip null */
+						if (!strcmp("null", direntry->d_name)) {
+							continue;
+						}
+
+						snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
+
+						int sensfd = ::open(devname, 0);
+
+						if (sensfd < 0) {
+							warn("failed opening device %s", devname);
+							continue;
+						}
+
+						int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
 						close(sensfd);
 
-						printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL");
+						printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
 					}
 
 					closedir(d);
 
-					warnx("directory listing ok (FS mounted and readable)");
-
 				} else {
 					/* failed opening dir */
 					warnx("FAILED LISTING DEVICE ROOT DIRECTORY");

src/modules/controllib/block/Block.cpp

@@ -41,10 +41,11 @@
 #include <string.h>
 #include <stdio.h>
 
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+
 #include "Block.hpp"
 #include "BlockParam.hpp"
-#include "../uorb/UOrbSubscription.hpp"
-#include "../uorb/UOrbPublication.hpp"
 
 namespace control
 {
@@ -100,7 +101,7 @@ void Block::updateParams()
 
 void Block::updateSubscriptions()
 {
-	UOrbSubscriptionBase *sub = getSubscriptions().getHead();
+	uORB::SubscriptionBase *sub = getSubscriptions().getHead();
 	int count = 0;
 
 	while (sub != NULL) {
@@ -118,7 +119,7 @@ void Block::updateSubscriptions()
 
 void Block::updatePublications()
 {
-	UOrbPublicationBase *pub = getPublications().getHead();
+	uORB::PublicationBase *pub = getPublications().getHead();
 	int count = 0;
 
 	while (pub != NULL) {

src/modules/controllib/block/Block.hpp

@@ -42,7 +42,13 @@
 #include <stdint.h>
 #include <inttypes.h>
 
-#include "List.hpp"
+#include <containers/List.hpp>
+
+// forward declaration
+namespace uORB {
+	class SubscriptionBase;
+	class PublicationBase;
+}
 
 namespace control
 {
@@ -55,8 +61,6 @@ static const uint8_t blockNameLengthMax = 80;
 
 // forward declaration
 class BlockParamBase;
-class UOrbSubscriptionBase;
-class UOrbPublicationBase;
 class SuperBlock;
 
 /**
@@ -79,15 +83,15 @@ public:
 protected:
 // accessors
 	SuperBlock *getParent() { return _parent; }
-	List<UOrbSubscriptionBase *> & getSubscriptions() { return _subscriptions; }
-	List<UOrbPublicationBase *> & getPublications() { return _publications; }
+	List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
+	List<uORB::PublicationBase *> & getPublications() { return _publications; }
 	List<BlockParamBase *> & getParams() { return _params; }
 // attributes
 	const char *_name;
 	SuperBlock *_parent;
 	float _dt;
-	List<UOrbSubscriptionBase *> _subscriptions;
-	List<UOrbPublicationBase *> _publications;
+	List<uORB::SubscriptionBase *> _subscriptions;
+	List<uORB::PublicationBase *> _publications;
 	List<BlockParamBase *> _params;
 };
 

src/modules/controllib/block/BlockParam.cpp

@@ -76,4 +76,29 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_pref
 		printf("error finding param: %s\n", fullname);
 };
 
+template <class T>
+BlockParam<T>::BlockParam(Block *block, const char *name,
+		bool parent_prefix) :
+	BlockParamBase(block, name, parent_prefix),
+	_val() {
+	update();
+}
+
+template <class T>
+T BlockParam<T>::get() { return _val; }
+
+template <class T>
+void BlockParam<T>::set(T val) { _val = val; }
+
+template <class T>
+void BlockParam<T>::update() {
+	if (_handle != PARAM_INVALID) param_get(_handle, &_val);
+}
+
+template <class T>
+BlockParam<T>::~BlockParam() {};
+
+template class __EXPORT BlockParam<float>;
+template class __EXPORT BlockParam<int>;
+
 } // namespace control

src/modules/controllib/block/BlockParam.hpp

@@ -42,7 +42,7 @@
 #include <systemlib/param/param.h>
 
 #include "Block.hpp"
-#include "List.hpp"
+#include <containers/List.hpp>
 
 namespace control
 {
@@ -70,38 +70,21 @@ protected:
  * Parameters that are tied to blocks for updating and nameing.
  */
 
-class __EXPORT BlockParamFloat : public BlockParamBase
+template <class T>
+class BlockParam : public BlockParamBase
 {
 public:
-	BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) :
-		BlockParamBase(block, name, parent_prefix),
-		_val() {
-		update();
-	}
-	float get() { return _val; }
-	void set(float val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
+	BlockParam(Block *block, const char *name,
+			bool parent_prefix=true);
+	T get();
+	void set(T val);
+	void update();
+	virtual ~BlockParam();
 protected:
-	float _val;
+	T _val;
 };
 
-class __EXPORT BlockParamInt : public BlockParamBase
-{
-public:
-	BlockParamInt(Block *block, const char *name, bool parent_prefix=true) :
-		BlockParamBase(block, name, parent_prefix),
-		_val() {
-		update();
-	}
-	int get() { return _val; }
-	void set(int val) { _val = val; }
-	void update() {
-		if (_handle != PARAM_INVALID) param_get(_handle, &_val);
-	}
-protected:
-	int _val;
-};
+typedef BlockParam<float> BlockParamFloat;
+typedef BlockParam<int> BlockParamInt;
 
 } // namespace control

src/modules/controllib/module.mk

@@ -37,7 +37,5 @@
 SRCS		 = test_params.c \
 		   block/Block.cpp \
 		   block/BlockParam.cpp \
-		   uorb/UOrbPublication.cpp \
-		   uorb/UOrbSubscription.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp

src/modules/controllib/uorb/blocks.hpp

@@ -62,8 +62,8 @@ extern "C" {
 }
 
 #include "../blocks.hpp"
-#include "UOrbSubscription.hpp"
-#include "UOrbPublication.hpp"
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 
 namespace control
 {
@@ -94,16 +94,16 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
 {
 protected:
 	// subscriptions
-	UOrbSubscription<vehicle_attitude_s> _att;
-	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
-	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
-	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<position_setpoint_triplet_s> _missionCmd;
-	UOrbSubscription<manual_control_setpoint_s> _manual;
-	UOrbSubscription<vehicle_status_s> _status;
-	UOrbSubscription<parameter_update_s> _param_update;
+	uORB::Subscription<vehicle_attitude_s> _att;
+	uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
+	uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
+	uORB::Subscription<vehicle_global_position_s> _pos;
+	uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
+	uORB::Subscription<manual_control_setpoint_s> _manual;
+	uORB::Subscription<vehicle_status_s> _status;
+	uORB::Subscription<parameter_update_s> _param_update;
 	// publications
-	UOrbPublication<actuator_controls_s> _actuators;
+	uORB::Publication<actuator_controls_s> _actuators;
 public:
 	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
 	virtual ~BlockUorbEnabledAutopilot();

src/modules/dataman/dataman.c

@@ -1,8 +1,10 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier
- *              Jean Cyr
+ *   Author: 	Jean Cyr
+ *              Lorenz Meier
+ *              Julian Oes
+ *              Thomas Gubler
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,16 +42,8 @@
 #include <nuttx/config.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
 #include <fcntl.h>
-#include <errno.h>
-#include <math.h>
-#include <poll.h>
-#include <time.h>
-#include <sys/ioctl.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/err.h>
 #include <queue.h>
 
 #include "dataman.h"
@@ -175,8 +169,10 @@ create_work_item(void)
 
 	/* Try to reuse item from free item queue */
 	lock_queue(&g_free_q);
+
 	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
 		g_free_q.size--;
+
 	unlock_queue(&g_free_q);
 
 	/* If we there weren't any free items then obtain memory for a new one */
@@ -289,11 +285,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
 	offset = calculate_offset(item, index);
 
 	/* If item type or index out of range, return error */
-	if (offset < 0) 
+	if (offset < 0)
 		return -1;
 
 	/* Make sure caller has not given us more data than we can handle */
-	if (count > DM_MAX_DATA_SIZE) 
+	if (count > DM_MAX_DATA_SIZE)
 		return -1;
 
 	/* Write out the data, prefixed with length and persistence level */
@@ -339,6 +335,7 @@ _read(dm_item_t item, unsigned char index, void *buf, size_t count)
 
 	/* Read the prefix and data */
 	len = -1;
+
 	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
 		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
 
@@ -492,7 +489,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 		return -1;
 
 	/* get a work item and queue up a write request */
-	if ((work = create_work_item()) == NULL) 
+	if ((work = create_work_item()) == NULL)
 		return -1;
 
 	work->func = dm_write_func;
@@ -599,17 +596,20 @@ task_main(int argc, char *argv[])
 
 	/* Open or create the data manager file */
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
 		sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
+
 	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
 		sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
+
 	fsync(g_task_fd);
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
@@ -767,10 +767,10 @@ dataman_main(int argc, char *argv[])
 		stop();
 	else if (!strcmp(argv[1], "status"))
 		status();
-    else if (!strcmp(argv[1], "poweronrestart"))
-        dm_restart(DM_INIT_REASON_POWER_ON);
-    else if (!strcmp(argv[1], "inflightrestart"))
-        dm_restart(DM_INIT_REASON_IN_FLIGHT);
+	else if (!strcmp(argv[1], "poweronrestart"))
+		dm_restart(DM_INIT_REASON_POWER_ON);
+	else if (!strcmp(argv[1], "inflightrestart"))
+		dm_restart(DM_INIT_REASON_IN_FLIGHT);
 	else
 		usage();
 

src/modules/dataman/module.mk

@@ -38,5 +38,3 @@
 MODULE_COMMAND	= dataman
 
 SRCS		= dataman.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

src/modules/fixedwing_backside/fixedwing.cpp

@@ -175,14 +175,14 @@ void BlockMultiModeBacksideAutopilot::update()
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
 					_pos.vel_n * _pos.vel_n +
-					_pos.vy * _pos.vy +
+					_pos.vel_e * _pos.vel_e +
 					_pos.vel_d * _pos.vel_d));
 
 		// limit velocity command between min/max velocity
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_missionCmd.current.alt - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -237,7 +237,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
 					_pos.vel_n * _pos.vel_n +
-					_pos.vy * _pos.vy +
+					_pos.vel_e * _pos.vel_e +
 					_pos.vel_d * _pos.vel_d));
 
 		// pitch channel -> rate of climb

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -108,14 +108,14 @@ private:
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
 	int		_control_task;			/**< task handle for sensor task */
 
-	int		_att_sub;				/**< vehicle attitude subscription */
-	int		_accel_sub;				/**< accelerometer subscription */
+	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_accel_sub;			/**< accelerometer subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle status subscription */
-	int 	_params_sub;			/**< notification of parameter updates */
-	int 	_manual_sub;			/**< notification of manual control updates */
+	int 		_params_sub;			/**< notification of parameter updates */
+	int 		_manual_sub;			/**< notification of manual control updates */
 	int		_global_pos_sub;		/**< global position subscription */
 
 	orb_advert_t	_rate_sp_pub;			/**< rate setpoint publication */
@@ -123,20 +123,19 @@ private:
 	orb_advert_t	_actuators_0_pub;		/**< actuator control group 0 setpoint */
 	orb_advert_t	_actuators_1_pub;		/**< actuator control group 1 setpoint (Airframe) */
 
-	struct vehicle_attitude_s				_att;			/**< vehicle attitude */
-	struct accel_report						_accel;			/**< body frame accelerations */
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct accel_report				_accel;			/**< body frame accelerations */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s						_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s			_vcontrol_mode;	/**< vehicle control mode */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
 	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
-	struct actuator_controls_s			_actuators_airframe;		/**< actuator control inputs */
-	struct vehicle_global_position_s		_global_pos;	/**< global position */
+	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-	bool		_airspeed_valid;		/**< flag if the airspeed measurement is valid */
 
 	struct {
 		float tconst;
@@ -166,6 +165,15 @@ private:
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -197,6 +205,12 @@ private:
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
+
+		param_t trim_roll;
+		param_t trim_pitch;
+		param_t trim_yaw;
+		param_t rollsp_offset_deg;
+		param_t pitchsp_offset_deg;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -230,7 +244,7 @@ private:
 	/**
 	 * Check for airspeed updates.
 	 */
-	bool		vehicle_airspeed_poll();
+	void		vehicle_airspeed_poll();
 
 	/**
 	 * Check for accel updates.
@@ -293,19 +307,18 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
 /* states */
-	_setpoint_valid(false),
-	_airspeed_valid(false)
+	_setpoint_valid(false)
 {
 	/* safely initialize structs */
-	_att = {0};
-	_accel = {0};
-	_att_sp = {0};
-	_manual = {0};
-	_airspeed = {0};
-	_vcontrol_mode = {0};
-	_actuators = {0};
-	_actuators_airframe = {0};
-	_global_pos = {0};
+	_att = {};
+	_accel = {};
+	_att_sp = {};
+	_manual = {};
+	_airspeed = {};
+	_vcontrol_mode = {};
+	_actuators = {};
+	_actuators_airframe = {};
+	_global_pos = {};
 
 
 	_parameter_handles.tconst = param_find("FW_ATT_TC");
@@ -335,6 +348,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
 
+	_parameter_handles.trim_roll = param_find("TRIM_ROLL");
+	_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
+	_parameter_handles.trim_yaw = param_find("TRIM_YAW");
+	_parameter_handles.rollsp_offset_deg = param_find("FW_RSP_OFF");
+	_parameter_handles.pitchsp_offset_deg = param_find("FW_PSP_OFF");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -395,6 +414,15 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
 
+	param_get(_parameter_handles.trim_roll, &(_parameters.trim_roll));
+	param_get(_parameter_handles.trim_pitch, &(_parameters.trim_pitch));
+	param_get(_parameter_handles.trim_yaw, &(_parameters.trim_yaw));
+	param_get(_parameter_handles.rollsp_offset_deg, &(_parameters.rollsp_offset_deg));
+	param_get(_parameter_handles.pitchsp_offset_deg, &(_parameters.pitchsp_offset_deg));
+	_parameters.rollsp_offset_rad = math::radians(_parameters.rollsp_offset_deg);
+	_parameters.pitchsp_offset_rad = math::radians(_parameters.pitchsp_offset_deg);
+
+
 	/* pitch control parameters */
 	_pitch_ctrl.set_time_constant(_parameters.tconst);
 	_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -452,7 +480,7 @@ FixedwingAttitudeControl::vehicle_manual_poll()
 	}
 }
 
-bool
+void
 FixedwingAttitudeControl::vehicle_airspeed_poll()
 {
 	/* check if there is a new position */
@@ -462,10 +490,7 @@ FixedwingAttitudeControl::vehicle_airspeed_poll()
 	if (airspeed_updated) {
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 //		warnx("airspeed poll: ind: %.4f,  true: %.4f", _airspeed.indicated_airspeed_m_s, _airspeed.true_airspeed_m_s);
-		return true;
 	}
-
-	return false;
 }
 
 void
@@ -539,7 +564,7 @@ FixedwingAttitudeControl::task_main()
 	parameters_update();
 
 	/* get an initial update for all sensor and status data */
-	(void)vehicle_airspeed_poll();
+	vehicle_airspeed_poll();
 	vehicle_setpoint_poll();
 	vehicle_accel_poll();
 	vehicle_control_mode_poll();
@@ -596,7 +621,7 @@ FixedwingAttitudeControl::task_main()
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
 
-			_airspeed_valid = vehicle_airspeed_poll();
+			vehicle_airspeed_poll();
 
 			vehicle_setpoint_poll();
 
@@ -636,9 +661,9 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is smaller than min, the sensor is not giving good readings */
-				if (!_airspeed_valid ||
-				    (_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
-				    !isfinite(_airspeed.indicated_airspeed_m_s)) {
+				if ((_airspeed.indicated_airspeed_m_s < 0.5f * _parameters.airspeed_min) ||
+				    !isfinite(_airspeed.indicated_airspeed_m_s) ||
+				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
 
 				} else {
@@ -648,13 +673,13 @@ FixedwingAttitudeControl::task_main()
 				float airspeed_scaling = _parameters.airspeed_trim / airspeed;
 				//warnx("aspd scale: %6.2f act scale: %6.2f", airspeed_scaling, actuator_scaling);
 
-				float roll_sp = 0.0f;
-				float pitch_sp = 0.0f;
+				float roll_sp = _parameters.rollsp_offset_rad;
+				float pitch_sp = _parameters.pitchsp_offset_rad;
 				float throttle_sp = 0.0f;
 
 				if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) {
-					roll_sp = _att_sp.roll_body;
-					pitch_sp = _att_sp.pitch_body;
+					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
 					/* reset integrals where needed */
@@ -670,9 +695,13 @@ FixedwingAttitudeControl::task_main()
 					 * With this mapping the stick angle is a 1:1 representation of
 					 * the commanded attitude. If more than 45 degrees are desired,
 					 * a scaling parameter can be applied to the remote.
+					 *
+					 * The trim gets subtracted here from the manual setpoint to get
+					 * the intended attitude setpoint. Later, after the rate control step the
+					 * trim is added again to the control signal.
 					 */
-					roll_sp = _manual.roll * 0.75f;
-					pitch_sp = _manual.pitch * 0.75f;
+					roll_sp = (_manual.roll - _parameters.trim_roll) * 0.75f + _parameters.rollsp_offset_rad;
+					pitch_sp = (_manual.pitch - _parameters.trim_pitch) * 0.75f + _parameters.pitchsp_offset_rad;
 					throttle_sp = _manual.throttle;
 					_actuators.control[4] = _manual.flaps;
 
@@ -685,7 +714,7 @@ FixedwingAttitudeControl::task_main()
 					att_sp.timestamp = hrt_absolute_time();
 					att_sp.roll_body = roll_sp;
 					att_sp.pitch_body = pitch_sp;
-					att_sp.yaw_body = 0.0f;
+					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
 					att_sp.thrust = throttle_sp;
 
 					/* lazily publish the setpoint only once available */
@@ -719,12 +748,12 @@ FixedwingAttitudeControl::task_main()
 							speed_body_u, speed_body_v, speed_body_w,
 							_roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
 
-					/* Run attitude RATE controllers which need the desired attitudes from above */
+					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
 					float roll_u = _roll_ctrl.control_bodyrate(_att.pitch,
 							_att.rollspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[0] = (isfinite(roll_u)) ? roll_u : 0.0f;
+					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
 						warnx("roll_u %.4f", roll_u);
 					}
@@ -733,7 +762,7 @@ FixedwingAttitudeControl::task_main()
 							_att.pitchspeed, _att.yawspeed,
 							_yaw_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u : 0.0f;
+					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
 						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
 								pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body);
@@ -743,7 +772,7 @@ FixedwingAttitudeControl::task_main()
 							_att.pitchspeed, _att.yawspeed,
 							_pitch_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
-					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u : 0.0f;
+					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
 						warnx("yaw_u %.4f", yaw_u);
 					}
@@ -757,10 +786,6 @@ FixedwingAttitudeControl::task_main()
 					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp);
 				}
 
-				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-				// 			_actuators.control[2], _actuators.control[3]);
-
 				/*
 				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
 				 * only once available

src/modules/fw_att_control/fw_att_control_params.c

@@ -176,3 +176,13 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 // @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
 // @Range		0.0 to 30
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
+
+// @DisplayName		Roll Setpoint Offset
+// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
+
+// @DisplayName		Pitch Setpoint Offset
+// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
+// @Range		-90.0 to 90.0
+PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -130,23 +130,23 @@ private:
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_control_mode_sub;			/**< vehicle status subscription */
+	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_sensor_combined_sub;			/**< for body frame accelerations */
+	int		_sensor_combined_sub;		/**< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
-	struct navigation_capabilities_s		_nav_capabilities;	/**< navigation capabilities */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
-	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;		/**< triplet of mission items */
-	struct sensor_combined_s				_sensor_combined;			/**< for body frame accelerations */
+	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
+	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
+	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
+	struct airspeed_s				_airspeed;			/**< airspeed */
+	struct vehicle_control_mode_s			_control_mode;			/**< vehicle status */
+	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
+	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -154,13 +154,13 @@ private:
 
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
-	float		_loiter_hold_lon;
+	double		_loiter_hold_lat;
+	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
 	bool		_loiter_hold;
 
-	float		_launch_lat;
-	float		_launch_lon;
+	double		_launch_lat;
+	double		_launch_lon;
 	float		_launch_alt;
 	bool		_launch_valid;
 
@@ -176,6 +176,8 @@ private:
 	bool launch_detected;
 	bool usePreTakeoffThrust;
 
+	bool last_manual;				///< true if the last iteration was in manual mode (used to determine when a reset is needed)
+
 	/* Landingslope object */
 	Landingslope landingslope;
 
@@ -184,7 +186,7 @@ private:
 	float target_bearing;
 
 	/* Launch detection */
-	LaunchDetector launchDetector;
+	launchdetection::LaunchDetector launchDetector;
 
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
@@ -192,7 +194,7 @@ private:
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Matrix<3, 3> _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -233,7 +235,6 @@ private:
 		float speedrate_p;
 
 		float land_slope_angle;
-		float land_slope_length;
 		float land_H1_virt;
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
@@ -278,7 +279,6 @@ private:
 		param_t speedrate_p;
 
 		param_t land_slope_angle;
-		param_t land_slope_length;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
@@ -346,6 +346,16 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+	/*
+	 * Reset takeoff state
+	 */
+	void reset_takeoff_state();
+
+	/*
+	 * Reset landing state
+	 */
+	void reset_landing_state();
 };
 
 namespace l1_control
@@ -362,6 +372,7 @@ FixedwingPositionControl	*g_control;
 
 FixedwingPositionControl::FixedwingPositionControl() :
 
+	_mavlink_fd(-1),
 	_task_should_exit(false),
 	_control_task(-1),
 
@@ -380,38 +391,34 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+
 /* states */
 	_setpoint_valid(false),
 	_loiter_hold(false),
-	_airspeed_error(0.0f),
-	_airspeed_valid(false),
-	_groundspeed_undershoot(0.0f),
-	_global_pos_valid(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
-	flare_curve_alt_last(0.0f),
-	_mavlink_fd(-1),
-	launchDetector(),
 	launch_detected(false),
-	usePreTakeoffThrust(false)
+	last_manual(false),
+	usePreTakeoffThrust(false),
+	flare_curve_alt_last(0.0f),
+	launchDetector(),
+	_airspeed_error(0.0f),
+	_airspeed_valid(false),
+	_groundspeed_undershoot(0.0f),
+	_global_pos_valid(false),
+	_att(),
+	_att_sp(),
+	_nav_capabilities(),
+	_manual(),
+	_airspeed(),
+	_control_mode(),
+	_global_pos(),
+	_pos_sp_triplet(),
+	_sensor_combined()
 {
-	/* safely initialize structs */
-	vehicle_attitude_s			_att = {0};
-	vehicle_attitude_setpoint_s		_att_sp = {0};
-	navigation_capabilities_s		_nav_capabilities = {0};
-	manual_control_setpoint_s		_manual = {0};
-	airspeed_s				_airspeed = {0};
-	vehicle_control_mode_s			_control_mode = {0};
-	vehicle_global_position_s		_global_pos = {0};
-	position_setpoint_triplet_s			_pos_sp_triplet = {0};
-	sensor_combined_s				_sensor_combined = {0};
-
-
-
-
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -431,7 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
 	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
-	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
@@ -520,7 +526,6 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
 
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
-	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
 	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
@@ -587,8 +592,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 
 		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat / 1e7f;
-			_launch_lon = _global_pos.lon / 1e7f;
+			_launch_lat = _global_pos.lat;
+			_launch_lon = _global_pos.lon;
 			_launch_alt = _global_pos.alt;
 			_launch_valid = true;
 		}
@@ -703,7 +708,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid) {
+	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -889,8 +894,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
+			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
+			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
@@ -916,7 +923,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
 				/* avoid climbout */
-				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
+				if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
 				{
 					flare_curve_alt = pos_sp_triplet.current.alt;
 					land_stayonground = true;
@@ -935,38 +942,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
 				flare_curve_alt_last = flare_curve_alt;
-
-			} else if (wp_distance < L_wp_distance) {
-
-				/* minimize speed to approach speed, stay on landing slope */
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, flare_pitch_angle_rad,
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
-				//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
-
-				if (!land_onslope) {
-
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
-					land_onslope = true;
-				}
-
 			} else {
 
 				 /* intersect glide slope:
-				 * if current position is higher or within 10m of slope follow the glide slope
-				 * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
-				 * */
+				  * minimize speed to approach speed
+				  * if current position is higher or within 10m of slope follow the glide slope
+				  * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+				  * */
 				float altitude_desired = _global_pos.alt;
 				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
 					/* stay on slope */
 					altitude_desired = landing_slope_alt_desired;
-					//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
+					if (!land_onslope) {
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+						land_onslope = true;
+					}
 				} else {
 					/* continue horizontally */
 					altitude_desired =  math::max(_global_pos.alt, L_altitude);
-					//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
 				}
 
 				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
@@ -996,6 +989,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				} else	{
 					/* no takeoff detection --> fly */
 					launch_detected = true;
+					warnx("launchdetection off");
 				}
 			}
 
@@ -1042,19 +1036,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		// mission is active
 		_loiter_hold = false;
 
-		/* reset land state */
+		/* reset landing state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
-			land_noreturn_horizontal = false;
-			land_noreturn_vertical = false;
-			land_stayonground = false;
-			land_motor_lim = false;
-			land_onslope = false;
+			reset_landing_state();
 		}
 
 		/* reset takeoff/launch state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_TAKEOFF) {
-			launch_detected = false;
-			usePreTakeoffThrust = false;
+			reset_takeoff_state();
 		}
 
 		if (was_circle_mode && !_l1_control.circle_mode()) {
@@ -1074,13 +1063,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
 		}
 
-		/* climb out control */
-		bool climb_out = false;
+		//XXX not used
 
-		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
-			climb_out = true;
-		}
+		/* climb out control */
+//		bool climb_out = false;
+//
+//		/* user wants to climb out */
+//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+//			climb_out = true;
+//		}
 
 		/* if in seatbelt mode, set airspeed based on manual control */
 
@@ -1149,6 +1140,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
+
+		/* reset landing and takeoff state */
+		if (!last_manual) {
+			reset_landing_state();
+			reset_takeoff_state();
+		}
 	}
 
 	if (usePreTakeoffThrust) {
@@ -1159,6 +1156,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	}
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
 
+	if (_control_mode.flag_control_position_enabled) {
+		last_manual = false;
+	} else {
+		last_manual = true;
+	}
+
 
 	return setpoint;
 }
@@ -1287,7 +1290,7 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;
@@ -1309,6 +1312,22 @@ FixedwingPositionControl::task_main()
 	_exit(0);
 }
 
+void FixedwingPositionControl::reset_takeoff_state()
+{
+	launch_detected = false;
+	usePreTakeoffThrust = false;
+	launchDetector.reset();
+}
+
+void FixedwingPositionControl::reset_landing_state()
+{
+	land_noreturn_horizontal = false;
+	land_noreturn_vertical = false;
+	land_stayonground = false;
+	land_motor_lim = false;
+	land_onslope = false;
+}
+
 int
 FixedwingPositionControl::start()
 {

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -245,7 +245,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
 /**
  * Maximum vertical acceleration
  *
- * This is the maximum vertical acceleration (in metres/second^2) 
+ * This is the maximum vertical acceleration (in metres/second square)
  * either up or down that the controller will use to correct speed 
  * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
  * allows for reasonably aggressive pitch changes if required to recover 
@@ -348,13 +348,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
  */
 PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 
-/**
- * Landing slope length
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
-
 /**
  *
  *

src/modules/gpio_led/gpio_led.c

@@ -51,7 +51,6 @@
 #include <systemlib/err.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_armed.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
 #include <modules/px4iofirmware/protocol.h>
@@ -63,8 +62,6 @@ struct gpio_led_s {
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
-	struct actuator_armed_s armed;
-	int actuator_armed_sub;
 	bool led_state;
 	int counter;
 };
@@ -81,6 +78,7 @@ void gpio_led_cycle(FAR void *arg);
 int gpio_led_main(int argc, char *argv[])
 {
 	if (argc < 2) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
 		     "\t-p\tUse pin:\n"
 		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
@@ -88,7 +86,14 @@ int gpio_led_main(int argc, char *argv[])
 		     "\t\ta1\tPX4IO ACC1\n"
 		     "\t\ta2\tPX4IO ACC2\n"
 		     "\t\tr1\tPX4IO RELAY1\n"
-		     "\t\tr2\tPX4IO RELAY2");
+		     "\t\tr2\tPX4IO RELAY2"
+		    );
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		errx(1, "usage: gpio_led {start|stop} [-p <n>]\n"
+		     "\t-p <n>\tUse specified AUX OUT pin number (default: 1)"
+		    );
+#endif
 
 	} else {
 
@@ -98,37 +103,70 @@ int gpio_led_main(int argc, char *argv[])
 			}
 
 			bool use_io = false;
-			int pin = GPIO_EXT_1;
+
+			/* by default use GPIO_EXT_1 on FMUv1 and GPIO_SERVO_1 on FMUv2 */
+			int pin = 1;
+
+			/* pin name to display */
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			char *pin_name = "PX4FMU GPIO_EXT1";
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+			char pin_name[] = "AUX OUT 1";
+#endif
 
 			if (argc > 2) {
 				if (!strcmp(argv[2], "-p")) {
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+
 					if (!strcmp(argv[3], "1")) {
 						use_io = false;
 						pin = GPIO_EXT_1;
+						pin_name = "PX4FMU GPIO_EXT1";
 
 					} else if (!strcmp(argv[3], "2")) {
 						use_io = false;
 						pin = GPIO_EXT_2;
+						pin_name = "PX4FMU GPIO_EXT2";
 
 					} else if (!strcmp(argv[3], "a1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC1;
+						pin_name = "PX4IO ACC1";
 
 					} else if (!strcmp(argv[3], "a2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_ACC2;
+						pin_name = "PX4IO ACC2";
 
 					} else if (!strcmp(argv[3], "r1")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER1;
+						pin_name = "PX4IO RELAY1";
 
 					} else if (!strcmp(argv[3], "r2")) {
 						use_io = true;
 						pin = PX4IO_P_SETUP_RELAYS_POWER2;
+						pin_name = "PX4IO RELAY2";
 
 					} else {
 						errx(1, "unsupported pin: %s", argv[3]);
 					}
+
+#endif
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+					unsigned int n = strtoul(argv[3], NULL, 10);
+
+					if (n >= 1 && n <= 6) {
+						use_io = false;
+						pin = 1 << (n - 1);
+						snprintf(pin_name, sizeof(pin_name), "AUX OUT %d", n);
+
+					} else {
+						errx(1, "unsupported pin: %s", argv[3]);
+					}
+
+#endif
 				}
 			}
 
@@ -142,21 +180,6 @@ int gpio_led_main(int argc, char *argv[])
 
 			} else {
 				gpio_led_started = true;
-				char pin_name[24];
-
-				if (use_io) {
-					if (pin & (PX4IO_P_SETUP_RELAYS_ACC1 | PX4IO_P_SETUP_RELAYS_ACC2)) {
-						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
-
-					} else {
-						sprintf(pin_name, "PX4IO RELAY%i", pin);
-					}
-
-				} else {
-					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
-
-				}
-
 				warnx("start, using pin: %s", pin_name);
 			}
 
@@ -186,6 +209,7 @@ void gpio_led_start(FAR void *arg)
 
 	if (priv->use_io) {
 		gpio_dev = PX4IO_DEVICE_PATH;
+
 	} else {
 		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
@@ -204,8 +228,10 @@ void gpio_led_start(FAR void *arg)
 	/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
-	/* subscribe to vehicle status topic */
+	/* initialize vehicle status structure */
 	memset(&priv->status, 0, sizeof(priv->status));
+
+	/* subscribe to vehicle status topic */
 	priv->vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
 	/* add worker to queue */
@@ -224,38 +250,33 @@ void gpio_led_cycle(FAR void *arg)
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
 	/* check for status updates*/
-	bool status_updated;
-	orb_check(priv->vehicle_status_sub, &status_updated);
+	bool updated;
+	orb_check(priv->vehicle_status_sub, &updated);
 
-	if (status_updated)
+	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
-
-	orb_check(priv->vehicle_status_sub, &status_updated);
-
-	if (status_updated)
-		orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed);
+	}
 
 	/* select pattern for current status */
 	int pattern = 0;
 
-	if (priv->armed.armed) {
-		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
+	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
+		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)
+
+	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
+		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) {
 			pattern = 0x3f;	// ****** solid (armed)
 
 		} else {
-			pattern = 0x2A;	// *_*_*_ fast blink (armed, battery warning)
+			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)
 		}
 
-	} else {
-		if (priv->armed.ready_to_arm) {
-			pattern = 0x00;	// ______ off (disarmed, preflight check)
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
+		pattern = 0x38;	// ***___ slow blink (disarmed, ready)
 
-		} else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) {
-			pattern = 0x38;	// ***___ slow blink (disarmed, ready)
+	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
+		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)
 
-		} else {
-			pattern = 0x28;	// *_*___ slow double blink (disarmed, not good to arm)
-		}
 	}
 
 	/* blink pattern */
@@ -266,6 +287,7 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
+
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}
@@ -273,8 +295,9 @@ void gpio_led_cycle(FAR void *arg)
 
 	priv->counter++;
 
-	if (priv->counter > 5)
+	if (priv->counter > 5) {
 		priv->counter = 0;
+	}
 
 	/* repeat cycle at 5 Hz */
 	if (gpio_led_started) {

src/modules/mavlink/mavlink.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 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
@@ -34,46 +33,18 @@
 
 /**
  * @file mavlink.c
- * MAVLink 1.0 protocol implementation.
+ * Adapter functions expected by the protocol library
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>
 #include <unistd.h>
-#include <pthread.h>
 #include <stdio.h>
-#include <math.h>
 #include <stdbool.h>
-#include <fcntl.h>
-#include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <drivers/drv_hrt.h>
-#include <time.h>
-#include <float.h>
-#include <unistd.h>
-#include <nuttx/sched.h>
-#include <sys/prctl.h>
-#include <termios.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <poll.h>
-
 #include <systemlib/param/param.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/err.h>
-#include <mavlink/mavlink_log.h>
-#include <commander/px4_custom_mode.h>
-
-#include "waypoints.h"
-#include "orb_topics.h"
-#include "mavlink_hil.h"
-#include "util.h"
-#include "waypoints.h"
-#include "mavlink_parameters.h"
-
-#include <uORB/topics/mission_result.h>
 
 /* define MAVLink specific parameters */
 /**
@@ -92,22 +63,6 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
  */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 
-__EXPORT int mavlink_main(int argc, char *argv[]);
-
-static int mavlink_thread_main(int argc, char *argv[]);
-
-/* thread state */
-volatile bool thread_should_exit = false;
-static volatile bool thread_running = false;
-static int mavlink_task;
-
-/* pthreads */
-static pthread_t receive_thread;
-static pthread_t uorb_receive_thread;
-
-/* terminate MAVLink on user request - disabled by default */
-static bool mavlink_link_termination_allowed = false;
-
 mavlink_system_t mavlink_system = {
 	100,
 	50,
@@ -117,365 +72,6 @@ mavlink_system_t mavlink_system = {
 	0
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
-/* XXX not widely used */
-uint8_t chan = MAVLINK_COMM_0;
-
-/* XXX probably should be in a header... */
-extern pthread_t receive_start(int uart);
-
-/* Allocate storage space for waypoints */
-static mavlink_wpm_storage wpm_s;
-mavlink_wpm_storage *wpm = &wpm_s;
-
-bool mavlink_hil_enabled = false;
-
-/* protocol interface */
-static int uart;
-static int baudrate;
-bool gcs_link = true;
-
-/* interface mode */
-static enum {
-	MAVLINK_INTERFACE_MODE_OFFBOARD,
-	MAVLINK_INTERFACE_MODE_ONBOARD
-} mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD;
-
-static struct mavlink_logbuffer lb;
-
-static void mavlink_update_system(void);
-static int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb);
-static void usage(void);
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval);
-
-
-
-int
-set_hil_on_off(bool hil_enabled)
-{
-	int ret = OK;
-
-	/* Enable HIL */
-	if (hil_enabled && !mavlink_hil_enabled) {
-
-		mavlink_hil_enabled = true;
-
-		/* ramp up some HIL-related subscriptions */
-		unsigned hil_rate_interval;
-
-		if (baudrate < 19200) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 38400) {
-			/* 10 Hz */
-			hil_rate_interval = 100;
-
-		} else if (baudrate < 115200) {
-			/* 20 Hz */
-			hil_rate_interval = 50;
-
-		} else {
-			/* 200 Hz */
-			hil_rate_interval = 5;
-		}
-
-		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, hil_rate_interval);
-	}
-
-	if (!hil_enabled && mavlink_hil_enabled) {
-		mavlink_hil_enabled = false;
-		orb_set_interval(mavlink_subs.spa_sub, 200);
-
-	} else {
-		ret = ERROR;
-	}
-
-	return ret;
-}
-
-void
-get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode)
-{
-	/* reset MAVLink mode bitfield */
-	*mavlink_base_mode = 0;
-	*mavlink_custom_mode = 0;
-
-	/**
-	 * Set mode flags
-	 **/
-
-	/* HIL */
-	if (v_status.hil_state == HIL_STATE_ON) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
-	}
-
-	/* arming state */
-	if (armed.armed) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
-	}
-
-	/* main state */
-	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
-	union px4_custom_mode custom_mode;
-	custom_mode.data = 0;
-	if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
-		/* use main state when navigator is not active */
-		if (v_status.main_state == MAIN_STATE_MANUAL) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-		} else if (v_status.main_state == MAIN_STATE_EASY) {
-			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-		} else if (v_status.main_state == MAIN_STATE_AUTO) {
-			/* this must not happen */
-			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (v_status.main_state == MAIN_STATE_OFFBOARD) {
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
-		}
-	} else {
-		/* use navigation state when navigator is active */
-		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
-		} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
-		}
-	}
-	*mavlink_custom_mode = custom_mode.data;
-
-	/**
-	 * Set mavlink state
-	 **/
-
-	/* set calibration state */
-	if (v_status.arming_state == ARMING_STATE_INIT
-			|| v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE
-			|| v_status.arming_state == ARMING_STATE_STANDBY_ERROR) {	// TODO review
-		*mavlink_state = MAV_STATE_UNINIT;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED) {
-		*mavlink_state = MAV_STATE_ACTIVE;
-	} else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
-		*mavlink_state = MAV_STATE_CRITICAL;
-	} else if (v_status.arming_state == ARMING_STATE_STANDBY) {
-		*mavlink_state = MAV_STATE_STANDBY;
-	} else if (v_status.arming_state == ARMING_STATE_REBOOT) {
-		*mavlink_state = MAV_STATE_POWEROFF;
-	} else {
-		warnx("Unknown mavlink state");
-		*mavlink_state = MAV_STATE_CRITICAL;
-	}
-}
-
-
-int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval)
-{
-	int ret = OK;
-
-	switch (mavlink_msg_id) {
-	case MAVLINK_MSG_ID_SCALED_IMU:
-		/* sensor sub triggers scaled IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_HIGHRES_IMU:
-		/* sensor sub triggers highres IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_RAW_IMU:
-		/* sensor sub triggers RAW IMU */
-		orb_set_interval(subs->sensor_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_ATTITUDE:
-		/* attitude sub triggers attitude */
-		orb_set_interval(subs->att_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
-		/* actuator_outputs triggers this message */
-		orb_set_interval(subs->act_0_sub, min_interval);
-		orb_set_interval(subs->act_1_sub, min_interval);
-		orb_set_interval(subs->act_2_sub, min_interval);
-		orb_set_interval(subs->act_3_sub, min_interval);
-		orb_set_interval(subs->actuators_sub, min_interval);
-		orb_set_interval(subs->actuators_effective_sub, min_interval);
-		orb_set_interval(subs->spa_sub, min_interval);
-		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_MANUAL_CONTROL:
-		/* manual_control_setpoint triggers this message */
-		orb_set_interval(subs->man_control_sp_sub, min_interval);
-		break;
-
-	case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
-		orb_set_interval(subs->debug_key_value, min_interval);
-		break;
-
-	default:
-		/* not found */
-		ret = ERROR;
-		break;
-	}
-
-	return ret;
-}
-
-
-/****************************************************************************
- * MAVLink text message logger
- ****************************************************************************/
-
-static int	mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg);
-
-static const struct file_operations mavlink_fops = {
-	.ioctl = mavlink_dev_ioctl
-};
-
-static int
-mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg)
-{
-	static unsigned int total_counter = 0;
-
-	switch (cmd) {
-	case (int)MAVLINK_IOC_SEND_TEXT_INFO:
-	case (int)MAVLINK_IOC_SEND_TEXT_CRITICAL:
-	case (int)MAVLINK_IOC_SEND_TEXT_EMERGENCY: {
-			const char *txt = (const char *)arg;
-			struct mavlink_logmessage msg;
-			strncpy(msg.text, txt, sizeof(msg.text));
-			mavlink_logbuffer_write(&lb, &msg);
-			total_counter++;
-			return OK;
-		}
-
-	default:
-		return ENOTTY;
-	}
-}
-
-#define MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED 0x10
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb)
-{
-	/* process baud rate */
-	int speed;
-
-	switch (baud) {
-	case 0:      speed = B0;      break;
-
-	case 50:     speed = B50;     break;
-
-	case 75:     speed = B75;     break;
-
-	case 110:    speed = B110;    break;
-
-	case 134:    speed = B134;    break;
-
-	case 150:    speed = B150;    break;
-
-	case 200:    speed = B200;    break;
-
-	case 300:    speed = B300;    break;
-
-	case 600:    speed = B600;    break;
-
-	case 1200:   speed = B1200;   break;
-
-	case 1800:   speed = B1800;   break;
-
-	case 2400:   speed = B2400;   break;
-
-	case 4800:   speed = B4800;   break;
-
-	case 9600:   speed = B9600;   break;
-
-	case 19200:  speed = B19200;  break;
-
-	case 38400:  speed = B38400;  break;
-
-	case 57600:  speed = B57600;  break;
-
-	case 115200: speed = B115200; break;
-
-	case 230400: speed = B230400; break;
-
-	case 460800: speed = B460800; break;
-
-	case 921600: speed = B921600; break;
-
-	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud);
-		return -EINVAL;
-	}
-
-	/* open uart */
-	warnx("UART is %s, baudrate is %d\n", uart_name, baud);
-	uart = open(uart_name, O_RDWR | O_NOCTTY);
-
-	/* Try to set baud rate */
-	struct termios uart_config;
-	int termios_state;
-	*is_usb = false;
-
-	/* Back up the original uart configuration to restore it after exit */
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
-		close(uart);
-		return -1;
-	}
-
-	/* Fill the struct for the new configuration */
-	tcgetattr(uart, &uart_config);
-
-	/* Clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-
-	/* USB serial is indicated by /dev/ttyACM0*/
-	if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/ttyACM1") != OK) {
-
-		/* Set baud rate */
-		if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-			warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
-			close(uart);
-			return -1;
-		}
-
-	}
-
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
-		close(uart);
-		return -1;
-	}
-
-	return uart;
-}
-
-void
-mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
-{
-	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
-}
-
 /*
  * Internal function to give access to the channel status for each channel
  */
@@ -493,362 +89,3 @@ extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_buffer[channel];
 }
-
-void mavlink_update_system(void)
-{
-	static bool initialized = false;
-	static param_t param_system_id;
-	static param_t param_component_id;
-	static param_t param_system_type;
-
-	if (!initialized) {
-		param_system_id = param_find("MAV_SYS_ID");
-		param_component_id = param_find("MAV_COMP_ID");
-		param_system_type = param_find("MAV_TYPE");
-		initialized = true;
-	}
-
-	/* update system and component id */
-	int32_t system_id;
-	param_get(param_system_id, &system_id);
-
-	if (system_id > 0 && system_id < 255) {
-		mavlink_system.sysid = system_id;
-	}
-
-	int32_t component_id;
-	param_get(param_component_id, &component_id);
-
-	if (component_id > 0 && component_id < 255) {
-		mavlink_system.compid = component_id;
-	}
-
-	int32_t system_type;
-	param_get(param_system_type, &system_type);
-
-	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
-	}
-}
-
-/**
- * MAVLink Protocol main function.
- */
-int mavlink_thread_main(int argc, char *argv[])
-{
-	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 10);
-
-	int ch;
-	char *device_name = "/dev/ttyS1";
-	baudrate = 57600;
-
-	/* work around some stupidity in task_create's argv handling */
-	argc -= 2;
-	argv += 2;
-
-	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
-		switch (ch) {
-		case 'b':
-			baudrate = strtoul(optarg, NULL, 10);
-
-			if (baudrate < 9600 || baudrate > 921600)
-				errx(1, "invalid baud rate '%s'", optarg);
-
-			break;
-
-		case 'd':
-			device_name = optarg;
-			break;
-
-		case 'e':
-			mavlink_link_termination_allowed = true;
-			break;
-
-		case 'o':
-			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
-			break;
-
-		default:
-			usage();
-			break;
-		}
-	}
-
-	struct termios uart_config_original;
-
-	bool usb_uart;
-
-	/* print welcome text */
-	warnx("MAVLink v1.0 serial interface starting...");
-
-	/* inform about mode */
-	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");
-
-	/* Flush stdout in case MAVLink is about to take it over */
-	fflush(stdout);
-
-	/* default values for arguments */
-	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);
-
-	if (uart < 0)
-		err(1, "could not open %s", device_name);
-
-	/* create the device node that's used for sending text log messages, etc. */
-	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);
-
-	/* Initialize system properties */
-	mavlink_update_system();
-
-	/* start the MAVLink receiver */
-	receive_thread = receive_start(uart);
-
-	/* start the ORB receiver */
-	uorb_receive_thread = uorb_receive_start();
-
-	/* initialize waypoint manager */
-	mavlink_wpm_init(wpm);
-
-	/* all subscriptions are now active, set up initial guess about rate limits */
-	if (baudrate >= 230400) {
-		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
-		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
-		/* 10 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
-
-	} else if (baudrate >= 115200) {
-		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-
-	} else if (baudrate >= 57600) {
-		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
-		/* 10 Hz / 100 ms ATTITUDE */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
-		/* 5 Hz / 200 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
-		/* 2 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);
-
-	} else {
-		/* very low baud rate, limit to 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
-		/* 1 Hz / 1000 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
-		/* 0.5 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
-		/* 0.1 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
-	}
-
-	int mission_result_sub = orb_subscribe(ORB_ID(mission_result));
-	struct mission_result_s mission_result;
-	memset(&mission_result, 0, sizeof(mission_result));
-
-	thread_running = true;
-
-	/* arm counter to go off immediately */
-	unsigned lowspeed_counter = 10;
-
-	while (!thread_should_exit) {
-
-		/* 1 Hz */
-		if (lowspeed_counter == 10) {
-			mavlink_update_system();
-
-			/* translate the current system state to mavlink state and mode */
-			uint8_t mavlink_state = 0;
-			uint8_t mavlink_base_mode = 0;
-			uint32_t mavlink_custom_mode = 0;
-			get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-			/* send heartbeat */
-			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state);
-
-			/* switch HIL mode if required */
-			if (v_status.hil_state == HIL_STATE_ON)
-				set_hil_on_off(true);
-			else if (v_status.hil_state == HIL_STATE_OFF)
-				set_hil_on_off(false);
-
-			/* send status (values already copied in the section above) */
-			mavlink_msg_sys_status_send(chan,
-						    v_status.onboard_control_sensors_present,
-						    v_status.onboard_control_sensors_enabled,
-						    v_status.onboard_control_sensors_health,
-						    v_status.load * 1000.0f,
-						    v_status.battery_voltage * 1000.0f,
-						    v_status.battery_current * 100.0f,
-						    v_status.battery_remaining * 100.0f,
-						    v_status.drop_rate_comm,
-						    v_status.errors_comm,
-						    v_status.errors_count1,
-						    v_status.errors_count2,
-						    v_status.errors_count3,
-						    v_status.errors_count4);
-			lowspeed_counter = 0;
-		}
-
-		lowspeed_counter++;
-
-		bool updated;
-		orb_check(mission_result_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result);
-
-			if (mission_result.mission_reached) {
-				mavlink_wpm_send_waypoint_reached((uint16_t)mission_result.mission_index);
-			}
-		}
-
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* check if waypoint has been reached against the last positions */
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		/* send parameters at 20 Hz (if queued for sending) */
-		mavlink_pm_queued_send();
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		/* sleep quarter the time */
-		usleep(25000);
-
-		mavlink_waypoint_eventloop(hrt_absolute_time());
-
-		if (baudrate > 57600) {
-			mavlink_pm_queued_send();
-		}
-
-		/* sleep 10 ms */
-		usleep(10000);
-
-		/* send one string at 10 Hz */
-		if (!mavlink_logbuffer_is_empty(&lb)) {
-			struct mavlink_logmessage msg;
-			int lb_ret = mavlink_logbuffer_read(&lb, &msg);
-
-			if (lb_ret == OK) {
-				mavlink_missionlib_send_gcs_string(msg.text);
-			}
-		}
-
-		/* sleep 15 ms */
-		usleep(15000);
-	}
-
-	/* wait for threads to complete */
-	pthread_join(receive_thread, NULL);
-	pthread_join(uorb_receive_thread, NULL);
-
-	/* Reset the UART flags to original state */
-	tcsetattr(uart, TCSANOW, &uart_config_original);
-
-	/* destroy log buffer */
-	//mavlink_logbuffer_destroy(&lb);
-
-	thread_running = false;
-
-	return 0;
-}
-
-static void
-usage()
-{
-	fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n"
-		"       mavlink stop\n"
-		"       mavlink status\n");
-	exit(1);
-}
-
-int mavlink_main(int argc, char *argv[])
-{
-
-	if (argc < 2) {
-		warnx("missing command");
-		usage();
-	}
-
-	if (!strcmp(argv[1], "start")) {
-
-		/* this is not an error */
-		if (thread_running)
-			errx(0, "mavlink already running");
-
-		thread_should_exit = false;
-		mavlink_task = task_spawn_cmd("mavlink",
-					  SCHED_DEFAULT,
-					  SCHED_PRIORITY_DEFAULT,
-					  2048,
-					  mavlink_thread_main,
-					  (const char **)argv);
-
-		while (!thread_running) {
-			usleep(200);
-		}
-
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop")) {
-
-		/* this is not an error */
-		if (!thread_running)
-			errx(0, "mavlink already stopped");
-
-		thread_should_exit = true;
-
-		while (thread_running) {
-			usleep(200000);
-			warnx(".");
-		}
-
-		warnx("terminated.");
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status")) {
-		if (thread_running) {
-			errx(0, "running");
-
-		} else {
-			errx(1, "not running");
-		}
-	}
-
-	warnx("unrecognized command");
-	usage();
-	/* not getting here */
-	return 0;
-}
-

src/modules/mavlink/mavlink_bridge_header.h

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 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
@@ -43,6 +42,8 @@
 #ifndef MAVLINK_BRIDGE_HEADER_H
 #define MAVLINK_BRIDGE_HEADER_H
 
+__BEGIN_DECLS
+
 #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
 
 /* use efficient approach, see mavlink_helpers.h */
@@ -73,11 +74,13 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
+void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
 extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
 #include <v1.0/common/mavlink.h>
 
+__END_DECLS
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */