DroneCAN SocketCAN driver add FMU support

src/drivers/uavcan/CMakeLists.txt

@@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
 set(UAVCAN_PLATFORM "generic")
 
 if(CONFIG_ARCH_CHIP)
-	if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
+	if(${CONFIG_NET_CAN} MATCHES "y")
+		set(UAVCAN_DRIVER "socketcan")
+		set(UAVCAN_TIMER 1)
+	elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
 		set(UAVCAN_DRIVER "kinetis")
 		set(UAVCAN_TIMER 1)
 	elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7")

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

@@ -360,18 +360,13 @@ public:
 	 * This function can either initialize the driver at a fixed bit rate, or it can perform
 	 * automatic bit rate detection. For theory please refer to the CiA application note #801.
 	 *
-	 * @param delay_callable    A callable entity that suspends execution for strictly more than one second.
-	 *                          The callable entity will be invoked without arguments.
-	 *                          @ref getRecommendedListeningDelay().
-	 *
-	 * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
+     * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
 	 *                          If auto detection was used, the function will update the argument
 	 *                          with established bit rate. In case of an error the value will be undefined.
 	 *
 	 * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
 	 */
-	template <typename DelayCallable>
-	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
+	int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
 	{
 		if (inout_bitrate > 0) {
 			return driver.init(inout_bitrate, CanIface::NormalMode);
@@ -389,7 +384,7 @@ public:
 
 				const int res = driver.init(inout_bitrate, CanIface::SilentMode);
 
-				delay_callable();
+				usleep(1000000);
 
 				if (res >= 0) {
 					for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {

src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt

@@ -6,6 +6,7 @@ add_compile_options(-Wno-unused-variable)
 add_library(uavcan_socketcan_driver STATIC
     src/socketcan.cpp
     src/thread.cpp
+    src/clock.cpp
     )
 
 add_dependencies(uavcan_socketcan_driver uavcan)

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp

@@ -46,6 +46,16 @@
 
 namespace uavcan_socketcan
 {
+namespace clock
+{
+/**
+ * Performs UTC phase and frequency adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ * This function is thread safe.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment);
+//FIXME
+}
 /**
  * Different adjustment modes can be used for time synchronization
  */

src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp

@@ -71,7 +71,7 @@ class CanIface : public uavcan::ICanIface
 	SystemClock clock;
 
 public:
-	uavcan::uint32_t socketInit(const char *can_iface_name);
+	uavcan::uint32_t socketInit(uint32_t index);
 
 	uavcan::int16_t send(const uavcan::CanFrame &frame,
 			     uavcan::MonotonicTime tx_deadline,
@@ -109,13 +109,13 @@ public:
 	CanDriver() : update_event_(*this)
 	{}
 
-	uavcan::int32_t initIface(uint32_t index, const char *name)
+	uavcan::int32_t initIface(uint32_t index)
 	{
 		if (index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) {
 			return -1;
 		}
 
-		return if_[index].socketInit(name);
+		return if_[index].socketInit(index);
 	}
 
 	/**
@@ -180,10 +180,10 @@ public:
 	 */
 	int init(uavcan::uint32_t bitrate)
 	{
-		driver.initIface(0, "can0");
-#if UAVCAN_SOCKETCAN_NUM_IFACES > 1
-		driver.initIface(1, "can1");
-#endif
+		for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
+			driver.initIface(i);
+		}
+
 		return driver.init(bitrate);
 	}
 
@@ -191,23 +191,20 @@ public:
 	 * This function can either initialize the driver at a fixed bit rate, or it can perform
 	 * automatic bit rate detection. For theory please refer to the CiA application note #801.
 	 *
-	 * @param delay_callable    A callable entity that suspends execution for strictly more than one second.
-	 *                          The callable entity will be invoked without arguments.
-	 *                          @ref getRecommendedListeningDelay().
-	 *
-	 * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
+	  * @param bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
 	 *                          If auto detection was used, the function will update the argument
 	 *                          with established bit rate. In case of an error the value will be undefined.
 	 *
 	 * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
 	 */
-	template <typename DelayCallable>
-	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = 1000000)
-	{
-		if (inout_bitrate > 0) {
-			return driver.init(inout_bitrate);
 
+	int init(uavcan::uint32_t &bitrate = 1000000)
+	{
+		if (bitrate > 0) {
+			return driver.init(bitrate);
 		}
+
+		return -1;
 	}
 
 	/**

src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/clock.cpp

@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *   Kinetis Port Author David Sidrane <david_s5@nscdg.com>
+ *   NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <uavcan_nuttx/thread.hpp>
+#include <uavcan_nuttx/socketcan.hpp>
+#include <px4_platform_common/log.h>
+
+namespace uavcan_socketcan
+{
+namespace clock
+{
+/**
+ * Performs UTC phase and frequency adjustment.
+ * The UTC time will be zero until first adjustment has been performed.
+ * This function is thread safe.
+ */
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+	//printf("Adjust UTC\n");
+	//clock::adjustUtc(adjustment);
+}
+
+}
+}

src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp

@@ -69,7 +69,7 @@ struct BitTimingSettings {
 
 } // namespace
 
-uavcan::uint32_t CanIface::socketInit(const char *can_iface_name)
+uavcan::uint32_t CanIface::socketInit(uint32_t index)
 {
 
 	struct sockaddr_can addr;
@@ -86,7 +86,7 @@ uavcan::uint32_t CanIface::socketInit(const char *can_iface_name)
 		return -1;
 	}
 
-	strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1);
+	snprintf(ifr.ifr_name, IFNAMSIZ, "can%li", index);
 	ifr.ifr_name[IFNAMSIZ - 1] = '\0';
 	ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name);
 
@@ -194,7 +194,7 @@ uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicT
 	_send_tv->tv_usec = tx_deadline.toUSec() % 1000000ULL;
 	_send_tv->tv_sec = (tx_deadline.toUSec() - _send_tv->tv_usec) / 1000000ULL;
 
-	res = sendmsg(_fd, &_send_msg, 0);
+	res = sendmsg(_fd, &_send_msg, MSG_DONTWAIT);
 
 	if (res > 0) {
 		return 1;
@@ -271,12 +271,10 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)())
 
 int CanDriver::init(uavcan::uint32_t bitrate)
 {
-	pfds[0].fd     = if_[0].getFD();
-	pfds[0].events = POLLIN | POLLOUT;
-#if UAVCAN_SOCKETCAN_NUM_IFACES > 1
-	pfds[1].fd     = if_[1].getFD();
-	pfds[1].events = POLLIN | POLLOUT;
-#endif
+	for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {
+		pfds[i].fd     = if_[i].getFD();
+		pfds[i].events = POLLIN | POLLOUT;
+	}
 
 	/*
 	 * TODO add filter configuration ioctl
@@ -308,8 +306,7 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks,
 	}
 
 	inout_masks.read = 0;
-	//FIXME NuttX SocketCAN implement POLLOUT
-	inout_masks.write = 0x3;
+	inout_masks.write = 0;
 
 	if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) {
 		for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) {

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

@@ -320,18 +320,14 @@ public:
 	 * This function can either initialize the driver at a fixed bit rate, or it can perform
 	 * automatic bit rate detection. For theory please refer to the CiA application note #801.
 	 *
-	 * @param delay_callable    A callable entity that suspends execution for strictly more than one second.
-	 *                          The callable entity will be invoked without arguments.
-	 *                          @ref getRecommendedListeningDelay().
-	 *
-	 * @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
+	* @param inout_bitrate     Fixed bit rate or zero. Zero invokes the bit rate detection process.
 	 *                          If auto detection was used, the function will update the argument
 	 *                          with established bit rate. In case of an error the value will be undefined.
 	 *
 	 * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
 	 */
 	template <typename DelayCallable>
-	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
+	int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
 	{
 		if (inout_bitrate > 0) {
 			return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
@@ -349,7 +345,7 @@ public:
 
 				const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
 
-				delay_callable();
+				usleep(1000000);
 
 				if (res >= 0) {
 					for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {

src/drivers/uavcan/uavcan_drivers/stm32h7/driver/include/uavcan_stm32h7/can.hpp

@@ -341,8 +341,7 @@ public:
 	 *
 	 * @return                  Negative value on error; non-negative on success. Refer to constants Err*.
 	 */
-	template <typename DelayCallable>
-	int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
+	int init(uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
 	{
 		if (inout_bitrate > 0) {
 			return driver.init(inout_bitrate, CanIface::NormalMode, enabledInterfaces_);
@@ -360,7 +359,7 @@ public:
 
 				const int res = driver.init(inout_bitrate, CanIface::SilentMode, enabledInterfaces_);
 
-				delay_callable();
+				usleep(1000000);
 
 				if (res >= 0) {
 					for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {

src/drivers/uavcan/uavcan_main.cpp

@@ -74,6 +74,8 @@
  */
 UavcanNode *UavcanNode::_instance;
 
+static UavcanNode::CanInitHelper *can = nullptr;
+
 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
 	ModuleParams(nullptr),
@@ -404,28 +406,15 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 		return -1;
 	}
 
-	/*
-	 * CAN driver init
-	 * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
-	 * shipped with libuavcan does not support deinitialization.
-	 */
-	static CanInitHelper *can = nullptr;
-
 	if (can == nullptr) {
 
 		can = new CanInitHelper(board_get_can_interfaces());
 
-		if (can == nullptr) {                    // We don't have exceptions so bad_alloc cannot be thrown
+		if (can == nullptr) {  // We don't have exceptions so bad_alloc cannot be thrown
 			PX4_ERR("Out of memory");
 			return -1;
 		}
 
-		const int can_init_res = can->init(bitrate);
-
-		if (can_init_res < 0) {
-			PX4_ERR("CAN driver init failed %i", can_init_res);
-			return can_init_res;
-		}
 	}
 
 	/*
@@ -438,15 +427,6 @@ UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 		return -1;
 	}
 
-	const int node_init_res = _instance->init(node_id, can->driver.updateEvent());
-
-	if (node_init_res < 0) {
-		delete _instance;
-		_instance = nullptr;
-		PX4_ERR("Node init failed %i", node_init_res);
-		return node_init_res;
-	}
-
 	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
 	_instance->_mixing_interface_esc.ScheduleNow();
 	_instance->_mixing_interface_servo.ScheduleNow();
@@ -634,6 +614,36 @@ UavcanNode::handle_time_sync(const uavcan::TimerEvent &)
 void
 UavcanNode::Run()
 {
+	if (!_node_init) {
+		// Node ID
+		int32_t node_id = 1;
+		(void)param_get(param_find("UAVCAN_NODE_ID"), &node_id);
+
+		if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
+			PX4_ERR("Invalid Node ID %" PRId32, node_id);
+			::exit(1);
+		}
+
+		// CAN bitrate
+		int32_t bitrate = 1000000;
+		(void)param_get(param_find("UAVCAN_BITRATE"), &bitrate);
+
+		/*
+		* CAN driver init
+		 * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
+		 * shipped with libuavcan does not support deinitialization.
+		 */
+		const int can_init_res = can->init(bitrate);
+
+		if (can_init_res < 0) {
+			PX4_ERR("CAN driver init failed %i", can_init_res);
+		}
+
+		_instance->init(node_id, can->driver.updateEvent());
+
+		_node_init = true;
+	}
+
 	pthread_mutex_lock(&_node_mutex);
 
 	if (_output_count == 0) {

src/drivers/uavcan/uavcan_main.hpp

@@ -220,6 +220,7 @@ private:
 
 	uavcan_node::Allocator	 _pool_allocator;
 
+	bool                    _node_init{false};
 	uavcan::Node<>			_node;				///< library instance
 	pthread_mutex_t			_node_mutex;
 

src/drivers/uavcannode/UavcanNode.cpp

@@ -367,7 +367,7 @@ void UavcanNode::Run()
 	if (!_initialized) {
 
 
-		const int can_init_res = _can->init(_bitrate);
+		const int can_init_res = _can->init((uint32_t)_bitrate);
 
 		if (can_init_res < 0) {
 			PX4_ERR("CAN driver init failed %i", can_init_res);