#!/bin/sh
#
# Start telemetry
#

eval `cat /etc/wfb.conf | grep "tab_wlan" | grep -v "#"`
eval `cat /etc/wfb.conf | grep "udp_addr" | grep -v "#"`
. /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
          telemetry_tx -r ${stream_tx} -u ${port_tx} -K ${keydir}/gs.key -B ${bandwidth} -M ${mcs_index} -S ${stbc} -L ${ldpc} -G ${guard_interval} -k ${fec_k} -p ${fec_p} ${wlan} >/dev/null >/dev/null &
        else
          telemetry_tx -p ${stream_tx} -u ${port_tx} -K ${keydir}/gs.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
    if [ -z "${link_id}" ]; then
      telemetry_rx -r ${stream_rx} -c 127.0.0.1 -u ${port_rx} -K ${keydir}/gs.key ${wlan} >/dev/null &
    else
      telemetry_rx -p ${stream_rx} -u ${port_rx} -K ${keydir}/gs.key -i ${link_id} ${wlan} >/dev/null &
    fi
  fi
  #for ethernet-modem
  if [ ${udp_addr} = "auto" ]; then
      ETH_IP=`ip route show 0.0.0.0/0 dev eth0 | cut -d\  -f3`
      if [ ! -z ${ETH_IP} ]; then
          echo found ethernet-modem, ip: ${ETH_IP}
          MAVROUTARG=" -e ${ETH_IP}:14550"
      fi
  fi
  #for tablet osd (FPV-VR) as usb
  PHONE_IP=`ip route show 0.0.0.0/0 dev usb0 | cut -d\  -f3`
  if [ ! -z ${PHONE_IP} ]; then
      echo found usb0 device, usb-modem ip: ${PHONE_IP}
      MAVROUTARG=" -e ${PHONE_IP}:14550"
  fi
  #for tablet osd (FPV-VR) as AP
  TAB_IP=`ip route show 0.0.0.0/0 dev ${tab_wlan} | cut -d\  -f3`
  if [ ! -z ${TAB_IP} ]; then
      echo found ${tab_wlan} device, tab ip: ${TAB_IP}
      MAVROUTARG=" -e ${TAB_IP}:14550"
  fi
}

start_all() {
	echo "Loading MAVLink telemetry service..."
	if [ ${fw} = "fpv" ]; then
		start_telemetry
	fi
	if [ ${router} -eq 1 ] || [ ${fw} = "lte" ]; then
	  /usr/bin/mavlink-routerd -c /etc/mavlink.conf ${MAVROUTARG} &
	else
    mavfwd --channels ${channels} --master ${serial} --baudrate ${baud} --out 127.0.0.1:${port_tx} --in 127.0.0.1:${port_rx} &
	fi
	echo "Done."
}

stop_all() {
	echo "Stopping telemetry services..."
	kill -9 $(pidof telemetry_rx)
	kill -9 $(pidof telemetry_tx)
	kill -9 $(pidof mavlink-routerd)
	kill -9 $(pidof mavfwd)
}

case "$1" in
  start)
    start_all
	;;
  stop)
    stop_all
	;;
	restart)
    stop_all
    start_all
	;;
    *)
	echo "Usage: $0 {start|stop|restart}"
	exit 1
esac