#!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 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
#
#------------------------------------------------------------------------------
#
# UART mapping on OMNIBUSF4SD:
#
# USART1        /dev/ttyS0      SerialRX
# USART4        /dev/ttyS1      TELEM1
# USART6        /dev/ttyS2      GPS
#

#------------------------------------------------------------------------------
#
# UART mapping on NXPhlitev3:
#
# LPUART0		/dev/ttyS0		P16 CONSOLE
# UART0			/dev/ttyS1		P7  IR
# UART1			/dev/ttyS2		P14-3, P15-2 RC input
# UART2			/dev/ttyS3		P3 GPS
# UART4			/dev/ttyS3		P10 TELEM1 (flow control)

#
# Set default paramter values.
#
set AUX_MODE pwm
set DATAMAN_OPT ""
set FAILSAFE none
set FAILSAFE_AUX none
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set FRC /fs/microsd/etc/rc.txt
set FMU_ARGS ""
set FMU_MODE pwm
set IO_FILE ""
set IO_PRESENT no
set LOG_FILE /fs/microsd/bootlog.txt
set MAVLINK_F default
set MAVLINK_COMPANION_DEVICE /dev/ttyS2
set MAV_TYPE none
set MIXER none
set MIXER_AUX none
set MIXER_FILE none
set MK_MODE none
set MKBLCTRL_ARG ""
set OUTPUT_MODE none
set PARAM_FILE /fs/microsd/params
set PWM_AUX_DISARMED p:PWM_AUX_DISARMED
set PWM_AUX_MAX p:PWM_AUX_MAX
set PWM_AUX_MIN p:PWM_AUX_MIN
set PWM_AUX_OUT none
set PWM_AUX_RATE p:PWM_AUX_RATE
set PWM_DISARMED p:PWM_DISARMED
set PWM_MAX p:PWM_MAX
set PWM_MIN p:PWM_MIN
set PWM_OUT none
set PWM_RATE p:PWM_RATE
set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers
set USE_IO no
set VEHICLE_TYPE none
set TUNE_ERR "ML<<CP4CP4CP4CP4CP4"
#
# Mount the procfs.
#
mount -t procfs /proc

#
# Start CDC/ACM serial driver.
#
sercon

#
# Print full system version.
#
ver all

#
# 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
		# Error tune.
		tune_control play -t 2

		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"
			tune_control play -m MNBG
			set LOG_FILE /dev/null
		fi
	else
		set LOG_FILE /dev/null
	fi
fi

# AEROCORE2 shouldn't have an sd card and CF2 may optionally have an sd card.
if ! ver hwcmp CRAZYFLIE AEROCORE2
then
	# Run no SD alarm.
	if [ $LOG_FILE == /dev/null ]
	then
		# Error tune.
		tune_control play -t 2
	fi
fi

#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
if [ -f $FRC ]
then
	sh $FRC
else
	#
	# Start the ORB (first app to start).
	#
	uorb start

	#
	# Start the tone_alarm driver.
	#
	tone_alarm start

	#
	# Play the startup tone.
	#
	tune_control play -t 1

	#
	# Waypoint storage.
	# REBOOTWORK this needs to start in parallel.
	#
	dataman start $DATAMAN_OPT

	#
	# Start the socket communication send_event handler.
	#
	send_event start

	#
	# Start the resource load monitor.
	#
	load_mon start

	#
	# Set the parameter file if mtd starts successfully.
	#
	if mtd start
	then
		set PARAM_FILE /fs/mtd_params
	fi

	#
	# Load parameters.
	#
	param select $PARAM_FILE
	if ! param load
	then
		param reset
	fi

	#
	# Start system state indicator.
	#
	if ! rgbled start
	then
		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_* TC_* CAL_ACC* CAL_GYRO*
		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

	###############################################################################
	#               Begin setup for board specific configurations.                #
	###############################################################################

	if ver hwcmp AEROCORE2
	then
		set DATAMAN_OPT "-f /fs/mtd_dataman"
	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

		set DATAMAN_OPT -i
	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 NXPHLITE_V3
	then
		param set SYS_FMU_TASK 1
	fi

	###############################################################################
	#                 End Setup for board specific configurations.                #
	###############################################################################

	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
		sh /etc/init.d/rc.autostart
	fi

	#
	# Override parameters from user configuration file.
	#
	if [ -f $FCONFIG ]
	then
		echo "Custom: ${FCONFIG}"
		sh $FCONFIG
	fi

	#
	# If autoconfig parameter was set, reset it and save parameters.
	#
	if [ $AUTOCNF == yes ]
	then
		# 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

		# Disable safety switch by default on Pixracer and OmnibusF4SD.
		if ver hwcmp PX4FMU_V4 OMNIBUS_F4SD
		then
			param set CBRK_IO_SAFETY 22027
		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

	#
	# 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
			tune_control play -m MLL32CP8MB

			if px4io start
			then
				# Try to safety px4 io so motor outputs don't go crazy.
				if ! px4io safety_on
				then
					# 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
					tune_control play -m MLL8CDE

					set IO_PRESENT yes
				else
					echo "PX4IO update failed" >> $LOG_FILE
					# Error tune.
					tune_control play -t 2
				fi
			else
				echo "PX4IO update failed" >> $LOG_FILE
				# Error tune.
				tune_control play -t 2
			fi
		fi
	fi

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

	if [ $USE_IO == yes -a $IO_PRESENT == no ]
	then
		echo "PX4IO not found" >> $LOG_FILE
		# Error tune.
		tune_control play -t 2
	fi

	if [ $IO_PRESENT == no -o $USE_IO == no ]
	then
		rc_input start
	fi

	#
	# 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

	# 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

	#
	# Check if UAVCAN is enabled, default to it for ESCs.
	#
	if param greater UAVCAN_ENABLE 0
	then
		# Start core UAVCAN module.
		if uavcan start
		then
			if param greater UAVCAN_ENABLE 1
			then
				# Start UAVCAN firmware update server and dynamic node ID allocation server.
				uavcan start fw

				if param greater UAVCAN_ENABLE 2
				then
					set OUTPUT_MODE uavcan_esc
				fi
			fi
		else
			# Error tune.
			tune_control play -t 2
		fi
	fi

	#
	# Start mavlink streams.
	#
	sh /etc/init.d/rc.mavlink

	#
	# Configure vehicle type specific parameters.
	# Note: rc.vehicle_setup is the entry point for rc.interface,
	#       rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps.
	#
	sh /etc/init.d/rc.vehicle_setup

	#
	# Start the navigator.
	#
	navigator start

	#
	# Start the standalone wind estimator.
	#
	if param compare WEST_EN 1
	then
		wind_estimator start
	fi

	#
	# Start a thermal calibration if required.
	#
	sh /etc/init.d/rc.thermal_cal

	#
	# Start the logger.
	#
	sh /etc/init.d/rc.logging

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

	#
	# Launch the flow sensor as a background task.
	#
	if ver hwcmp PX4FMU_V2 PX4FMU_V4 PX4FMU_V4PRO MINDPX_V2 PX4FMU_V5 OMNIBUS_F4SD
	then
		px4flow start &
	fi

	#
	# Start any custom addons.
	#
	if [ -f $FEXTRAS ]
	then
		echo "Addons script: ${FEXTRAS}"
		sh $FEXTRAS
	fi
#
# End of autostart.
#
fi

#
# Unset all script parameters to free RAM.
#
unset AUX_MODE
unset DATAMAN_OPT
unset FAILSAFE
unset FAILSAFE_AUX
unset FCONFIG
unset FEXTRAS
unset FRC
unset FMU_ARGS
unset FMU_MODE
unset IO_FILE
unset IO_PRESENT
unset LOG_FILE
unset MAVLINK_F
unset MAVLINK_COMPANION_DEVICE
unset MAV_TYPE
unset MIXER
unset MIXER_AUX
unset MIXER_FILE
unset MK_MODE
unset MKBLCTRL_ARG
unset OUTPUT_DEV
unset OUTPUT_MODE
unset PARAM_FILE
unset PWM_AUX_DISARMED
unset PWM_AUX_MAX
unset PWM_AUX_MIN
unset PWM_AUX_OUT
unset PWM_AUX_RATE
unset PWM_OUT
unset PWM_RATE
unset PWM_DISARMED
unset PWM_MAX
unset PWM_MIN
unset SDCARD_MIXERS_PATH
unset USE_IO
unset VEHICLE_TYPE
unset TUNE_ERR

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