From b7cc0bd1398bd7058937b98a18cd529896fe11f4 Mon Sep 17 00:00:00 2001
From: Benjamin Perseghetti <bperseghetti@rudislabs.com>
Date: Mon, 6 Jun 2022 19:37:20 -0400
Subject: [PATCH] Include can-utils.

---
 .../nxp/ucans32k146/nuttx-config/nsh/defconfig  |  5 +++++
 boards/nxp/ucans32k146/pca9685.px4board         |  2 +-
 src/drivers/cyphal/ParamManager.hpp             |  3 ++-
 src/drivers/cyphal/SubscriptionManager.hpp      | 17 ++++++++++++++++-
 src/drivers/cyphal/parameters.c                 |  9 +++++++++
 5 files changed, 33 insertions(+), 3 deletions(-)

diff --git a/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig b/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig
index 54fd7511e4..11abb786d2 100644
--- a/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig
+++ b/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig
@@ -25,6 +25,11 @@ CONFIG_BCH=y
 CONFIG_BOARDCTL_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=3997
 CONFIG_BUILTIN=y
+CONFIG_CAN=y
+CONFIG_CANUTILS_CANDUMP=y
+CONFIG_CANUTILS_CANSEND=y
+CONFIG_CAN_EXTID=y
+CONFIG_CAN_FD=y
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
diff --git a/boards/nxp/ucans32k146/pca9685.px4board b/boards/nxp/ucans32k146/pca9685.px4board
index 78c01dd376..94422db2fc 100644
--- a/boards/nxp/ucans32k146/pca9685.px4board
+++ b/boards/nxp/ucans32k146/pca9685.px4board
@@ -1,3 +1,3 @@
-CONFIG_DRIVERS_PCA9685_UCAN=y
 CONFIG_DRIVERS_GPS=n
+CONFIG_DRIVERS_PCA9685_UCAN=y
 CONFIG_EXAMPLES_PCA_PWM_TEST=y
diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp
index c9c0cd7b62..823acae183 100644
--- a/src/drivers/cyphal/ParamManager.hpp
+++ b/src/drivers/cyphal/ParamManager.hpp
@@ -116,7 +116,7 @@ public:
 private:
 
 
-	const UavcanParamBinder _uavcan_params[13] {
+	const UavcanParamBinder _uavcan_params[14] {
 		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.servo.0.id",              "UCAN1_SERVO_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.gps.0.id",                "UCAN1_GPS_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
@@ -130,6 +130,7 @@ private:
 		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		{"uavcan.pub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS_P",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.uorb.pca_pwm.0.id",       "UCAN1_PCA_PWM_S",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_SUB"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_SUB"},
 	};
diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp
index 1e25a65d52..57ce24bd46 100644
--- a/src/drivers/cyphal/SubscriptionManager.hpp
+++ b/src/drivers/cyphal/SubscriptionManager.hpp
@@ -61,13 +61,18 @@
 #define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0
 #endif
 
+#ifndef CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER
+#define CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER 0
+#endif
+
 /* Preprocessor calculation of Subscribers count */
 
 #define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \
 	CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \
 	CONFIG_CYPHAL_BMS_SUBSCRIBER + \
-	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER
+	CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER + \
+	CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER
 
 #include <px4_platform_common/defines.h>
 #include <drivers/drv_hrt.h>
@@ -179,6 +184,16 @@ private:
 			"uorb.sensor_gps",
 			0
 		},
+#endif
+#if CONFIG_CYPHAL_UORB_PCA_PWM_SUBSCRIBER
+		{
+			[](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
+			{
+				return new uORB_over_UAVCAN_Subscriber<pca_pwm_s>(handle, pmgr, ORB_ID(pca_pwm));
+			},
+			"uorb.pca_pwm",
+			0
+		},
 #endif
 	};
 };
diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c
index 52d3db5b35..aaa0d93df1 100644
--- a/src/drivers/cyphal/parameters.c
+++ b/src/drivers/cyphal/parameters.c
@@ -151,6 +151,15 @@ PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1);
  */
 PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1);
 
+/**
+ * pca_pwm uORB over Cyphal subscription port ID.
+ *
+ * @min -1
+ * @max 6143
+ * @group Cyphal
+ */
+PARAM_DEFINE_INT32(UCAN1_PCA_PWM_S, -1);
+
 // Publication Port IDs
 
 /**