#!nsh
#
# PX4FMU startup script.

#
# 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 TUNE_OUT_ERROR ML<<CP4CP4CP4CP4CP4

#
# Try to mount the microSD card.
#
echo "[init] Looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
	set LOG_FILE /fs/microsd/bootlog.txt
	echo "[init] microSD card mounted at /fs/microsd"
	# 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 ]
then
	echo "[init] Executing init script: $RC_FILE"
	sh $RC_FILE
	set MODE custom
else
	echo "[init] Init script not found: $RC_FILE"
fi

# if this is an APM build then there will be a rc.APM script
# from an EXTERNAL_SCRIPTS build option
if [ -f /etc/init.d/rc.APM ]
then
	if sercon
	then
		echo "[init] USB interface connected"
	fi

	echo "[init] 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"

	#
	# Start CDC/ACM serial driver
	#
	sercon
	
	#
	# Start the ORB (first app to start)
	#
	uorb start
	
	#
	# 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
		echo "[init] Parameters loaded: $PARAM_FILE"
	else
		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
	fi
	
	#
	# Start system state indicator
	#
	if rgbled start
	then
		echo "[init] Using external RGB Led"
	else
		if blinkm start
		then
			echo "[init] Using blinkm"
			blinkm systemstate
		fi
	fi
	
	#
	# Set default values
	#
	set HIL no
	set VEHICLE_TYPE none
	set MIXER none
	set USE_IO yes
	set OUTPUT_MODE none
	set PWM_OUTPUTS none
	set PWM_RATE none
	set PWM_DISARMED none
	set PWM_MIN none
	set PWM_MAX none
	set MKBLCTRL_MODE none
	set FMU_MODE pwm
	set MAV_TYPE none
	
	#
	# Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts
	#
	if param compare SYS_AUTOCONFIG 1
	then
		set DO_AUTOCONFIG yes
	else
		set DO_AUTOCONFIG no
	fi
		
	#
	# Set parameters and env variables for selected AUTOSTART
	#
	if param compare SYS_AUTOSTART 0
	then
		echo "[init] Don't try to find autostart script"
	else
		sh /etc/init.d/rc.autostart
	fi

	#
	# Override parameters from user configuration file
	#
	if [ -f $CONFIG_FILE ]
	then
		echo "[init] Reading config: $CONFIG_FILE"
		sh $CONFIG_FILE
	else
		echo "[init] Config file not found: $CONFIG_FILE"
	fi

	#
	# If autoconfig parameter was set, reset it and save parameters
	#
	if [ $DO_AUTOCONFIG == yes ]
	then
		param set SYS_AUTOCONFIG 0
		param save
	fi
	
	set IO_PRESENT no
	
	if [ $USE_IO == yes ]
	then
		#
		# Check if PX4IO present and update firmware if needed
		#
		if [ -f /etc/extras/px4io-v2_default.bin ]
		then
			set IO_FILE /etc/extras/px4io-v2_default.bin
		else
			set IO_FILE /etc/extras/px4io-v1_default.bin
		fi
		
		if px4io checkcrc $IO_FILE
		then
			echo "[init] PX4IO CRC OK"
			echo "PX4IO CRC OK" >> $LOG_FILE
	
			set IO_PRESENT yes
		else
			echo "[init] Trying to update"
			echo "PX4IO Trying to update" >> $LOG_FILE
			
			tone_alarm MLL32CP8MB
			
			if px4io forceupdate 14662 $IO_FILE
			then
				usleep 500000
				if px4io checkcrc $IO_FILE
				then
					echo "[init] PX4IO CRC OK, update successful"
					echo "PX4IO CRC OK after updating" >> $LOG_FILE
					tone_alarm MLL8CDE
	
					set IO_PRESENT yes
				else
					echo "[init] ERROR: PX4IO update failed"
					echo "PX4IO update failed" >> $LOG_FILE
					tone_alarm $TUNE_OUT_ERROR
				fi
			else
				echo "[init] ERROR: PX4IO update failed"
				echo "PX4IO update failed" >> $LOG_FILE
				tone_alarm $TUNE_OUT_ERROR
			fi
		fi
		
		if [ $IO_PRESENT == no ]
		then
			echo "[init] ERROR: PX4IO not found"
			tone_alarm $TUNE_OUT_ERROR
		fi
	fi
	
	#
	# Set default output if not set
	#
	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
		echo "[init] ERROR: PX4IO not found, disabling output"
		
		# Avoid using ttyS0 for MAVLink on FMUv1
		if hw_ver compare PX4FMU_V1
		then
			set FMU_MODE serial
		fi
	fi

	if [ $OUTPUT_MODE == ardrone ]
	then
		set FMU_MODE gpio_serial
	fi

	if [ $HIL == yes ]
	then
		set OUTPUT_MODE hil
		if hw_ver compare PX4FMU_V1
		then
			set FMU_MODE serial
		fi
	else
		# Try to get an USB console if not in HIL mode
		nshterm /dev/ttyACM0 &
	fi
	
	#
	# Start the Commander (needs to be this early for in-air-restarts)
	#
	commander start
	
	#
	# Start primary output
	#
	set TTYS1_BUSY no
	
	# If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output
	if [ $OUTPUT_MODE != none ]
	then
		if [ $OUTPUT_MODE == io ]
		then
			echo "[init] Use PX4IO PWM as primary output"
			if px4io start
			then
				echo "[init] PX4IO started"
				sh /etc/init.d/rc.io
			else
				echo "[init] ERROR: PX4IO start failed"
				tone_alarm $TUNE_OUT_ERROR
			fi
		fi
		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
		then
			echo "[init] Use FMU as primary output"
			if fmu mode_$FMU_MODE
			then
				echo "[init] FMU mode_$FMU_MODE started"
			else
				echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
				tone_alarm $TUNE_OUT_ERROR
			fi
			
			if hw_ver compare PX4FMU_V1
			then
				if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
				then
					set TTYS1_BUSY yes
				fi
				if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
				then
					set TTYS1_BUSY yes
				fi
			fi
		fi
		if [ $OUTPUT_MODE == mkblctrl ]
		then
			echo "[init] Use MKBLCTRL as primary output"
			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
				echo "[init] MKBLCTRL started"
			else
				echo "[init] ERROR: MKBLCTRL start failed"
				tone_alarm $TUNE_OUT_ERROR
			fi
			
		fi		
		if [ $OUTPUT_MODE == hil ]
		then
			echo "[init] Use HIL as primary output"
			if hil mode_port2_pwm8
			then
				echo "[init] HIL output started"
			else
				echo "[init] ERROR: HIL output start failed"
				tone_alarm $TUNE_OUT_ERROR
			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
					echo "[init] PX4IO started"
					sh /etc/init.d/rc.io
				else
					echo "[init] ERROR: PX4IO start failed"
					tone_alarm $TUNE_OUT_ERROR
				fi
			fi
		else
			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
			then
				if fmu mode_$FMU_MODE
				then
					echo "[init] FMU mode_$FMU_MODE started"
				else
					echo "[init] ERROR: FMU mode_$FMU_MODE start failed"
					tone_alarm $TUNE_OUT_ERROR
				fi
	
				if hw_ver compare PX4FMU_V1
				then
					if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ]
					then
						set TTYS1_BUSY yes
					fi
					if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
					then
						set TTYS1_BUSY yes
					fi
				fi
			fi
		fi
	fi
	
	#
	# MAVLink
	#
	set EXIT_ON_END no
	
	if [ $HIL == yes ]
	then
		sleep 1
		mavlink start -b 230400 -d /dev/ttyACM0 
		usleep 5000
	else
		if [ $TTYS1_BUSY == yes ]
		then
			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
			mavlink start -d /dev/ttyS0
			usleep 5000
			
			# Exit from nsh to free port for mavlink
			set EXIT_ON_END yes
		else
			# Start MAVLink on default port: ttyS1
			mavlink start
			usleep 5000
		fi
	fi
	
	#
	# Start the datamanager
	#
	dataman start
	
	#
	# Start the navigator
	#
	navigator start
	
	#
	# Sensors, Logging, GPS
	#
	echo "[init] Start sensors"
	sh /etc/init.d/rc.sensors

	if [ $HIL == no ]
	then
		echo "[init] Start logging"
		sh /etc/init.d/rc.logging
		
		echo "[init] Start GPS"
		gps start
	fi
	
	#
	# Start up ARDrone Motor interface
	#
	if [ $OUTPUT_MODE == ardrone ]
	then
		ardrone_interface start -d /dev/ttyS1
	fi

	#
	# Fixed wing setup
	#
	if [ $VEHICLE_TYPE == fw ]
	then
		echo "[init] Vehicle type: FIXED WING"
		
		if [ $MIXER == none ]
		then
			# Set default mixer for fixed wing if not defined
			set MIXER FMU_AERT
		fi
		
		if [ $MAV_TYPE == none ]
		then
			# Use MAV_TYPE = 1 (fixed wing) if not defined
			set MAV_TYPE 1
		fi
		
		param set MAV_TYPE $MAV_TYPE
		
		# Load mixer and configure outputs
		sh /etc/init.d/rc.interface
		
		# Start standard fixedwing apps
		sh /etc/init.d/rc.fw_apps
	fi

	#
	# Multicopters setup
	#
	if [ $VEHICLE_TYPE == mc ]
	then
		echo "[init] Vehicle type: MULTICOPTER"

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

		if [ $MAV_TYPE == none ]
		then
			# Use mixer to detect vehicle type
			if [ $MIXER == FMU_quad_x -o $MIXER == FMU_quad_+ ]
			then
				set MAV_TYPE 2
			fi
			if [ $MIXER == FMU_quad_w ]
			then
				set MAV_TYPE 2
			fi
			if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ]
			then
				set MAV_TYPE 13
			fi
			if [ $MIXER == hexa_cox ]
			then
				set MAV_TYPE 13
			fi
			if [ $MIXER == FMU_octo_x -o $MIXER == FMU_octo_+ ]
			then
				set MAV_TYPE 14
			fi
			if [ $MIXER == FMU_octo_cox ]
			then
				set MAV_TYPE 14
			fi
		fi
		
		# Still no MAV_TYPE found
		if [ $MAV_TYPE == none ]
		then
			echo "Unknown MAV_TYPE"
		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

	#
	# Generic setup (autostart ID not found)
	#
	if [ $VEHICLE_TYPE == none ]
	then
		echo "[init] Vehicle type: No autostart ID found"

	fi

	# Start any custom addons
	if [ -f $EXTRAS_FILE ]
	then
		echo "[init] Starting addons script: $EXTRAS_FILE"
		sh $EXTRAS_FILE
	else
		echo "[init] Addons script not found: $EXTRAS_FILE"
	fi

	if [ $EXIT_ON_END == yes ]
	then
		exit
	fi

# End of autostart
fi