#!nsh

echo "[init] PX4FMU v1, v2 with or without IO on Hex"

#
# 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.004
	param set MC_ATTRATE_I 0.0
	param set MC_ATTRATE_P 0.12
	param set MC_ATT_D 0.0
	param set MC_ATT_I 0.0
	param set MC_ATT_P 7.0
	param set MC_YAWPOS_D 0.0
	param set MC_YAWPOS_I 0.0
	param set MC_YAWPOS_P 2.0
	param set MC_YAWRATE_D 0.005
	param set MC_YAWRATE_I 0.2
	param set MC_YAWRATE_P 0.3
	param set NAV_TAKEOFF_ALT 3.0
	param set MPC_TILT_MAX 0.5
	param set MPC_THR_MAX 0.7
	param set MPC_THR_MIN 0.3
	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
 
#
# Force some key parameters to sane values
# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
# 13 = hexarotor
#
param set MAV_TYPE 13

set EXIT_ON_END no

#
# Start and configure PX4IO or FMU interface
#
if px4io detect
then
	# Start MAVLink (depends on orb)
	mavlink start
	usleep 5000

	sh /etc/init.d/rc.io
	# Set PWM values for DJI ESCs
	px4io idle 900 900 900 900 900 900
	px4io min 1200 1200 1200 1200 1200 1200
	px4io max 1900 1900 1900 1900 1900 1900
else
	# Start MAVLink (on UART1 / ttyS0)
	mavlink start -d /dev/ttyS0
	usleep 5000
	fmu mode_pwm
	param set BAT_V_SCALING 0.004593
	set EXIT_ON_END yes
fi

#
# Load mixer
#
mixer load /dev/pwm_output $MIXER

#
# Set PWM output frequency to 400 Hz
#
pwm -u 400 -m 0xff

#
# Start common for all multirotors apps
#
sh /etc/init.d/rc.multirotor

if [ $EXIT_ON_END == yes ]
then
	exit
fi