#!/bin/sh
# shellcheck disable=SC2154

# Simulator IMU data provided at 250 Hz
param set-default IMU_INTEG_RATE 250

# For simulation, allow registering modes while armed for developer convenience
param set-default COM_MODE_ARM_CHK 1

if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then

	echo "INFO  [init] SIH simulator"

	if [ -n "${PX4_HOME_LAT}" ]; then
		param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
	fi

	if [ -n "${PX4_HOME_LON}" ]; then
		param set SIH_LOC_LON0 ${PX4_HOME_LON}
	fi
	if [ -n "${PX4_HOME_ALT}" ]; then
		param set SIH_LOC_H0 ${PX4_HOME_ALT}
	fi

	if simulator_sih start; then

		if param compare -s SENS_EN_BAROSIM 1
		then
			sensor_baro_sim start
		fi
		if param compare -s SENS_EN_GPSSIM 1
		then
			sensor_gps_sim start
		fi
		if param compare -s SENS_EN_MAGSIM 1
		then
			sensor_mag_sim start
		fi
		if param compare -s SENS_EN_ARSPDSIM 1
		then
			sensor_airspeed_sim start
		fi
		if param compare -s SENS_EN_AGPSIM 1
		then
			sensor_agp_sim start
		fi


	else
		echo "ERROR [init] simulator_sih failed to start"
		exit 1
	fi

elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
    # Run Gazebo (gz)
    . px4-rc.gzsim

elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then
    # Run jMAVSim
    . px4-rc.jmavsim

else
    # Run simulator_mavlink module for interface with gazebo-classic
    . px4-rc.mavlinksim
fi