#!nsh
#
# USB HIL start
#

echo "[HIL] HILS quadrotor + starting.."

#
# Load default params for this platform
#
if param compare SYS_AUTOCONFIG 1
then
        # Set all params here, then disable autoconfig
        param set SYS_AUTOCONFIG 0

        param set MC_ATTRATE_D 0.0
        param set MC_ATTRATE_I 0.0
        param set MC_ATTRATE_P 0.05
        param set MC_ATT_D 0.0
        param set MC_ATT_I 0.0
        param set MC_ATT_P 3.0
        param set MC_YAWPOS_D 0.0
        param set MC_YAWPOS_I 0.0
        param set MC_YAWPOS_P 2.1
        param set MC_YAWRATE_D 0.0
        param set MC_YAWRATE_I 0.0
        param set MC_YAWRATE_P 0.05
        param set NAV_TAKEOFF_ALT 3.0
        param set MPC_TILT_MAX 0.5
        param set MPC_THR_MAX 0.5
        param set MPC_THR_MIN 0.1
        param set MPC_XY_D 0
        param set MPC_XY_P 0.5
        param set MPC_XY_VEL_D 0
        param set MPC_XY_VEL_I 0
        param set MPC_XY_VEL_MAX 3
        param set MPC_XY_VEL_P 0.2
        param set MPC_Z_D 0
        param set MPC_Z_P 1
        param set MPC_Z_VEL_D 0
        param set MPC_Z_VEL_I 0.1
        param set MPC_Z_VEL_MAX 2
        param set MPC_Z_VEL_P 0.20

        param save
fi

# Allow USB some time to come up
sleep 1
# Tell MAVLink that this link is "fast"
mavlink start -b 230400 -d /dev/ttyACM0

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

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

#
# 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_quad_+.mix

#
# Start position estimator
#
position_estimator_inav start

#
# Start attitude control
#
multirotor_att_control start

#
# Start position control
#
multirotor_pos_control start

echo "[HIL] setup done, running"