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

#
# Start CDC/ACM serial driver
#
sercon

#
# 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
	# Start playing the startup tune
	tone_alarm start
else
	tone_alarm MBAGP
	if mkfatfs /dev/mmcsd0
	then
		if mount -t vfat /dev/mmcsd0 /fs/microsd
		then
			echo "[i] microSD card formatted"
		else
			echo "[i] 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
	echo "[i] Executing script: $FRC"
	sh $FRC
	set MODE custom
fi
unset FRC

if [ $MODE == autostart ]
then

	#
	# 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
	else
		if param reset
		then
		fi
	fi

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

	# Currently unused, but might be useful down the road
	#if pca8574 start
	#then
	#fi

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

	#
	# Set default values
	#
	set HIL no
	set VEHICLE_TYPE none
	set MIXER none
	set MIXER_AUX 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 PWM_AUX_OUT none
	set PWM_AUX_RATE none
	set PWM_ACHDIS none
	set PWM_AUX_DISARMED none
	set PWM_AUX_MIN none
	set PWM_AUX_MAX none
	set FAILSAFE_AUX none
	set MK_MODE none
	set FMU_MODE pwm
	set AUX_MODE pwm
	set MAVLINK_F default
	set EXIT_ON_END no
	set MAV_TYPE none
	set FAILSAFE none
	set USE_IO yes

	#
	# Set USE_IO flag
	#
	if param compare SYS_USE_IO 1
	then
		if ver hwcmp PX4FMU_V4
		then
			set USE_IO no
		fi

		if ver hwcmp MINDPX_V2
		then
			set USE_IO no
		fi
	else
		set USE_IO no
	fi

	# should set to 0.8 for mindpx-v2 borad.
	if param compare INAV_LIDAR_ERR 0.5
	then
		if ver hwcmp MINDPX_V2
		then
			param set INAV_LIDAR_ERR 0.8
			param save
		fi
	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
	unset MODE

	#
	# Wipe incompatible settings for boards not having two outputs
	if ver hwcmp PX4FMU_V4
	then
		set MIXER_AUX none
	fi

	#
	# Override parameters from user configuration file
	#
	set FCONFIG /fs/microsd/etc/config.txt
	if [ -f $FCONFIG ]
	then
		echo "[i] Custom: $FCONFIG"
		sh $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
	unset AUTOCNF

	set IO_PRESENT no

	if [ $USE_IO == yes ]
	then
		#
		# 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
		else
			set IO_FILE /etc/extras/px4io-v1.bin
		fi

		if px4io checkcrc ${IO_FILE}
		then
			echo "PX4IO CRC OK" >> $LOG_FILE

			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 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
		unset IO_FILE

		if [ $IO_PRESENT == no ]
		then
			echo "ERROR: PX4IO not found" >> $LOG_FILE
			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

		# 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
		unset HIL
	else
		unset HIL
		gps start
	fi

	# waypoint storage
	# REBOOTWORK this needs to start in parallel
	if dataman start
	then
	fi

	#
	# Sensors System (start before Commander so Preflight checks are properly run)
	#
	sh /etc/init.d/rc.sensors

	# Needs to be this early for in-air-restarts
	if [ $OUTPUT_MODE == hil ]
	then
	    commander start -hil
	else
	    commander start
	fi

	#
	# Start CPU load monitor
	#
	load_mon start

	#
	# Start primary output
	#
	set TTYS1_BUSY no

	#
	# Check if UAVCAN is enabled, default to it for ESCs
	#
	if param greater UAVCAN_ENABLE 2
	then
		set OUTPUT_MODE uavcan_esc
	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
				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
				sh /etc/init.d/rc.io
			else
				echo "PX4IO start failed" >> $LOG_FILE
				tone_alarm $TUNE_ERR
			fi
		fi

		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == ardrone ]
		then
			if fmu mode_$FMU_MODE
			then
			else
				echo "FMU start failed" >> $LOG_FILE
				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 [ $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
				echo "ERROR: MK start failed" >> $LOG_FILE
				tone_alarm $TUNE_ERR
			fi
			unset MKBLCTRL_ARG
		fi
		unset MK_MODE

		if [ $OUTPUT_MODE == hil ]
		then
			if pwm_out_sim mode_port2_pwm8
			then
			else
				echo "PWM SIM start failed" >> $LOG_FILE
				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
					sh /etc/init.d/rc.io
				else
					echo "PX4IO start failed" >> $LOG_FILE
					tone_alarm $TUNE_ERR
				fi
			fi
		else
			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
			then
				if fmu mode_$FMU_MODE
				then
				else
					echo "FMU mode_$FMU_MODE start failed" >> $LOG_FILE
					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 1200 -d /dev/ttyS0"

			# Exit from nsh to free port for mavlink
			set EXIT_ON_END yes
		else
			set MAVLINK_F "-r 1200"
			# Avoid using ttyS1 for MAVLink on FMUv4
			if ver hwcmp PX4FMU_V4
			then
				set MAVLINK_F "-r 1200 -d /dev/ttyS1"
				# Start MAVLink on Wifi (ESP8266 port)
				mavlink start -r 20000 -m config -b 921600 -d /dev/ttyS0
			fi
		fi
	fi

	mavlink start $MAVLINK_F
	unset MAVLINK_F

	#
	# MAVLink onboard / TELEM2
	#
	if ver hwcmp PX4FMU_V1
	then
	else
		# 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 /dev/ttyS2
		fi
		if param compare SYS_COMPANION 921600
		then
			mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 80000 -x
		fi
		if param compare SYS_COMPANION 57600
		then
			mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 5000 -x
		fi
		if param compare SYS_COMPANION 157600
		then
			mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
		fi
		if param compare SYS_COMPANION 257600
		then
			mavlink start -d /dev/ttyS2 -b 57600 -m magic -r 5000 -x
		fi
		if param compare SYS_COMPANION 357600
		then
			mavlink start -d /dev/ttyS2 -b 57600 -r 1000
		fi
		if param compare SYS_COMPANION 1921600
		then
			mavlink start -d /dev/ttyS2 -b 921600 -r 20000
		fi
		# Sensors on the PWM interface bank
		# clear pins 5 and 6
		if param compare SENS_EN_LL40LS 1
		then
			set AUX_MODE pwm4
		fi
		if param greater TRIG_MODE 0
		then
			# Get FMU driver out of the way
			set MIXER_AUX none
			set AUX_MODE none
			camera_trigger start
		fi
	fi

	#
	# Starting stuff according to UAVCAN_ENABLE value
	#
	if param greater UAVCAN_ENABLE 0
	then
		if uavcan start
		then
		else
			tone_alarm $TUNE_ERR
		fi
	fi

	if param greater UAVCAN_ENABLE 1
	then
		if uavcan start fw
		then
		else
			tone_alarm $TUNE_ERR
		fi
	fi

	#
	# Optional drivers
	#

	# Sensors on the PWM interface bank
	if param compare SENS_EN_LL40LS 1
	then
		if pwm_input start
		then
			if ll40ls start pwm
			then
			fi
		fi
	fi

	# sf0x lidar sensor
	if param compare SENS_EN_SF0X 1
	then
		sf0x start
	fi

	if ver hwcmp PX4FMU_V4
	then
		frsky_telemetry start -d /dev/ttyS6
	fi

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

	if ver hwcmp MINDPX_V2
	then
		#mindxp also need flow
		px4flow start &
	fi

	# Start USB shell if no microSD present, MAVLink else
	if [ $LOG_FILE == /dev/null ]
	then
		# Try to get an USB console
		nshterm /dev/ttyACM0 &
	else
		mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
	fi

	#
	# Logging
	#
	if ver hwcmp PX4FMU_V1
	then
		if sdlog2 start -r 30 -a -b 2 -t
		then
		fi
	else
		# check if we should increase logging rate for ekf2 replay message logging
		if param greater EKF2_REC_RPL 0
		then
			if param compare SYS_LOGGER 0
			then
				if sdlog2 start -r 500 -e -b 18 -t
				then
				fi
			else
				if logger start -r 500
				then
				fi
			fi
		else
			if param compare SYS_LOGGER 0
			then
				if sdlog2 start -r 100 -a -b 9 -t
				then
				fi
			else
				if logger start -b 9
				then
				fi
			fi
		fi
	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 "FIXED WING"

		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
		echo "MULTICOPTER"

		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 == sk450_deadcat ]
			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
			echo "Unknown MAV_TYPE"
			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
		echo "VTOL"

		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

	#
	# Rover setup
	#
	if [ $VEHICLE_TYPE == rover ]
	then
		# 10 is MAV_TYPE_GROUND_ROVER
		set MAV_TYPE 10

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

		# Start standard rover apps
		sh /etc/init.d/rc.axialracing_ax10_apps

		param set MAV_TYPE 10
	fi

	#
	# For snapdragon, we need a passthrough mode
	# Do not run any mavlink instances since we need the serial port for
	# communication with Snapdragon.
	#
	if [ $VEHICLE_TYPE == passthrough ]
	then
		mavlink stop-all
		commander stop

		# Stop multicopter attitude controller if it is running, the controls come
		# from Snapdragon.
		if mc_att_control stop
		then
		fi

		# Start snapdragon interface on serial port.
		if ver hwcmp PX4FMU_V2
		then
			# On Pixfalcon use the standard telemetry port (Telem 1).
			snapdragon_rc_pwm start -d /dev/ttyS1
			px4io start
		fi

		if ver hwcmp PX4FMU_V4
		then
			# On Pixracer use Telem 2 port (TL2).
			snapdragon_rc_pwm start -d /dev/ttyS2
			fmu mode_pwm4
		fi

		pwm failsafe -c 1234 -p 900
		pwm disarmed -c 1234 -p 900

		# Arm straightaway.
		pwm arm
		# Use 400 Hz PWM on all channels.
		pwm rate -a -r 400
	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 "[i] No autostart ID found"
	fi

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

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

# 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

if [ $EXIT_ON_END == yes ]
then
	echo "NSH EXIT"
	exit
fi
unset EXIT_ON_END