From 700e6da25917e499d498ff19a03c365b42117e93 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Tue, 7 Jun 2022 13:17:05 -0400
Subject: [PATCH] full canfd support

---
 boards/nxp/ucans32k146/pca9685.px4board                 | 2 ++
 src/drivers/cyphal/CanardSocketCAN.cpp                  | 5 ++++-
 src/drivers/cyphal/Cyphal.cpp                           | 4 ++++
 src/drivers/cyphal/Kconfig                              | 8 ++++++++
 src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp | 1 +
 src/drivers/cyphal/SubscriptionManager.cpp              | 2 ++
 6 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/boards/nxp/ucans32k146/pca9685.px4board b/boards/nxp/ucans32k146/pca9685.px4board
index 94422db2fc..8b60dc8160 100644
--- a/boards/nxp/ucans32k146/pca9685.px4board
+++ b/boards/nxp/ucans32k146/pca9685.px4board
@@ -1,3 +1,5 @@
 CONFIG_DRIVERS_GPS=n
+CONFIG_CYPHAL_CANFD_SUPPORT=y
+CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER=y
 CONFIG_DRIVERS_PCA9685_UCAN=y
 CONFIG_EXAMPLES_PCA_PWM_TEST=y
diff --git a/src/drivers/cyphal/CanardSocketCAN.cpp b/src/drivers/cyphal/CanardSocketCAN.cpp
index ef1c3afc8b..a3a1237a89 100644
--- a/src/drivers/cyphal/CanardSocketCAN.cpp
+++ b/src/drivers/cyphal/CanardSocketCAN.cpp
@@ -56,8 +56,11 @@ int CanardSocketCAN::init()
 	struct ifreq ifr;
 
 	//FIXME HOTFIX to make this code compile
+#if CONFIG_CYPHAL_CANFD_SUPPORT
+	bool can_fd = 1;
+#else
 	bool can_fd = 0;
-
+#endif
 	_can_fd = can_fd;
 
 	/* open socket */
diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp
index dd6fd8b711..9f4e6e93cb 100644
--- a/src/drivers/cyphal/Cyphal.cpp
+++ b/src/drivers/cyphal/Cyphal.cpp
@@ -117,7 +117,11 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 		return -1;
 	}
 
+#if CONFIG_CYPHAL_CANFD_SUPPORT
+	bool can_fd = true;
+#else
 	bool can_fd = false;
+#endif
 
 	if (can_fd) {
 		_instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD);
diff --git a/src/drivers/cyphal/Kconfig b/src/drivers/cyphal/Kconfig
index 050cd891f8..0e3fd07695 100644
--- a/src/drivers/cyphal/Kconfig
+++ b/src/drivers/cyphal/Kconfig
@@ -41,6 +41,9 @@ if DRIVERS_CYPHAL
         help
             When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined
 
+    config CYPHAL_CANFD_SUPPORT
+        bool "Enable CANFD Support in Cyphal"
+        default n
 
     menu "Publisher support"
 
@@ -87,6 +90,11 @@ if DRIVERS_CYPHAL
         config CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
             bool "uORB sensor_gps Subscriber"
             default n
+
+        config CYPHAL_UORB_PCA_PWM_SUBSCRIBER
+            bool "uORB pca_pwm Subscriber"
+            default n
+
     endmenu
 
     menu "Advertised Services"
diff --git a/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp
index a80edf292d..adfe026d59 100644
--- a/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp
+++ b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp
@@ -47,6 +47,7 @@
 #include <uORB/topics/actuator_outputs.h>
 
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/pca_pwm.h>
 
 template <class T>
 class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber
diff --git a/src/drivers/cyphal/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp
index 3f50850f0f..30eaedca97 100644
--- a/src/drivers/cyphal/SubscriptionManager.cpp
+++ b/src/drivers/cyphal/SubscriptionManager.cpp
@@ -71,6 +71,8 @@ void SubscriptionManager::subscribe()
 
 void SubscriptionManager::updateDynamicSubscriptions()
 {
+	PX4_INFO("Cyphal sub count: %d", UAVCAN_SUB_COUNT);
+
 	for (auto &sub : _uavcan_subs) {
 
 		bool found_subscriber = false;