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

# Serial map:
# /dev/ttyS0: GPS
# /dev/ttyS1: GB
# /dev/ttyS2: nsh shell / console
# /dev/ttyS3: payload
# /dev/ttyS4: esc bus
# /dev/ttyS5: RC input

#
# Start CDC/ACM serial driver
#
sercon

set LOG_FILE /fs/microsd/bootlog.txt
set DATAMAN_OPT -r

#
# Start the ORB (first app to start)
# tone_alarm and tune_control
# is dependent.
#
uorb start

#
# Start the tone_alarm driver.
#
tone_alarm start

#
# Try to mount the microSD card.
#
if mount -t vfat /dev/mmcsd0 /fs/microsd
then
	echo "microSD present"
	unset DATAMAN_OPT
else
	if mkfatfs /dev/mmcsd0
	then
		if mount -t vfat /dev/mmcsd0 /fs/microsd
		then
			echo "microSD card formatted"
			unset DATAMAN_OPT
		else
			echo "ERROR [init] Format failed"
			tune_control play -t 17
			set LOG_FILE /dev/null
		fi
	else
		set LOG_FILE /dev/null
	fi
fi

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

#
# Start system state indicator
#
if rgbled_pwm start
then
fi

if rgbled start
then
fi

#
# Set parameters and env variables for selected AUTOSTART
#
if param compare SYS_AUTOSTART 0
then
	param set SYS_AUTOSTART 6001
	param set SYS_AUTOCONFIG 1
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 VEHICLE_TYPE none
set MIXER 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 FMU_MODE pwm
set MAV_TYPE none
set FAILSAFE none

# Start canned airframe config
sh /etc/init.d/rc.autostart

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

gps start -d /dev/ttyS0

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

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

commander start

#
# Start CPU load monitor
#
load_mon start

# Start MAVLink on the gimbal port
#mavlink start -r 1200 -d /dev/ttyS1

# Start MAVLink on USB, developers can use the MAVLink shell
mavlink start -r 60000 -d /dev/ttyACM0 -m config

#
# Logging
#
#if logger start -b 2 -t
#then
#fi

#
# Fixed wing setup
#
if [ $VEHICLE_TYPE == fw ]
then
	echo "INFO  [init] Fixedwing"

	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 "INFO  [init] Multicopter"

	if [ $MIXER == none ]
	then
		echo "INFO  [init] 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 ]
		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
	fi

	# Still no MAV_TYPE found
	if [ $MAV_TYPE == none ]
	then
		echo "WARN  [init] 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 "INFO  [init] VTOL"

	if [ $MIXER == none ]
	then
		echo "WARN  [init] 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 "WARN  [init] 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

#
# UGV/Rover setup
#
if [ $VEHICLE_TYPE == ugv ]
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.ugv_apps

	param set MAV_TYPE 10
fi

unset MIXER
unset MAV_TYPE
unset OUTPUT_MODE

#
# Start the RC input driver
#
if fmu mode_pwm1
then
fi

#
# Use 400 Hz PWM output for landing gear (frequency also affects RGB LED)
#
pwm rate -c 1 -r 400

#
# Load the gear mixer onto fmu
#
mixer load /dev/px4fmu /etc/mixers/gear.mix

#
# Start the navigator
#
if navigator start
then
fi

# There is no further script processing, so we can free some RAM
# XXX potentially unset all script variables.
unset TUNE_ERR
unset LOG_FILE
unset DATAMAN_OPT

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