#!/bin/sh
# 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.
#
#------------------------------------------------------------------------------

#
# Set default paramter values.
# Do not add intra word spaces
# it wastes flash
#
set R /
set AUTOCNF no
set AUX_MODE pwm
set FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set FMU_MODE pwm
set FRC /fs/microsd/etc/rc.txt
set IOFW "/etc/extras/px4_io-v2_default.bin"
set IO_PRESENT no
set LOGGER_ARGS ""
set LOGGER_BUF  14
set MAV_TYPE none
set MIXER none
set MIXER_AUX none
set MIXER_FILE none
set MIXER_EXTRA none
set OUTPUT_MODE none
set PARAM_FILE /fs/microsd/params
set PWM_OUT none
set PWM_MAIN_RATE p:PWM_MAIN_RATE
set PWM_AUX_OUT none
set PWM_AUX_RATE p:PWM_AUX_RATE
set PWM_EXTRA_OUT none
set PWM_EXTRA_RATE p:PWM_EXTRA_RATE
set EXTRA_MIXER_MODE none
set RC_INPUT_ARGS ""
set SDCARD_MIXERS_PATH /fs/microsd/etc/mixers
set STARTUP_TUNE 1
set USE_IO no
set VEHICLE_TYPE none

#
# 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.
		set STARTUP_TUNE 2
		if hardfault_log commit
		then
			hardfault_log reset
		fi
	fi

	# Prevent MacOS and Ubuntu from creating unnecessary temporary files on the microSD card

	# block MacOS Spotlight indexing (.Spotlight-V100 folder)
	if [ ! -f "/fs/microsd/.metadata_never_index" ]; then
		cat > /fs/microsd/.metadata_never_index
	fi

	# block MacOS trashes
	if [ ! -f "/fs/microsd/.Trashes" ]; then
		cat > /fs/microsd/.Trashes
	fi

	# block MacOS logging of filesystem events
	if [ ! -d "/fs/microsd/.fseventsd" ]; then
		mkdir /fs/microsd/.fseventsd
	fi

	if [ ! -f "/fs/microsd/.fseventsd/no_log" ]; then
		cat > /fs/microsd/.fseventsd/no_log
	fi

	# block Ubuntu trash
	if [ ! -f "/fs/microsd/.Trash-1000" ]; then
		cat > /fs/microsd/.Trash-1000
	fi

else
	# tune SD_INIT
	set STARTUP_TUNE 14 # tune 14 = SD_INIT
	if mkfatfs /dev/mmcsd0
	then
		if mount -t vfat /dev/mmcsd0 /fs/microsd
		then
			echo "INFO [init] card formatted"
		else
			set STARTUP_TUNE 15 # tune 15 = SD_ERROR
			echo "ERROR [init] format failed"
		fi
	fi
fi

#
# Look for an init script on the microSD card.
# Disable autostart if the script found.
#
if [ -f $FRC ]
then
	. $FRC
else

	#
	# Set the parameter file the board supports params on
	# MTD device.
	#
	if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
	then
		set PARAM_FILE /fs/mtd_params
	fi

	#
	# Load parameters.
	#
	# if the board has a storage for (factory) calibration data
	if mft query -q -k MTD -s MTD_CALDATA -v /fs/mtd_caldata
	then
		param load /fs/mtd_caldata
	fi

	param select $PARAM_FILE
	if ! param import
	then
		param reset_all
	fi
	if ver hwtypecmp V5X00 V5X90 V5Xa0
	then
		netman update -i eth0
	fi

	#
	# Set AUTOCNF flag to use it in AUTOSTART scripts.
	#
	if param greater SYS_AUTOCONFIG 0
	then
		# Wipe out params except RC*, flight modes, total flight time, calibration parameters, next flight UUID
		param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT*
		set AUTOCNF yes
	fi

	#
	# Optional board architecture defaults: rc.board_arch_defaults
	#
	set BOARD_ARCH_RC_DEFAULTS ${R}etc/init.d/rc.board_arch_defaults
	if [ -f $BOARD_ARCH_RC_DEFAULTS ]
	then
		echo "Board architecture defaults: ${BOARD_ARCH_RC_DEFAULTS}"
		. $BOARD_ARCH_RC_DEFAULTS
	fi
	unset BOARD_ARCH_RC_DEFAULTS

	#
	# Optional board defaults: rc.board_defaults
	#
	set BOARD_RC_DEFAULTS ${R}etc/init.d/rc.board_defaults
	if [ -f $BOARD_RC_DEFAULTS ]
	then
		echo "Board defaults: ${BOARD_RC_DEFAULTS}"
		. $BOARD_RC_DEFAULTS
	fi
	unset BOARD_RC_DEFAULTS

	#
	# Start the tone_alarm driver.
	# Needs to be started after the parameters are loaded (for CBRK_BUZZER).
	#
	tone_alarm start

	#
	# Play the startup tune (if not disabled or there is an error)
	#
	param compare CBRK_BUZZER 782090
	if [ $? != 0 -o $STARTUP_TUNE != 1 ]
	then
		tune_control play -t $STARTUP_TUNE
	fi

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

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

	#
	# Start the resource load monitor.
	#
	load_mon start

	#
	# Start system state indicator.
	#
	rgbled start -X -q
	rgbled_ncp5623c start -X -q

	#
	# Set parameters and env variables for selected AUTOSTART.
	#
	if ! param compare SYS_AUTOSTART 0
	then
		. ${R}etc/init.d/rc.autostart
	fi

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

	#
	# If autoconfig parameter was set, reset it and save parameters.
	#
	if [ $AUTOCNF = yes ]
	then
		param set SYS_AUTOCONFIG 0
	fi

	#
	# Check if UAVCAN is enabled, default to it for ESCs.
	#
	if param greater -s 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
				tune_control play -t 1
				if param greater UAVCAN_ENABLE 2
				then
					set OUTPUT_MODE uavcan_esc
				fi
			fi
		else
			tune_control play error
		fi
	else
		if param greater -s UAVCAN_V1_ENABLE 0
		then
			uavcan_v1 start
		fi
	fi

	#
	# Check if PX4IO present and update firmware if needed.
	# Assumption IOFW set to firmware file and IO_PRESENT = no
	#

	if [ -f $IOFW ]
	then
		# Check for the mini using build with px4io fw file
		# but not a px4IO
		if ver hwtypecmp V540 V560
		then
			param set SYS_USE_IO 0
		else
			if px4io checkcrc ${IOFW}
			then
				set IO_PRESENT yes
			else
				# tune Program PX4IO
				tune_control play -t 16 # tune 16 = PROG_PX4IO

				if px4io update ${IOFW}
				then
					usleep 10000
					tune_control stop
					if px4io checkcrc ${IOFW}
					then
						tune_control play -t 17 # tune 17 = PROG_PX4IO_OK
						set IO_PRESENT yes
					else
						tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
					fi
				else
					tune_control stop
				fi
			fi
		fi
	fi

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

	if [ $USE_IO = yes -a $IO_PRESENT = no ]
	then
		echo "PX4IO not found"
		tune_control play error
	fi

	#
	# RC update (map raw RC input to calibrate manual control)
	#  start before commander
	#
	rc_update start

	#
	# Sensors System (start before Commander so Preflight checks are properly run).
	# Commander needs to be this early for in-air-restarts.
	#
	if param greater SYS_HITL 0
	then
		set OUTPUT_MODE hil
		sensors start -h
		commander start -h
		# disable GPS
		param set GPS_1_CONFIG 0

		# start the simulator in hardware if needed
		if param compare SYS_HITL 2
		then
			sih start
		fi

	else
		#
		# board sensors: rc.sensors
		#
		set BOARD_RC_SENSORS ${R}etc/init.d/rc.board_sensors
		if [ -f $BOARD_RC_SENSORS ]
		then
			echo "Board sensors: ${BOARD_RC_SENSORS}"
			. $BOARD_RC_SENSORS
		fi
		unset BOARD_RC_SENSORS

		. ${R}etc/init.d/rc.sensors

		if param compare -s BAT1_SOURCE 2
		then
			esc_battery start
		fi

		if ! param compare BAT1_SOURCE 1
		then
			battery_status start
		fi

		commander start
	fi

	# Sensors on the PWM interface bank.
	if param compare -s SENS_EN_LL40LS 1
	then
		# Clear pins 5 and 6.
		set FMU_MODE pwm4
		set AUX_MODE pwm4
	fi


	# Check if ATS is enabled
	if param compare FD_EXT_ATS_EN 1
	then
		# Clear pins 5 and 6.
		set FMU_MODE pwm4
		set AUX_MODE pwm4
	fi

	if param greater -s TRIG_MODE 0
	then
		# We ONLY support trigger on pins 5+6 or 7+8 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
			if param compare TRIG_PINS 78
			then
				# clear pins 7 and 8
				set FMU_MODE pwm6
				set AUX_MODE pwm6
			else
				set FMU_MODE none
				set AUX_MODE none
			fi
		fi

		camera_trigger start
		camera_feedback start
	fi

	#
	# Optional board mavlink streams: rc.board_mavlink
	#
	set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink
	if [ -f $BOARD_RC_MAVLINK ]
	then
		echo "Board extras: ${BOARD_RC_MAVLINK}"
		. $BOARD_RC_MAVLINK
	fi
	unset BOARD_RC_MAVLINK

	#
	# Start UART/Serial device drivers.
	# Note: rc.serial is auto-generated from Tools/serial/generate_config.py
	#
	. ${R}etc/init.d/rc.serial

	if [ $IO_PRESENT = no ]
	then
		# Must be started after the serial config is read
		rc_input start $RC_INPUT_ARGS
	fi

	#
	# Configure vehicle type specific parameters.
	# Note: rc.vehicle_setup is the entry point for rc.interface,
	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
	#
	. ${R}etc/init.d/rc.vehicle_setup

	# Camera capture driver
	if param greater -s CAM_CAP_FBACK 0
	then
		if camera_capture start
		then
			camera_capture on
		fi
	fi

	#
	# Start the navigator.
	#
	navigator start

	#
	# Start a thermal calibration if required.
	#
	. ${R}etc/init.d/rc.thermal_cal

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

	# Check for flow sensor
	if param compare -s SENS_EN_PX4FLOW 1
	then
		px4flow start -X
	fi

	# Blacksheep telemetry
	if param compare -s TEL_BST_EN 1
	then
		bst start -X
	fi

	if param compare -s IMU_GYRO_FFT_EN 1
	then
		gyro_fft start
	fi

	if param compare -s IMU_GYRO_CAL_EN 1
	then
		gyro_calibration start
	fi

	#
	# Optional board supplied extras: rc.board_extras
	#
	set BOARD_RC_EXTRAS ${R}etc/init.d/rc.board_extras
	if [ -f $BOARD_RC_EXTRAS ]
	then
		echo "Board extras: ${BOARD_RC_EXTRAS}"
		. $BOARD_RC_EXTRAS
	fi
	unset BOARD_RC_EXTRAS

	#
	# Start any custom addons from the sdcard.
	#
	if [ -f $FEXTRAS ]
	then
		echo "Addons script: ${FEXTRAS}"
		. $FEXTRAS
	fi

	#
	# Start the logger.
	#
	. ${R}etc/init.d/rc.logging

	#
	# Set additional parameters and env variables for selected AUTOSTART.
	#
	if ! param compare SYS_AUTOSTART 0
	then
		. ${R}etc/init.d/rc.autostart.post
	fi

	#
	# Bootloader upgrade
	#
	if param compare -s SYS_BL_UPDATE 1
	then
		if [ -f "/etc/extras/bootloader.bin" ]
		then
			param set SYS_BL_UPDATE 0
			param save
			echo "bootloader update..."
			bl_update "/etc/extras/bootloader.bin"
			echo "bootloader update done, rebooting"
			reboot
		fi
	fi

#
# End of autostart.
#
fi

#
# Unset all script parameters to free RAM.
#
unset R
unset AUTOCNF
unset AUX_MODE
unset FCONFIG
unset FEXTRAS
unset FMU_MODE
unset FRC
unset IO_PRESENT
unset IOFW
unset LOGGER_ARGS
unset LOGGER_BUF
unset MAV_TYPE
unset MIXER
unset MIXER_AUX
unset MIXER_FILE
unset OUTPUT_MODE
unset PARAM_FILE
unset PWM_AUX_OUT
unset PWM_AUX_RATE
unset PWM_MAIN_RATE
unset PWM_OUT
unset PWM_EXTRA_OUT
unset PWM_EXTRA_RATE
unset RC_INPUT_ARGS
unset SDCARD_MIXERS_PATH
unset STARTUP_TUNE
unset USE_IO
unset VEHICLE_TYPE

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