#!nsh
#
# PX4FMU startup script.
#
#  NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
#

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

set FRC /fs/microsd/etc/rc.txt
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt

set TUNE_ERR ML<<CP4CP4CP4CP4CP4

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

#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
if [ -f $FRC ]
then
	echo "[i] Executing init script: $FRC"
	sh $FRC
	set MODE custom
else
	echo "[i] Init script not found: $FRC"
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 "[i] USB interface connected"
	fi

	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 "[i] AUTOSTART mode"

	#
	# Start CDC/ACM serial driver
	#
	sercon

	# Try to get an USB console
	nshterm /dev/ttyACM0 &

	#
	# 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 "[param] Loaded: $PARAM_FILE"
	else
		echo "[param] FAILED loading $PARAM_FILE"
		if param reset
		then
		fi
	fi

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

	if pca8574 start
	then
	fi

	#
	# Set default values
	#
	set HIL no
	set VEHICLE_TYPE none
	set MIXER none
	set OUTPUT_MODE none
	set PWM_OUT none
	set PWM_RATE none
	set PWM_DISARMED none
	set PWM_MIN none
	set PWM_MAX none
	set MK_MODE none
	set FMU_MODE pwm
	set MAVLINK_F default
	set EXIT_ON_END no
	set MAV_TYPE none
	set LOAD_DAPPS yes
	set GPS yes
	set GPS_FAKE no
	set FAILSAFE none

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

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

	#
	# Set parameters and env variables for selected AUTOSTART
	#
	if param compare SYS_AUTOSTART 0
	then
		echo "[i] No autostart"
	else
		sh /etc/init.d/rc.autostart
	fi

	#
	# Override parameters from user configuration file
	#
	if [ -f $FCONFIG ]
	then
		echo "[i] Config: $FCONFIG"
		sh $FCONFIG
	else
		echo "[i] Config not found: $FCONFIG"
	fi
	unset FCONFIG

	#
	# If autoconfig parameter was set, reset it and save parameters
	#
	if [ $AUTOCNF == 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 "PX4IO CRC OK" >> $LOG_FILE

			set IO_PRESENT yes
		else
			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 "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
				tone_alarm $TUNE_ERR
			fi
		fi

		if [ $IO_PRESENT == no ]
		then
			echo "[i] ERROR: PX4IO not found"
			tone_alarm $TUNE_ERR
		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 "[i] ERROR: PX4IO not found, disabling output"

		# Avoid using ttyS0 for MAVLink on FMUv1
		if ver hwcmp 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 ver hwcmp PX4FMU_V1
		then
			set FMU_MODE serial
		fi
	fi

	# waypoint storage
	if dataman start
	then
	fi

	# 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 == uavcan_esc ]
		then
			if param compare UAVCAN_ENABLE 0
			then
				echo "[i] OVERRIDING UAVCAN_ENABLE = 1"
				param set UAVCAN_ENABLE 1
			fi
		fi

		if [ $OUTPUT_MODE == io -o $OUTPUT_MODE == uavcan_esc ]
		then
			if px4io start
			then
				echo "[i] PX4IO started"
				sh /etc/init.d/rc.io
			else
				echo "[i] ERROR: PX4IO start failed"
				tone_alarm $TUNE_ERR
			fi
		fi

		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
		then
			if fmu mode_$FMU_MODE
			then
				echo "[i] FMU mode_$FMU_MODE started"
			else
				echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
				tone_alarm $TUNE_ERR
			fi

			if ver hwcmp 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
			set MKBLCTRL_ARG ""
			if [ $MK_MODE == x ]
			then
				set MKBLCTRL_ARG "-mkmode x"
			fi
			if [ $MK_MODE == + ]
			then
				set MKBLCTRL_ARG "-mkmode +"
			fi

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

		fi

		if [ $OUTPUT_MODE == hil ]
		then
			if hil mode_port2_pwm8
			then
				echo "[i] HIL output started"
			else
				echo "[i] ERROR: HIL output start failed"
				tone_alarm $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
					echo "[i] PX4IO started"
					sh /etc/init.d/rc.io
				else
					echo "[i] ERROR: PX4IO start failed"
					tone_alarm $TUNE_ERR
				fi
			fi
		else
			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
			then
				if fmu mode_$FMU_MODE
				then
					echo "[i] FMU mode_$FMU_MODE started"
				else
					echo "[i] ERROR: FMU mode_$FMU_MODE start failed"
					tone_alarm $TUNE_ERR
				fi

				if ver hwcmp 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

	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_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_F "-r 1000"
		fi
	fi

	mavlink start $MAVLINK_F
	unset MAVLINK_F

	#
	# MAVLink onboard / TELEM2
	#
	if ver hwcmp PX4FMU_V2
	then
		if param compare SYS_COMPANION 921600
		then
			mavlink start -d /dev/ttyS2 -b 921600 -m onboard
		fi
	fi

	# UAVCAN
	#
	sh /etc/init.d/rc.uavcan

	#
	# Sensors, Logging, GPS
	#
	sh /etc/init.d/rc.sensors
	sh /etc/init.d/rc.logging

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

	#
	# 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 "[i] FIXED WING"

		if [ $MIXER == none ]
		then
			# Set default mixer for fixed wing if not defined
			set MIXER FMU_AERT
		fi

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

		param set MAV_TYPE $MAV_TYPE

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

		# Start standard fixedwing apps
		if [ $LOAD_DAPPS == yes ]
		then
			sh /etc/init.d/rc.fw_apps
		fi
	fi

	#
	# Multicopters setup
	#
	if [ $VEHICLE_TYPE == mc ]
	then
		echo "[i] MULTICOPTER"

		if [ $MIXER == none ]
		then
			echo "Mixer undefined"
		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 == FMU_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
		if [ $LOAD_DAPPS == yes ]
		then
			sh /etc/init.d/rc.mc_apps
		fi
	fi

	#
	# VTOL setup
	#
	if [ $VEHICLE_TYPE == vtol ]
	then
		echo "[init] Vehicle type: VTOL"

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

		if [ $MAV_TYPE == none ]
		then
			# Use mixer to detect vehicle type
			if [ $MIXER == FMU_caipirinha_vtol ]
			then
				set MAV_TYPE 19
			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 vtol apps
		if [ $LOAD_DAPPS == yes ]
		then
			sh /etc/init.d/rc.vtol_apps
		fi
	fi

	#
	# Start the navigator
	#
	navigator start

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

	fi

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

	if [ $EXIT_ON_END == yes ]
	then
		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