#!/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 FCONFIG /fs/microsd/etc/config.txt
set FEXTRAS /fs/microsd/etc/extras.txt
set FRC /fs/microsd/etc/rc.txt
set IOFW "/etc/extras/px4_io-v2_default.bin"
set LOGGER_ARGS ""
set LOGGER_BUF 8
set PARAM_FILE ""
set PARAM_BACKUP_FILE ""
set RC_INPUT_ARGS ""
set STORAGE_AVAILABLE no
set SDCARD_EXT_PATH /fs/microsd/ext_autostart
set SDCARD_FORMAT no
set STARTUP_TUNE 1
set VEHICLE_TYPE none

# Airframe parameter versioning
# Value set to 1 by default but can optionally be overridden in the airframe configuration startup script.
# Airframe maintainers can ensure a reset to the airframe defaults during an update by increasing by one.
# e.g. add line "set PARAM_DEFAULTS_VER 2" in your airframe file to build the first update that enforces a reset.
set PARAM_DEFAULTS_VER 1

#
# Print full system version.
#
ver all

#
# Try to mount the microSD card.
#
if [ -b "/dev/mmcsd0" ]
then
	if mount -t vfat /dev/mmcsd0 /fs/microsd
	then
		if [ -f "/fs/microsd/.format" ]
		then
			echo "INFO [init] format /dev/mmcsd0 requested (/fs/microsd/.format)"
			set SDCARD_FORMAT yes
			rm /fs/microsd/.format
			umount /fs/microsd

		else
			set STORAGE_AVAILABLE yes
		fi
	fi

	if [ $STORAGE_AVAILABLE = no -o $SDCARD_FORMAT = yes ]
	then
		echo "INFO [init] formatting /dev/mmcsd0"
		set STARTUP_TUNE 15 # tune 15 = SD_ERROR (overridden to SD_INIT if format + mount succeeds)

		if mkfatfs -F 32 /dev/mmcsd0
		then
			echo "INFO [init] card formatted"

			if mount -t vfat /dev/mmcsd0 /fs/microsd
			then
				set STORAGE_AVAILABLE yes
				set STARTUP_TUNE 14 # tune 14 = SD_INIT
			else
				echo "ERROR [init] card mount failed"
			fi
		else
			echo "ERROR [init] format failed"
		fi
	fi
else
	# Is there a device mounted for storage
	if mft query -q -k MTD -s MTD_PARAMETERS -v /mnt/microsd
	then
		set STORAGE_AVAILABLE yes
	fi
fi

if [ $STORAGE_AVAILABLE = yes ]
then
	if hardfault_log check
	then
		set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
		if hardfault_log commit
		then
			hardfault_log reset
		fi
	fi

	# Check for an update of the ext_autostart folder, and replace the old one with it
	if [ -e /fs/microsd/ext_autostart_new ]
	then
		echo "Updating external autostart files"
		rm -r $SDCARD_EXT_PATH
		mv /fs/microsd/ext_autostart_new $SDCARD_EXT_PATH
	fi

	set PARAM_FILE /fs/microsd/params
	set PARAM_BACKUP_FILE "/fs/microsd/parameters_backup.bson"
fi

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

	# Load param file location from kconfig
	. ${R}etc/init.d/rc.filepaths

	# Check if /fs/mtd_params is a valid BSON file
	if ! bsondump docsize /fs/mtd_caldata
	then
		echo "New /fs/mtd_caldata size is:"
		bsondump docsize /fs/mtd_caldata
	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
		echo "ERROR [init] param import failed"
		set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE

		bsondump $PARAM_FILE

		if [ -d "/fs/microsd" ]
		then
			# try to make a backup copy
			cp $PARAM_FILE /fs/microsd/param_import_fail.bson

			# try importing from backup file
			if [ -f $PARAM_BACKUP_FILE ]
			then
				echo "[init] importing from parameter backup"

				# dump current backup file contents for comparison
				bsondump $PARAM_BACKUP_FILE

				param import $PARAM_BACKUP_FILE

				# overwrite invalid $PARAM_FILE with backup
				cp $PARAM_BACKUP_FILE $PARAM_FILE
			fi

			param status

			dmesg >> /fs/microsd/param_import_fail.txt &
		fi
	fi

	if [ $STORAGE_AVAILABLE = yes ]
	then
		param select-backup $PARAM_BACKUP_FILE
	fi

	if mft query -q -k MFT -s MFT_ETHERNET -v 1
	then
		netman update -i eth0
	fi

	# To trigger a parameter reset during boot SYS_AUTCONFIG was set to 1 before
	if param greater SYS_AUTOCONFIG 0
	then
		# Reset parameters except airframe, parameter version, RC calibration, sensor calibration, flight modes, total flight time, flight UUID
		param reset_all SYS_AUTOSTART SYS_PARAM_VER RC* CAL_* COM_FLTMODE* LND_FLIGHT* TC_* COM_FLIGHT*
	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

	#
	# Optional additional init file: rc.additional_init
	#
	set BOARD_RC_ADDITIONAL_INIT ${R}etc/init.d/rc.additional_init
	if [ -f $BOARD_RC_ADDITIONAL_INIT ]
	then
		echo "Board additional init: ${BOARD_RC_ADDITIONAL_INIT}"
		. $BOARD_RC_ADDITIONAL_INIT
	fi
	unset BOARD_RC_ADDITIONAL_INIT

	# Load airframe configuration based on SYS_AUTOSTART parameter if successful VEHICLE_TYPE gets set
	# Run autogenerated ROMFS airframe script
	. ${R}etc/init.d/rc.autostart

	if [ ${VEHICLE_TYPE} = none ]
	then
		# Run external airframe script on SD card
		if [ $STORAGE_AVAILABLE = yes ]
		then
			. ${R}etc/init.d/rc.autostart_ext
		else
			echo "ERROR [init] SD not mounted, skipping external airframe"
		fi
	fi

	if [ ${VEHICLE_TYPE} = none ]
	then
		echo "ERROR [init] No airframe file found for SYS_AUTOSTART value"
		param set SYS_AUTOSTART 0
		tune_control play error
	fi

	# Check parameter version and reset upon airframe configuration version mismatch.
	# Reboot required because "param reset_all" would reset all "param set" lines from airframe.
	if ! param compare SYS_PARAM_VER ${PARAM_DEFAULTS_VER}
	then
		echo "Switched to different parameter version. Resetting parameters."
		param set SYS_PARAM_VER ${PARAM_DEFAULTS_VER}
		param set SYS_AUTOCONFIG 1
		param save
		reboot
	fi

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

	#
	# Waypoint storage.
	# REBOOTWORK this needs to start in parallel.
	#
%ifdef CONFIG_MODULES_DATAMAN
	if param compare -s SYS_DM_BACKEND 1
	then
		dataman start -r
	else
		if param compare SYS_DM_BACKEND 0
		then
			# dataman start default
			dataman start
		fi
	fi
%endif

	#
	# Start the socket communication send_event handler.
	#
%ifdef CONFIG_MODULES_SEND_EVENT
	send_event start
%endif

	#
	# Start the hardfault streamer.
	#
	if param compare -s SYS_HF_MAV 1
	then
		hardfault_stream start
	fi

	#
	# Start the resource load monitor.
	#
%ifdef CONFIG_MODULES_LOAD_MON
	load_mon start
%endif

	#
	# Start system state indicator.
	#
%ifdef CONFIG_DRIVERS_LIGHTS_RGBLED
	rgbled start -X -q
%endif
%ifdef CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C
	rgbled_ncp5623c start -X -q
%endif
%ifdef CONFIG_DRIVERS_LIGHTS_RGBLED_LP5562
	rgbled_lp5562 start -X -q
%endif
%ifdef CONFIG_DRIVERS_LIGHTS_RGBLED_IS31FL3195
	rgbled_is31fl3195 start -X -q
%endif

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


	#
	# Sensors System (start before Commander so Preflight checks are properly run).
	#
	if param greater SYS_HITL 0
	then
		sensors start -h

		# disable GPS
		param set GPS_1_CONFIG 0

		# start the simulator in hardware if needed
		if param compare SYS_HITL 2
		then
%ifdef CONFIG_MODULES_SIMULATION_SIMULATOR_SIH
			simulator_sih start
%endif
%ifdef CONFIG_MODULES_SIMULATION_SENSOR_BARO_SIM
			sensor_baro_sim start
%endif
%ifdef CONFIG_MODULES_SIMULATION_SENSOR_MAG_SIM
			sensor_mag_sim start
%endif
%ifdef CONFIG_MODULES_SIMULATION_SENSOR_GPS_SIM
			sensor_gps_sim start
%endif
%ifdef CONFIG_MODULES_SIMULATION_SENSOR_AGP_SIM
			sensor_agp_sim start
%endif
		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

%ifdef CONFIG_MODULES_SENSORS
%ifdef CONFIG_MODULES_ESC_BATTERY
		if param compare -s BAT1_SOURCE 2
		then
			esc_battery start
		fi
%endif

%ifdef CONFIG_MODULES_BATTERY_STATUS
		if ! param compare BAT1_SOURCE 1
		then
			battery_status start
		fi
%endif
%endif

%ifdef CONFIG_MODULES_SENSORS
		sensors start
%endif
	fi

	#
	# state estimator selection
	#
%ifdef CONFIG_MODULES_EKF2
	if param compare -s EKF2_EN 1
	then
		ekf2 start &
	fi
%endif

%ifdef CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR
	if param compare -s LPE_EN 1
	then
		local_position_estimator start
	fi
%endif

%ifdef CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q
	if param compare -s ATT_EN 1
	then
		attitude_estimator_q start
	fi
%endif

%ifdef CONFIG_DRIVERS_PX4IO
	#
	# px4io
	#
	if px4io supported
	then
		# Check if PX4IO present and update firmware if needed.
		if [ -f $IOFW ]
		then
			if ! px4io checkcrc ${IOFW}
			then
				# 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
					else
						tune_control play -t 18 # tune 18 = PROG_PX4IO_ERR
					fi
				else
					tune_control stop
				fi
			fi

			if ! px4io start
			then
				echo "PX4IO start failed"
				set STARTUP_TUNE 2 # tune 2 = ERROR_TUNE
			fi
		fi
	fi
%endif

	# Heater driver for temperature regulated IMUs.
	# The heater needs to start after px4io.
%ifdef CONFIG_DRIVERS_HEATER
	if param compare -s SENS_EN_THERMAL 1
	then
		heater start
	fi
%endif


	#
	# RC update (map raw RC input to calibrate manual control)
	#  start before commander
	#
%ifdef CONFIG_MODULES_RC_UPDATE
	rc_update start
%endif
%ifdef CONFIG_MODULES_MANUAL_CONTROL
	manual_control start
%endif

	# Start camera trigger, capture and PPS before pwm_out as they might access
	# pwm pins
%ifdef CONFIG_DRIVERS_CAMERA_TRIGGER
	if param greater -s TRIG_MODE 0
	then
		camera_trigger start
%ifdef CONFIG_MODULES_CAMERA_FEEDBACK
		camera_feedback start
%endif
	fi
%endif
	# PPS capture driver
%ifdef CONFIG_DRIVERS_PPS_CAPTURE
	if param greater -s PPS_CAP_ENABLE 0
	then
		pps_capture start
	fi
%endif
	# RPM capture driver
%ifdef CONFIG_DRIVERS_RPM_CAPTURE
	if param greater -s RPM_CAP_ENABLE 0
	then
		rpm_capture start
	fi
%endif
	# Camera capture driver
%ifdef CONFIG_DRIVERS_CAMERA_CAPTURE
	if param greater -s CAM_CAP_FBACK 0
	then
		if camera_capture start
		then
			camera_capture on
		fi
	fi
%endif

	#
	# Commander
	#
%ifdef CONFIG_MODULES_COMMANDER
	if param greater SYS_HITL 0
	then
		commander start -h

%ifdef CONFIG_MODULES_SIMULATION_PWM_OUT_SIM
		if ! pwm_out_sim start -m hil
		then
			tune_control play error
		fi
%endif

	else
		commander start

%ifdef CONFIG_DRIVERS_DSHOT
		dshot start
%endif
%ifdef CONFIG_DRIVERS_PWM_OUT
		pwm_out start
%endif
	fi
%endif

	#
	# Configure vehicle type specific parameters.
	# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.
	. ${R}etc/init.d/rc.vehicle_setup

	# Pre-takeoff continuous magnetometer calibration
%ifdef CONFIG_MODULES_MAG_BIAS_ESTIMATOR
	if param compare -s MBE_ENABLE 1
	then
		mag_bias_estimator start
	fi
%endif

	#
	# 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 mavlink: ${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

	# Must be started after the serial config is read
	rc_input start $RC_INPUT_ARGS

	# Manages USB interface
	if param greater -s SYS_USB_AUTO -1
	then
		if ! cdcacm_autostart start
		then
			sercon
			echo "Starting MAVLink on /dev/ttyACM0"
			mavlink start -d /dev/ttyACM0
		fi
	fi

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

	#
	# Start the navigator.
	#
%ifdef CONFIG_MODULES_NAVIGATOR
	navigator start
%endif

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

	#
	# Start gimbal to control mounts such as gimbals, disabled by default.
	#
%ifdef CONFIG_MODULES_GIMBAL
	if param greater -s MNT_MODE_IN -1
	then
		gimbal start
	fi
%endif

	# Blacksheep telemetry
%ifdef CONFIG_DRIVERS_TELEMETRY_BST
	if param compare -s TEL_BST_EN 1
	then
		bst start -X
	fi
%endif

%ifdef CONFIG_MODULES_GYRO_FFT
	if param compare -s IMU_GYRO_FFT_EN 1
	then
		gyro_fft start
	fi
%endif

%ifdef CONFIG_MODULES_GYRO_CALIBRATION
	if param compare -s IMU_GYRO_CAL_EN 1
	then
		gyro_calibration start
	fi
%endif

	# Check for px4flow sensor
%ifdef CONFIG_DRIVERS_OPTICAL_FLOW_PX4FLOW
	if param compare -s SENS_EN_PX4FLOW 1
	then
		px4flow start -X &
	fi
%endif

%ifdef CONFIG_MODULES_PAYLOAD_DELIVERER
	payload_deliverer start
%endif

%ifdef CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL
	if param compare -s ICE_EN 1
	then
		internal_combustion_engine_control start
	fi
%endif

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

%ifdef CONFIG_MODULES_LOGGER
	#
	# Start the logger.
	#
	. ${R}etc/init.d/rc.logging
%endif

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


	set BOARD_BOOTLOADER_UPGRADE ${R}etc/init.d/rc.board_bootloader_upgrade
	if [ -f $BOARD_BOOTLOADER_UPGRADE ]
	then
		sh $BOARD_BOOTLOADER_UPGRADE
	fi
	unset BOARD_BOOTLOADER_UPGRADE

	#
	# Check if UAVCAN is enabled, default to it for ESCs.
	#
%ifdef CONFIG_DRIVERS_UAVCAN
	if param greater -s UAVCAN_ENABLE 0
	then
		# Start core UAVCAN module.
		if ! uavcan start
		then
			tune_control play error
		fi
	else
%ifdef CONFIG_DRIVERS_CYPHAL
		if param greater -s CYPHAL_ENABLE 0
		then
			cyphal start
		fi
%endif
	fi
%else
%ifdef CONFIG_DRIVERS_CYPHAL
	if param greater -s CYPHAL_ENABLE 0
	then
		cyphal start
	fi
%endif
%endif
%ifdef CONFIG_MODULES_ZENOH
	if param greater -s ZENOH_ENABLE 0
	then
		zenoh start
	fi
%endif

#
# End of autostart.
#
fi

#
# Unset all script parameters to free RAM.
#
unset R
unset FCONFIG
unset FEXTRAS
unset FRC
unset IOFW
unset LOGGER_ARGS
unset LOGGER_BUF
unset PARAM_FILE
unset PARAM_BACKUP_FILE
unset PARAM_DEFAULTS_VER
unset RC_INPUT_ARGS
unset STORAGE_AVAILABLE
unset SDCARD_EXT_PATH
unset SDCARD_FORMAT
unset STARTUP_TUNE
unset VEHICLE_TYPE

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