SYS_FMU_TASK: add param to start fmu as task (default=work queue)

ROMFS/px4fmu_common/init.d/rc.interface

@@ -157,7 +157,7 @@ then
 
 	if [ $MIXER_AUX_FILE != none ]
 	then
-		if fmu mode_${AUX_MODE}
+		if fmu mode_${AUX_MODE} $FMU_ARGS
 		then
 			# Append aux mixer to main device
 			if [ $OUTPUT_MODE == hil ]

ROMFS/px4fmu_common/init.d/rcS

@@ -190,6 +190,7 @@ then
 	set MK_MODE none
 	set FMU_MODE pwm
 	set AUX_MODE pwm
+	set FMU_ARGS ""
 	set MAVLINK_F default
 	set MAVLINK_COMPANION_DEVICE /dev/ttyS2
 	set EXIT_ON_END no
@@ -198,6 +199,11 @@ then
 	set USE_IO yes
 	set LOGGER_BUF 16
 
+	if param compare SYS_FMU_TASK 1
+	then
+		set FMU_ARGS "-t"
+	fi
+
 	#
 	# Set USE_IO flag
 	#
@@ -535,7 +541,7 @@ then
 
 		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
-			if fmu mode_$FMU_MODE
+			if fmu mode_$FMU_MODE $FMU_ARGS
 			then
 			else
 				echo "FMU start failed" >> $LOG_FILE
@@ -604,7 +610,7 @@ then
 		else
 			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
 			then
-				if fmu mode_${FMU_MODE}
+				if fmu mode_${FMU_MODE} $FMU_ARGS
 				then
 				else
 					echo "FMU mode_${FMU_MODE} start failed" >> $LOG_FILE
@@ -1034,7 +1040,7 @@ then
 		then
 			# On Pixracer use Telem 2 port (TL2).
 			snapdragon_rc_pwm start -d /dev/ttyS2
-			fmu mode_pwm4
+			fmu mode_pwm4 $FMU_ARGS
 		fi
 
 		pwm failsafe -c 1234 -p 900

src/modules/systemlib/system_params.c

@@ -72,10 +72,27 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
  * @boolean
  * @min 0
  * @max 1
+ * @reboot_required true
  * @group System
  */
 PARAM_DEFINE_INT32(SYS_USE_IO, 1);
 
+/**
+ * Run the FMU as a task to reduce latency
+ *
+ * If true, the FMU will run in a separate task instead of on the work queue.
+ * Set this if low latency is required, for example for racing.
+ *
+ * This is a trade-off between RAM usage and latency: running as a task, it
+ * requires a separate stack and directly polls on the control topics, whereas
+ * running on the work queue, it runs at a fixed update rate.
+ *
+ * @boolean
+ * @reboot_required true
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_FMU_TASK, 0);
+
 /**
  * Set restart type
  *