#!nsh
# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
set +e
# Un comment the line below to help debug scripts by printing a trace of the script commands
#set -x
# PX4FMU startup script.
#
# NOTE: environment variable references:
#    If the dollar sign ('$') is followed by a left bracket ('{') then the
#    variable name is terminated with the right bracket character ('}').
#    Otherwise, the variable name goes to the end of the argument.
#
#
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#
# UART mapping on OMNIBUSF4SD:
#
# USART1		/dev/ttyS0		SerialRX
# USART4		/dev/ttyS1		TELEM1
# USART6		/dev/ttyS2		GPS
#
# UART mapping on FMUv2/3/4:
#
# UART1			/dev/ttyS0		IO debug (except v4, there ttyS0 is the wifi)
# USART2		/dev/ttyS1		TELEM1 (flow control)
# USART3		/dev/ttyS2		TELEM2 (flow control)
# UART4
# UART7							CONSOLE
# UART8							SERIAL4
#
#
# UART mapping on FMUv5:
#
# UART1			/dev/ttyS0		GPS
# USART2		/dev/ttyS1		TELEM1 (flow control)
# USART3		/dev/ttyS2		TELEM2 (flow control)
# UART4			/dev/ttyS3		TELEM4
# USART6		/dev/ttyS4		TELEM3 (flow control)
# UART7			/dev/ttyS5		?
# UART8			/dev/ttyS6		CONSOLE

#
# Mount the procfs.
#
mount -t procfs /proc

#
# Start CDC/ACM serial driver
#
sercon

# print full system version
ver all

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

set TUNE_ERR "ML<<CP4CP4CP4CP4CP4"
set LOG_FILE /fs/microsd/bootlog.txt

#
# Try to mount the microSD card.
#
# REBOOTWORK this needs to start after the flight control loop
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
	if hardfault_log check
	then
		tone_alarm error
		if hardfault_log commit
		then
			hardfault_log reset
			tone_alarm stop
		fi
	fi
else
	tone_alarm MBAGP
	if mkfatfs /dev/mmcsd0
	then
		if mount -t vfat /dev/mmcsd0 /fs/microsd
		then
			echo "INFO  [init] card formatted"
		else
			echo "ERROR [init] format failed"
			tone_alarm MNBG
			set LOG_FILE /dev/null
		fi
	else
		set LOG_FILE /dev/null
	fi
fi

#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
set FRC /fs/microsd/etc/rc.txt
if [ -f $FRC ]
then
	sh $FRC
	set MODE custom
fi
unset FRC

if [ $MODE == autostart ]
then

	#
	# Start the ORB (first app to start)
	#
	uorb start

	# Start tone driver
	tone_alarm start

	# play startup tone
	tune_control play -t 1

	#
	# Load parameters
	#
	set PARAM_FILE /fs/microsd/params
	if mtd start
	then
		set PARAM_FILE /fs/mtd_params
	fi

	param select $PARAM_FILE
	if param load
	then
	else
		if param reset
		then
		fi
	fi

	#
	# Start system state indicator
	#
	if rgbled start
	then
	else
		if blinkm start
		then
			blinkm systemstate
		fi
	fi

	# FMUv5 may have both PWM I2C RGB LED support
	rgbled_pwm start

	#
	# Set AUTOCNF flag to use it in AUTOSTART scripts
	#
	if param compare SYS_AUTOCONFIG 1
	then
		# Wipe out params except RC*, flight modes and total flight time
		param reset_nostart RC* COM_FLTMODE* LND_FLIGHT_T_*
		set AUTOCNF yes
	else
		set AUTOCNF no

		#
		# Release 1.4.0 transitional support:
		# set to old default if unconfigured.
		# this preserves the previous behaviour
		#
		if param compare BAT_N_CELLS 0
		then
			param set BAT_N_CELLS 3
		fi
	fi

	#
	# Set default values
	#
	set VEHICLE_TYPE none
	set MIXER none
	set MIXER_AUX none
	set OUTPUT_MODE none
	set PWM_OUT none
	set PWM_RATE p:PWM_RATE
	set PWM_DISARMED p:PWM_DISARMED
	set PWM_MIN p:PWM_MIN
	set PWM_MAX p:PWM_MAX
	set PWM_AUX_OUT none
	set PWM_AUX_RATE none
	set PWM_ACHDIS none
	set PWM_AUX_DISARMED p:PWM_AUX_DISARMED
	set PWM_AUX_MIN p:PWM_AUX_MIN
	set PWM_AUX_MAX p:PWM_AUX_MAX
	set FAILSAFE_AUX none
	set MK_MODE none
	set FMU_MODE pwm
	set AUX_MODE pwm
	set FMU_ARGS ""
	set MAVLINK_F default
	set MAVLINK_COMPANION_DEVICE /dev/ttyS2
	set MAV_TYPE none
	set FAILSAFE none
	set USE_IO no
	set LOGGER_BUF 14

	if ver hwcmp PX4FMU_V5
	then
		set LOGGER_BUF 64
	fi

	if ver hwcmp CRAZYFLIE
	then
		if param compare SYS_AUTOSTART 0
		then
			param set SYS_AUTOSTART 4900
			set AUTOCNF yes
		fi
	fi

	if ver hwcmp AEROFC_V1
	then
		if param compare SYS_AUTOSTART 0
		then
			set AUTOCNF yes
		fi

		# We don't allow changing AUTOSTART as it doesn't work in
		# other configurations
		param set SYS_AUTOSTART 4070
	fi

	if ver hwcmp NXPHLITE_V3
	then
		param set SYS_FMU_TASK 1
		set MAVLINK_COMPANION_DEVICE /dev/ttyS4
	fi

	if ver hwcmp OMNIBUS_F4SD
	then
		set MAVLINK_COMPANION_DEVICE /dev/ttyS1
	fi

	#
	# Set USE_IO flag
	#
	if param compare SYS_USE_IO 1
	then
		set USE_IO yes
	fi

	if param compare SYS_FMU_TASK 1
	then
		set FMU_ARGS "-t"
	fi

	#
	# Set parameters and env variables for selected AUTOSTART
	#
	if param compare SYS_AUTOSTART 0
	then
	else
		sh /etc/init.d/rc.autostart
	fi
	unset MODE

	#
	# If mount (gimbal) control is enabled and output mode is AUX, set the aux
	# mixer to mount (override the airframe-specific MIXER_AUX setting)
	#
	if param compare MNT_MODE_IN -1
	then
	else
		if param compare MNT_MODE_OUT 0
		then
			set MIXER_AUX mount
		fi
	fi

	#
	# Override parameters from user configuration file
	#
	set FCONFIG /fs/microsd/etc/config.txt
	if [ -f $FCONFIG ]
	then
		echo "Custom: ${FCONFIG}"
		sh $FCONFIG
	fi
	unset FCONFIG

	#
	# If autoconfig parameter was set, reset it and save parameters
	#
	if [ $AUTOCNF == yes ]
	then
		# Disable safety switch by default on Pixracer and OmnibusF4SD
		if ver hwcmp PX4FMU_V4 OMNIBUS_F4SD
		then
			param set CBRK_IO_SAFETY 22027
		fi

		# Run FMU as task on Pixracer and on boards with enough RAM
		if ver hwcmp PX4FMU_V4 PX4FMU_V4PRO PX4FMU_V5
		then
			param set SYS_FMU_TASK 1
		fi

		if ver hwcmp OMNIBUS_F4SD
		then
			param set SYS_FMU_TASK 1
			param set SYS_HAS_MAG 0
			# use the Q attitude estimator, it works w/o mag or GPS
			param set SYS_MC_EST_GROUP 1
			param set ATT_ACC_COMP 0
			param set ATT_W_ACC 0.4000
			param set ATT_W_GYRO_BIAS 0.0000
		fi

		param set SYS_AUTOCONFIG 0
	fi
	unset AUTOCNF

	set IO_PRESENT no

	#
	# Check if PX4IO present and update firmware if needed
	#
	if [ -f /etc/extras/px4io-v2.bin ]
	then
		set IO_FILE /etc/extras/px4io-v2.bin

		if px4io checkcrc ${IO_FILE}
		then
			set IO_PRESENT yes
		else
			tone_alarm MLL32CP8MB

			if px4io start
			then
				# try to safe px4 io so motor outputs dont go crazy
				if px4io safety_on
				then
					# success! no-op
				else
					# px4io did not respond to the safety command
					px4io stop
				fi
			fi

			if px4io forceupdate 14662 ${IO_FILE}
			then
				usleep 10000
				if px4io checkcrc ${IO_FILE}
				then
					echo "PX4IO CRC OK after updating" >> $LOG_FILE
					tone_alarm MLL8CDE

					set IO_PRESENT yes
				else
					echo "PX4IO update failed" >> $LOG_FILE
					tone_alarm ${TUNE_ERR}
				fi
			else
				echo "PX4IO update failed" >> $LOG_FILE
				tune_control play -m ${TUNE_ERR}
			fi
		fi
	fi
	unset IO_FILE

	if [ $USE_IO == yes -a $IO_PRESENT == no ]
	then
		echo "PX4IO not found" >> $LOG_FILE
		tune_control play -m ${TUNE_ERR}
	fi

	#
	# Set default output if not set
	#
	if [ $OUTPUT_MODE == none ]
	then
		if [ $USE_IO == yes ]
		then
			set OUTPUT_MODE io
		else
			set OUTPUT_MODE fmu
		fi
	fi

	if [ $OUTPUT_MODE == io -a $IO_PRESENT != yes ]
	then
		# Need IO for output but it not present, disable output
		set OUTPUT_MODE none
	fi

	if [ $OUTPUT_MODE == tap_esc ]
	then
		set FMU_MODE rcin
	fi

	set DATAMAN_OPT ""
	if ver hwcmp AEROFC_V1
	then
		set DATAMAN_OPT -i
	fi
	if ver hwcmp AEROCORE2
	then
		set DATAMAN_OPT "-f /fs/mtd_dataman"
	fi
	# waypoint storage
	# REBOOTWORK this needs to start in parallel
	if dataman start $DATAMAN_OPT
	then
	fi
	unset DATAMAN_OPT

	#
	# Sensors System (start before Commander so Preflight checks are properly run)
	# commander Needs to be this early for in-air-restarts
	if param compare SYS_HITL 1
	then
		set OUTPUT_MODE hil
		sensors start -h
		commander start --hil
	else
		if ver hwcmp PX4_SAME70XPLAINED_V1
		then
			gps start -d /dev/ttyS2
		else
			gps start
		fi
		sh /etc/init.d/rc.sensors
		commander start
	fi
	send_event start
	load_mon start

	#
	# Check if UAVCAN is enabled, default to it for ESCs
	#
	if param greater UAVCAN_ENABLE 2
	then
		set OUTPUT_MODE uavcan_esc
	fi

	# Sensors on the PWM interface bank
	if param compare SENS_EN_LL40LS 1
	then
		# clear pins 5 and 6
		set FMU_MODE pwm4
		set AUX_MODE pwm4
	fi
	if param greater TRIG_MODE 0
	then
		# We ONLY support trigger on pins 5 and 6 when simultanously using AUX for actuator output
		if param compare TRIG_PINS 56
		then
			# clear pins 5 and 6
			set FMU_MODE pwm4
			set AUX_MODE pwm4
		else
			set FMU_MODE none
			set AUX_MODE none
		fi
		camera_trigger start

		param set CAM_FBACK_MODE 1
		camera_feedback start
	fi

	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
	if [ $OUTPUT_MODE != none ]
	then
		if [ $OUTPUT_MODE == uavcan_esc ]
		then
			if param compare UAVCAN_ENABLE 0
			then
				param set UAVCAN_ENABLE 3
			fi
		fi

		if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
		then
			if px4io start
			then
				sh /etc/init.d/rc.io
			else
				echo "PX4IO start failed" >> $LOG_FILE
				tune_control play -m  ${TUNE_ERR}
			fi
		fi

		if [ $OUTPUT_MODE == fmu ]
		then
			if fmu mode_$FMU_MODE $FMU_ARGS
			then
			else
				echo "FMU start failed" >> $LOG_FILE
				tune_control play -m ${TUNE_ERR}
			fi
		fi

		if [ $OUTPUT_MODE == mkblctrl ]
		then
			set MKBLCTRL_ARG ""
			if [ $MKBLCTRL_MODE == x ]
			then
				set MKBLCTRL_ARG "-mkmode x"
			fi
			if [ $MKBLCTRL_MODE == + ]
			then
				set MKBLCTRL_ARG "-mkmode +"
			fi

			if mkblctrl $MKBLCTRL_ARG
			then
			else
				tune_control play -m ${TUNE_ERR}
			fi
			unset MKBLCTRL_ARG
		fi
		unset MK_MODE

		if [ $OUTPUT_MODE == hil ]
		then
			if pwm_out_sim start
			then
			else
				tune_control play -m ${TUNE_ERR}
			fi
		fi

		#
		# Start IO or FMU for RC PPM input if needed
		#
		if [ $IO_PRESENT == yes ]
		then
			if [ $OUTPUT_MODE != io ]
			then
				if px4io start
				then
					sh /etc/init.d/rc.io
				else
					echo "PX4IO start failed" >> $LOG_FILE
					tune_control play -m ${TUNE_ERR}
				fi
			fi
		else
			if [ $OUTPUT_MODE != fmu ]
			then
				if fmu mode_${FMU_MODE} $FMU_ARGS
				then
				else
					echo "FMU mode_${FMU_MODE} start failed" >> $LOG_FILE
					tune_control play -m ${TUNE_ERR}
				fi
			fi
		fi
	fi

	if [ $MAVLINK_F == default ]
	then
		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
		set MAVLINK_F "-r 1200 -f"

		# Use ttyS1 for MAVLink on FMUv4 in addition to ttyS0 (debug)
		if ver hwcmp PX4FMU_V4
		then
			set MAVLINK_F "-r 1200 -d /dev/ttyS1"
			# Start MAVLink on Wifi (ESP8266 port)
			mavlink start -r 20000 -b 921600 -d /dev/ttyS0
		fi

		# Use ttyS3 (TELEM4) for MAVLink on FMUv5 in addition to ttyS1 (TELEM1) and ttyS2 (TELEM2)
		if ver hwcmp PX4FMU_V5
		then
			mavlink start -r 2000 -b 57600 -d /dev/ttyS3
		fi

		if ver hwcmp AEROFC_V1
		then
			set MAVLINK_F "-r 1200 -d /dev/ttyS3"

			# Only start mavlink if the Benewake TFMini or LeddarOne isn't being used
			if param greater SENS_EN_TFMINI 0
			then
				set MAVLINK_F none
			fi
			if param greater SENS_EN_LEDDAR1 0
			then
				set MAVLINK_F none				
			fi
		fi

		if ver hwcmp CRAZYFLIE OMNIBUS_F4SD
		then
			# Avoid using either of the two available serials
			set MAVLINK_F none
		fi
fi

	if [ "x$MAVLINK_F" == xnone ]
	then
	else
		mavlink start ${MAVLINK_F}
	fi
	unset MAVLINK_F

	#
	# MAVLink onboard / TELEM2
	#
	# XXX We need a better way for runtime eval of shell variables,
	# but this works for now
	if param compare SYS_COMPANION 10
	then
		frsky_telemetry start -d ${MAVLINK_COMPANION_DEVICE}
	else
		if ver hwcmp PX4FMU_V4 PX4FMU_V4PRO MINDPX_V2
		then
			# This is TELEM4 on Pixhawk 3 Pro
			frsky_telemetry start -d /dev/ttyS6
		fi
	fi

	if param compare SYS_COMPANION 20
	then
		syslink start
		mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
	fi
	if param compare SYS_COMPANION 921600
	then
		if ver hwcmp AEROFC_V1
		then
			if protocol_splitter start ${MAVLINK_COMPANION_DEVICE}
			then
				mavlink start -d /dev/mavlink -b 921600 -m onboard -r 5000 -x
				micrortps_client start -d /dev/rtps -b 921600 -l -1 -s 2000
			else
				 mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -m onboard -r 80000 -x -f
			fi
		else
			mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -m onboard -r 80000 -x -f
		fi
	fi
	if param compare SYS_COMPANION 57600
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m onboard -r 5000 -x -f
	fi
	if param compare SYS_COMPANION 460800
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 460800 -m onboard -r 5000 -x -f
	fi
	if param compare SYS_COMPANION 157600
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m osd -r 1000
	fi
	if param compare SYS_COMPANION 257600
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m magic -r 5000 -x -f
	fi
	if param compare SYS_COMPANION 319200
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 19200 -r 1000 -f
	fi
	if param compare SYS_COMPANION 338400
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 38400 -r 1000 -f
	fi
	if param compare SYS_COMPANION 357600
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -r 1000 -f
	fi
	if param compare SYS_COMPANION 3115200
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 115200 -r 1000 -f
	fi
	if param compare SYS_COMPANION 419200
	then
		iridiumsbd start -d ${MAVLINK_COMPANION_DEVICE}
		mavlink start -d /dev/iridium -b 19200 -m iridium -r 10
	fi
	if param compare SYS_COMPANION 519200
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 19200 -m minimal -r 1000
	fi
	if param compare SYS_COMPANION 538400
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 38400 -m minimal -r 1000
	fi
	if param compare SYS_COMPANION 557600
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m minimal -r 1000
	fi
	if param compare SYS_COMPANION 5115200
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 115200 -m minimal -r 1000
	fi
	if param compare SYS_COMPANION 6460800
	then
		micrortps_client start -t UART -d /dev/ttyS2 -b 460800
	fi
	if param compare SYS_COMPANION 1921600
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -r 20000
	fi
	if param compare SYS_COMPANION 1500000
	then
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 1500000 -m onboard -r 10000 -x -f
	fi

	unset MAVLINK_COMPANION_DEVICE

	#
	# Starting stuff according to UAVCAN_ENABLE value
	#
	if param greater UAVCAN_ENABLE 0
	then
		# Start core UAVCAN module
		if uavcan start
		then
			if param greater UAVCAN_ENABLE 1
			then
				# Reduce logger buffer to free up some RAM for UAVCAN servers
				set LOGGER_BUF 6
				# Start UAVCAN firmware update server and dynamic node ID allocation server
				uavcan start fw
			fi
		else
			tone_alarm ${TUNE_ERR}
		fi
	fi

	if ver hwcmp PX4FMU_V2 PX4FMU_V4 PX4FMU_V4PRO MINDPX_V2 PX4FMU_V5 OMNIBUS_F4SD
	then
		# Check for flow sensor - as it is a background task, launch it last
		px4flow start &
	fi

	if ver hwcmp AEROFC_V1
	then
		# don't start mavlink ttyACM0 on aerofc_v1
	else
		# Start MAVLink
		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
	fi

	#
	# Fixed wing setup
	#
	if [ $VEHICLE_TYPE == fw ]
	then
		if [ $MIXER == none ]
		then
			# Set default mixer for fixed wing if not defined
			set MIXER AERT
		fi

		if [ $MAV_TYPE == none ]
		then
			# Use MAV_TYPE = 1 (fixed wing) if not defined
			set MAV_TYPE 1
		fi

		param set MAV_TYPE ${MAV_TYPE}

		# Load mixer and configure outputs
		sh /etc/init.d/rc.interface

		# Start standard fixedwing apps
		sh /etc/init.d/rc.fw_apps
	fi

	#
	# Multicopters setup
	#
	if [ $VEHICLE_TYPE == mc ]
	then
		if [ $MIXER == none ]
		then
			echo "Mixer undefined"
		fi

		if [ $MAV_TYPE == none ]
		then
			# Use mixer to detect vehicle type
			if [ $MIXER == quad_x -o $MIXER == quad_+ ]
			then
				set MAV_TYPE 2
			fi
			if [ $MIXER == quad_w -o $MIXER == quad_dc ]
			then
				set MAV_TYPE 2
			fi
			if [ $MIXER == quad_h ]
			then
				set MAV_TYPE 2
			fi
			if [ $MIXER == tri_y_yaw- -o $MIXER == tri_y_yaw+ ]
			then
				set MAV_TYPE 15
			fi
			if [ $MIXER == hexa_x -o $MIXER == hexa_+ ]
			then
				set MAV_TYPE 13
			fi
			if [ $MIXER == hexa_cox ]
			then
				set MAV_TYPE 13
			fi
			if [ $MIXER == octo_x -o $MIXER == octo_+ ]
			then
				set MAV_TYPE 14
			fi
			if [ $MIXER == octo_cox -o $MIXER == octo_cox_w ]
			then
				set MAV_TYPE 14
			fi
			if [ $MIXER == coax ]
			then
				set MAV_TYPE 3
			fi
		fi

		# Still no MAV_TYPE found
		if [ $MAV_TYPE == none ]
		then
			param set MAV_TYPE 2
		else
			param set MAV_TYPE ${MAV_TYPE}
		fi

		# Load mixer and configure outputs
		sh /etc/init.d/rc.interface

		# Start standard multicopter apps
		sh /etc/init.d/rc.mc_apps
	fi

	#
	# VTOL setup
	#
	if [ $VEHICLE_TYPE == vtol ]
	then
		if [ $MIXER == none ]
		then
			echo "VTOL mixer undefined"
		fi

		if [ $MAV_TYPE == none ]
		then
			# Use mixer to detect vehicle type
			if [ $MIXER == caipirinha_vtol ]
			then
				set MAV_TYPE 19
			fi
			if [ $MIXER == firefly6 ]
			then
				set MAV_TYPE 21
			fi
			if [ $MIXER == quad_x_pusher_vtol ]
			then
				set MAV_TYPE 22
			fi
		fi

		# Still no MAV_TYPE found
		if [ $MAV_TYPE == none ]
		then
			echo "Unknown MAV_TYPE"
			param set MAV_TYPE 19
		else
			param set MAV_TYPE ${MAV_TYPE}
		fi

		# Load mixer and configure outputs
		sh /etc/init.d/rc.interface

		# Start standard vtol apps
		sh /etc/init.d/rc.vtol_apps
	fi

	#
	# UGV setup
	#
	if [ $VEHICLE_TYPE == ugv ]
	then
		if [ $MIXER == none ]
		then
			# Set default mixer for UGV if not defined
			set MIXER ugv_generic
		fi

		if [ $MAV_TYPE == none ]
		then
			# Use MAV_TYPE = 10 (UGV) if not defined
			set MAV_TYPE 10
		fi

		param set MAV_TYPE ${MAV_TYPE}

		# Load mixer and configure outputs
		sh /etc/init.d/rc.interface

		# Start standard UGV apps
		sh /etc/init.d/rc.ugv_apps
	fi

	unset MIXER
	unset MAV_TYPE
	unset OUTPUT_MODE

	#
	# Start the navigator
	#
	navigator start

	#
	# Generic setup (autostart ID not found)
	#
	if [ $VEHICLE_TYPE == none ]
	then
		echo "No autostart ID found"
		ekf2 start
	fi

	# Start any custom addons
	set FEXTRAS /fs/microsd/etc/extras.txt
	if [ -f $FEXTRAS ]
	then
		echo "Addons script: ${FEXTRAS}"
		sh $FEXTRAS
	fi
	unset FEXTRAS

	if ver hwcmp CRAZYFLIE
	then
		# CF2 shouldn't have an sd card
	else

		if ver hwcmp AEROCORE2
		then
			# AEROCORE2 shouldn't have an sd card
		else

			# Run no SD alarm
			if [ $LOG_FILE == /dev/null ]
			then
				# Play SOS
				tone_alarm error
			fi

		fi

	fi

	#
	# Check if we should start a thermal calibration
	# TODO move further up and don't start unnecessary services if we are calibrating
	#
	set TEMP_CALIB_ARGS ""
	if param compare SYS_CAL_GYRO 1
	then
		set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -g"
		param set SYS_CAL_GYRO 0
	fi
	if param compare SYS_CAL_ACCEL 1
	then
		set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -a"
		param set SYS_CAL_ACCEL 0
	fi
	if param compare SYS_CAL_BARO 1
	then
		set TEMP_CALIB_ARGS "${TEMP_CALIB_ARGS} -b"
		param set SYS_CAL_BARO 0
	fi
	if [ "x$TEMP_CALIB_ARGS" != "x" ]
	then
		send_event temperature_calibration ${TEMP_CALIB_ARGS}
	fi
	unset TEMP_CALIB_ARGS

	# vmount to control mounts such as gimbals, disabled by default.
	if param compare MNT_MODE_IN -1
	then
	else
		if vmount start
		then
		fi
	fi

	sh /etc/init.d/rc.logging

# End of autostart
fi

# There is no further script processing, so we can free some RAM
# XXX potentially unset all script variables.
unset TUNE_ERR

# Boot is complete, inform MAVLink app(s) that the system is now fully up and running
mavlink boot_complete