#!/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 df_hmc5883_wrapper start
	qshell df_trone_wrapper start
	#qshell df_isl29501_wrapper start

# Qualcomm 200qx microheli platform
elif param compare SYS_AUTOSTART 4200
then
	param set CAL_GYRO0_ID 100
	param set CAL_ACC0_ID 100
	param set CAL_MAG0_ID 101
	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 RC_MAP_THROTTLE 1
	param set RC_MAP_ROLL 2
	param set RC_MAP_PITCH 3
	param set RC_MAP_YAW 4
	param set RC_MAP_MODE_SW 5
	param set RC_MAP_POSCTL_SW 6
	param set RC1_MAX 1900
	param set RC1_MIN 1099
	param set RC1_TRIM 1099
	param set RC1_REV 1
	param set RC2_MAX 1900
	param set RC2_MIN 1099
	param set RC2_TRIM 1500
	param set RC2_REV -1
	param set RC3_MAX 1900
	param set RC3_MIN 1099
	param set RC3_TRIM 1500
	param set RC3_REV 1
	param set RC4_MAX 1900
	param set RC4_MIN 1099
	param set RC4_TRIM 1500
	param set RC4_REV -1
	param set RC5_MAX 1900
	param set RC5_MIN 1099
	param set RC5_TRIM 1500
	param set RC5_REV 1
	param set RC6_MAX 1900
	param set RC6_MIN 1099
	param set RC6_TRIM 1099
	param set RC6_REV 1
	param set ATT_W_MAG 0.00
	param set PE_MAG_NOISE 1.0f
	param set SENS_BOARD_ROT 0
	param set RC_RECEIVER_TYPE 1
	param set UART_ESC_MODEL 2
	param set UART_ESC_BAUDRTE 250000
	param set UART_ESC_MOTOR1 4
	param set UART_ESC_MOTOR2 2
	param set UART_ESC_MOTOR3 1
	param set UART_ESC_MOTOR4 3

	qshell uart_esc start -D /dev/tty-2
	qshell rc_receiver start -D /dev/tty-1

# Qualcomm internal 210qc platform
elif param compare SYS_AUTOSTART 4210
then
	param set CAL_GYRO0_ID 100
	param set CAL_ACC0_ID 100
	param set CAL_MAG0_ID 101
	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 RC_MAP_THROTTLE 1
	param set RC_MAP_ROLL 2
	param set RC_MAP_PITCH 3
	param set RC_MAP_YAW 4
	param set RC_MAP_MODE_SW 5
	param set RC_MAP_POSCTL_SW 6
	param set RC1_MAX 1900
	param set RC1_MIN 1099
	param set RC1_TRIM 1099
	param set RC1_REV 1
	param set RC2_MAX 1900
	param set RC2_MIN 1099
	param set RC2_TRIM 1500
	param set RC2_REV -1
	param set RC3_MAX 1900
	param set RC3_MIN 1099
	param set RC3_TRIM 1500
	param set RC3_REV 1
	param set RC4_MAX 1900
	param set RC4_MIN 1099
	param set RC4_TRIM 1500
	param set RC4_REV -1
	param set RC5_MAX 1900
	param set RC5_MIN 1099
	param set RC5_TRIM 1500
	param set RC5_REV 1
	param set RC6_MAX 1900
	param set RC6_MIN 1099
	param set RC6_TRIM 1099
	param set RC6_REV 1
	param set ATT_W_MAG 0.00
	param set PE_MAG_NOISE 1.0f
	param set SENS_BOARD_ROT 0
	param set CAL_GYRO0_XOFF -0.0028
	param set CAL_GYRO0_YOFF -0.0047
	param set CAL_GYRO0_ZOFF -0.0034
	param set CAL_GYRO0_XSCALE 1.0000
	param set CAL_GYRO0_YSCALE 1.0000
	param set CAL_GYRO0_ZSCALE 1.0000
	param set CAL_MAG0_XOFF 0.0000
	param set CAL_MAG0_YOFF 0.0000
	param set CAL_MAG0_ZOFF 0.0000
	param set CAL_MAG0_XSCALE 1.0000
	param set CAL_MAG0_YSCALE 1.0000
	param set CAL_MAG0_ZSCALE 1.0000
	param set CAL_ACC0_XOFF -0.0941
	param set CAL_ACC0_YOFF 0.1168
	param set CAL_ACC0_ZOFF -0.0398
	param set CAL_ACC0_XSCALE 1.0004
	param set CAL_ACC0_YSCALE 0.9974
	param set CAL_ACC0_ZSCALE 0.9951
	param set RC_RECEIVER_TYPE 1
	param set UART_ESC_MODEL 2
	param set UART_ESC_BAUDRTE 250000
	param set UART_ESC_MOTOR1 4
	param set UART_ESC_MOTOR2 2
	param set UART_ESC_MOTOR3 1
	param set UART_ESC_MOTOR4 3

	uart_esc start -D /dev/tty-2
	rc_receiver start -D /dev/tty-1
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 df_mpu9250_wrapper start
qshell df_bmp280_wrapper 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_pos_control start
qshell mc_att_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