Merge branch 'master' of https://github.com/PX4/Firmware into vtol_merge

2026-05-22 01:12:31 +00:00 · 2014-12-15 22:34:01 +01:00
parent 51a7fbeee0 aa40c69853
commit 736f57f436
93 changed files with 3081 additions and 3721 deletions
--- a/2
+++ b/2
--- a/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/10015_tbs_discovery
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.mc_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	# TODO review MC_YAWRATE_I
 	param set MC_ROLL_P 8.0
@@ -26,5 +26,5 @@ fi

 set MIXER FMU_quad_w

-set PWM_OUTPUTS 1234
+set PWM_OUT 1234
 set PWM_MIN 1200
--- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.mc_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	# TODO tune roll/pitch separately
 	param set MC_ROLL_P 7.0
@@ -29,7 +29,7 @@ fi

 set MIXER FMU_quad_w

-set PWM_OUTPUTS 1234
+set PWM_OUT 1234

 set PWM_MIN 1200
 set PWM_MAX 1950
--- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.mc_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	# TODO tune roll/pitch separately
 	param set MC_ROLL_P 7.0
@@ -31,4 +31,4 @@ set MIXER FMU_quad_w
 set PWM_MIN 1210
 set PWM_MAX 2100

-set PWM_OUTPUTS 1234
+set PWM_OUT 1234
--- a/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
+++ b/ROMFS/px4fmu_common/init.d/1005_rc_fw_Malolo1.hil
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.fw_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set FW_AIRSPD_MIN 12
 	param set FW_AIRSPD_TRIM 25
--- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
 set MIXER FMU_hexa_cox

 # Need to set all 8 channels
-set PWM_OUTPUTS 12345678
+set PWM_OUT 12345678
--- a/ROMFS/px4fmu_common/init.d/12001_octo_cox
+++ b/ROMFS/px4fmu_common/init.d/12001_octo_cox
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults

 set MIXER FMU_octo_cox

-set PWM_OUTPUTS 12345678
+set PWM_OUT 12345678
--- a/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
+++ b/ROMFS/px4fmu_common/init.d/2102_3dr_skywalker
@@ -2,4 +2,4 @@

 sh /etc/init.d/rc.fw_defaults

-set MIXER FMU_AERT
+set MIXER skywalker
--- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer
@@ -4,5 +4,5 @@ sh /etc/init.d/rc.fw_defaults

 set MIXER FMU_Q
 # Provide ESC a constant 1000 us pulse while disarmed
-set PWM_OUTPUTS 4
+set PWM_OUT 4
 set PWM_DISARMED 1000
--- a/ROMFS/px4fmu_common/init.d/3031_phantom
+++ b/ROMFS/px4fmu_common/init.d/3031_phantom
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.fw_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set FW_AIRSPD_MIN 13
 	param set FW_AIRSPD_TRIM 15
@@ -36,5 +36,5 @@ fi
 set MIXER phantom

 # Provide ESC a constant 1000 us pulse
-set PWM_OUTPUTS 4
+set PWM_OUT 4
 set PWM_DISARMED 1000
--- a/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/3032_skywalker_x5
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.fw_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set FW_AIRSPD_MIN 15
 	param set FW_AIRSPD_TRIM 20
--- a/ROMFS/px4fmu_common/init.d/3033_wingwing
+++ b/ROMFS/px4fmu_common/init.d/3033_wingwing
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.fw_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set BAT_N_CELLS 2
 	param set FW_AIRSPD_MAX 15
@@ -44,5 +44,5 @@ fi

 set MIXER wingwing
 # Provide ESC a constant 1000 us pulse
-set PWM_OUTPUTS 4
+set PWM_OUT 4
 set PWM_DISARMED 1000
--- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
+++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
@@ -7,7 +7,7 @@

 sh /etc/init.d/rc.fw_defaults

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then

 	# TODO: these are the X5 default parameters, update them to the caipi
--- a/ROMFS/px4fmu_common/init.d/4001_quad_x
+++ b/ROMFS/px4fmu_common/init.d/4001_quad_x
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults

 set MIXER FMU_quad_x

-set PWM_OUTPUTS 1234
+set PWM_OUT 1234
--- a/ROMFS/px4fmu_common/init.d/4008_ardrone
+++ b/ROMFS/px4fmu_common/init.d/4008_ardrone
@@ -8,7 +8,7 @@ sh /etc/init.d/rc.mc_defaults
 #
 # Load default params for this platform
 #
-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
    # Set all params here, then disable autoconfig
    param set MC_ROLL_P 6.0
--- a/ROMFS/px4fmu_common/init.d/4010_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330
@@ -7,7 +7,7 @@

 sh /etc/init.d/4001_quad_x

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
--- a/ROMFS/px4fmu_common/init.d/4011_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450
@@ -7,7 +7,7 @@

 sh /etc/init.d/4001_quad_x

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	# TODO REVIEW
 	param set MC_ROLL_P 7.0
--- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can
+++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can
@@ -7,7 +7,7 @@

 sh /etc/init.d/4001_quad_x

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	# TODO REVIEW
 	param set MC_ROLL_P 7.0
--- a/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
+++ b/ROMFS/px4fmu_common/init.d/4020_hk_micro_pcb
@@ -9,7 +9,7 @@ echo "HK Micro PCB Quad"

 sh /etc/init.d/4001_quad_x

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
--- a/ROMFS/px4fmu_common/init.d/5001_quad_+
+++ b/ROMFS/px4fmu_common/init.d/5001_quad_+
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults

 set MIXER FMU_quad_+

-set PWM_OUTPUTS 1234
+set PWM_OUT 1234
--- a/ROMFS/px4fmu_common/init.d/6001_hexa_x
+++ b/ROMFS/px4fmu_common/init.d/6001_hexa_x
@@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
 set MIXER FMU_hexa_x

 # Need to set all 8 channels
-set PWM_OUTPUTS 12345678
+set PWM_OUT 12345678
--- a/ROMFS/px4fmu_common/init.d/7001_hexa_+
+++ b/ROMFS/px4fmu_common/init.d/7001_hexa_+
@@ -10,4 +10,4 @@ sh /etc/init.d/rc.mc_defaults
 set MIXER FMU_hexa_+

 # Need to set all 8 channels
-set PWM_OUTPUTS 12345678
+set PWM_OUT 12345678
--- a/ROMFS/px4fmu_common/init.d/8001_octo_x
+++ b/ROMFS/px4fmu_common/init.d/8001_octo_x
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults

 set MIXER FMU_octo_x

-set PWM_OUTPUTS 12345678
+set PWM_OUT 12345678
--- a/ROMFS/px4fmu_common/init.d/9001_octo_+
+++ b/ROMFS/px4fmu_common/init.d/9001_octo_+
@@ -9,4 +9,4 @@ sh /etc/init.d/rc.mc_defaults

 set MIXER FMU_octo_+

-set PWM_OUTPUTS 12345678
+set PWM_OUT 12345678
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -2,7 +2,7 @@

 set VEHICLE_TYPE fw

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 #
 # Default parameters for FW
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -8,12 +8,11 @@ then
 	#
 	# Load mixer
 	#
-	set MIXERSD /fs/microsd/etc/mixers/$MIXER.mix
 	
-	# Use the mixer file from the SD-card if it exists
-	if [ -f $MIXERSD ]
+	#Use the mixer file from the SD-card if it exists
+	if [ -f /fs/microsd/etc/mixers/$MIXER.mix ]
 	then
-		set MIXER_FILE $MIXERSD
+		set MIXER_FILE /fs/microsd/etc/mixers/$MIXER.mix
 	else
 		set MIXER_FILE /etc/mixers/$MIXER.mix
 	fi
@@ -32,29 +31,31 @@ then
 	
 	if mixer load $OUTPUT_DEV $MIXER_FILE
 	then
-		echo "[init] Mixer loaded: $MIXER_FILE"
+		echo "[i] Mixer: $MIXER_FILE"
 	else
-		echo "[init] Error loading mixer: $MIXER_FILE"
-		tone_alarm $TUNE_OUT_ERROR
+		echo "[i] Error loading mixer: $MIXER_FILE"
+		tone_alarm $TUNE_ERR
 	fi
+
+	unset MIXER_FILE
 else
 	if [ $MIXER != skip ]
 	then
-		echo "[init] Mixer not defined"
-		tone_alarm $TUNE_OUT_ERROR
+		echo "[i] Mixer not defined"
+		tone_alarm $TUNE_ERR
 	fi
 fi

 if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]
 then
-	if [ $PWM_OUTPUTS != none ]
+	if [ $PWM_OUT != none ]
 	then
 		#
 		# Set PWM output frequency
 		#
 		if [ $PWM_RATE != none ]
 		then
-			pwm rate -c $PWM_OUTPUTS -r $PWM_RATE
+			pwm rate -c $PWM_OUT -r $PWM_RATE
 		fi
 		
 		#
@@ -62,15 +63,15 @@ then
 		#
 		if [ $PWM_DISARMED != none ]
 		then
-			pwm disarmed -c $PWM_OUTPUTS -p $PWM_DISARMED
+			pwm disarmed -c $PWM_OUT -p $PWM_DISARMED
 		fi
 		if [ $PWM_MIN != none ]
 		then
-			pwm min -c $PWM_OUTPUTS -p $PWM_MIN
+			pwm min -c $PWM_OUT -p $PWM_MIN
 		fi
 		if [ $PWM_MAX != none ]
 		then
-			pwm max -c $PWM_OUTPUTS -p $PWM_MAX
+			pwm max -c $PWM_OUT -p $PWM_MAX
 		fi
 	fi

--- a/ROMFS/px4fmu_common/init.d/rc.io
+++ b/ROMFS/px4fmu_common/init.d/rc.io
@@ -16,5 +16,5 @@ then
 	set PX4IO_LIMIT 200
 fi

-echo "[init] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
+echo "[i] Set PX4IO update rate limit: $PX4IO_LIMIT Hz"
 px4io limit $PX4IO_LIMIT
--- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults
@@ -2,7 +2,7 @@

 set VEHICLE_TYPE mc

-if [ $DO_AUTOCONFIG == yes ]
+if [ $AUTOCNF == yes ]
 then
 	param set MC_ROLL_P 7.0
 	param set MC_ROLLRATE_P 0.1
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -56,7 +56,7 @@ fi

 if meas_airspeed start
 then
-	echo "[init] Using MEAS airspeed sensor"
+	echo "[i] Using MEAS airspeed sensor"
 else
 	if ets_airspeed start
 	then
@@ -71,6 +71,10 @@ if px4flow start
 then
 fi

+if ll40ls start
+then
+fi
+
 #
 # Start sensors -> preflight_check
 #
--- a/ROMFS/px4fmu_common/init.d/rc.uavcan
+++ b/ROMFS/px4fmu_common/init.d/rc.uavcan
@@ -10,9 +10,9 @@ then
 		# First sensor publisher to initialize takes lowest instance ID
 		# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
 		sleep 1
-		echo "[init] UAVCAN started"
+		echo "[i] UAVCAN started"
 	else
-		echo "[init] ERROR: Could not start UAVCAN"
-		tone_alarm $TUNE_OUT_ERROR
+		echo "[i] ERROR: Could not start UAVCAN"
+		tone_alarm $TUNE_ERR
 	fi
 fi
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -1,46 +1,46 @@
 #!nsh
 #
 # PX4FMU startup script.
+#
+#  NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
+#

 #
 # Default to auto-start mode.
 #
 set MODE autostart

-set RC_FILE /fs/microsd/etc/rc.txt
-set CONFIG_FILE /fs/microsd/etc/config.txt
-set EXTRAS_FILE /fs/microsd/etc/extras.txt
+set FRC /fs/microsd/etc/rc.txt
+set FCONFIG /fs/microsd/etc/config.txt
+set FEXTRAS /fs/microsd/etc/extras.txt

-set TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4
+set TUNE_ERR ML<<CP4CP4CP4CP4CP4

 #
 # Try to mount the microSD card.
 #
-echo "[init] Looking for microSD..."
+echo "[i] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	set LOG_FILE /fs/microsd/bootlog.txt
-	echo "[init] microSD mounted: /fs/microsd"
+	echo "[i] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
 	set LOG_FILE /dev/null
-	echo "[init] No microSD card found"
-	# Play SOS
-	tone_alarm error
 fi

 #
 # Look for an init script on the microSD card.
 # Disable autostart if the script found.
 #
-if [ -f $RC_FILE ]
+if [ -f $FRC ]
 then
-	echo "[init] Executing init script: $RC_FILE"
-	sh $RC_FILE
+	echo "[i] Executing init script: $FRC"
+	sh $FRC
 	set MODE custom
 else
-	echo "[init] Init script not found: $RC_FILE"
+	echo "[i] Init script not found: $FRC"
 fi

 # if this is an APM build then there will be a rc.APM script
@@ -49,17 +49,17 @@ if [ -f /etc/init.d/rc.APM ]
 then
 	if sercon
 	then
-		echo "[init] USB interface connected"
+		echo "[i] USB interface connected"
 	fi

-	echo "[init] Running rc.APM"
+	echo "[i] Running rc.APM"
 	# if APM startup is successful then nsh will exit
 	sh /etc/init.d/rc.APM
 fi

 if [ $MODE == autostart ]
 then
-	echo "[init] AUTOSTART mode"
+	echo "[i] AUTOSTART mode"

 	#
 	# Start CDC/ACM serial driver
@@ -117,31 +117,31 @@ then
 	set VEHICLE_TYPE none
 	set MIXER none
 	set OUTPUT_MODE none
-	set PWM_OUTPUTS none
+	set PWM_OUT none
 	set PWM_RATE none
 	set PWM_DISARMED none
 	set PWM_MIN none
 	set PWM_MAX none
-	set MKBLCTRL_MODE none
+	set MK_MODE none
 	set FMU_MODE pwm
-	set MAVLINK_FLAGS default
+	set MAVLINK_F default
 	set EXIT_ON_END no
 	set MAV_TYPE none
-	set LOAD_DEFAULT_APPS yes
+	set LOAD_DAPPS yes
 	set GPS yes
 	set GPS_FAKE no
 	set FAILSAFE none

 	#
-	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
+	# Set AUTOCNF flag to use it in AUTOSTART scripts
 	#
 	if param compare SYS_AUTOCONFIG 1
 	then
 		# Wipe out params
 		param reset_nostart
-		set DO_AUTOCONFIG yes
+		set AUTOCNF yes
 	else
-		set DO_AUTOCONFIG no
+		set AUTOCNF no
 	fi

 	#
@@ -159,7 +159,7 @@ then
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[init] No autostart"
+		echo "[i] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -167,18 +167,19 @@ then
 	#
 	# Override parameters from user configuration file
 	#
-	if [ -f $CONFIG_FILE ]
+	if [ -f $FCONFIG ]
 	then
-		echo "[init] Config: $CONFIG_FILE"
-		sh $CONFIG_FILE
+		echo "[i] Config: $FCONFIG"
+		sh $FCONFIG
 	else
-		echo "[init] Config not found: $CONFIG_FILE"
+		echo "[i] Config not found: $FCONFIG"
 	fi
+	unset FCONFIG

 	#
 	# If autoconfig parameter was set, reset it and save parameters
 	#
-	if [ $DO_AUTOCONFIG == yes ]
+	if [ $AUTOCNF == yes ]
 	then
 		param set SYS_AUTOCONFIG 0
 		param save
@@ -219,18 +220,18 @@ then
 					set IO_PRESENT yes
 				else
 					echo "PX4IO update failed" >> $LOG_FILE
-					tone_alarm $TUNE_OUT_ERROR
+					tone_alarm $TUNE_ERR
 				fi
 			else
 				echo "PX4IO update failed" >> $LOG_FILE
-				tone_alarm $TUNE_OUT_ERROR
+				tone_alarm $TUNE_ERR
 			fi
 		fi

 		if [ $IO_PRESENT == no ]
 		then
-			echo "[init] ERROR: PX4IO not found"
-			tone_alarm $TUNE_OUT_ERROR
+			echo "[i] ERROR: PX4IO not found"
+			tone_alarm $TUNE_ERR
 		fi
 	fi

@@ -251,7 +252,7 @@ then
 	then
 		# Need IO for output but it not present, disable output
 		set OUTPUT_MODE none
-		echo "[init] ERROR: PX4IO not found, disabling output"
+		echo "[i] ERROR: PX4IO not found, disabling output"

 		# Avoid using ttyS0 for MAVLink on FMUv1
 		if ver hwcmp PX4FMU_V1
@@ -294,7 +295,7 @@ then
 		then
 			if param compare UAVCAN_ENABLE 0
 			then
-				echo "[init] OVERRIDING UAVCAN_ENABLE = 1"
+				echo "[i] OVERRIDING UAVCAN_ENABLE = 1"
 				param set UAVCAN_ENABLE 1
 			fi
 		fi
@@ -303,11 +304,11 @@ then
 		then
 			if px4io start
 			then
-				echo "[init] PX4IO started"
+				echo "[i] PX4IO started"
 				sh /etc/init.d/rc.io
 			else
-				echo "[init] ERROR: PX4IO start failed"
-				tone_alarm $TUNE_OUT_ERROR
+				echo "[i] ERROR: PX4IO start failed"
+				tone_alarm $TUNE_ERR
 			fi
 		fi

@@ -315,10 +316,10 @@ then
 		then
 			if fmu mode_$FMU_MODE
 			then
-				echo "[init] FMU mode_$FMU_MODE started"
+				echo "[i] FMU mode_$FMU_MODE started"
 			else
-				echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
-				tone_alarm $TUNE_OUT_ERROR
+				echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
+				tone_alarm $TUNE_ERR
 			fi

 			if ver hwcmp PX4FMU_V1
@@ -337,21 +338,21 @@ then
 		if [ $OUTPUT_MODE == mkblctrl ]
 		then
 			set MKBLCTRL_ARG ""
-			if [ $MKBLCTRL_MODE == x ]
+			if [ $MK_MODE == x ]
 			then
 				set MKBLCTRL_ARG "-mkmode x"
 			fi
-			if [ $MKBLCTRL_MODE == + ]
+			if [ $MK_MODE == + ]
 			then
 				set MKBLCTRL_ARG "-mkmode +"
 			fi

 			if mkblctrl $MKBLCTRL_ARG
 			then
-				echo "[init] MKBLCTRL started"
+				echo "[i] MK started"
 			else
-				echo "[init] ERROR: MKBLCTRL start failed"
-				tone_alarm $TUNE_OUT_ERROR
+				echo "[i] ERROR: MK start failed"
+				tone_alarm $TUNE_ERR
 			fi

 		fi
@@ -360,10 +361,10 @@ then
 		then
 			if hil mode_port2_pwm8
 			then
-				echo "[init] HIL output started"
+				echo "[i] HIL output started"
 			else
-				echo "[init] ERROR: HIL output start failed"
-				tone_alarm $TUNE_OUT_ERROR
+				echo "[i] ERROR: HIL output start failed"
+				tone_alarm $TUNE_ERR
 			fi
 		fi

@@ -376,10 +377,11 @@ then
 			then
 				if px4io start
 				then
+					echo "[i] PX4IO started"
 					sh /etc/init.d/rc.io
 				else
-					echo "[init] ERROR: PX4IO start failed"
-					tone_alarm $TUNE_OUT_ERROR
+					echo "[i] ERROR: PX4IO start failed"
+					tone_alarm $TUNE_ERR
 				fi
 			fi
 		else
@@ -387,10 +389,10 @@ then
 			then
 				if fmu mode_$FMU_MODE
 				then
-					echo "[init] FMU mode_$FMU_MODE started"
+					echo "[i] FMU mode_$FMU_MODE started"
 				else
-					echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
-					tone_alarm $TUNE_OUT_ERROR
+					echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
+					tone_alarm $TUNE_ERR
 				fi

 				if ver hwcmp PX4FMU_V1
@@ -408,23 +410,24 @@ then
 		fi
 	fi

-	if [ $MAVLINK_FLAGS == default ]
+	if [ $MAVLINK_F == default ]
 	then
 		# 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"
+			set MAVLINK_F "-r 1000 -d /dev/ttyS0"

 			# 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"
+			set MAVLINK_F "-r 1000"
 		fi
 	fi

-	mavlink start $MAVLINK_FLAGS
+	mavlink start $MAVLINK_F
+	unset MAVLINK_F

 	#
 	# UAVCAN
@@ -439,14 +442,16 @@ then

 	if [ $GPS == yes ]
 	then
+		echo "[i] Start GPS"
 		if [ $GPS_FAKE == yes ]
 		then
-			echo "[init] Faking GPS"
+			echo "[i] Faking GPS"
 			gps start -f
 		else
 			gps start
 		fi
 	fi
+	unset GPS_FAKE

 	#
 	# Start up ARDrone Motor interface
@@ -461,7 +466,7 @@ then
 	#
 	if [ $VEHICLE_TYPE == fw ]
 	then
-		echo "[init] Vehicle type: FIXED WING"
+		echo "[i] FIXED WING"

 		if [ $MIXER == none ]
 		then
@@ -481,7 +486,7 @@ then
 		sh /etc/init.d/rc.interface

 		# Start standard fixedwing apps
-		if [ $LOAD_DEFAULT_APPS == yes ]
+		if [ $LOAD_DAPPS == yes ]
 		then
 			sh /etc/init.d/rc.fw_apps
 		fi
@@ -492,11 +497,11 @@ then
 	#
 	if [ $VEHICLE_TYPE == mc ]
 	then
-		echo "[init] Vehicle type: MULTICOPTER"
+		echo "[i] MULTICOPTER"

 		if [ $MIXER == none ]
 		then
-			echo "Default mixer for multicopter not defined"
+			echo "Mixer undefined"
 		fi

 		if [ $MAV_TYPE == none ]
@@ -540,7 +545,7 @@ then
 		sh /etc/init.d/rc.interface

 		# Start standard multicopter apps
-		if [ $LOAD_DEFAULT_APPS == yes ]
+		if [ $LOAD_DAPPS == yes ]
 		then
 			sh /etc/init.d/rc.mc_apps
 		fi
@@ -595,24 +600,38 @@ then
 	#
 	if [ $VEHICLE_TYPE == none ]
 	then
-		echo "[init] Vehicle type: No autostart ID found"
+		echo "[i] No autostart ID found"

 	fi

 	# Start any custom addons
-	if [ -f $EXTRAS_FILE ]
+	if [ -f $FEXTRAS ]
 	then
-		echo "[init] Starting addons script: $EXTRAS_FILE"
-		sh $EXTRAS_FILE
+		echo "[i] Addons script: $FEXTRAS"
+		sh $FEXTRAS
 	else
-		echo "[init] No addons script: $EXTRAS_FILE"
+		echo "[i] No addons script: $FEXTRAS"
 	fi
+	unset FEXTRAS

 	if [ $EXIT_ON_END == yes ]
 	then
-		echo "[init] Exit from nsh"
+		echo "Exit from nsh"
 		exit
 	fi
+	unset EXIT_ON_END
+
+	# Run no SD alarm last
+	if [ $LOG_FILE == /dev/null ]
+	then
+		echo "[i] No microSD card found"
+		# Play SOS
+		tone_alarm error
+	fi

 # End of autostart
 fi
+
+# There is no further processing, so we can free some RAM
+# XXX potentially unset all script variables.
+unset TUNE_ERR
--- a/ROMFS/px4fmu_common/mixers/skywalker.mix
+++ b/ROMFS/px4fmu_common/mixers/skywalker.mix
@@ -0,0 +1,64 @@
+Mixer for Skywalker Airframe
+==================================================
+
+This file defines mixers suitable for controlling a fixed wing aircraft with
+aileron, rudder, elevator and throttle controls using PX4FMU. The configuration
+assumes the aileron servo(s) are connected to PX4FMU servo output 0, the
+elevator to output 1, the rudder to output 2 and the throttle to output 3.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+Aileron mixer
+-------------
+Two scalers total (output, roll).
+
+This mixer assumes that the aileron servos are set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+As there is only one output, if using two servos adjustments to compensate for
+differences between the servos must be made mechanically.  To obtain the correct
+motion using a Y  cable, the servos can be positioned reversed from one another.
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 0  -10000  -10000      0 -10000  10000
+
+Elevator mixer
+------------
+Two scalers total (output, roll).
+
+This mixer assumes that the elevator servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:       10000   10000      0 -10000  10000
+S: 0 1   10000   10000      0 -10000  10000
+
+Rudder mixer
+------------
+Two scalers total (output, yaw).
+
+This mixer assumes that the rudder servo is set up correctly mechanically;
+depending on the actual configuration it may be necessary to reverse the scaling
+factors (to reverse the servo movement) and adjust the offset, scaling and
+endpoints to suit.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 2  10000  10000      0 -10000  10000
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
--- a/ROMFS/px4fmu_test/init.d/rc.standalone
+++ b/ROMFS/px4fmu_test/init.d/rc.standalone
@@ -3,11 +3,11 @@
 # Flight startup script for PX4FMU standalone configuration.
 #

-echo "[init] doing standalone PX4FMU startup..."
+echo "[i] doing standalone PX4FMU startup..."

 #
 # Start the ORB
 #
 uorb start

-echo "[init] startup done"
+echo "[i] startup done"
--- a/ROMFS/px4fmu_test/init.d/rcS
+++ b/ROMFS/px4fmu_test/init.d/rcS
@@ -6,7 +6,7 @@ uorb start

 if sercon
 then
-	echo "[init] USB interface connected"
+	echo "[i] USB interface connected"

 	# Try to get an USB console
 	nshterm /dev/ttyACM0 &
@@ -15,14 +15,14 @@ fi
 #
 # Try to mount the microSD card.
 #
-echo "[init] looking for microSD..."
+echo "[i] looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
-	echo "[init] card mounted at /fs/microsd"
+	echo "[i] card mounted at /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
-	echo "[init] no microSD card found"
+	echo "[i] no microSD card found"
 	# Play SOS
 	tone_alarm error
 fi
--- a/makefiles/config_px4fmu-v2_test.mk
+++ b/makefiles/config_px4fmu-v2_test.mk
@@ -50,16 +50,25 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/conversion

 #
-# Modules to test-build, but not useful for test environment
+# Example modules to test-build
+#
+MODULES		+= examples/flow_position_estimator
+MODULES		+= examples/fixedwing_control
+MODULES		+= examples/hwtest
+MODULES		+= examples/matlab_csv_serial
+MODULES		+= examples/px4_daemon_app
+MODULES		+= examples/px4_mavlink_debug
+MODULES		+= examples/px4_simple_app
+
+#
+# Drivers / modules to test build, but not useful for test environment
 #
 MODULES		+= modules/attitude_estimator_so3
 MODULES		+= drivers/pca8574
-MODULES		+= examples/flow_position_estimator

 #
-# Libraries
+# Tests
 #
-LIBRARIES	+= lib/mathlib/CMSIS

 MODULES 	+= modules/unit_test
 MODULES		+= modules/mavlink/mavlink_tests
--- a/src/drivers/ll40ls/ll40ls.cpp
+++ b/src/drivers/ll40ls/ll40ls.cpp
@@ -89,7 +89,7 @@

 /* Device limits */
 #define LL40LS_MIN_DISTANCE (0.00f)
-#define LL40LS_MAX_DISTANCE (14.00f)
+#define LL40LS_MAX_DISTANCE (60.00f)

 #define LL40LS_CONVERSION_INTERVAL 100000 /* 100ms */

@@ -581,6 +581,8 @@ LL40LS::collect()
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
+	report.minimum_distance = get_minimum_distance();
+	report.maximum_distance = get_maximum_distance();
 	if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
 		report.valid = 1;
 	}
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -520,6 +520,8 @@ MB12XX::collect()
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
+	report.minimum_distance = get_minimum_distance();
+	report.maximum_distance = get_maximum_distance();
 	report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;

 	/* publish it, if we are the primary */
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -564,6 +564,8 @@ SF0X::collect()
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
+	report.minimum_distance = get_minimum_distance();
+	report.maximum_distance = get_maximum_distance();
 	report.valid = valid && (si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0);

 	/* publish it */
--- a/src/drivers/trone/trone.cpp
+++ b/src/drivers/trone/trone.cpp
@@ -558,6 +558,8 @@ TRONE::collect()
 	report.timestamp = hrt_absolute_time();
 	report.error_count = perf_event_count(_comms_errors);
 	report.distance = si_units;
+	report.minimum_distance = get_minimum_distance();
+	report.maximum_distance = get_maximum_distance();
 	report.valid = crc8(val, 2) == val[2] && si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;


--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -1,7 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -31,6 +30,7 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
+
 /**
 * @file main.c
 *
@@ -55,7 +55,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -106,11 +106,9 @@ static void usage(const char *reason);
 *
 * @param att_sp The current attitude setpoint - the values the system would like to reach.
 * @param att The current attitude. The controller should make the attitude match the setpoint
- * @param speed_body The velocity of the system. Currently unused.
 * @param rates_sp The angular rate setpoint. This is the output of the controller.
 */
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
 	struct actuator_controls_s *actuators);

 /**
@@ -125,7 +123,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 * @param att The current attitude
 * @param att_sp The attitude setpoint. This is the output of the controller
 */
-void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
 	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp);

 /* Variables */
@@ -135,8 +133,7 @@ static int deamon_task;				/**< Handle of deamon task / thread */
 static struct params p;
 static struct param_handles ph;

-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	float speed_body[], struct vehicle_rates_setpoint_s *rates_sp, 
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, 
 	struct actuator_controls_s *actuators)
 {

@@ -173,7 +170,7 @@ void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const st
 	actuators->control[1] = pitch_err * p.pitch_p;
 }

-void control_heading(const struct vehicle_global_position_s *pos, const struct vehicle_global_position_setpoint_s *sp,
+void control_heading(const struct vehicle_global_position_s *pos, const struct position_setpoint_s *sp,
 	const struct vehicle_attitude_s *att, struct vehicle_attitude_setpoint_s *att_sp)
 {

@@ -186,7 +183,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v
 	/* calculate heading error */
 	float yaw_err = att->yaw - bearing;
 	/* apply control gain */
-	float roll_command = yaw_err * p.hdng_p;
+	att_sp->roll_body = yaw_err * p.hdng_p;

 	/* limit output, this commonly is a tuning parameter, too */
 	if (att_sp->roll_body < -0.6f) {
@@ -253,7 +250,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	memset(&manual_sp, 0, sizeof(manual_sp));
 	struct vehicle_status_s vstatus;
 	memset(&vstatus, 0, sizeof(vstatus));
-	struct vehicle_global_position_setpoint_s global_sp;
+	struct position_setpoint_s global_sp;
 	memset(&global_sp, 0, sizeof(global_sp));

 	/* output structs - this is what is sent to the mixer */
@@ -275,17 +272,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])

 	/* subscribe to topics. */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	int manual_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
-	int global_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 	int param_sub = orb_subscribe(ORB_ID(parameter_update));

 	/* Setup of loop */
-	float speed_body[3] = {0.0f, 0.0f, 0.0f};
-	/* RC failsafe check */
-	bool throttle_half_once = false;
+
 	struct pollfd fds[2] = {{ .fd = param_sub, .events = POLLIN },
 				{ .fd = att_sub, .events = POLLIN }};

@@ -339,25 +333,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				/* get a local copy of attitude */
 				orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);

-				if (global_sp_updated)
-					orb_copy(ORB_ID(vehicle_global_position_setpoint), global_sp_sub, &global_sp);
-
-				/* currently speed in body frame is not used, but here for reference */
-				if (pos_updated) {
-					orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
-
-					if (att.R_valid) {
-						speed_body[0] = att.R[0][0] * global_pos.vx + att.R[0][1] * global_pos.vy + att.R[0][2] * global_pos.vz;
-						speed_body[1] = att.R[1][0] * global_pos.vx + att.R[1][1] * global_pos.vy + att.R[1][2] * global_pos.vz;
-						speed_body[2] = att.R[2][0] * global_pos.vx + att.R[2][1] * global_pos.vy + att.R[2][2] * global_pos.vz;
-
-					} else {
-						speed_body[0] = 0;
-						speed_body[1] = 0;
-						speed_body[2] = 0;
-
-						warnx("Did not get a valid R\n");
-					}
+				if (global_sp_updated) {
+					struct position_setpoint_triplet_s triplet;
+					orb_copy(ORB_ID(position_setpoint_triplet), global_sp_sub, &triplet);
+					memcpy(&global_sp, &triplet.current, sizeof(global_sp));
 				}

 				if (manual_sp_updated)
@@ -365,106 +344,14 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 					orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);

 				/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
-				if (isfinite(manual_sp.throttle) &&
-							    (manual_sp.throttle >= 0.6f) &&
-							    (manual_sp.throttle <= 1.0f)) {
-					throttle_half_once = true;
+				if (isfinite(manual_sp.z) &&
+							    (manual_sp.z >= 0.6f) &&
+							    (manual_sp.z <= 1.0f)) {
 				}

 				/* get the system status and the flight mode we're in */
 				orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus);

-				/* control */
-
-#warning fix this
-#if 0
-				if (vstatus.navigation_state == NAVIGATION_STATE_AUTO_ ||
-				    vstatus.navigation_state == NAVIGATION_STATE_STABILIZED) {
-
-				    	/* simple heading control */
-				    	control_heading(&global_pos, &global_sp, &att, &att_sp);
-
-				    	/* nail pitch and yaw (rudder) to zero. This example only controls roll (index 0) */
-				    	actuators.control[1] = 0.0f;
-				    	actuators.control[2] = 0.0f;
-					
-					/* simple attitude control */
-					control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
-
-					/* pass through throttle */
-					actuators.control[3] = att_sp.thrust;
-
-					/* set flaps to zero */
-					actuators.control[4] = 0.0f;
-
-				} else if (vstatus.navigation_state == NAVIGATION_STATE_MANUAL) {
-				/* if in manual mode, decide between attitude stabilization (SAS) and full manual pass-through */
-				} else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) {
-					if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
-
-						/* if the RC signal is lost, try to stay level and go slowly back down to ground */
-						if (vstatus.rc_signal_lost && throttle_half_once) {
-
-							/* put plane into loiter */
-							att_sp.roll_body = 0.3f;
-							att_sp.pitch_body = 0.0f;
-
-							/* limit throttle to 60 % of last value if sane */
-							if (isfinite(manual_sp.throttle) &&
-							    (manual_sp.throttle >= 0.0f) &&
-							    (manual_sp.throttle <= 1.0f)) {
-								att_sp.thrust = 0.6f * manual_sp.throttle;
-
-							} else {
-								att_sp.thrust = 0.0f;
-							}
-
-							att_sp.yaw_body = 0;
-
-							// XXX disable yaw control, loiter
-
-						} else {
-
-							att_sp.roll_body = manual_sp.roll;
-							att_sp.pitch_body = manual_sp.pitch;
-							att_sp.yaw_body = 0;
-							att_sp.thrust = manual_sp.throttle;
-						}
-
-						att_sp.timestamp = hrt_absolute_time();
-
-						/* attitude control */
-						control_attitude(&att_sp, &att, speed_body, &rates_sp, &actuators);
-
-						/* pass through throttle */
-						actuators.control[3] = att_sp.thrust;
-
-						/* pass through flaps */
-						if (isfinite(manual_sp.flaps)) {
-							actuators.control[4] = manual_sp.flaps;
-
-						} else {
-							actuators.control[4] = 0.0f;
-						}
-
-					} else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-						/* directly pass through values */
-						actuators.control[0] = manual_sp.roll;
-						/* positive pitch means negative actuator -> pull up */
-						actuators.control[1] = manual_sp.pitch;
-						actuators.control[2] = manual_sp.yaw;
-						actuators.control[3] = manual_sp.throttle;
-
-						if (isfinite(manual_sp.flaps)) {
-							actuators.control[4] = manual_sp.flaps;
-
-						} else {
-							actuators.control[4] = 0.0f;
-						}
-					}
-				}
-#endif
-
 				/* publish rates */
 				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp);

@@ -474,6 +361,10 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				    isfinite(actuators.control[2]) &&
 				    isfinite(actuators.control[3])) {
 					orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+
+					if (verbose) {
+						warnx("published");
+					}
 				}
 			}
 		}
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -1,387 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file flow_speed_control.c
- *
- * Optical flow speed controller
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <stdbool.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <debug.h>
-#include <termios.h>
-#include <time.h>
-#include <math.h>
-#include <sys/prctl.h>
-#include <drivers/drv_hrt.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_bodyframe_speed_setpoint.h>
-#include <uORB/topics/filtered_bottom_flow.h>
-#include <systemlib/systemlib.h>
-#include <systemlib/perf_counter.h>
-#include <systemlib/err.h>
-#include <poll.h>
-#include <mavlink/mavlink_log.h>
-
-#include "flow_speed_control_params.h"
-
-
-static bool thread_should_exit = false;		/**< Deamon exit flag */
-static bool thread_running = false;		/**< Deamon status flag */
-static int deamon_task;				/**< Handle of deamon task / thread */
-
-__EXPORT int flow_speed_control_main(int argc, char *argv[]);
-
-/**
- * Mainloop of position controller.
- */
-static int flow_speed_control_thread_main(int argc, char *argv[]);
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
-	exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- * 
- * The actual stack size should be set in the call
- * to task_spawn_cmd().
- */
-int flow_speed_control_main(int argc, char *argv[])
-{
-	if (argc < 1)
-		usage("missing command");
-
-	if (!strcmp(argv[1], "start"))
-	{
-		if (thread_running)
-		{
-			printf("flow speed control already running\n");
-			/* this is not an error */
-			exit(0);
-		}
-
-		thread_should_exit = false;
-		deamon_task = task_spawn_cmd("flow_speed_control",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 6,
-					 4096,
-					 flow_speed_control_thread_main,
-					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "stop"))
-	{
-		thread_should_exit = true;
-		exit(0);
-	}
-
-	if (!strcmp(argv[1], "status"))
-	{
-		if (thread_running)
-			printf("\tflow speed control app is running\n");
-		else
-			printf("\tflow speed control app not started\n");
-
-		exit(0);
-	}
-
-	usage("unrecognized command");
-	exit(1);
-}
-
-static int
-flow_speed_control_thread_main(int argc, char *argv[])
-{
-	/* welcome user */
-	thread_running = true;
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd,"[fsc] started");
-
-	uint32_t counter = 0;
-
-	/* structures */
-	struct actuator_armed_s armed;
-	memset(&armed, 0, sizeof(armed));
-	struct vehicle_control_mode_s control_mode;
-	memset(&control_mode, 0, sizeof(control_mode));
-	struct filtered_bottom_flow_s filtered_flow;
-	memset(&filtered_flow, 0, sizeof(filtered_flow));
-	struct vehicle_bodyframe_speed_setpoint_s speed_sp;
-	memset(&speed_sp, 0, sizeof(speed_sp));
-	struct vehicle_attitude_setpoint_s att_sp;
-	memset(&att_sp, 0, sizeof(att_sp));
-
-	/* subscribe to attitude, motor setpoints and system state */
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow));
-	int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint));
-
-	orb_advert_t att_sp_pub;
-	bool attitude_setpoint_adverted = false;
-
-	/* parameters init*/
-	struct flow_speed_control_params params;
-	struct flow_speed_control_param_handles param_handles;
-	parameters_init(&param_handles);
-	parameters_update(&param_handles, &params);
-
-	/* register the perf counter */
-	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_speed_control_runtime");
-	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_speed_control_interval");
-	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_speed_control_err");
-
-	static bool sensors_ready = false;
-	static bool status_changed = false;
-
-	while (!thread_should_exit)
-	{
-		/* wait for first attitude msg to be sure all data are available */
-		if (sensors_ready)
-		{
-			/* polling */
-			struct pollfd fds[2] = {
-				{ .fd = vehicle_bodyframe_speed_setpoint_sub, .events = POLLIN }, // speed setpoint from pos controller
-				{ .fd = parameter_update_sub,   .events = POLLIN }
-			};
-
-			/* wait for a position update, check for exit condition every 5000 ms */
-			int ret = poll(fds, 2, 500);
-
-			if (ret < 0)
-			{
-				/* poll error, count it in perf */
-				perf_count(mc_err_perf);
-			}
-			else if (ret == 0)
-			{
-				/* no return value, ignore */
-//				printf("[flow speed control] no bodyframe speed setpoints updates\n");
-			}
-			else
-			{
-				/* parameter update available? */
-				if (fds[1].revents & POLLIN)
-				{
-					/* read from param to clear updated flag */
-					struct parameter_update_s update;
-					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
-
-					parameters_update(&param_handles, &params);
-					mavlink_log_info(mavlink_fd,"[fsp] parameters updated.");
-				}
-
-				/* only run controller if position/speed changed */
-				if (fds[0].revents & POLLIN)
-				{
-					perf_begin(mc_loop_perf);
-
-					/* get a local copy of the armed topic */
-					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-					/* get a local copy of the control mode */
-					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-					/* get a local copy of filtered bottom flow */
-					orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow);
-					/* get a local copy of bodyframe speed setpoint */
-					orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp);
-					/* get a local copy of control mode */
-					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);
-
-					if (control_mode.flag_control_velocity_enabled)
-					{
-						/* calc new roll/pitch */
-						float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p;
-						float roll_body  =  (speed_sp.vy - filtered_flow.vy) * params.speed_p;
-
-						if(status_changed == false)
-							mavlink_log_info(mavlink_fd,"[fsc] flow SPEED control engaged");
-
-						status_changed = true;
-
-						/* limit roll and pitch corrections */
-						if((pitch_body <= params.limit_pitch) && (pitch_body >= -params.limit_pitch))
-						{
-							att_sp.pitch_body = pitch_body;
-						}
-						else
-						{
-							if(pitch_body > params.limit_pitch)
-								att_sp.pitch_body = params.limit_pitch;
-							if(pitch_body < -params.limit_pitch)
-								att_sp.pitch_body = -params.limit_pitch;
-						}
-
-						if((roll_body <= params.limit_roll) && (roll_body >= -params.limit_roll))
-						{
-							att_sp.roll_body = roll_body;
-						}
-						else
-						{
-							if(roll_body > params.limit_roll)
-								att_sp.roll_body = params.limit_roll;
-							if(roll_body < -params.limit_roll)
-								att_sp.roll_body = -params.limit_roll;
-						}
-
-						/* set yaw setpoint forward*/
-						att_sp.yaw_body = speed_sp.yaw_sp;
-
-						/* add trim from parameters */
-						att_sp.roll_body = att_sp.roll_body + params.trim_roll;
-						att_sp.pitch_body = att_sp.pitch_body + params.trim_pitch;
-
-						att_sp.thrust = speed_sp.thrust_sp;
-						att_sp.timestamp = hrt_absolute_time();
-
-						/* publish new attitude setpoint */
-						if(isfinite(att_sp.pitch_body) && isfinite(att_sp.roll_body) && isfinite(att_sp.yaw_body) && isfinite(att_sp.thrust))
-						{
-							if (attitude_setpoint_adverted)
-							{
-								orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-							}
-							else
-							{
-								att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
-								attitude_setpoint_adverted = true;
-							}
-						}
-						else
-						{
-							warnx("NaN in flow speed controller!");
-						}
-					}
-					else
-					{
-						if(status_changed == true)
-							mavlink_log_info(mavlink_fd,"[fsc] flow SPEED controller disengaged.");
-
-						status_changed = false;
-
-						/* reset attitude setpoint */
-						att_sp.roll_body = 0.0f;
-						att_sp.pitch_body = 0.0f;
-						att_sp.thrust = 0.0f;
-						att_sp.yaw_body = 0.0f;
-					}
-
-					/* measure in what intervals the controller runs */
-					perf_count(mc_interval_perf);
-					perf_end(mc_loop_perf);
-				}
-			}
-
-			counter++;
-		}
-		else
-		{
-			/* sensors not ready waiting for first attitude msg */
-
-			/* polling */
-			struct pollfd fds[1] = {
-				{ .fd = vehicle_attitude_sub, .events = POLLIN },
-			};
-
-			/* wait for a flow msg, check for exit condition every 5 s */
-			int ret = poll(fds, 1, 5000);
-
-			if (ret < 0)
-			{
-				/* poll error, count it in perf */
-				perf_count(mc_err_perf);
-			}
-			else if (ret == 0)
-			{
-				/* no return value, ignore */
-				mavlink_log_info(mavlink_fd,"[fsc] no attitude received.");
-			}
-			else
-			{
-				if (fds[0].revents & POLLIN)
-				{
-					sensors_ready = true;
-					mavlink_log_info(mavlink_fd,"[fsp] initialized.");
-				}
-			}
-		}
-	}
-
-	mavlink_log_info(mavlink_fd,"[fsc] ending now...");
-
-	thread_running = false;
-
-	close(parameter_update_sub);
-	close(vehicle_attitude_sub);
-	close(vehicle_bodyframe_speed_setpoint_sub);
-	close(filtered_bottom_flow_sub);
-	close(armed_sub);
-	close(control_mode_sub);
-	close(att_sp_pub);
-
-	perf_print_counter(mc_loop_perf);
-	perf_free(mc_loop_perf);
-
-	fflush(stdout);
-	return 0;
-}
--- a/src/examples/flow_speed_control/flow_speed_control_params.c
+++ b/src/examples/flow_speed_control/flow_speed_control_params.c
@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file flow_speed_control_params.c
- * 
- */
-
-#include "flow_speed_control_params.h"
-
-/* controller parameters */
-PARAM_DEFINE_FLOAT(FSC_S_P, 0.1f);
-PARAM_DEFINE_FLOAT(FSC_L_PITCH, 0.4f);
-PARAM_DEFINE_FLOAT(FSC_L_ROLL, 0.4f);
-
-int parameters_init(struct flow_speed_control_param_handles *h)
-{
-	/* PID parameters */
-	h->speed_p	 			=	param_find("FSC_S_P");
-	h->limit_pitch 			=	param_find("FSC_L_PITCH");
-	h->limit_roll 			=	param_find("FSC_L_ROLL");
-	h->trim_roll 			=	param_find("TRIM_ROLL");
-	h->trim_pitch 			=	param_find("TRIM_PITCH");
-
-
-	return OK;
-}
-
-int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p)
-{
-	param_get(h->speed_p, &(p->speed_p));
-	param_get(h->limit_pitch, &(p->limit_pitch));
-	param_get(h->limit_roll, &(p->limit_roll));
-	param_get(h->trim_roll, &(p->trim_roll));
-	param_get(h->trim_pitch, &(p->trim_pitch));
-
-	return OK;
-}
--- a/src/examples/flow_speed_control/flow_speed_control_params.h
+++ b/src/examples/flow_speed_control/flow_speed_control_params.h
@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Samuel Zihlmann <samuezih@ee.ethz.ch>
- *   		 Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/*
- * @file flow_speed_control_params.h
- * 
- * Parameters for speed controller
- */
-
-#include <systemlib/param/param.h>
-
-struct flow_speed_control_params {
-	float speed_p;
-	float limit_pitch;
-	float limit_roll;
-	float trim_roll;
-	float trim_pitch;
-};
-
-struct flow_speed_control_param_handles {
-	param_t speed_p;
-	param_t limit_pitch;
-	param_t limit_roll;
-	param_t trim_roll;
-	param_t trim_pitch;
-};
-
-/**
- * Initialize all parameter handles and values
- *
- */
-int parameters_init(struct flow_speed_control_param_handles *h);
-
-/**
- * Update all parameters
- *
- */
-int parameters_update(const struct flow_speed_control_param_handles *h, struct flow_speed_control_params *p);
--- a/src/examples/flow_speed_control/module.mk
+++ b/src/examples/flow_speed_control/module.mk
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Build flow speed control
-#
-
-MODULE_COMMAND	= flow_speed_control
-
-SRCS		= flow_speed_control_main.c \
-		  flow_speed_control_params.c
--- a/src/examples/hwtest/hwtest.c
+++ b/src/examples/hwtest/hwtest.c
@@ -1,7 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author:    Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -34,7 +33,8 @@
 /**
 * @file hwtest.c
 *
- * Simple functional hardware test.
+ * Simple output test.
+ * @ref Documentation https://pixhawk.org/dev/examples/write_output
 *
 * @author Lorenz Meier <lm@inf.ethz.ch>
 */
@@ -45,30 +45,80 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_armed.h>

 __EXPORT int ex_hwtest_main(int argc, char *argv[]);

 int ex_hwtest_main(int argc, char *argv[])
 {
-    struct actuator_controls_s actuators;
-    memset(&actuators, 0, sizeof(actuators));
-    orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);
+	warnx("DO NOT FORGET TO STOP THE COMMANDER APP!");
+	warnx("(run <commander stop> to do so)");
+    warnx("usage: http://px4.io/dev/examples/write_output");

-    int i;
-    float rcvalue = -1.0f;
-    hrt_abstime stime;
+	struct actuator_controls_s actuators;
+	memset(&actuators, 0, sizeof(actuators));
+	orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators);

-    while (true) {
-        stime = hrt_absolute_time();
-        while (hrt_absolute_time() - stime < 1000000) {
-            for (i=0; i<8; i++)
-                actuators.control[i] = rcvalue;
-            actuators.timestamp = hrt_absolute_time(); 
-            orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
-        }
-        warnx("servos set to %.1f", rcvalue);
-        rcvalue *= -1.0f;
-    }
+	struct actuator_armed_s arm;
+	memset(&arm, 0 , sizeof(arm));

-    return OK;
+	arm.timestamp = hrt_absolute_time();
+	arm.ready_to_arm = true;
+	arm.armed = true;
+	orb_advert_t arm_pub_fd = orb_advertise(ORB_ID(actuator_armed), &arm);
+	orb_publish(ORB_ID(actuator_armed), arm_pub_fd, &arm);
+
+	/* read back values to validate */
+	int arm_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
+	orb_copy(ORB_ID(actuator_armed), arm_sub_fd, &arm);
+
+	if (arm.ready_to_arm && arm.armed) {
+		warnx("Actuator armed");
+
+	} else {
+		errx(1, "Arming actuators failed");
+	}
+
+	hrt_abstime stime;
+
+	int count = 0;
+
+	while (count != 36) {
+		stime = hrt_absolute_time();
+
+		while (hrt_absolute_time() - stime < 1000000) {
+			for (int i = 0; i != 2; i++) {
+				if (count <= 5) {
+					actuators.control[i] = -1.0f;
+
+				} else if (count <= 10) {
+					actuators.control[i] = -0.7f;
+
+				} else if (count <= 15) {
+					actuators.control[i] = -0.5f;
+
+				} else if (count <= 20) {
+					actuators.control[i] = -0.3f;
+
+				} else if (count <= 25) {
+					actuators.control[i] = 0.0f;
+
+				} else if (count <= 30) {
+					actuators.control[i] = 0.3f;
+
+				} else {
+					actuators.control[i] = 0.5f;
+				}
+			}
+
+			actuators.timestamp = hrt_absolute_time();
+			orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators);
+			usleep(10000);
+		}
+
+		warnx("count %i", count);
+		count++;
+	}
+
+	return OK;
 }
--- a/src/examples/math_demo/math_demo.cpp
+++ b/src/examples/math_demo/math_demo.cpp
@@ -1,106 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
- *   Author: James Goppert
- *
- * 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 math_demo.cpp
- * @author James Goppert
- * Demonstration of math library
- */
-
-#include <nuttx/config.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <systemlib/systemlib.h>
-#include <mathlib/mathlib.h>
-
-/**
- * Management function.
- */
-extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
-
-/**
- * Test function
- */
-void test();
-
-/**
- * Print the correct usage.
- */
-static void usage(const char *reason);
-
-static void
-usage(const char *reason)
-{
-    if (reason)
-        fprintf(stderr, "%s\n", reason);
-    fprintf(stderr, "usage: math_demo {test}\n\n");
-    exit(1);
-}
-
-/**
- * The deamon app only briefly exists to start
- * the background job. The stack size assigned in the
- * Makefile does only apply to this management task.
- * 
- * The actual stack size should be set in the call
- * to task_create().
- */
-int math_demo_main(int argc, char *argv[])
-{
-
-    if (argc < 1)
-        usage("missing command");
-
-    if (!strcmp(argv[1], "test")) {
-        test();
-        exit(0);
-    }
-
-    usage("unrecognized command");
-    exit(1);
-}
-
-void test()
-{
-    printf("beginning math lib test\n");
-    using namespace math;
-    vectorTest();
-    matrixTest();
-    vector3Test();
-    eulerAnglesTest();
-    quaternionTest();
-    dcmTest();
-}
--- a/src/examples/math_demo/module.mk
+++ b/src/examples/math_demo/module.mk
@@ -1,41 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Mathlib / operations demo application
-#
-
-MODULE_COMMAND		= math_demo
-MODULE_STACKSIZE	= 12000
-
-SRCS			= math_demo.cpp
--- a/src/modules/attitude_estimator_ekf/AttitudeEKF.m
+++ b/src/modules/attitude_estimator_ekf/AttitudeEKF.m
@@ -0,0 +1,298 @@
+function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
+    = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
+
+
+%LQG Postion Estimator and Controller
+% Observer:
+%        x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
+%        x[n+1|n] = Ax[n|n] + Bu[n]
+%
+% $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $
+%
+%
+% Arguments:
+% approx_prediction: if 1 then the exponential map is approximated with a
+% first order taylor approximation. has at the moment not a big influence
+% (just 1st or 2nd order approximation) we should change it to rodriquez
+% approximation.
+% use_inertia_matrix: set to true if you have the inertia matrix J for your
+% quadrotor
+% xa_apo_k: old state vectotr
+% zFlag: if sensor measurement is available [gyro, acc, mag]
+% dt: dt in s
+% z: measurements [gyro, acc, mag]
+% q_rotSpeed: process noise gyro
+% q_rotAcc: process noise gyro acceleration
+% q_acc: process noise acceleration
+% q_mag: process noise magnetometer
+% r_gyro: measurement noise gyro
+% r_accel: measurement noise accel
+% r_mag: measurement noise mag
+% J: moment of inertia matrix
+
+
+% Output:
+% xa_apo: updated state vectotr
+% Pa_apo: updated state covariance matrix
+% Rot_matrix: rotation matrix
+% eulerAngles: euler angles
+% debugOutput: not used
+
+
+%% model specific parameters
+
+
+
+% compute once the inverse of the Inertia
+persistent Ji;
+if isempty(Ji)
+    Ji=single(inv(J));
+end
+
+%% init
+persistent x_apo
+if(isempty(x_apo))
+    gyro_init=single([0;0;0]);
+    gyro_acc_init=single([0;0;0]);
+    acc_init=single([0;0;-9.81]);
+    mag_init=single([1;0;0]);
+    x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
+    
+end
+
+persistent P_apo
+if(isempty(P_apo))
+    %     P_apo = single(eye(NSTATES) * 1000);
+    P_apo = single(200*ones(12));
+end
+
+debugOutput = single(zeros(4,1));
+
+%% copy the states
+wx=  x_apo(1);   % x  body angular rate
+wy=  x_apo(2);   % y  body angular rate
+wz=  x_apo(3);   % z  body angular rate
+
+wax=  x_apo(4);  % x  body angular acceleration
+way=  x_apo(5);  % y  body angular acceleration
+waz=  x_apo(6);  % z  body angular acceleration
+
+zex=  x_apo(7);  % x  component gravity vector
+zey=  x_apo(8);  % y  component gravity vector
+zez=  x_apo(9);  % z  component gravity vector
+
+mux=  x_apo(10); % x  component magnetic field vector
+muy=  x_apo(11); % y  component magnetic field vector
+muz=  x_apo(12); % z  component magnetic field vector
+
+
+
+
+%% prediction section
+% compute the apriori state estimate from the previous aposteriori estimate
+%body angular accelerations
+if (use_inertia_matrix==1)
+    wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
+else
+    wak =[wax;way;waz];
+end
+
+%body angular rates
+wk =[wx;  wy; wz] + dt*wak;
+
+%derivative of the prediction rotation matrix
+O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
+
+%prediction of the earth z vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    zek =(O*dt+single(eye(3)))*[zex;zey;zez];
+    
+else
+    zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
+    %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
+    %precision
+end
+
+
+
+%prediction of the magnetic vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    muk =(O*dt+single(eye(3)))*[mux;muy;muz];
+else
+     muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
+    %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
+    %precision
+end
+
+x_apr=[wk;wak;zek;muk];
+
+% compute the apriori error covariance estimate from the previous
+%aposteriori estimate
+
+EZ=[0,zez,-zey;
+    -zez,0,zex;
+    zey,-zex,0]';
+MA=[0,muz,-muy;
+    -muz,0,mux;
+    muy,-mux,0]';
+
+E=single(eye(3));
+Z=single(zeros(3));
+
+A_lin=[ Z,  E,  Z,  Z
+    Z,  Z,  Z,  Z
+    EZ, Z,  O,  Z
+    MA, Z,  Z,  O];
+
+A_lin=eye(12)+A_lin*dt;
+
+%process covariance matrix
+
+persistent Q
+if (isempty(Q))
+    Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
+        q_rotAcc,q_rotAcc,q_rotAcc,...
+        q_acc,q_acc,q_acc,...
+        q_mag,q_mag,q_mag]);
+end
+
+P_apr=A_lin*P_apo*A_lin'+Q;
+
+
+%% update
+if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
+    
+%     R=[r_gyro,0,0,0,0,0,0,0,0;
+%         0,r_gyro,0,0,0,0,0,0,0;
+%         0,0,r_gyro,0,0,0,0,0,0;
+%         0,0,0,r_accel,0,0,0,0,0;
+%         0,0,0,0,r_accel,0,0,0,0;
+%         0,0,0,0,0,r_accel,0,0,0;
+%         0,0,0,0,0,0,r_mag,0,0;
+%         0,0,0,0,0,0,0,r_mag,0;
+%         0,0,0,0,0,0,0,0,r_mag];
+     R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
+    %observation matrix
+    %[zw;ze;zmk];
+    H_k=[  E,     Z,      Z,    Z;
+        Z,     Z,      E,    Z;
+        Z,     Z,      Z,    E];
+    
+    y_k=z(1:9)-H_k*x_apr;
+    
+    
+    %S_k=H_k*P_apr*H_k'+R;
+     S_k=H_k*P_apr*H_k';
+     S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
+    K_k=(P_apr*H_k'/(S_k));
+    
+    
+    x_apo=x_apr+K_k*y_k;
+    P_apo=(eye(12)-K_k*H_k)*P_apr;
+else
+    if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
+        
+        R=[r_gyro,0,0;
+            0,r_gyro,0;
+            0,0,r_gyro];
+        R_v=[r_gyro,r_gyro,r_gyro];
+        %observation matrix
+        
+        H_k=[  E,     Z,      Z,    Z];
+        
+        y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
+        
+       % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
+        S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
+        S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
+        K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
+        
+        
+        x_apo=x_apr+K_k*y_k;
+        P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
+    else
+        if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
+            
+%             R=[r_gyro,0,0,0,0,0;
+%                 0,r_gyro,0,0,0,0;
+%                 0,0,r_gyro,0,0,0;
+%                 0,0,0,r_accel,0,0;
+%                 0,0,0,0,r_accel,0;
+%                 0,0,0,0,0,r_accel];
+            
+            R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
+            %observation matrix
+            
+            H_k=[  E,     Z,      Z,    Z;
+                Z,     Z,      E,    Z];
+            
+            y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
+            
+           % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+            S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+            S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+            K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+            
+            
+            x_apo=x_apr+K_k*y_k;
+            P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+        else
+            if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
+%                 R=[r_gyro,0,0,0,0,0;
+%                     0,r_gyro,0,0,0,0;
+%                     0,0,r_gyro,0,0,0;
+%                     0,0,0,r_mag,0,0;
+%                     0,0,0,0,r_mag,0;
+%                     0,0,0,0,0,r_mag];
+                  R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
+                %observation matrix
+                
+                H_k=[  E,     Z,      Z,    Z;
+                    Z,     Z,      Z,    E];
+                
+                y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
+                
+                %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+                S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+                S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+                K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+                
+                
+                x_apo=x_apr+K_k*y_k;
+                P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+            else
+                x_apo=x_apr;
+                P_apo=P_apr;
+            end
+        end
+    end
+end
+
+
+
+%% euler anglels extraction
+z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
+m_n_b = x_apo(10:12)./norm(x_apo(10:12));
+
+y_n_b=cross(z_n_b,m_n_b);
+y_n_b=y_n_b./norm(y_n_b);
+
+x_n_b=(cross(y_n_b,z_n_b));
+x_n_b=x_n_b./norm(x_n_b);
+
+
+xa_apo=x_apo;
+Pa_apo=P_apo;
+% rotation matrix from earth to body system
+Rot_matrix=[x_n_b,y_n_b,z_n_b];
+
+
+phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
+theta=-asin(Rot_matrix(1,3));
+psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
+eulerAngles=[phi;theta;psi];
+
--- a/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
+++ b/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj
@@ -0,0 +1,502 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
+  <configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
+    <profile key="profile.mex">
+      <param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
+      <param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
+      <param.RanInstrumentedMex>false</param.RanInstrumentedMex>
+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
+      <param.SpecifiedWorkingFolder />
+      <param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
+      <param.SpecifiedBuildFolder />
+      <param.SearchPaths />
+      <param.ResponsivenessChecks>true</param.ResponsivenessChecks>
+      <param.ExtrinsicCalls>true</param.ExtrinsicCalls>
+      <param.IntegrityChecks>true</param.IntegrityChecks>
+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
+      <param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
+      <param.EnableVariableSizing>true</param.EnableVariableSizing>
+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
+      <param.StackUsageMax>200000</param.StackUsageMax>
+      <param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
+      <param.GenerateComments>true</param.GenerateComments>
+      <param.MATLABSourceComments>false</param.MATLABSourceComments>
+      <param.ReservedNameArray />
+      <param.EnableScreener>true</param.EnableScreener>
+      <param.EnableDebugging>false</param.EnableDebugging>
+      <param.GenerateReport>true</param.GenerateReport>
+      <param.LaunchReport>false</param.LaunchReport>
+      <param.CustomSourceCode />
+      <param.CustomHeaderCode />
+      <param.CustomInitializer />
+      <param.CustomTerminator />
+      <param.CustomInclude />
+      <param.CustomSource />
+      <param.CustomLibrary />
+      <param.PostCodeGenCommand />
+      <param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
+      <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
+      <param.RecursionLimit>100</param.RecursionLimit>
+      <param.TargetLang>option.TargetLang.C</param.TargetLang>
+      <param.EchoExpressions>true</param.EchoExpressions>
+      <param.InlineThreshold>10</param.InlineThreshold>
+      <param.InlineThresholdMax>200</param.InlineThresholdMax>
+      <param.InlineStackLimit>4000</param.InlineStackLimit>
+      <param.EnableMemcpy>true</param.EnableMemcpy>
+      <param.MemcpyThreshold>64</param.MemcpyThreshold>
+      <param.EnableOpenMP>true</param.EnableOpenMP>
+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
+      <param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
+      <unset>
+        <param.MergeInstrumentationResults />
+        <param.BuiltInstrumentedMex />
+        <param.RanInstrumentedMex />
+        <param.WorkingFolder />
+        <param.SpecifiedWorkingFolder />
+        <param.BuildFolder />
+        <param.SpecifiedBuildFolder />
+        <param.SearchPaths />
+        <param.ResponsivenessChecks />
+        <param.ExtrinsicCalls />
+        <param.IntegrityChecks />
+        <param.SaturateOnIntegerOverflow />
+        <param.GlobalDataSyncMethod />
+        <param.EnableVariableSizing />
+        <param.DynamicMemoryAllocation />
+        <param.DynamicMemoryAllocationThreshold />
+        <param.StackUsageMax />
+        <param.FilePartitionMethod />
+        <param.GenerateComments />
+        <param.MATLABSourceComments />
+        <param.ReservedNameArray />
+        <param.EnableScreener />
+        <param.EnableDebugging />
+        <param.GenerateReport />
+        <param.LaunchReport />
+        <param.CustomSourceCode />
+        <param.CustomHeaderCode />
+        <param.CustomInitializer />
+        <param.CustomTerminator />
+        <param.CustomInclude />
+        <param.CustomSource />
+        <param.CustomLibrary />
+        <param.PostCodeGenCommand />
+        <param.ProposeFixedPointDataTypes />
+        <param.mex.GenCodeOnly />
+        <param.ConstantFoldingTimeout />
+        <param.RecursionLimit />
+        <param.TargetLang />
+        <param.EchoExpressions />
+        <param.InlineThreshold />
+        <param.InlineThresholdMax />
+        <param.InlineStackLimit />
+        <param.EnableMemcpy />
+        <param.MemcpyThreshold />
+        <param.EnableOpenMP />
+        <param.InitFltsAndDblsToZero />
+        <param.ConstantInputs />
+      </unset>
+    </profile>
+    <profile key="profile.c">
+      <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
+      <param.SpecifiedWorkingFolder />
+      <param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
+      <param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
+      <param.SearchPaths />
+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
+      <param.PurelyIntegerCode>false</param.PurelyIntegerCode>
+      <param.SupportNonFinite>false</param.SupportNonFinite>
+      <param.EnableVariableSizing>false</param.EnableVariableSizing>
+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
+      <param.StackUsageMax>4000</param.StackUsageMax>
+      <param.MultiInstanceCode>false</param.MultiInstanceCode>
+      <param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
+      <param.GenerateComments>true</param.GenerateComments>
+      <param.MATLABSourceComments>true</param.MATLABSourceComments>
+      <param.MATLABFcnDesc>false</param.MATLABFcnDesc>
+      <param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
+      <param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
+      <param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
+      <param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
+      <param.MaxIdLength>31</param.MaxIdLength>
+      <param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
+      <param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
+      <param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
+      <param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
+      <param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
+      <param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
+      <param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
+      <param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
+      <param.ReservedNameArray />
+      <param.EnableScreener>true</param.EnableScreener>
+      <param.Verbose>false</param.Verbose>
+      <param.GenerateReport>true</param.GenerateReport>
+      <param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
+      <param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
+      <param.LaunchReport>true</param.LaunchReport>
+      <param.CustomSourceCode />
+      <param.CustomHeaderCode />
+      <param.CustomInitializer />
+      <param.CustomTerminator />
+      <param.CustomInclude />
+      <param.CustomSource />
+      <param.CustomLibrary />
+      <param.PostCodeGenCommand />
+      <param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
+      <param.SameHardware>true</param.SameHardware>
+      <param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
+      <param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
+      <var.instance.enabled.Production>true</var.instance.enabled.Production>
+      <param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
+      <param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
+      <param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
+      <param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
+      <param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
+      <param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
+      <param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
+      <param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
+      <param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
+      <param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
+      <param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
+      <param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
+      <param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
+      <param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
+      <param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
+      <param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
+      <param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
+      <var.instance.enabled.Target>false</var.instance.enabled.Target>
+      <param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
+      <param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
+      <param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
+      <param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
+      <param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
+      <param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
+      <param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
+      <param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
+      <param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
+      <param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
+      <param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
+      <param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
+      <param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
+      <param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
+      <param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
+      <param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
+      <param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
+      <param.CustomToolchainOptions />
+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
+      <param.RecursionLimit>100</param.RecursionLimit>
+      <param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
+      <param.TargetLang>option.TargetLang.C</param.TargetLang>
+      <param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
+      <param.CCompilerCustomOptimizations />
+      <param.GenerateMakefile>true</param.GenerateMakefile>
+      <param.BuildToolEnable>false</param.BuildToolEnable>
+      <param.MakeCommand>make_rtw</param.MakeCommand>
+      <param.TemplateMakefile>default_tmf</param.TemplateMakefile>
+      <param.BuildToolConfiguration />
+      <param.InlineThreshold>10</param.InlineThreshold>
+      <param.InlineThresholdMax>200</param.InlineThresholdMax>
+      <param.InlineStackLimit>4000</param.InlineStackLimit>
+      <param.EnableMemcpy>true</param.EnableMemcpy>
+      <param.MemcpyThreshold>64</param.MemcpyThreshold>
+      <param.EnableOpenMP>true</param.EnableOpenMP>
+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
+      <param.PassStructByReference>false</param.PassStructByReference>
+      <param.UseECoderFeatures>true</param.UseECoderFeatures>
+      <unset>
+        <param.WorkingFolder />
+        <param.SpecifiedWorkingFolder />
+        <param.SearchPaths />
+        <param.SaturateOnIntegerOverflow />
+        <param.PurelyIntegerCode />
+        <param.DynamicMemoryAllocation />
+        <param.DynamicMemoryAllocationThreshold />
+        <param.MultiInstanceCode />
+        <param.GenerateComments />
+        <param.MATLABFcnDesc />
+        <param.DataTypeReplacement />
+        <param.ConvertIfToSwitch />
+        <param.PreserveExternInFcnDecls />
+        <param.ParenthesesLevel />
+        <param.MaxIdLength />
+        <param.CustomSymbolStrGlobalVar />
+        <param.CustomSymbolStrType />
+        <param.CustomSymbolStrField />
+        <param.CustomSymbolStrFcn />
+        <param.CustomSymbolStrTmpVar />
+        <param.CustomSymbolStrMacro />
+        <param.CustomSymbolStrEMXArray />
+        <param.CustomSymbolStrEMXArrayFcn />
+        <param.ReservedNameArray />
+        <param.EnableScreener />
+        <param.Verbose />
+        <param.GenerateReport />
+        <param.GenerateCodeMetricsReport />
+        <param.GenerateCodeReplacementReport />
+        <param.CustomInclude />
+        <param.CustomSource />
+        <param.CustomLibrary />
+        <param.SameHardware />
+        <var.instance.enabled.Production />
+        <param.HardwareSizeChar.Production />
+        <param.HardwareSizeShort.Production />
+        <param.HardwareSizeInt.Production />
+        <param.HardwareSizeLong.Production />
+        <param.HardwareSizeLongLong.Production />
+        <param.HardwareSizeFloat.Production />
+        <param.HardwareSizeDouble.Production />
+        <param.HardwareSizeWord.Production />
+        <param.HardwareSizePointer.Production />
+        <param.HardwareEndianness.Production />
+        <param.HardwareLongLongMode.Production />
+        <param.HardwareDivisionRounding.Production />
+        <var.instance.enabled.Target />
+        <param.HardwareSizeChar.Target />
+        <param.HardwareSizeShort.Target />
+        <param.HardwareSizeInt.Target />
+        <param.HardwareSizeLongLong.Target />
+        <param.HardwareSizeFloat.Target />
+        <param.HardwareSizeDouble.Target />
+        <param.HardwareEndianness.Target />
+        <param.HardwareAtomicFloatSize.Target />
+        <param.CustomToolchainOptions />
+        <param.ConstantFoldingTimeout />
+        <param.RecursionLimit />
+        <param.IncludeTerminateFcn />
+        <param.TargetLang />
+        <param.CCompilerCustomOptimizations />
+        <param.GenerateMakefile />
+        <param.BuildToolEnable />
+        <param.MakeCommand />
+        <param.TemplateMakefile />
+        <param.BuildToolConfiguration />
+        <param.InlineThreshold />
+        <param.InlineThresholdMax />
+        <param.InlineStackLimit />
+        <param.EnableMemcpy />
+        <param.MemcpyThreshold />
+        <param.EnableOpenMP />
+        <param.InitFltsAndDblsToZero />
+        <param.UseECoderFeatures />
+      </unset>
+    </profile>
+    <param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
+    <param.version>R2012a</param.version>
+    <param.HasECoderFeatures>true</param.HasECoderFeatures>
+    <param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
+    <param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
+    <param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
+    <param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
+    <param.SILDebugging>false</param.SILDebugging>
+    <param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
+    <param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
+    <param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
+    <param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
+    <param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
+    <param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
+    <param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
+    <param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
+    <param.artifact>option.target.artifact.lib</param.artifact>
+    <param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
+    <param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
+    <param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
+    <param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
+    <param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
+    <param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
+    <param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
+    <param.UsePreconditions>false</param.UsePreconditions>
+    <param.FeatureFlags />
+    <param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
+    <param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
+    <param.ComputedFixedPointData />
+    <param.UserFixedPointData />
+    <param.DefaultWordLength>16</param.DefaultWordLength>
+    <param.DefaultFractionLength>4</param.DefaultFractionLength>
+    <param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
+    <param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
+    <param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
+    <param.StaticAnalysisTimeout />
+    <param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
+    <param.LogAllIOValues>false</param.LogAllIOValues>
+    <param.LogHistogram>false</param.LogHistogram>
+    <param.ShowCoverage>true</param.ShowCoverage>
+    <param.ExcludedFixedPointVerificationFiles />
+    <param.ExcludedFixedPointSimulationFiles />
+    <param.InstrumentedBuildChecksum />
+    <param.FixedPointStaticAnalysisChecksum />
+    <param.InstrumentedMexFile />
+    <param.FixedPointValidationChecksum />
+    <param.FixedPointSourceCodeChecksum />
+    <param.FixedPointFunctionReplacements />
+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
+    <param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
+    <param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
+    <unset>
+      <param.outputfile />
+      <param.version />
+      <param.HasECoderFeatures />
+      <param.CallGeneratedCodeFromTest />
+      <param.VerificationMode />
+      <param.SILDebugging />
+      <param.AutoInferUseVariableSize />
+      <param.AutoInferUseUnboundedSize />
+      <param.AutoInferVariableSizeThreshold />
+      <param.AutoInferUnboundedSizeThreshold />
+      <param.mex.outputfile />
+      <param.grt.outputfile />
+      <param.FixedPointTypeProposalMode />
+      <param.DefaultProposedFixedPointType />
+      <param.MinMaxSafetyMargin />
+      <param.OptimizeWholeNumbers />
+      <param.LaunchInstrumentationReport />
+      <param.OpenInstrumentationReportInBrowser />
+      <param.CreatePrintableInstrumentationReport />
+      <param.EnableAutoExtrinsicCalls />
+      <param.UsePreconditions />
+      <param.FeatureFlags />
+      <param.FixedPointMode />
+      <param.AutoScaleLoopIndexVariables />
+      <param.ComputedFixedPointData />
+      <param.UserFixedPointData />
+      <param.DefaultWordLength />
+      <param.DefaultFractionLength />
+      <param.FixedPointSafetyMargin />
+      <param.FixedPointFimath />
+      <param.FixedPointTypeSource />
+      <param.StaticAnalysisTimeout />
+      <param.StaticAnalysisGlobalRangesOnly />
+      <param.LogAllIOValues />
+      <param.LogHistogram />
+      <param.ShowCoverage />
+      <param.ExcludedFixedPointVerificationFiles />
+      <param.ExcludedFixedPointSimulationFiles />
+      <param.InstrumentedBuildChecksum />
+      <param.FixedPointStaticAnalysisChecksum />
+      <param.InstrumentedMexFile />
+      <param.FixedPointValidationChecksum />
+      <param.FixedPointSourceCodeChecksum />
+      <param.FixedPointFunctionReplacements />
+      <param.GeneratedFixedPointFileSuffix />
+      <param.DefaultFixedPointSignedness />
+    </unset>
+    <fileset.entrypoints>
+      <file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
+        <Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
+          <Input Name="approx_prediction">
+            <Class>uint8</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="use_inertia_matrix">
+            <Class>uint8</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="zFlag">
+            <Class>uint8</Class>
+            <UserDefined>false</UserDefined>
+            <Size>3 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="dt">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="z">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>9 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_rotSpeed">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_rotAcc">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_acc">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_mag">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="r_gyro">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="r_accel">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="r_mag">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="J">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>3 x 3</Size>
+            <Complex>false</Complex>
+          </Input>
+        </Inputs>
+      </file>
+    </fileset.entrypoints>
+    <fileset.testbench>
+      <file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
+    </fileset.testbench>
+    <fileset.package />
+    <build-deliverables>
+      <file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
+    </build-deliverables>
+    <workflow />
+    <matlab>
+      <root>/opt/matlab/r2013b</root>
+      <toolboxes>
+        <toolbox name="fixedpoint" />
+      </toolboxes>
+    </matlab>
+    <platform>
+      <unix>true</unix>
+      <mac>false</mac>
+      <windows>false</windows>
+      <win2k>false</win2k>
+      <winxp>false</winxp>
+      <vista>false</vista>
+      <linux>true</linux>
+      <solaris>false</solaris>
+      <osver>3.16.1-1-ARCH</osver>
+      <os32>false</os32>
+      <os64>true</os64>
+      <arch>glnxa64</arch>
+      <matlab>true</matlab>
+    </platform>
+  </configuration>
+</deployment-project>
+
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -38,6 +38,7 @@
 *
 * @author Tobias Naegeli <naegelit@student.ethz.ch>
 * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
 */

 #include <nuttx/config.h>
@@ -74,8 +75,7 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-#include "codegen/attitudeKalmanfilter_initialize.h"
-#include "codegen/attitudeKalmanfilter.h"
+#include "codegen/AttitudeEKF.h"
 #include "attitude_estimator_ekf_params.h"
 #ifdef __cplusplus
 }
@@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      14000,
+					      7200,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -206,10 +206,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */

+	float debugOutput[4] = { 0.0f };
+
 	int overloadcounter = 19;

 	/* Initialize filter */
-	attitudeKalmanfilter_initialize();
+	AttitudeEKF_initialize();

 	/* store start time to guard against too slow update rates */
 	uint64_t last_run = hrt_absolute_time();
@@ -273,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};

-	struct attitude_estimator_ekf_params ekf_params;
+	struct attitude_estimator_ekf_params ekf_params = { 0 };

-	struct attitude_estimator_ekf_param_handles ekf_param_handles;
+	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };

 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
@@ -504,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}

-					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
-							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
+					/* Call the estimator */
+					AttitudeEKF(false, // approx_prediction
+							(unsigned char)ekf_params.use_moment_inertia,
+							update_vect,
+							dt,
+							z_k,
+							ekf_params.q[0], // q_rotSpeed,
+							ekf_params.q[1], // q_rotAcc
+							ekf_params.q[2], // q_acc
+							ekf_params.q[3], // q_mag
+							ekf_params.r[0], // r_gyro
+							ekf_params.r[1], // r_accel
+							ekf_params.r[2], // r_mag
+							ekf_params.moment_inertia_J,
+							x_aposteriori,
+							P_aposteriori,
+							Rot_matrix,
+							euler,
+							debugOutput);

 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -44,28 +44,96 @@

 /* Extended Kalman Filter covariances */

-/* gyro process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
-/* gyro offsets process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);

-/* gyro measurement noise */
+/**
+ * Body angular rate process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
+
+/**
+ * Body angular acceleration process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
+
+/**
+ * Acceleration process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
+
+/**
+ * Magnet field vector process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
+
+/**
+ * Gyro measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
-/* accel measurement noise */
+
+/**
+ * Accel measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
-/* mag measurement noise */
+
+/**
+ * Mag measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
-/* offset estimation - UNUSED */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);

 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);

 PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);

+/**
+ * Moment of inertia matrix diagonal entry (1, 1)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (2, 2)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (3, 3)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
+
+/**
+ * Moment of inertia enabled in estimator
+ *
+ * If set to != 0 the moment of inertia will be used in the estimator
+ *
+ * @group attitude_ekf
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_J_EN, 0);
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->q1 	=	param_find("EKF_ATT_V3_Q1");
 	h->q2 	=	param_find("EKF_ATT_V3_Q2");
 	h->q3 	=	param_find("EKF_ATT_V3_Q3");
-	h->q4 	=	param_find("EKF_ATT_V3_Q4");

 	h->r0 	=	param_find("EKF_ATT_V4_R0");
 	h->r1 	=	param_find("EKF_ATT_V4_R1");
 	h->r2 	=	param_find("EKF_ATT_V4_R2");
-	h->r3 	=	param_find("EKF_ATT_V4_R3");

 	h->mag_decl   =	param_find("ATT_MAG_DECL");

 	h->acc_comp   =	param_find("ATT_ACC_COMP");

+	h->moment_inertia_J[0]  =   param_find("ATT_J11");
+	h->moment_inertia_J[1]  =   param_find("ATT_J22");
+	h->moment_inertia_J[2]  =   param_find("ATT_J33");
+	h->use_moment_inertia	=   param_find("ATT_J_EN");
+
 	return OK;
 }

@@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q1, &(p->q[1]));
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
-	param_get(h->q4, &(p->q[4]));

 	param_get(h->r0, &(p->r[0]));
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
-	param_get(h->r3, &(p->r[3]));

 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI_F / 180.0f;

 	param_get(h->acc_comp, &(p->acc_comp));

+	for (int i = 0; i < 3; i++) {
+		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
+	}
+	param_get(h->use_moment_inertia, &(p->use_moment_inertia));
+
 	return OK;
 }
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -42,8 +42,10 @@
 #include <systemlib/param/param.h>

 struct attitude_estimator_ekf_params {
-	float r[9];
-	float q[12];
+	float r[3];
+	float q[4];
+	float moment_inertia_J[9];
+	int32_t use_moment_inertia;
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
@@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
 };

 struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3;
-	param_t q0, q1, q2, q3, q4;
+	param_t r0, r1, r2;
+	param_t q0, q1, q2, q3;
+	param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
+	param_t use_moment_inertia;
 	param_t mag_decl;
 	param_t acc_comp;
 };
--- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.c
--- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h
@@ -0,0 +1,26 @@
+/*
+ * AttitudeEKF.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_H__
+#define __ATTITUDEEKF_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "AttitudeEKF_types.h"
+
+/* Function Declarations */
+extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
+extern void AttitudeEKF_initialize(void);
+extern void AttitudeEKF_terminate(void);
+#endif
+/* End of code generation (AttitudeEKF.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
+++ b/src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h
@@ -0,0 +1,17 @@
+/*
+ * AttitudeEKF_types.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_TYPES_H__
+#define __ATTITUDEEKF_TYPES_H__
+
+/* Include files */
+#include "rtwtypes.h"
+
+#endif
+/* End of code generation (AttitudeEKF_types.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
@@ -1,34 +0,0 @@
-/*
- * attitudeKalmanfilter.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_H__
-#define __ATTITUDEKALMANFILTER_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
-#endif
-/* End of code generation (attitudeKalmanfilter.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
@@ -1,31 +0,0 @@
-/*
- * attitudeKalmanfilter_initialize.c
- *
- * Code generation for function 'attitudeKalmanfilter_initialize'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "attitudeKalmanfilter_initialize.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void attitudeKalmanfilter_initialize(void)
-{
-  rt_InitInfAndNaN(8U);
-}
-
-/* End of code generation (attitudeKalmanfilter_initialize.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
@@ -1,34 +0,0 @@
-/*
- * attitudeKalmanfilter_initialize.h
- *
- * Code generation for function 'attitudeKalmanfilter_initialize'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter_initialize(void);
-#endif
-/* End of code generation (attitudeKalmanfilter_initialize.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
@@ -1,31 +0,0 @@
-/*
- * attitudeKalmanfilter_terminate.c
- *
- * Code generation for function 'attitudeKalmanfilter_terminate'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "attitudeKalmanfilter_terminate.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-void attitudeKalmanfilter_terminate(void)
-{
-  /* (no terminate code required) */
-}
-
-/* End of code generation (attitudeKalmanfilter_terminate.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
@@ -1,34 +0,0 @@
-/*
- * attitudeKalmanfilter_terminate.h
- *
- * Code generation for function 'attitudeKalmanfilter_terminate'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
-#define __ATTITUDEKALMANFILTER_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter_terminate(void);
-#endif
-/* End of code generation (attitudeKalmanfilter_terminate.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ b/src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
@@ -1,16 +0,0 @@
-/*
- * attitudeKalmanfilter_types.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
-#define __ATTITUDEKALMANFILTER_TYPES_H__
-
-/* Type Definitions */
-
-#endif
-/* End of code generation (attitudeKalmanfilter_types.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/cross.c
+++ b/src/modules/attitude_estimator_ekf/codegen/cross.c
@@ -1,37 +0,0 @@
-/*
- * cross.c
- *
- * Code generation for function 'cross'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "cross.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
-{
-  c[0] = a[1] * b[2] - a[2] * b[1];
-  c[1] = a[2] * b[0] - a[0] * b[2];
-  c[2] = a[0] * b[1] - a[1] * b[0];
-}
-
-/* End of code generation (cross.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/cross.h
+++ b/src/modules/attitude_estimator_ekf/codegen/cross.h
@@ -1,34 +0,0 @@
-/*
- * cross.h
- *
- * Code generation for function 'cross'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __CROSS_H__
-#define __CROSS_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
-#endif
-/* End of code generation (cross.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/eye.c
+++ b/src/modules/attitude_estimator_ekf/codegen/eye.c
@@ -1,51 +0,0 @@
-/*
- * eye.c
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "eye.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void b_eye(real_T I[144])
-{
-  int32_T i;
-  memset(&I[0], 0, 144U * sizeof(real_T));
-  for (i = 0; i < 12; i++) {
-    I[i + 12 * i] = 1.0;
-  }
-}
-
-/*
- *
- */
-void eye(real_T I[9])
-{
-  int32_T i;
-  memset(&I[0], 0, 9U * sizeof(real_T));
-  for (i = 0; i < 3; i++) {
-    I[i + 3 * i] = 1.0;
-  }
-}
-
-/* End of code generation (eye.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/eye.h
+++ b/src/modules/attitude_estimator_ekf/codegen/eye.h
@@ -1,35 +0,0 @@
-/*
- * eye.h
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __EYE_H__
-#define __EYE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_eye(real_T I[144]);
-extern void eye(real_T I[9]);
-#endif
-/* End of code generation (eye.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
+++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.c
@@ -1,357 +0,0 @@
-/*
- * mrdivide.c
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "mrdivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
-{
-  real32_T b_A[9];
-  int32_T rtemp;
-  int32_T k;
-  real32_T b_B[36];
-  int32_T r1;
-  int32_T r2;
-  int32_T r3;
-  real32_T maxval;
-  real32_T a21;
-  real32_T Y[36];
-  for (rtemp = 0; rtemp < 3; rtemp++) {
-    for (k = 0; k < 3; k++) {
-      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
-    }
-  }
-
-  for (rtemp = 0; rtemp < 12; rtemp++) {
-    for (k = 0; k < 3; k++) {
-      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
-    }
-  }
-
-  r1 = 0;
-  r2 = 1;
-  r3 = 2;
-  maxval = (real32_T)fabs(b_A[0]);
-  a21 = (real32_T)fabs(b_A[1]);
-  if (a21 > maxval) {
-    maxval = a21;
-    r1 = 1;
-    r2 = 0;
-  }
-
-  if ((real32_T)fabs(b_A[2]) > maxval) {
-    r1 = 2;
-    r2 = 1;
-    r3 = 0;
-  }
-
-  b_A[r2] /= b_A[r1];
-  b_A[r3] /= b_A[r1];
-  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
-  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
-  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
-  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
-  if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
-    rtemp = r2;
-    r2 = r3;
-    r3 = rtemp;
-  }
-
-  b_A[3 + r3] /= b_A[3 + r2];
-  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
-  for (k = 0; k < 12; k++) {
-    Y[3 * k] = b_B[r1 + 3 * k];
-    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
-    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
-      + r3];
-    Y[2 + 3 * k] /= b_A[6 + r3];
-    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
-    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
-    Y[1 + 3 * k] /= b_A[3 + r2];
-    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
-    Y[3 * k] /= b_A[r1];
-  }
-
-  for (rtemp = 0; rtemp < 3; rtemp++) {
-    for (k = 0; k < 12; k++) {
-      y[k + 12 * rtemp] = Y[rtemp + 3 * k];
-    }
-  }
-}
-
-/*
- *
- */
-void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
-{
-  real32_T b_A[36];
-  int8_T ipiv[6];
-  int32_T i3;
-  int32_T iy;
-  int32_T j;
-  int32_T c;
-  int32_T ix;
-  real32_T temp;
-  int32_T k;
-  real32_T s;
-  int32_T jy;
-  int32_T ijA;
-  real32_T Y[72];
-  for (i3 = 0; i3 < 6; i3++) {
-    for (iy = 0; iy < 6; iy++) {
-      b_A[iy + 6 * i3] = B[i3 + 6 * iy];
-    }
-
-    ipiv[i3] = (int8_T)(1 + i3);
-  }
-
-  for (j = 0; j < 5; j++) {
-    c = j * 7;
-    iy = 0;
-    ix = c;
-    temp = (real32_T)fabs(b_A[c]);
-    for (k = 2; k <= 6 - j; k++) {
-      ix++;
-      s = (real32_T)fabs(b_A[ix]);
-      if (s > temp) {
-        iy = k - 1;
-        temp = s;
-      }
-    }
-
-    if (b_A[c + iy] != 0.0F) {
-      if (iy != 0) {
-        ipiv[j] = (int8_T)((j + iy) + 1);
-        ix = j;
-        iy += j;
-        for (k = 0; k < 6; k++) {
-          temp = b_A[ix];
-          b_A[ix] = b_A[iy];
-          b_A[iy] = temp;
-          ix += 6;
-          iy += 6;
-        }
-      }
-
-      i3 = (c - j) + 6;
-      for (jy = c + 1; jy + 1 <= i3; jy++) {
-        b_A[jy] /= b_A[c];
-      }
-    }
-
-    iy = c;
-    jy = c + 6;
-    for (k = 1; k <= 5 - j; k++) {
-      temp = b_A[jy];
-      if (b_A[jy] != 0.0F) {
-        ix = c + 1;
-        i3 = (iy - j) + 12;
-        for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
-          b_A[ijA] += b_A[ix] * -temp;
-          ix++;
-        }
-      }
-
-      jy += 6;
-      iy += 6;
-    }
-  }
-
-  for (i3 = 0; i3 < 12; i3++) {
-    for (iy = 0; iy < 6; iy++) {
-      Y[iy + 6 * i3] = A[i3 + 12 * iy];
-    }
-  }
-
-  for (jy = 0; jy < 6; jy++) {
-    if (ipiv[jy] != jy + 1) {
-      for (j = 0; j < 12; j++) {
-        temp = Y[jy + 6 * j];
-        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
-        Y[(ipiv[jy] + 6 * j) - 1] = temp;
-      }
-    }
-  }
-
-  for (j = 0; j < 12; j++) {
-    c = 6 * j;
-    for (k = 0; k < 6; k++) {
-      iy = 6 * k;
-      if (Y[k + c] != 0.0F) {
-        for (jy = k + 2; jy < 7; jy++) {
-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
-        }
-      }
-    }
-  }
-
-  for (j = 0; j < 12; j++) {
-    c = 6 * j;
-    for (k = 5; k > -1; k += -1) {
-      iy = 6 * k;
-      if (Y[k + c] != 0.0F) {
-        Y[k + c] /= b_A[k + iy];
-        for (jy = 0; jy + 1 <= k; jy++) {
-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
-        }
-      }
-    }
-  }
-
-  for (i3 = 0; i3 < 6; i3++) {
-    for (iy = 0; iy < 12; iy++) {
-      y[iy + 12 * i3] = Y[i3 + 6 * iy];
-    }
-  }
-}
-
-/*
- *
- */
-void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
-{
-  real32_T b_A[81];
-  int8_T ipiv[9];
-  int32_T i2;
-  int32_T iy;
-  int32_T j;
-  int32_T c;
-  int32_T ix;
-  real32_T temp;
-  int32_T k;
-  real32_T s;
-  int32_T jy;
-  int32_T ijA;
-  real32_T Y[108];
-  for (i2 = 0; i2 < 9; i2++) {
-    for (iy = 0; iy < 9; iy++) {
-      b_A[iy + 9 * i2] = B[i2 + 9 * iy];
-    }
-
-    ipiv[i2] = (int8_T)(1 + i2);
-  }
-
-  for (j = 0; j < 8; j++) {
-    c = j * 10;
-    iy = 0;
-    ix = c;
-    temp = (real32_T)fabs(b_A[c]);
-    for (k = 2; k <= 9 - j; k++) {
-      ix++;
-      s = (real32_T)fabs(b_A[ix]);
-      if (s > temp) {
-        iy = k - 1;
-        temp = s;
-      }
-    }
-
-    if (b_A[c + iy] != 0.0F) {
-      if (iy != 0) {
-        ipiv[j] = (int8_T)((j + iy) + 1);
-        ix = j;
-        iy += j;
-        for (k = 0; k < 9; k++) {
-          temp = b_A[ix];
-          b_A[ix] = b_A[iy];
-          b_A[iy] = temp;
-          ix += 9;
-          iy += 9;
-        }
-      }
-
-      i2 = (c - j) + 9;
-      for (jy = c + 1; jy + 1 <= i2; jy++) {
-        b_A[jy] /= b_A[c];
-      }
-    }
-
-    iy = c;
-    jy = c + 9;
-    for (k = 1; k <= 8 - j; k++) {
-      temp = b_A[jy];
-      if (b_A[jy] != 0.0F) {
-        ix = c + 1;
-        i2 = (iy - j) + 18;
-        for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
-          b_A[ijA] += b_A[ix] * -temp;
-          ix++;
-        }
-      }
-
-      jy += 9;
-      iy += 9;
-    }
-  }
-
-  for (i2 = 0; i2 < 12; i2++) {
-    for (iy = 0; iy < 9; iy++) {
-      Y[iy + 9 * i2] = A[i2 + 12 * iy];
-    }
-  }
-
-  for (jy = 0; jy < 9; jy++) {
-    if (ipiv[jy] != jy + 1) {
-      for (j = 0; j < 12; j++) {
-        temp = Y[jy + 9 * j];
-        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
-        Y[(ipiv[jy] + 9 * j) - 1] = temp;
-      }
-    }
-  }
-
-  for (j = 0; j < 12; j++) {
-    c = 9 * j;
-    for (k = 0; k < 9; k++) {
-      iy = 9 * k;
-      if (Y[k + c] != 0.0F) {
-        for (jy = k + 2; jy < 10; jy++) {
-          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
-        }
-      }
-    }
-  }
-
-  for (j = 0; j < 12; j++) {
-    c = 9 * j;
-    for (k = 8; k > -1; k += -1) {
-      iy = 9 * k;
-      if (Y[k + c] != 0.0F) {
-        Y[k + c] /= b_A[k + iy];
-        for (jy = 0; jy + 1 <= k; jy++) {
-          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
-        }
-      }
-    }
-  }
-
-  for (i2 = 0; i2 < 9; i2++) {
-    for (iy = 0; iy < 12; iy++) {
-      y[iy + 12 * i2] = Y[i2 + 9 * iy];
-    }
-  }
-}
-
-/* End of code generation (mrdivide.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
+++ b/src/modules/attitude_estimator_ekf/codegen/mrdivide.h
@@ -1,36 +0,0 @@
-/*
- * mrdivide.h
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __MRDIVIDE_H__
-#define __MRDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
-#endif
-/* End of code generation (mrdivide.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/norm.c
+++ b/src/modules/attitude_estimator_ekf/codegen/norm.c
@@ -1,54 +0,0 @@
-/*
- * norm.c
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "norm.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-real32_T norm(const real32_T x[3])
-{
-  real32_T y;
-  real32_T scale;
-  int32_T k;
-  real32_T absxk;
-  real32_T t;
-  y = 0.0F;
-  scale = 1.17549435E-38F;
-  for (k = 0; k < 3; k++) {
-    absxk = (real32_T)fabs(x[k]);
-    if (absxk > scale) {
-      t = scale / absxk;
-      y = 1.0F + y * t * t;
-      scale = absxk;
-    } else {
-      t = absxk / scale;
-      y += t * t;
-    }
-  }
-
-  return scale * (real32_T)sqrt(y);
-}
-
-/* End of code generation (norm.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/norm.h
+++ b/src/modules/attitude_estimator_ekf/codegen/norm.h
@@ -1,34 +0,0 @@
-/*
- * norm.h
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __NORM_H__
-#define __NORM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real32_T norm(const real32_T x[3]);
-#endif
-/* End of code generation (norm.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.c
@@ -1,38 +0,0 @@
-/*
- * rdivide.c
- *
- * Code generation for function 'rdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/* Include files */
-#include "rt_nonfinite.h"
-#include "attitudeKalmanfilter.h"
-#include "rdivide.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-
-/* Function Definitions */
-
-/*
- *
- */
-void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
-{
-  int32_T i;
-  for (i = 0; i < 3; i++) {
-    z[i] = x[i] / y;
-  }
-}
-
-/* End of code generation (rdivide.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/rdivide.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rdivide.h
@@ -1,34 +0,0 @@
-/*
- * rdivide.h
- *
- * Code generation for function 'rdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RDIVIDE_H__
-#define __RDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
-#endif
-/* End of code generation (rdivide.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.c
@@ -1,139 +0,0 @@
-/*
- * rtGetInf.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/*
- * Abstract:
- *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf
- */
-#include "rtGetInf.h"
-#define NumBitsPerChar	8U
-
-/* Function: rtGetInf ==================================================
- * Abstract:
- * Initialize rtInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetInf(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T inf = 0.0;
-  if (bitsPerReal == 32U) {
-    inf = rtGetInfF();
-  } else {
-    uint16_T one = 1U;
-    enum {
-      LittleEndian,
-      BigEndian
-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-    switch (machByteOrder) {
-     case LittleEndian:
-      {
-        union {
-          LittleEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        inf = tmpVal.fltVal;
-        break;
-      }
-
-     case BigEndian:
-      {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        inf = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return inf;
-}
-
-/* Function: rtGetInfF ==================================================
- * Abstract:
- * Initialize rtInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetInfF(void)
-{
-  IEEESingle infF;
-  infF.wordL.wordLuint = 0x7F800000U;
-  return infF.wordL.wordLreal;
-}
-
-/* Function: rtGetMinusInf ==================================================
- * Abstract:
- * Initialize rtMinusInf needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetMinusInf(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T minf = 0.0;
-  if (bitsPerReal == 32U) {
-    minf = rtGetMinusInfF();
-  } else {
-    uint16_T one = 1U;
-    enum {
-      LittleEndian,
-      BigEndian
-    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-    switch (machByteOrder) {
-     case LittleEndian:
-      {
-        union {
-          LittleEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0xFFF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        minf = tmpVal.fltVal;
-        break;
-      }
-
-     case BigEndian:
-      {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0xFFF00000U;
-        tmpVal.bitVal.words.wordL = 0x00000000U;
-        minf = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return minf;
-}
-
-/* Function: rtGetMinusInfF ==================================================
- * Abstract:
- * Initialize rtMinusInfF needed by the generated code.
- * Inf is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetMinusInfF(void)
-{
-  IEEESingle minfF;
-  minfF.wordL.wordLuint = 0xFF800000U;
-  return minfF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetInf.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetInf.h
@@ -1,23 +0,0 @@
-/*
- * rtGetInf.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RTGETINF_H__
-#define __RTGETINF_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetInf(void);
-extern real32_T rtGetInfF(void);
-extern real_T rtGetMinusInf(void);
-extern real32_T rtGetMinusInfF(void);
-
-#endif
-/* End of code generation (rtGetInf.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c
@@ -1,96 +0,0 @@
-/*
- * rtGetNaN.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/*
- * Abstract:
- *       MATLAB for code generation function to initialize non-finite, NaN
- */
-#include "rtGetNaN.h"
-#define NumBitsPerChar	8U
-
-/* Function: rtGetNaN ==================================================
- * Abstract:
- * Initialize rtNaN needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real_T rtGetNaN(void)
-{
-  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
-  real_T nan = 0.0;
-  if (bitsPerReal == 32U) {
-    nan = rtGetNaNF();
-  } else {
-  uint16_T one = 1U;
-  enum {
-    LittleEndian,
-    BigEndian
-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-  switch (machByteOrder) {
-   case LittleEndian:
-    {
-      union {
-        LittleEndianIEEEDouble bitVal;
-        real_T fltVal;
-      } tmpVal;
-
-      tmpVal.bitVal.words.wordH = 0xFFF80000U;
-      tmpVal.bitVal.words.wordL = 0x00000000U;
-      nan = tmpVal.fltVal;
-      break;
-    }
-
-   case BigEndian:
-    {
-        union {
-          BigEndianIEEEDouble bitVal;
-          real_T fltVal;
-        } tmpVal;
-
-        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
-        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
-        nan = tmpVal.fltVal;
-        break;
-      }
-    }
-  }
-
-  return nan;
-}
-
-/* Function: rtGetNaNF ==================================================
- * Abstract:
- * Initialize rtNaNF needed by the generated code.
- * NaN is initialized as non-signaling. Assumes IEEE.
- */
-real32_T rtGetNaNF(void)
-{
-  IEEESingle nanF = { { 0 } };
-  uint16_T one = 1U;
-  enum {
-    LittleEndian,
-    BigEndian
-  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
-  switch (machByteOrder) {
-   case LittleEndian:
-    {
-      nanF.wordL.wordLuint = 0xFFC00000U;
-      break;
-    }
-
-   case BigEndian:
-    {
-      nanF.wordL.wordLuint = 0x7FFFFFFFU;
-      break;
-    }
-  }
-
-  return nanF.wordL.wordLreal;
-}
-
-/* End of code generation (rtGetNaN.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h
@@ -1,21 +0,0 @@
-/*
- * rtGetNaN.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RTGETNAN_H__
-#define __RTGETNAN_H__
-
-#include <stddef.h>
-#include "rtwtypes.h"
-#include "rt_nonfinite.h"
-
-extern real_T rtGetNaN(void);
-extern real32_T rtGetNaNF(void);
-
-#endif
-/* End of code generation (rtGetNaN.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_defines.h
@@ -1,24 +0,0 @@
-/*
- * rt_defines.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RT_DEFINES_H__
-#define __RT_DEFINES_H__
-
-#include <stdlib.h>
-
-#define RT_PI	3.14159265358979323846
-#define RT_PIF	3.1415927F
-#define RT_LN_10	2.30258509299404568402
-#define RT_LN_10F	2.3025851F
-#define RT_LOG10E	0.43429448190325182765
-#define RT_LOG10EF	0.43429449F
-#define RT_E	2.7182818284590452354
-#define RT_EF	2.7182817F
-#endif
-/* End of code generation (rt_defines.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c
@@ -1,87 +0,0 @@
-/*
- * rt_nonfinite.c
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-/*
- * Abstract:
- *      MATLAB for code generation function to initialize non-finites,
- *      (Inf, NaN and -Inf).
- */
-#include "rt_nonfinite.h"
-#include "rtGetNaN.h"
-#include "rtGetInf.h"
-
-real_T rtInf;
-real_T rtMinusInf;
-real_T rtNaN;
-real32_T rtInfF;
-real32_T rtMinusInfF;
-real32_T rtNaNF;
-
-/* Function: rt_InitInfAndNaN ==================================================
- * Abstract:
- * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
- * generated code. NaN is initialized as non-signaling. Assumes IEEE.
- */
-void rt_InitInfAndNaN(size_t realSize)
-{
-  (void) (realSize);
-  rtNaN = rtGetNaN();
-  rtNaNF = rtGetNaNF();
-  rtInf = rtGetInf();
-  rtInfF = rtGetInfF();
-  rtMinusInf = rtGetMinusInf();
-  rtMinusInfF = rtGetMinusInfF();
-}
-
-/* Function: rtIsInf ==================================================
- * Abstract:
- * Test if value is infinite
- */
-boolean_T rtIsInf(real_T value)
-{
-  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
-}
-
-/* Function: rtIsInfF =================================================
- * Abstract:
- * Test if single-precision value is infinite
- */
-boolean_T rtIsInfF(real32_T value)
-{
-  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
-}
-
-/* Function: rtIsNaN ==================================================
- * Abstract:
- * Test if value is not a number
- */
-boolean_T rtIsNaN(real_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan(value)? TRUE:FALSE;
-#else
-  return (value!=value)? 1U:0U;
-#endif
-}
-
-/* Function: rtIsNaNF =================================================
- * Abstract:
- * Test if single-precision value is not a number
- */
-boolean_T rtIsNaNF(real32_T value)
-{
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-  return _isnan((real_T)value)? true:false;
-#else
-  return (value!=value)? 1U:0U;
-#endif
-}
-
-
-/* End of code generation (rt_nonfinite.c) */
--- a/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h
@@ -1,53 +0,0 @@
-/*
- * rt_nonfinite.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __RT_NONFINITE_H__
-#define __RT_NONFINITE_H__
-
-#if defined(_MSC_VER) && (_MSC_VER <= 1200)
-#include <float.h>
-#endif
-#include <stddef.h>
-#include "rtwtypes.h"
-
-extern real_T rtInf;
-extern real_T rtMinusInf;
-extern real_T rtNaN;
-extern real32_T rtInfF;
-extern real32_T rtMinusInfF;
-extern real32_T rtNaNF;
-extern void rt_InitInfAndNaN(size_t realSize);
-extern boolean_T rtIsInf(real_T value);
-extern boolean_T rtIsInfF(real32_T value);
-extern boolean_T rtIsNaN(real_T value);
-extern boolean_T rtIsNaNF(real32_T value);
-
-typedef struct {
-  struct {
-    uint32_T wordH;
-    uint32_T wordL;
-  } words;
-} BigEndianIEEEDouble;
-
-typedef struct {
-  struct {
-    uint32_T wordL;
-    uint32_T wordH;
-  } words;
-} LittleEndianIEEEDouble;
-
-typedef struct {
-  union {
-    real32_T wordLreal;
-    uint32_T wordLuint;
-  } wordL;
-} IEEESingle;
-
-#endif
-/* End of code generation (rt_nonfinite.h) */
--- a/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/src/modules/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -1,9 +1,9 @@
 /*
 * rtwtypes.h
 *
- * Code generation for function 'attitudeKalmanfilter'
+ * Code generation for function 'AttitudeEKF'
 *
- * C source code generated on: Sat Jan 19 15:25:29 2013
+ * C source code generated on: Thu Aug 21 11:17:28 2014
 *
 */

@@ -22,11 +22,12 @@

 /*=======================================================================* 
 * Target hardware information
- *   Device type: Generic->MATLAB Host Computer
+ *   Device type: ARM Compatible->ARM Cortex
 *   Number of bits:     char:   8    short:   16    int:  32
- *                       long:  32      native word size:  32
+ *                       long:  32
+ *                       native word size:  32
 *   Byte ordering: LittleEndian
- *   Signed integer division rounds to: Zero
+ *   Signed integer division rounds to: Undefined
 *   Shift right on a signed integer as arithmetic shift: on
 *=======================================================================*/

@@ -65,48 +66,48 @@ typedef char_T byte_T;
 *===========================================================================*/
 #define CREAL_T	
   typedef struct {  
-     real32_T re;  
-     real32_T im;  
+      real32_T re;  
+      real32_T im;  
   } creal32_T;  

   typedef struct {  
-     real64_T re;  
-     real64_T im;  
+      real64_T re;  
+      real64_T im;  
   } creal64_T;  

   typedef struct {  
-     real_T re;  
-     real_T im;  
+      real_T re;  
+      real_T im;  
   } creal_T;  

   typedef struct {  
-     int8_T re;  
-     int8_T im;  
+      int8_T re;  
+      int8_T im;  
   } cint8_T;  

   typedef struct {  
-     uint8_T re;  
-     uint8_T im;  
+      uint8_T re;  
+      uint8_T im;  
   } cuint8_T;  

   typedef struct {  
-     int16_T re;  
-     int16_T im;  
+      int16_T re;  
+      int16_T im;  
   } cint16_T;  

   typedef struct {  
-     uint16_T re;  
-     uint16_T im;  
+      uint16_T re;  
+      uint16_T im;  
   } cuint16_T;  

   typedef struct {  
-     int32_T re;  
-     int32_T im;  
+      int32_T re;  
+      int32_T im;  
   } cint32_T;  

   typedef struct {  
-     uint32_T re;  
-     uint32_T im;  
+      uint32_T re;  
+      uint32_T im;  
   } cuint32_T;  


--- a/src/modules/attitude_estimator_ekf/module.mk
+++ b/src/modules/attitude_estimator_ekf/module.mk
@@ -39,16 +39,6 @@ MODULE_COMMAND	 = attitude_estimator_ekf

 SRCS		 = attitude_estimator_ekf_main.cpp \
 		   attitude_estimator_ekf_params.c \
-		   codegen/eye.c \
-		   codegen/attitudeKalmanfilter.c \
-		   codegen/mrdivide.c \
-		   codegen/rdivide.c \
-		   codegen/attitudeKalmanfilter_initialize.c \
-		   codegen/attitudeKalmanfilter_terminate.c \
-		   codegen/rt_nonfinite.c \
-		   codegen/rtGetInf.c \
-		   codegen/rtGetNaN.c \
-		   codegen/norm.c \
-		   codegen/cross.c
+		   codegen/AttitudeEKF.c

 MODULE_STACKSIZE = 1200
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -263,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
-	const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
+	const char *orientation_strs[6] = { "back", "front", "left", "right", "up", "down" };

 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));

@@ -294,12 +294,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float

 		/* inform user which axes are still needed */
 		mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
-				 (!data_collected[0]) ? "front " : "",
-				 (!data_collected[1]) ? "back " : "",
+				 (!data_collected[5]) ? "down " : "",
+				 (!data_collected[0]) ? "back " : "",
+				 (!data_collected[1]) ? "front " : "",
 				 (!data_collected[2]) ? "left " : "",
 				 (!data_collected[3]) ? "right " : "",
-				 (!data_collected[4]) ? "up " : "",
-				 (!data_collected[5]) ? "down " : "");
+				 (!data_collected[4]) ? "up " : "");

 		/* allow user enough time to read the message */
 		sleep(3);
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (telemetry_lost[i] &&
 				    hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) {

-					mavlink_log_critical(mavlink_fd, "data link %i regained", i);
+					mavlink_log_info(mavlink_fd, "data link %i regained", i);
 					telemetry_lost[i] = false;
 					have_link = true;

@@ -1711,7 +1711,7 @@ int commander_thread_main(int argc, char *argv[])
 				telemetry_last_dl_loss[i]  = hrt_absolute_time();

 				if (!telemetry_lost[i]) {
-					mavlink_log_critical(mavlink_fd, "data link %i lost", i);
+					mavlink_log_info(mavlink_fd, "data link %i lost", i);
 					telemetry_lost[i] = true;
 				}
 			}
@@ -1726,7 +1726,7 @@ int commander_thread_main(int argc, char *argv[])

 		} else {
 			if (!status.data_link_lost) {
-				mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST");
+				mavlink_log_info(mavlink_fd, "ALL DATA LINKS LOST");
 				status.data_link_lost = true;
 				status.data_link_lost_counter++;
 				status_changed = true;
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -831,14 +831,32 @@ FixedwingAttitudeControl::task_main()
 				 * - manual control is disabled (another app may send the setpoint, but it should
 				 *   for sure not be set from the remote control values)
 				 */
-				if (_vcontrol_mode.flag_control_velocity_enabled ||
-						_vcontrol_mode.flag_control_position_enabled ||
+				if (_vcontrol_mode.flag_control_auto_enabled ||
 						!_vcontrol_mode.flag_control_manual_enabled) {
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					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 */
+					if (_att_sp.roll_reset_integral) {
+						_roll_ctrl.reset_integrator();
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
+				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
+ 					/* 
+					 * Velocity should be controlled and manual is enabled.
+					*/
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+											+ _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					throttle_sp = _att_sp.thrust;
+
 					/* reset integrals where needed */
 					if (_att_sp.roll_reset_integral) {
 						_roll_ctrl.reset_integrator();
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -163,6 +163,9 @@ private:

 	perf_counter_t	_loop_perf;			/**< loop performance counter */

+	float	_hold_alt;				/**< hold altitude for velocity mode */
+	hrt_abstime _control_position_last_called; /**<last call of control_position  */
+
 	/* land states */
 	bool land_noreturn_horizontal;
 	bool land_noreturn_vertical;
@@ -198,6 +201,8 @@ private:
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
 	bool						_was_pos_control_mode;
+	bool						_was_velocity_control_mode;
+	bool						_was_alt_control_mode;

 	struct {
 		float l1_period;
@@ -316,6 +321,11 @@ private:
 	 */
 	void		vehicle_status_poll();

+	/**
+	 * Check for manual setpoint updates.
+	 */
+	bool		vehicle_manual_control_setpoint_poll();
+
 	/**
 	 * Check for airspeed updates.
 	 */
@@ -439,6 +449,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),

+	_hold_alt(0.0f),
+	_control_position_last_called(0),
+
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
@@ -692,6 +705,22 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }

+bool
+FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
+{
+	bool manual_updated;
+
+	/* Check if manual setpoint has changed */
+	orb_check(_manual_control_sub, &manual_updated);
+
+	if (manual_updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
+	}
+
+	return manual_updated;
+}
+
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -852,6 +881,12 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
+	float dt = FLT_MIN; // Using non zero value to a avoid division by zero
+	if (_control_position_last_called > 0) {
+		dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
+	}
+	_control_position_last_called = hrt_absolute_time();
+
 	bool setpoint = true;

 	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
@@ -888,6 +923,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}

 		_was_pos_control_mode = true;
+		_was_velocity_control_mode = false;
+
+		/* reset hold altitude */
+		_hold_alt = _global_pos.alt;

 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1209,12 +1248,59 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}

-	} else {
+	} else if (_control_mode.flag_control_velocity_enabled) {
+		const float deadBand = (60.0f/1000.0f);
+		const float factor = 1.0f - deadBand;
+		if (!_was_velocity_control_mode) {
+			_hold_alt = _global_pos.alt;
+			_was_alt_control_mode = false;
+		}
+		_was_velocity_control_mode = true;
+		_was_pos_control_mode = false;
+		// Get demanded airspeed
+		float altctrl_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.z;

+		// Get demanded vertical velocity from pitch control
+		float pitch = 0.0f;
+		if (_manual.x > deadBand) {
+			pitch = (_manual.x - deadBand) / factor;
+		} else if (_manual.x < - deadBand) {
+			pitch = (_manual.x + deadBand) / factor;
+		}
+		if (pitch > 0.0f) {
+			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
+			_was_alt_control_mode = false;
+		} else if (pitch < 0.0f) {
+			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
+			_was_alt_control_mode = false;
+		} else if (!_was_alt_control_mode) {
+			_hold_alt = _global_pos.alt;
+			_was_alt_control_mode = true;
+		}
+		tecs_update_pitch_throttle(_hold_alt,
+									altctrl_airspeed,
+									eas2tas,
+									math::radians(_parameters.pitch_limit_min),
+									math::radians(_parameters.pitch_limit_max),
+									_parameters.throttle_min,
+									_parameters.throttle_max,
+									_parameters.throttle_cruise,
+									false,
+									math::radians(_parameters.pitch_limit_min),
+									_global_pos.alt,
+									ground_speed,
+									TECS_MODE_NORMAL);
+	} else {
+		_was_velocity_control_mode = false;
 		_was_pos_control_mode = false;

 		/** MANUAL FLIGHT **/

+		/* reset hold altitude */
+		_hold_alt = _global_pos.alt;
+
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;

@@ -1350,6 +1436,7 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
+			vehicle_manual_control_setpoint_poll();
 			// vehicle_baro_poll();

 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -810,9 +810,6 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;

-	MavlinkOrbSubscription *_sensor_combined_sub;
-	uint64_t _sensor_combined_time;
-
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -828,9 +825,7 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0),
-		_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
-		_sensor_combined_time(0)
+		_airspeed_time(0)
 	{}

 	void send(const hrt_abstime t)
@@ -840,14 +835,12 @@ protected:
 		struct actuator_armed_s armed;
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
-		struct sensor_combined_s sensor_combined;

 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
 		updated |= _armed_sub->update(&_armed_time, &armed);
 		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
-		updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);

 		if (updated) {
 			mavlink_vfr_hud_t msg;
@@ -856,7 +849,7 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = sensor_combined.baro_alt_meter;
+			msg.alt = pos.alt;
 			msg.climb = -pos.vel_d;

 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -2180,7 +2173,7 @@ protected:
 			msg.id = 0;
 			msg.orientation = 0;
 			msg.min_distance = range.minimum_distance * 100;
-			msg.max_distance = range.minimum_distance * 100;
+			msg.max_distance = range.maximum_distance * 100;
 			msg.current_distance = range.distance * 100;
 			msg.covariance = 20;

--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -41,6 +41,7 @@
 #define SYSTEMLIB_H_
 #include <float.h>
 #include <stdint.h>
+#include <sched.h>

 __BEGIN_DECLS

--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -61,7 +61,7 @@ struct home_position_s

 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
-	float alt;				/**< Altitude in meters				*/
+	float alt;				/**< Altitude in meters	(AMSL)		*/

 	float x;				/**< X coordinate in meters			*/
 	float y;				/**< Y coordinate in meters			*/
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -83,7 +83,7 @@ struct mission_item_s {
 	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
 	double lat;			/**< latitude in degrees				*/
 	double lon;			/**< longitude in degrees				*/
-	float altitude;			/**< altitude in meters					*/
+	float altitude;			/**< altitude in meters	(AMSL)			*/
 	float yaw;			/**< in radians NED -PI..+PI, NAN means don't change yaw		*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
 	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -65,7 +65,7 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|  */
 	VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|  */
 	VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|  */
-	VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
+	VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -79,7 +79,7 @@ struct vehicle_gps_position_s {
 	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */

 	uint64_t timestamp_time;			/**< Timestamp for time information */
-	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
+	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */

 	uint8_t satellites_used;			/**< Number of satellites used */
 };
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -46,6 +46,8 @@
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>

+#include <uORB/topics/esc_status.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>

@@ -612,10 +614,32 @@ UavcanNode::print_info()

 	if (_outputs.noutputs != 0) {
 		printf("ESC output: ");
+
 		for (uint8_t i=0; i<_outputs.noutputs; i++) {
 			printf("%d ", (int)(_outputs.output[i]*1000));
 		}
 		printf("\n");
+
+		// ESC status
+		int esc_sub = orb_subscribe(ORB_ID(esc_status));
+		struct esc_status_s esc;
+		memset(&esc, 0, sizeof(esc));
+		orb_copy(ORB_ID(esc_status), esc_sub, &esc);
+
+		printf("ESC Status:\n");
+		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d\t",    esc.esc[i].esc_address);
+			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
+			printf("%3.2f\t", (double)esc.esc[i].esc_current);
+			printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
+			printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
+			printf("%d\t",    esc.esc[i].esc_rpm);
+			printf("%d",      esc.esc[i].esc_errorcount);
+			printf("\n");
+		}
+
+		orb_unsubscribe(esc_sub);
 	}

 	// Sensor bridges
--- a/2
+++ b/2