Multiple stages for UAVCAN_ENABLE

ROMFS/px4fmu_common/init.d/rc.uavcan

@@ -3,7 +3,26 @@
 # 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
@@ -16,3 +35,14 @@ then
 		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