Merged master in yaw_acceptance_fix

21 files changed, 375 insertions, 130 deletions

diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
index 26bea688a..084dff140 100644
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -18,9 +18,9 @@ then
 	param set MC_PITCHRATE_P 0.13
 	param set MC_PITCHRATE_I 0.0
 	param set MC_PITCHRATE_D 0.004
-	param set MC_YAW_P 0.5
-	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.1
+	param set MC_YAW_P 2.5
+	param set MC_YAWRATE_P 0.25
+	param set MC_YAWRATE_I 0.25
 	param set MC_YAWRATE_D 0.0
 
 	param set BAT_V_SCALING 0.00989
diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
new file mode 100644
index 000000000..6179855f6
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
@@ -0,0 +1,35 @@
+#!nsh
+#
+# Steadidrone QU4D
+#
+# Thomas Gubler <thomasgubler@gmail.com>
+# Lorenz Meier <lm@inf.ethz.ch>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	# TODO tune roll/pitch separately
+	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 7.0
+	param set MC_PITCHRATE_P 0.13
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.004
+	param set MC_YAW_P 0.5
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_D 0.0
+
+	param set BAT_N_CELLS 4
+fi
+
+set MIXER FMU_quad_w
+
+set PWM_MIN 1210
+set PWM_MAX 2100
+
+set PWM_OUTPUTS 1234
diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
index 38e65435b..daa04a4de 100644
--- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -1,7 +1,5 @@
 #!nsh
 #
-# UNTESTED UNTESTED!
-#
 # Generic 10" Hexa coaxial geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
@@ -9,7 +7,7 @@
 
 sh /etc/init.d/rc.mc_defaults
 
-set MIXER hexa_cox
+set MIXER FMU_hexa_cox
 
 # We only can run one channel group with one rate, so set all 8 channels
 set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/12001_octo_cox b/ROMFS/px4fmu_common/init.d/12001_octo_cox
index 99f902a6d..8703f5f2f 100644
--- a/ROMFS/px4fmu_common/init.d/12001_octo_cox
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -7,6 +7,6 @@
 
 sh /etc/init.d/rc.mc_defaults
 
-set MIXER octo_cox
+set MIXER FMU_octo_cox
 
 set PWM_OUTPUTS 12345678
diff --git a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
index 465a22c53..3ab2ac3d1 100644
--- a/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/2100_mpx_easystar
@@ -5,4 +5,4 @@
 
 sh /etc/init.d/rc.fw_defaults
  
-set MIXER FMU_RET
+set MIXER easystar
diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom
index 6cbd23643..d05c3174f 100644
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -10,8 +10,8 @@ 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_AIRSPD_TRIM 15
+	param set FW_AIRSPD_MAX 25
 	param set FW_ATT_TC 0.3
 	param set FW_L1_DAMPING 0.75
 	param set FW_L1_PERIOD 15
@@ -23,12 +23,12 @@ then
 	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_P_ROLLFF 1
 	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_LIM 50
 	param set FW_R_RMAX 0
 	param set FW_T_HRATE_P 0.01
 	param set FW_T_RLL2THR 15
@@ -37,3 +37,7 @@ then
 fi
 
 set MIXER FMU_Q
+
+# Provide ESC a constant 1000 us pulse
+set PWM_OUTPUTS 4
+set PWM_DISARMED 1000
diff --git a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
index bf5a87068..465166f25 100644
--- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -23,7 +23,7 @@ then
 	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_P_ROLLFF 1
 	param set FW_RR_FF 0.3
 	param set FW_RR_I 0
 	param set FW_RR_IMAX 0.2
diff --git a/ROMFS/px4fmu_common/init.d/4008_ardrone b/ROMFS/px4fmu_common/init.d/4008_ardrone
index 0c3cd09dd..e6007db0e 100644
--- a/ROMFS/px4fmu_common/init.d/4008_ardrone
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -14,19 +14,20 @@ sh /etc/init.d/rc.mc_defaults
 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.05
+    param set MC_ROLL_P 6.0
+    param set MC_ROLLRATE_P 0.14
+    param set MC_ROLLRATE_I 0.1
+    param set MC_ROLLRATE_D 0.002
+    param set MC_PITCH_P 6.0
+    param set MC_PITCHRATE_P 0.14
+    param set MC_PITCHRATE_I 0.1
+    param set MC_PITCHRATE_D 0.002
+    param set MC_YAW_P 2.0
+    param set MC_YAWRATE_P 0.2
+    param set MC_YAWRATE_I 0.2
     param set MC_YAWRATE_D 0.0
-    param set MC_YAW_FF 0.15
+    param set MC_YAW_FF 0.8
+    
     param set BAT_V_SCALING 0.00838095238
 fi
 
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 5ec735d39..b365bd642 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -195,6 +195,11 @@ then
 	sh /etc/init.d/10016_3dr_iris
 fi
 
+if param compare SYS_AUTOSTART 10017
+then
+	sh /etc/init.d/10017_steadidrone_qu4d
+fi
+
 #
 # Hexa Coaxial
 #
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps
index 429abc5ec..9aca3fc5f 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps
@@ -6,7 +6,7 @@
 #
 # Start the attitude and position estimator
 #
-fw_att_pos_estimator start
+ekf_att_pos_estimator start
 
 #
 # Start attitude controller
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index 4cd73e23f..3a50fcf56 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -11,6 +11,4 @@ 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
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io
index 6e8fdbc0f..e23aebd87 100644
--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -11,7 +11,7 @@ px4io recovery
 # Adjust PX4IO update rate limit
 #
 set PX4IO_LIMIT 400
-if hw_ver compare PX4FMU_V1
+if ver hwcmp PX4FMU_V1
 then
 	set PX4IO_LIMIT 200
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index c5aef8273..25f31a7e0 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -5,12 +5,12 @@
 
 if [ -d /fs/microsd ]
 then
-	if hw_ver compare PX4FMU_V1
+	if ver hwcmp PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 8 -t
+		sdlog2 start -r 50 -a -b 4 -t
 	else
 		echo "Start sdlog2 at 200Hz"
-		sdlog2 start -r 200 -a -b 16 -t
+		sdlog2 start -r 200 -a -b 22 -t
 	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps
index ed3939757..268eb9bba 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_apps
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps
@@ -5,6 +5,7 @@
 #
 
 attitude_estimator_ekf start
+#ekf_att_pos_estimator start
 position_estimator_inav start
 
 mc_att_control start
diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
index e96c5b493..0df320f49 100644
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -32,9 +32,16 @@ then
 	param set MPC_Z_VEL_D 0.0
 	param set MPC_Z_VEL_MAX 3
 	param set MPC_Z_FF 0.5
-	param set MPC_TILT_MAX 1.0
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
 	param set MPC_LAND_SPEED 1.0
-	param set MPC_LAND_TILT 0.3
+
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.7
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.0
+
+	param set NAV_ACCEPT_RAD 2.0
 fi
 
 set PWM_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 235be2431..1e14930fe 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -22,7 +22,7 @@ then
 	echo "[init] Using L3GD20(H)"
 fi
 
-if hw_ver compare PX4FMU_V2
+if ver hwcmp PX4FMU_V2
 then
 	if lsm303d start
 	then
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb
index b7b556945..1f8d8b862 100644
--- a/ROMFS/px4fmu_common/init.d/rc.usb
+++ b/ROMFS/px4fmu_common/init.d/rc.usb
@@ -3,16 +3,22 @@
 # USB MAVLink start
 #
 
-echo "Starting MAVLink on this USB console"
-
-mavlink start -r 10000 -d /dev/ttyACM0
+mavlink start -r 10000 -d /dev/ttyACM0 -x
 # Enable a number of interesting streams we want via USB
 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW -r 10
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
+usleep 100000
 mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30
-mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 10
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
+usleep 100000
+mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20
+usleep 100000
 
 # Exit shell to make it available to MAVLink
 exit
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 503dbb83e..f70e0ed77 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -20,7 +20,7 @@ echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	set LOG_FILE /fs/microsd/bootlog.txt
-	echo "[init] microSD card mounted at /fs/microsd"
+	echo "[init] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
@@ -65,12 +65,12 @@ then
 	# Start CDC/ACM serial driver
 	#
 	sercon
-	
+
 	#
 	# Start the ORB (first app to start)
 	#
 	uorb start
-	
+
 	#
 	# Load parameters
 	#
@@ -79,29 +79,33 @@ then
 	then
 		set PARAM_FILE /fs/mtd_params
 	fi
-	
+
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[init] Parameters loaded: $PARAM_FILE"
+		echo "[init] Params loaded: $PARAM_FILE"
 	else
-		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
+		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
 	fi
-	
+
 	#
 	# Start system state indicator
 	#
 	if rgbled start
 	then
-		echo "[init] Using external RGB Led"
+		echo "[init] RGB Led"
 	else
 		if blinkm start
 		then
-			echo "[init] Using blinkm"
+			echo "[init] BlinkM"
 			blinkm systemstate
 		fi
 	fi
-	
+
+	if pca8574 start
+	then
+	fi
+
 	#
 	# Set default values
 	#
@@ -120,7 +124,9 @@ then
 	set EXIT_ON_END no
 	set MAV_TYPE none
 	set LOAD_DEFAULT_APPS yes
-	
+	set GPS yes
+	set GPS_FAKE no
+
 	#
 	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
 	#
@@ -130,7 +136,7 @@ then
 	else
 		set DO_AUTOCONFIG no
 	fi
-	
+
 	#
 	# Set USE_IO flag
 	#
@@ -140,13 +146,13 @@ then
 	else
 		set USE_IO no
 	fi
-		
+
 	#
 	# Set parameters and env variables for selected AUTOSTART
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[init] Don't try to find autostart script"
+		echo "[init] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -156,10 +162,10 @@ then
 	#
 	if [ -f $CONFIG_FILE ]
 	then
-		echo "[init] Reading config: $CONFIG_FILE"
+		echo "[init] Config: $CONFIG_FILE"
 		sh $CONFIG_FILE
 	else
-		echo "[init] Config file not found: $CONFIG_FILE"
+		echo "[init] Config not found: $CONFIG_FILE"
 	fi
 
 	#
@@ -170,9 +176,9 @@ then
 		param set SYS_AUTOCONFIG 0
 		param save
 	fi
-	
+
 	set IO_PRESENT no
-	
+
 	if [ $USE_IO == yes ]
 	then
 		#
@@ -184,19 +190,19 @@ then
 		else
 			set IO_FILE /etc/extras/px4io-v1_default.bin
 		fi
-		
+
 		if px4io checkcrc $IO_FILE
 		then
 			echo "[init] PX4IO CRC OK"
 			echo "PX4IO CRC OK" >> $LOG_FILE
-	
+
 			set IO_PRESENT yes
 		else
 			echo "[init] Trying to update"
 			echo "PX4IO Trying to update" >> $LOG_FILE
-			
+
 			tone_alarm MLL32CP8MB
-			
+
 			if px4io forceupdate 14662 $IO_FILE
 			then
 				usleep 500000
@@ -205,7 +211,7 @@ then
 					echo "[init] PX4IO CRC OK, update successful"
 					echo "PX4IO CRC OK after updating" >> $LOG_FILE
 					tone_alarm MLL8CDE
-	
+
 					set IO_PRESENT yes
 				else
 					echo "[init] ERROR: PX4IO update failed"
@@ -218,14 +224,14 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		if [ $IO_PRESENT == no ]
 		then
 			echo "[init] ERROR: PX4IO not found"
 			tone_alarm $TUNE_OUT_ERROR
 		fi
 	fi
-	
+
 	#
 	# Set default output if not set
 	#
@@ -244,9 +250,9 @@ then
 		# Need IO for output but it not present, disable output
 		set OUTPUT_MODE none
 		echo "[init] ERROR: PX4IO not found, disabling output"
-		
+
 		# Avoid using ttyS0 for MAVLink on FMUv1
-		if hw_ver compare PX4FMU_V1
+		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
 		fi
@@ -260,25 +266,25 @@ then
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
-		if hw_ver compare PX4FMU_V1
+		if ver hwcmp PX4FMU_V1
 		then
 			set FMU_MODE serial
 		fi
-	else
-		# Try to get an USB console if not in HIL mode
-		nshterm /dev/ttyACM0 &
 	fi
-	
+
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
 	commander start
-	
+
 	#
 	# Start primary output
 	#
 	set TTYS1_BUSY no
-	
+
 	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
 	if [ $OUTPUT_MODE != none ]
 	then
@@ -294,6 +300,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
+
 		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
 		then
 			echo "[init] Use FMU as primary output"
@@ -304,8 +311,8 @@ then
 				echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
 				tone_alarm $TUNE_OUT_ERROR
 			fi
-			
-			if hw_ver compare PX4FMU_V1
+
+			if ver hwcmp PX4FMU_V1
 			then
 				if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
 				then
@@ -317,6 +324,7 @@ then
 				fi
 			fi
 		fi
+
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			echo "[init] Use MKBLCTRL as primary output"
@@ -329,7 +337,7 @@ then
 			then
 				set MKBLCTRL_ARG "-mkmode +"
 			fi
-			
+
 			if mkblctrl $MKBLCTRL_ARG
 			then
 				echo "[init] MKBLCTRL started"
@@ -337,8 +345,9 @@ then
 				echo "[init] ERROR: MKBLCTRL start failed"
 				tone_alarm $TUNE_OUT_ERROR
 			fi
-			
-		fi		
+
+		fi
+
 		if [ $OUTPUT_MODE == hil ]
 		then
 			echo "[init] Use HIL as primary output"
@@ -350,7 +359,7 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		
+
 		#
 		# Start IO or FMU for RC PPM input if needed
 		#
@@ -377,8 +386,8 @@ then
 					echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
 					tone_alarm $TUNE_OUT_ERROR
 				fi
-	
-				if hw_ver compare PX4FMU_V1
+
+				if ver hwcmp PX4FMU_V1
 				then
 					if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
 					then
@@ -392,64 +401,50 @@ then
 			fi
 		fi
 	fi
-	
+
 	#
 	# MAVLink
 	#
-	
 	if [ $MAVLINK_FLAGS == default ]
 	then
-		if [ $HIL == yes ]
+		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
+		if [ $TTYS1_BUSY == yes ]
 		then
-			sleep 1
-			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
-			usleep 5000
+			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+			set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+
+			# Exit from nsh to free port for mavlink
+			set EXIT_ON_END yes
 		else
-			# 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
+			# Start MAVLink on default port: ttyS1
+			set MAVLINK_FLAGS "-r 1000"
 		fi
 	fi
 
 	mavlink start $MAVLINK_FLAGS
-	usleep 5000
-	
-	#
-	# Start the datamanager
-	#
-	dataman start
-	
-	#
-	# Start the navigator
-	#
-	navigator start
-	
+
 	#
 	# Sensors, Logging, GPS
 	#
-	echo "[init] Start sensors"
 	sh /etc/init.d/rc.sensors
 
-	if [ $HIL == no ]
+	#
+	# Start logging in all modes, including HIL
+	#
+	sh /etc/init.d/rc.logging
+
+	if [ $GPS == yes ]
 	then
-		echo "[init] Start logging"
-		sh /etc/init.d/rc.logging
-		
 		echo "[init] Start GPS"
-		gps start
+		if [ $GPS_FAKE == yes ]
+		then
+			echo "[init] Faking GPS"
+			gps start -f
+		else
+			gps start
+		fi
 	fi
-	
+
 	#
 	# Start up ARDrone Motor interface
 	#
@@ -464,24 +459,24 @@ then
 	if [ $VEHICLE_TYPE == fw ]
 	then
 		echo "[init] Vehicle type: FIXED WING"
-		
+
 		if [ $MIXER == none ]
 		then
 			# Set default mixer for fixed wing if not defined
 			set MIXER FMU_AERT
 		fi
-		
+
 		if [ $MAV_TYPE == none ]
 		then
 			# Use MAV_TYPE = 1 (fixed wing) if not defined
 			set MAV_TYPE 1
 		fi
-		
+
 		param set MAV_TYPE $MAV_TYPE
-		
+
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
-		
+
 		# Start standard fixedwing apps
 		if [ $LOAD_DEFAULT_APPS == yes ]
 		then
@@ -516,7 +511,7 @@ then
 			then
 				set MAV_TYPE 13
 			fi
-			if [ $MIXER == hexa_cox ]
+			if [ $MIXER == FMU_hexa_cox ]
 			then
 				set MAV_TYPE 13
 			fi
@@ -529,7 +524,7 @@ then
 				set MAV_TYPE 14
 			fi
 		fi
-		
+
 		# Still no MAV_TYPE found
 		if [ $MAV_TYPE == none ]
 		then
@@ -537,10 +532,10 @@ then
 		else
 			param set MAV_TYPE $MAV_TYPE
 		fi
-		
+
 		# Load mixer and configure outputs
 		sh /etc/init.d/rc.interface
-		
+
 		# Start standard multicopter apps
 		if [ $LOAD_DEFAULT_APPS == yes ]
 		then
@@ -549,6 +544,16 @@ then
 	fi
 
 	#
+	# Start the datamanager
+	#
+	dataman start
+
+	#
+	# Start the navigator
+	#
+	navigator start
+
+	#
 	# Generic setup (autostart ID not found)
 	#
 	if [ $VEHICLE_TYPE == none ]
@@ -563,7 +568,7 @@ then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
 	else
-		echo "[init] Addons script not found: $EXTRAS_FILE"
+		echo "[init] No addons script: $EXTRAS_FILE"
 	fi
 
 	if [ $EXIT_ON_END == yes ]
diff --git a/ROMFS/px4fmu_common/mixers/hexa_cox.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix
index 497786feb..497786feb 100644
--- a/ROMFS/px4fmu_common/mixers/hexa_cox.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hexa_cox.mix
diff --git a/ROMFS/px4fmu_common/mixers/README b/ROMFS/px4fmu_common/mixers/README
new file mode 100644
index 000000000..60311232e
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/README
@@ -0,0 +1,154 @@
+PX4 mixer definitions
+=====================
+
+Files in this directory implement example mixers that can be used as a basis
+for customisation, or for general testing purposes.
+
+Mixer basics
+------------
+
+Mixers combine control values from various sources (control tasks, user inputs,
+etc.) and produce output values suitable for controlling actuators; servos,
+motors, switches and so on.
+
+An actuator derives its value from the combination of one or more control
+values. Each of the control values is scaled according to the actuator's
+configuration and then combined to produce the actuator value, which may then be
+further scaled to suit the specific output type.
+
+Internally, all scaling is performed using floating point values. Inputs and
+outputs are clamped to the range -1.0 to 1.0.
+
+control    control   control
+   |          |         |
+   v          v         v
+ scale      scale     scale
+   |          |         |
+   |          v         |
+   +-------> mix <------+
+              |
+            scale
+              |
+              v
+             out
+
+Scaling
+-------
+
+Basic scalers provide linear scaling of the input to the output.
+
+Each scaler allows the input value to be scaled independently for inputs
+greater/less than zero. An offset can be applied to the output, and lower and
+upper boundary constraints can be applied. Negative scaling factors cause the
+output to be inverted (negative input produces positive output).
+
+Scaler pseudocode:
+
+if (input < 0)
+    output = (input * NEGATIVE_SCALE) + OFFSET
+else
+    output = (input * POSITIVE_SCALE) + OFFSET
+
+if (output < LOWER_LIMIT)
+    output = LOWER_LIMIT
+if (output > UPPER_LIMIT)
+    output = UPPER_LIMIT
+
+Syntax
+------
+
+Mixer definitions are text files; lines beginning with a single capital letter
+followed by a colon are significant. All other lines are ignored, meaning that
+explanatory text can be freely mixed with the definitions.
+
+Each file may define more than one mixer; the allocation of mixers to actuators
+is specific to the device reading the mixer definition, and the number of
+actuator outputs generated by a mixer is specific to the mixer.
+
+A mixer begins with a line of the form
+
+	<tag>: <mixer arguments>
+
+The tag selects the mixer type; 'M' for a simple summing mixer, 'R' for a 
+multirotor mixer, etc.
+
+Null Mixer
+..........
+
+A null mixer consumes no controls and generates a single actuator output whose
+value is always zero.  Typically a null mixer is used as a placeholder in a
+collection of mixers in order to achieve a specific pattern of actuator outputs.
+
+The null mixer definition has the form:
+
+  Z:
+
+Simple Mixer
+............
+
+A simple mixer combines zero or more control inputs into a single actuator
+output.  Inputs are scaled, and the mixing function sums the result before
+applying an output scaler.
+
+A simple mixer definition begins with:
+
+  M: <control count>
+  O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+
+If <control count> is zero, the sum is effectively zero and the mixer will
+output a fixed value that is <offset> constrained by <lower limit> and <upper
+limit>.
+
+The second line defines the output scaler with scaler parameters as discussed
+above. Whilst the calculations are performed as floating-point operations, the
+values stored in the definition file are scaled by a factor of 10000; i.e. an
+offset of -0.5 is encoded as -5000.
+
+The definition continues with <control count> entries describing the control
+inputs and their scaling, in the form:
+
+  S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
+
+The <group> value identifies the control group from which the scaler will read,
+and the <index> value an offset within that group.  These values are specific to
+the device reading the mixer definition.
+
+When used to mix vehicle controls, mixer group zero is the vehicle attitude
+control group, and index values zero through three are normally roll, pitch,
+yaw and thrust respectively.
+
+The remaining fields on the line configure the control scaler with parameters as
+discussed above. Whilst the calculations are performed as floating-point
+operations, the values stored in the definition file are scaled by a factor of
+10000; i.e. an offset of -0.5 is encoded as -5000.
+
+Multirotor Mixer
+................
+
+The multirotor mixer combines four control inputs (roll, pitch, yaw, thrust)
+into a set of actuator outputs intended to drive motor speed controllers.
+
+The mixer definition is a single line of the form:
+
+R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
+
+The supported geometries include:
+
+  4x - quadrotor in X configuration
+  4+ - quadrotor in + configuration
+  6x - hexcopter in X configuration
+  6+ - hexcopter in + configuration
+  8x - octocopter in X configuration
+  8+ - octocopter in + configuration
+  
+Each of the roll, pitch and yaw scale values determine scaling of the roll,
+pitch and yaw controls relative to the thrust control.  Whilst the calculations
+are performed as floating-point operations, the values stored in the definition
+file are scaled by a factor of 10000; i.e. an factor of 0.5 is encoded as 5000.
+
+Roll, pitch and yaw inputs are expected to range from -1.0 to 1.0, whilst the
+thrust input ranges from 0.0 to 1.0.  Output for each actuator is in the 
+range -1.0 to 1.0.
+
+In the case where an actuator saturates, all actuator values are rescaled so that 
+the saturating actuator is limited to 1.0.
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/mixers/easystar.mix b/ROMFS/px4fmu_common/mixers/easystar.mix
new file mode 100644
index 000000000..0051ffdbb
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/easystar.mix
@@ -0,0 +1,31 @@
+EASYSTAR / EASYSTAR II MIXER
+============================
+
+Aileron mixer
+-------------
+One output - would be easy to add support for 2 servos
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 0  10000  10000      0 -10000  10000
+
+Elevator mixer
+------------
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 1  -10000  -10000      0 -10000  10000
+
+Rudder mixer
+------------
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 2  -10000  -10000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000