#!nsh
#
# PX4FMU startup script.

#
# Default to auto-start mode.  An init script on the microSD card
# can change this to prevent automatic startup of the flight script.
#
set MODE autostart

set logfile /fs/microsd/bootlog.txt

#
# Try to mount the microSD card.
#
echo "[init] looking for microSD..."
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
	echo "[init] card mounted at /fs/microsd"
	# Start playing the startup tune
	tone_alarm start
else
	echo "[init] no microSD card found"
	# Play SOS
	tone_alarm error
fi

#
# Look for an init script on the microSD card.
#
# To prevent automatic startup in the current flight mode,
# the script should set MODE to some other value.
#
if [ -f /fs/microsd/etc/rc ]
then
	echo "[init] reading /fs/microsd/etc/rc"
	sh /fs/microsd/etc/rc
fi
# Also consider rc.txt files
if [ -f /fs/microsd/etc/rc.txt ]
then
	echo "[init] reading /fs/microsd/etc/rc.txt"
	sh /fs/microsd/etc/rc.txt
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 "Running rc.APM"
	# if APM startup is successful then nsh will exit
	sh /etc/init.d/rc.APM
fi

if [ $MODE == autostart ]
then
	#
	# Start CDC/ACM serial driver
	#
	sercon
	
	#
	# Start the ORB (first app to start)
	#
	uorb start
	
	#
	# Load microSD params
	#
	#if ramtron start
	#then
	#	param select /ramtron/params
	#	if [ -f /ramtron/params ]
	#	then
	#		param load /ramtron/params
	#	fi
	#else
		param select /fs/microsd/params
		if [ -f /fs/microsd/params ]
		then
			if param load /fs/microsd/params
			then
				echo "Parameters loaded"
			else
				echo "Parameter file corrupt - ignoring"
			fi
		fi
	#fi
	
	#
	# Start system state indicator
	#
	if rgbled start
	then
		echo "Using external RGB Led"
	else
		if blinkm start
		then
			blinkm systemstate
		fi
	fi

	set USE_IO no
	set FRAME_TYPE none
	set PWM_RATE none
	set PWM_DISARMED none
	set PWM_MIN none
	set PWM_MAX none
	
	if param compare SYS_AUTOCONFIG 1
	then
		set DO_AUTOCONFIG yes
	else
		set DO_AUTOCONFIG no
	fi
	
	#
	# Start the Commander (needs to be this early for in-air-restarts)
	#
	commander start
	
	#
	# Set parameters and env variables for selected AUTOSTART (HIL setups)
	#
	sh /etc/init.d/rc.autostart_hil
	
	if [ $MODE == hil ]
	then
		#
		# Do common HIL setup depending on env variables
		#
		# Allow USB some time to come up
		sleep 1

		# Start MAVLink on USB port
		mavlink start -b 230400 -d /dev/ttyACM0
		usleep 5000
		
		# Create a fake HIL /dev/pwm_output interface
		hil mode_pwm
		
		# Sensors
		echo "Start sensors"
		sh /etc/init.d/rc.sensors
		
		#
		# Fixed wing setup
		#
		if [ $FRAME_TYPE == fw ]
		then
			echo "Setup FIXED WING"
		fi
	
		#
		# Multicopters setup
		#
		if [ $FRAME_TYPE == mc ]
		then
			echo "Setup MULTICOPTER"
			
			# Load mixer and configure outputs
			sh /etc/init.d/rc.mc_interface
			
			# Start common multicopter apps
			sh /etc/init.d/rc.mc_apps
		fi
	else
		# Try to get an USB console if not in HIL mode
		nshterm /dev/ttyACM0 &
	fi
		
	#
	# Upgrade PX4IO firmware
	#
	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 start
	then
		echo "PX4IO OK"
		echo "PX4IO OK" >> $logfile
	fi
	
	if px4io checkcrc $io_file
	then
		echo "PX4IO CRC OK"
		echo "PX4IO CRC OK" >> $logfile
		set USE_IO yes
	else
		echo "PX4IO CRC failure"
		echo "PX4IO CRC failure" >> $logfile
		tone_alarm MBABGP
		if px4io forceupdate 14662 $io_file
		then
			usleep 500000
			if px4io start
			then
				echo "PX4IO restart OK"
				echo "PX4IO restart OK" >> $logfile
				tone_alarm MSPAA
				set USE_IO yes
			else
				echo "PX4IO restart failed"
				echo "PX4IO restart failed" >> $logfile
				if hw_ver compare PX4FMU_V2
				then
					tone_alarm MNGGG
					sleep 10
					reboot
				fi
			fi
		else
			echo "PX4IO update failed"
			echo "PX4IO update failed" >> $logfile
			tone_alarm MNGGG
		fi
	fi

	set EXIT_ON_END no
	
	#
	# Set parameters and env variables for selected AUTOSTART
	#
	sh /etc/init.d/rc.autostart
	
	#
	# If autoconfig parameter was set, reset it and save parameters
	#
	if [ $DO_AUTOCONFIG == yes ]
	then
		param set SYS_AUTOCONFIG 0
		param save
	fi
	
	if [ $MODE == autostart ]
	then	
		#
		# Do common setup depending on env variables
		#
		if [ $USE_IO == yes ]
		then
			echo "Use IO"
			
			# Start MAVLink on default port: ttyS1
			mavlink start
			usleep 5000	
			
			sh /etc/init.d/rc.io
		else
			echo "Don't use IO"
			
			# Start MAVLink on ttyS0
			mavlink start -d /dev/ttyS0
			usleep 5000
			
			# Configure FMU for PWM outputs
			fmu mode_pwm
			
			# Exit from nsh to free port for mavlink
			set EXIT_ON_END yes
		fi
		
		# Sensors
		echo "Start sensors"
		sh /etc/init.d/rc.sensors
		
		# Logging
		sh /etc/init.d/rc.logging
		
		# GPS interface
		gps start
		
		#
		# Fixed wing setup
		#
		if [ $FRAME_TYPE == fw ]
		then
			echo "Setup FIXED WING"
		fi
	
		#
		# Multicopters setup
		#
		if [ $FRAME_TYPE == mc ]
		then
			echo "Setup MULTICOPTER"
			
			# Load mixer and configure outputs
			sh /etc/init.d/rc.mc_interface
			
			# Start common multicopter apps
			sh /etc/init.d/rc.mc_apps
		fi
	fi

	# Start any custom extensions
	if [ -f /fs/microsd/etc/rc.local ]
	then
		sh /fs/microsd/etc/rc.local
	fi
	
	# If none of the autostart scripts triggered, get a minimal setup
	if [ $MODE == autostart ]
	then
		# Telemetry port is on both FMU boards ttyS1
		# but the AR.Drone motors can be get 'flashed'
		# if starting MAVLink on them - so do not
		# start it as default (default link: USB)
	
		# Start commander
		commander start
	
		# Start px4io if present
		if px4io detect
		then
			px4io start
		else
			if fmu mode_serial
			then
				echo "FMU driver (no PWM) started"
			fi
		fi
	
		# Start sensors
		sh /etc/init.d/rc.sensors
	
		# Start one of the estimators
		attitude_estimator_ekf start
	
		# Start GPS
		gps start
	
	fi

	if [ $EXIT_ON_END == yes ]
	then
		exit
	fi

# End of autostart
fi