uavcan:Support runtime setting of CAN interfaces

src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp

@@ -340,7 +340,7 @@ public:
 
 	CanDriver driver;
 
-	CanInitHelper() :
+	CanInitHelper(uint32_t unused = 0x7) :
 		driver(queue_storage_)
 	{
 	}

src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp

@@ -234,6 +234,7 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
 #if UAVCAN_STM32_NUM_IFACES > 1
 	CanIface if1_;
 #endif
+	uint32_t enabledInterfaces_;
 
 	virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks,
 				       const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces],
@@ -249,6 +250,7 @@ public:
 #if UAVCAN_STM32_NUM_IFACES > 1
 		, if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
 #endif
+		, enabledInterfaces_(0x7)
 	{
 		uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check();
 	}
@@ -267,7 +269,7 @@ public:
 	 * Returns zero if OK.
 	 * Returns negative value if failed (e.g. invalid bitrate).
 	 */
-	int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
+	int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, const uavcan::uint32_t EnabledInterfaces);
 
 	virtual CanIface *getIface(uavcan::uint8_t iface_index);
 
@@ -296,9 +298,11 @@ public:
 	enum { BitRateAutoDetect = 0 };
 
 	CanDriver driver;
+	uint32_t enabledInterfaces_;
 
-	CanInitHelper() :
+	CanInitHelper(const uavcan::uint32_t EnabledInterfaces = 0x7) :
-		driver(queue_storage_)
+		driver(queue_storage_),
+		enabledInterfaces_(EnabledInterfaces)
 	{ }
 
 	/**
@@ -309,7 +313,7 @@ public:
 	 */
 	int init(uavcan::uint32_t bitrate)
 	{
-		return driver.init(bitrate, CanIface::NormalMode);
+		return driver.init(bitrate, CanIface::NormalMode, enabledInterfaces_);
 	}
 
 	/**
@@ -330,7 +334,7 @@ public:
 	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
 	{
 		if (inout_bitrate > 0) {
-			return driver.init(inout_bitrate, CanIface::NormalMode);
+			return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
 
 		} else {
 			static const uavcan::uint32_t StandardBitRates[] = {
@@ -343,7 +347,7 @@ public:
 			for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
 				inout_bitrate = StandardBitRates[br];
 
-				const int res = driver.init(inout_bitrate, CanIface::SilentMode);
+				const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
 
 				delay_callable();
 
@@ -351,7 +355,7 @@ public:
 					for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
 						if (!driver.getIface(iface)->isRxBufferEmpty()) {
 							// Re-initializing in normal mode
-							return driver.init(inout_bitrate, CanIface::NormalMode);
+							return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
 						}
 					}
 				}

src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp

@@ -917,7 +917,8 @@ void CanDriver::initOnce()
 #endif
 }
 
-int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
+int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode,
+		    const uavcan::uint32_t enabledInterfaces)
 {
 	int res = 0;
 
@@ -934,28 +935,33 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod
 	/*
 	 * CAN1
 	 */
-	UAVCAN_STM32_LOG("Initing iface 0...");
+	if (enabledInterfaces & 1) {
-	ifaces[0] = &if0_;                          // This link must be initialized first,
+		UAVCAN_STM32_LOG("Initing iface 0...");
-	res = if0_.init(bitrate, mode);             // otherwise an IRQ may fire while the interface is not linked yet;
+		ifaces[0] = &if0_;                          // This link must be initialized first,
+		res = if0_.init(bitrate, mode);             // otherwise an IRQ may fire while the interface is not linked yet;
 
-	if (res < 0) {                              // a typical race condition.
+		if (res < 0) {                              // a typical race condition.
-		UAVCAN_STM32_LOG("Iface 0 init failed %i", res);
+			UAVCAN_STM32_LOG("Iface 0 init failed %i", res);
-		ifaces[0] = UAVCAN_NULLPTR;
+			ifaces[0] = UAVCAN_NULLPTR;
-		goto fail;
+			goto fail;
+		}
 	}
 
 	/*
 	 * CAN2
 	 */
 #if UAVCAN_STM32_NUM_IFACES > 1
-	UAVCAN_STM32_LOG("Initing iface 1...");
-	ifaces[1] = &if1_;                          // Same thing here.
-	res = if1_.init(bitrate, mode);
 
-	if (res < 0) {
+	if (enabledInterfaces & 2) {
-		UAVCAN_STM32_LOG("Iface 1 init failed %i", res);
+		UAVCAN_STM32_LOG("Initing iface 1...");
-		ifaces[1] = UAVCAN_NULLPTR;
+		ifaces[1] = &if1_;                          // Same thing here.
-		goto fail;
+		res = if1_.init(bitrate, mode);
+
+		if (res < 0) {
+			UAVCAN_STM32_LOG("Iface 1 init failed %i", res);
+			ifaces[1] = UAVCAN_NULLPTR;
+			goto fail;
+		}
 	}
 
 #endif

src/drivers/uavcan/uavcan_main.cpp

@@ -518,7 +518,7 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 
 	if (can == nullptr) {
 
-		can = new CanInitHelper();
+		can = new CanInitHelper(board_get_can_interfaces());
 
 		if (can == nullptr) {                    // We don't have exceptions so bad_alloc cannot be thrown
 			PX4_ERR("Out of memory");