bizhang_-obav/ROMFS/px4fmu_common/init.d/rc.mavlink

#!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.
#

###############################################################################
#               Begin Setup for board specific configurations.                #
###############################################################################

if ! ver hwcmp AEROFC_V1
then
	# Start MAVLink
	mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
fi

if ver hwcmp NXPHLITE_V3
then
	set MAVLINK_COMPANION_DEVICE none
	set MAVLINK_F "-r 1200 -d /dev/ttyS4"
fi

if ver hwcmp OMNIBUS_F4SD
then
	set MAVLINK_COMPANION_DEVICE /dev/ttyS1
fi

###############################################################################
#                 End Setup for board specific configurations.                #
###############################################################################

if [ "x${MAVLINK_F}" == xdefault ]
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
	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
#----------------------------------------------------------------
#
# Sys companion setups not dependant on MAVLINK_COMPANION_DEVICE
#

if ! param compare SYS_COMPANION 10
then
	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

if param compare SYS_COMPANION 6460800
then
	micrortps_client start -t UART -d /dev/ttyS2 -b 460800
fi

#----------------------------------------------------------------
#
# Sys companion setups dependant on MAVLINK_COMPANION_DEVICE
#

if [ "x${MAVLINK_COMPANION_DEVICE}" != xnone ]
then
	if param compare SYS_COMPANION 10
	then
		frsky_telemetry start -d ${MAVLINK_COMPANION_DEVICE}
	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 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 4115200
	then
		usleep 200000 			# add a sleep here to make sure that the module is powered
		if iridiumsbd start -d ${MAVLINK_COMPANION_DEVICE}
		then
			mavlink start -d /dev/iridium -m iridium -b 115200
		else
			tune_control play -t 20
		fi
	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

	#
	# 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 140000 -x -f
	fi
fi