#!/usr/bin/bash

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

# Main SITL startup script


# check for ekf2 replay
if [ "$replay_mode" == "ekf2" ]
then
	sh etc/init.d-posix/rc.replay
	exit 0
fi


# TODO: In the future we want to share rc.autostart with NuttX
#source rc.autostart

# Use the variable set by sitl_run.sh to choose the model settings.
if [ "$PX4_SIM_MODEL" == "iris" ]; then
	set REQUESTED_AUTOSTART 10016

elif [ "$PX4_SIM_MODEL" == "typhoon_h480" ]; then
	set REQUESTED_AUTOSTART 6011
else
	echo "Unknown model"
	exit -1
fi

uorb start
if [ -f eeprom/parameters ]
then
	param load
fi

if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART
then
	set AUTOCNF no
else
	set AUTOCNF yes
fi


if [ $AUTOCNF == yes ]
then
	param set SYS_AUTOSTART $REQUESTED_AUTOSTART

	param set BAT_N_CELLS 3
	param set CAL_ACC0_ID 1376264
	param set CAL_ACC0_XOFF 0.01
	param set CAL_ACC0_XSCALE 1.01
	param set CAL_ACC0_YOFF -0.01
	param set CAL_ACC0_YSCALE 1.01
	param set CAL_ACC0_ZOFF 0.01
	param set CAL_ACC0_ZSCALE 1.01
	param set CAL_ACC1_ID 1310728
	param set CAL_ACC1_XOFF 0.01
	param set CAL_GYRO0_ID 2293768
	param set CAL_GYRO0_XOFF 0.01
	param set CAL_MAG0_ID 196616
	param set CAL_MAG0_XOFF 0.01
	param set COM_DISARM_LAND 3
	param set COM_OBL_ACT 2
	param set COM_OBL_RC_ACT 0
	param set COM_OF_LOSS_T 5
	param set COM_RC_IN_MODE 1
	param set EKF2_AID_MASK 1
	param set EKF2_ANGERR_INIT 0.01
	param set EKF2_GBIAS_INIT 0.01
	param set EKF2_HGT_MODE 0
	param set EKF2_MAG_TYPE 1
	param set MC_PITCH_P 6
	param set MC_PITCHRATE_P 0.2
	param set MC_ROLL_P 6
	param set MC_ROLLRATE_P 0.2
	param set MIS_TAKEOFF_ALT 2.5
	param set MPC_HOLD_MAX_Z 2.0
	param set MPC_Z_VEL_I 0.15
	param set MPC_Z_VEL_P 0.6
	param set NAV_ACC_RAD 2.0
	param set NAV_DLL_ACT 2
	param set RTL_DESCEND_ALT 5.0
	param set RTL_LAND_DELAY 5
	param set RTL_RETURN_ALT 30.0
	param set SDLOG_DIRS_MAX 7
	param set SENS_BOARD_ROT 0
	param set SENS_BOARD_X_OFF 0.000001
	param set SYS_RESTART_TYPE 2
fi


# Use environment variable PX4_ESTIMATOR to choose estimator.
if [ "$PX4_ESTIMATOR" == "ekf2" ]; then
	param set SYS_MC_EST_GROUP 2
elif [ "$PX4_ESTIMATOR" == "lpe" ]; then
	param set SYS_MC_EST_GROUP 1
fi

dataman start
replay tryapplyparams
simulator start -s
tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start
sensors start
commander start
land_detector start multicopter
navigator start

if param compare SYS_MC_EST_GROUP 1
then
	attitude_estimator_q start
	local_position_estimator start
elif param compare SYS_MC_EST_GROUP 2
then
	ekf2 start
else
	echo "No estimator chosen"
	exit -1
fi

mc_pos_control start
mc_att_control start

# TODO: eventually we want to re-use the existing autostart
#       infrastructure already available on NuttX.
if param compare SYS_AUTOSTART 10016
then
	sh etc/init.d-posix/10016_iris

elif param compare SYS_AUTOSTART 6011
then
	sh etc/init.d-posix/6011_typhoon_h480
fi


mavlink start -x -u 14556 -r 4000000
mavlink start -x -u 14557 -r 4000000 -m onboard -o 14540
mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
mavlink stream -r 50 -s ATTITUDE -u 14556
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
mavlink stream -r 20 -s RC_CHANNELS -u 14556
mavlink stream -r 250 -s HIGHRES_IMU -u 14556
mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556

# Required for the typhoon
mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14556
mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14557
mavlink stream -r 10 -s MOUNT_ORIENTATION -u 14557

logger start -e -t -b 1000

mavlink boot_complete
replay trystart