#!/bin/sh
#
# Start telemetry
#

. /etc/datalink.conf
. /etc/telemetry.conf

keydir="/etc"

fw=$(grep "BUILD_OPTION" "/etc/os-release" | cut -d= -f2)

start_telemetry() {
  if [ ! -f /usr/bin/telemetry_rx -a ! -f /usr/bin/telemetry_tx ]; then
	  ln -s /usr/bin/wfb_rx /usr/bin/telemetry_rx ; chmod +x /usr/bin/telemetry_rx
	  ln -s /usr/bin/wfb_tx /usr/bin/telemetry_tx ; chmod +x /usr/bin/telemetry_tx
  else
	if [ ${one_way} = "false" ]; then
    if [ -z "${link_id}" ]; then
      telemetry2_rx -r ${stream_rx} -u ${port_rx} -K ${keydir}/drone.key ${wlan} >/dev/null &
    else
      telemetry_rx -p ${stream_rx} -u ${port_rx} -K ${keydir}/drone.key -i ${link_id} ${wlan} >/dev/null &
    fi
	fi
    if [ -z "${link_id}" ]; then
      telemetry2_tx -r ${stream_tx} -u ${port_tx} -K ${keydir}/drone.key -B ${bandwidth} -M ${mcs_index} -S ${stbc} -L ${ldpc} -G ${guard_interval} -k ${fec_k} -p ${fec_p} ${wlan} >/dev/null &
     else
      telemetry_tx -p ${stream_tx} -u ${port_tx} -K ${keydir}/drone.key -B ${bandwidth} -M ${mcs_index} -S ${stbc} -L ${ldpc} -G ${guard_interval} -k ${fec_k} -n ${fec_n} -T ${fec_timeout} -i ${link_id} ${wlan} >/dev/null &
    fi
  fi
}

case "$1" in
  start)
	echo "Loading MAVLink telemetry service..."

	if [ ${router} -eq 1 ] || [ ${fw} = "lte" ]; then
	  /usr/bin/mavlink-routerd &
	else
	  mavfwd --master ${serial} --baudrate ${baud} --out 127.0.0.1:${port_tx} --in 127.0.0.1:${port_rx} &
	fi
	if [ ${fw} = "fpv" ]; then
		start_telemetry
	fi
	echo "Done."
	;;
  stop)
	echo "Stopping telemetry services..."
	kill -9 $(pidof telemetry_rx)
	kill -9 $(pidof telemetry_tx)
	kill -9 $(pidof mavlink-routerd)
	kill -9 $(pidof mavfwd)
	;;
    *)
	echo "Usage: $0 {start|stop}"
	exit 1
esac