#!nsh
#
# MAVLINK stream startup script.
#
# NOTE: Script variables are declared/initialized/unset in the rcS script.
#
# NOTE: Normal mode uses baud rate of 57600 (default) and data rate of 1000 bytes/s.
#
#------------------------------------------------------------------------------
#
# UART mapping on FMUv2/3/4:
#
# UART1         /dev/ttyS0      IO debug (except v4, there ttyS0 is the wifi)
# USART2        /dev/ttyS1      TELEM1 (flow control)
# USART3        /dev/ttyS2      TELEM2 (flow control)
# UART4
# UART7                         CONSOLE
# UART8                         SERIAL4
#
#------------------------------------------------------------------------------
#
# UART mapping on FMUv5:
#
# UART1         /dev/ttyS0      GPS
# USART2        /dev/ttyS1      TELEM1 (flow control)
# USART3        /dev/ttyS2      TELEM2 (flow control)
# UART4         /dev/ttyS3      TELEM4
# USART6        /dev/ttyS4      TELEM3 (flow control)
# UART7         /dev/ttyS5
# UART8         /dev/ttyS6      CONSOLE
#
#------------------------------------------------------------------------------
#
# UART mapping on OMNIBUSF4SD:
#
# USART1        /dev/ttyS0      SerialRX
# USART4        /dev/ttyS1      TELEM1
# USART6        /dev/ttyS2      GPS
#



if [ $MAVLINK_F == default ]
then
	# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
	set MAVLINK_F "-r 1200 -f"

	if ver hwcmp AEROFC_V1
	then
		set MAVLINK_F "-r 1200 -d /dev/ttyS3"

		# Only start mavlink if the Benewake TFMini or LeddarOne isn't being used
		if param greater SENS_EN_TFMINI 0
		then
			set MAVLINK_F none
		fi
		if param greater SENS_EN_LEDDAR1 0
		then
			set MAVLINK_F none
		fi
	fi

	# Use ttyS1 for MAVLink on FMUv4 in addition to ttyS0 (debug)
	if ver hwcmp PX4FMU_V4
	then
		set MAVLINK_F "-r 1200 -d /dev/ttyS1"
		# Start MAVLink on Wifi (ESP8266 port)
		mavlink start -r 20000 -b 921600 -d /dev/ttyS0
	fi

	# Use ttyS3 (TELEM4) for MAVLink on FMUv5 in addition to ttyS1 (TELEM1) and ttyS2 (TELEM2)
	if ver hwcmp PX4FMU_V5
	then
		mavlink start -r 2000 -b 57600 -d /dev/ttyS3
	fi

	if ver hwcmp CRAZYFLIE OMNIBUS_F4SD
	then
		# Avoid using either of the two available serials
		set MAVLINK_F none
	fi
fi

if [ "x$MAVLINK_F" == xnone ]
then
else
	mavlink start ${MAVLINK_F}
fi

#
# MAVLink onboard / TELEM2
#
# XXX We need a better way for runtime eval of shell variables,
# but this works for now
if param compare SYS_COMPANION 10
then
	frsky_telemetry start -d ${MAVLINK_COMPANION_DEVICE}
else
	if ver hwcmp PX4FMU_V4 PX4FMU_V4PRO MINDPX_V2
	then
		# This is TELEM4 on Pixhawk 3 Pro
		frsky_telemetry start -d /dev/ttyS6 -t 15
	fi
fi

if param compare SYS_COMPANION 20
then
	syslink start
	mavlink start -d /dev/bridge0 -b 57600 -m osd -r 40000
fi

#
# 19200 Baud Rate.
#
if param compare SYS_COMPANION 319200
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 19200 -r 1000 -f
fi
if param compare SYS_COMPANION 419200
then
	iridiumsbd start -d ${MAVLINK_COMPANION_DEVICE}
	mavlink start -d /dev/iridium -b 19200 -m iridium -r 10
fi
if param compare SYS_COMPANION 519200
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 19200 -m minimal -r 1000
fi

#
# 38400 Baud Rate.
#
if param compare SYS_COMPANION 338400
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 38400 -r 1000 -f
fi
if param compare SYS_COMPANION 538400
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 38400 -m minimal -r 1000
fi

#
# 57600 Baud Rate.
#
if param compare SYS_COMPANION 57600
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m onboard -r 5000 -x -f
fi
if param compare SYS_COMPANION 157600
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m osd -r 1000
fi
if param compare SYS_COMPANION 257600
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m magic -r 5000 -x -f
fi
if param compare SYS_COMPANION 357600
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -r 1000 -f
fi
if param compare SYS_COMPANION 557600
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -m minimal -r 1000
fi

#
# 115200 Baud Rate.
#
if param compare SYS_COMPANION 3115200
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 115200 -r 1000 -f
fi
if param compare SYS_COMPANION 5115200
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 115200 -m minimal -r 1000
fi

#
# 460800 Baud Rate.
#
if param compare SYS_COMPANION 460800
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 460800 -m onboard -r 5000 -x -f
fi
if param compare SYS_COMPANION 6460800
then
	micrortps_client start -t UART -d /dev/ttyS2 -b 460800
fi

#
# 921600 Baud Rate.
#
if param compare SYS_COMPANION 921600
then
	if ver hwcmp AEROFC_V1
	then
		if protocol_splitter start ${MAVLINK_COMPANION_DEVICE}
		then
			mavlink start -d /dev/mavlink -b 921600 -m onboard -r 5000 -x
			micrortps_client start -d /dev/rtps -b 921600 -l -1 -s 2000
		else
			mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -m onboard -r 80000 -x -f
		fi
	else
		mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -m onboard -r 80000 -x -f
	fi
fi
if param compare SYS_COMPANION 1921600
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 921600 -r 20000
fi

#
# 1500000 Baud Rate.
#
if param compare SYS_COMPANION 1500000
then
	mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 1500000 -m onboard -r 10000 -x -f
fi