#!/bin/sh

# PX4 commands need the 'px4-' prefix in bash.
# (px4-alias.sh is expected to be in the PATH)
. px4-alias.sh

uorb start
muorb start

# We need to wait until the DSP side is ready before
# sending commands with qshell.
sleep 1

# default, generic quad platform
if param compare SYS_AUTOSTART 4100
then
	param set MAV_TYPE 2

	qshell pwm_out_rc_in start -d /dev/tty-2

	qshell gps start -d /dev/tty-4
	qshell hmc5883 start

# Qualcomm 200qx microheli platform
elif param compare SYS_AUTOSTART 4200
then
	param set MAV_TYPE 2
	param set MC_YAW_P 7.0
	param set MC_YAWRATE_P 0.08
	param set MC_YAWRATE_I 0.0
	param set MC_YAWRATE_D 0
	param set MC_PITCH_P 7.0
	param set MC_PITCHRATE_P 0.08
	param set MC_PITCHRATE_I 0.0
	param set MC_PITCHRATE_D 0.0001
	param set MC_ROLL_P 7.0
	param set MC_ROLLRATE_P 0.08
	param set MC_ROLLRATE_I 0.0
	param set MC_ROLLRATE_D 0.0001
	param set SENS_BOARD_ROT 0

# Qualcomm internal 210qc platform
elif param compare SYS_AUTOSTART 4210
then
	param set MAV_TYPE 2
	param set MC_YAW_P 12
	param set MC_YAWRATE_P 0.08
	param set MC_YAWRATE_I 0.0
	param set MC_YAWRATE_D 0
	param set MC_PITCH_P 7.0
	param set MC_PITCHRATE_P 0.08
	param set MC_PITCHRATE_I 0.0
	param set MC_PITCHRATE_D 0.001
	param set MC_ROLL_P 7.0
	param set MC_ROLLRATE_P 0.08
	param set MC_ROLLRATE_I 0.0
	param set MC_ROLLRATE_D 0.001
	param set SENS_BOARD_ROT 0

else
	echo "NO AIRFRAME CHOSEN!"
	echo "Please set parameter to select airframe:"
	echo "    SYS_AUTOSTART 4100: generic (flight) Quad"
	echo "    SYS_AUTOSTART 4200: Microheli 200QX"
fi


qshell mpu9250 start
qshell bmp280 start
qshell rc_update start
qshell sensors start

if param compare SYS_MC_EST_GROUP 1
then
	qshell attitude_estimator_q start
	qshell local_position_estimator start

elif param compare SYS_MC_EST_GROUP 2
then
	param set EKF2_GBIAS_INIT 0.01
	param set EKF2_ANGERR_INIT 0.01
	qshell ekf2 start
else
	echo "No estimator chosen"
	exit -1
fi

qshell commander start
qshell land_detector start multicopter
qshell mc_hover_thrust_estimator start
qshell mc_pos_control start
qshell mc_att_control start
qshell mc_rate_control start

logger start -b 200 -t

param set MAV_BROADCAST 1
dataman start
navigator start
mavlink start -u 14556 -r 1000000
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink stream -u 14556 -s RC_CHANNELS -r 20
mavlink boot_complete