#!nsh
#
# USB HIL start
#

echo "[HIL] starting.."

uorb start

# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyS1

# Create a fake HIL /dev/pwm_output interface
hil mode_pwm

#
# Load microSD params
#
echo "[init] loading microSD params"
param select /fs/microsd/params
if [ -f /fs/microsd/params ]
then
	param load /fs/microsd/params
fi

#
# Force some key parameters to sane values
# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
#              see https://pixhawk.ethz.ch/mavlink/
#
param set MAV_TYPE 1

#
# Start the commander (depends on orb, mavlink)
#
commander start

#
# Check if we got an IO
#
if px4io start
then
	echo "IO started"
else
	fmu mode_serial
	echo "FMU started"
fi

#
# Start the sensors (depends on orb, px4io)
#
sh /etc/init.d/rc.sensors

#
# Start the attitude estimator (depends on orb)
#
att_pos_estimator_ekf start
 
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
fw_pos_control_l1 start
fw_att_control start

echo "[HIL] setup done, running"