#!nsh
#
# UAVCAN initialization script.
#

#
# Mirriring the UAVCAN_ENABLE param value to an eponymous environment variable.
# TODO there should be a smarter way.
#
set UAVCAN_ENABLE 0
if param compare UAVCAN_ENABLE 1
then
    set UAVCAN_ENABLE 1
fi
if param compare UAVCAN_ENABLE 2
then
    set UAVCAN_ENABLE 2
fi

echo "[i] UAVCAN_ENABLE is $UAVCAN_ENABLE"

#
# Starting stuff according to UAVCAN_ENABLE value
#
if [ $UAVCAN_ENABLE -ge 1 ]
then
	if uavcan start
	then
		# First sensor publisher to initialize takes lowest instance ID
		# This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs
		sleep 1
		echo "[i] UAVCAN started"
	else
		echo "[i] ERROR: Could not start UAVCAN"
		tone_alarm $TUNE_ERR
	fi
fi

if [ $UAVCAN_ENABLE -ge 2 ]
then
        if uavcan start fw
        then
                echo "[i] UAVCAN servers started"
        else
                echo "[i] ERROR: Could not start UAVCAN servers"
                tone_alarm $TUNE_ERR
        fi
fi