From e267dc020640a5f13ed22b3826aaacd12c977d37 Mon Sep 17 00:00:00 2001
From: JacobCrabill <jacob@flyvoly.com>
Date: Wed, 17 Feb 2021 21:35:35 -0800
Subject: [PATCH] uavcan_v1: Start adding framework for Subscribers

---
 src/drivers/uavcan_v1/Uavcan.cpp | 19 ++++++++++++++-
 src/drivers/uavcan_v1/Uavcan.hpp | 41 ++++++++++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 1 deletion(-)

diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp
index cf59af44a6..df1a04b6d7 100644
--- a/src/drivers/uavcan_v1/Uavcan.cpp
+++ b/src/drivers/uavcan_v1/Uavcan.cpp
@@ -56,7 +56,12 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
 UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
-	_can_interface(interface)
+	_can_interface(interface),
+	_gps0_sub("uavcan.sub.gps.0.id"),
+	_gps1_sub("uavcan.sub.gps.1.id"),
+	_bms0_sub("uavcan.sub.bms.0.id"),
+	_bms1_sub("uavcan.sub.bms.1.id"),
+	_subscribers({_gps0_sub, _gps1_sub, _bms0_sub, _bms1_sub})
 {
 	pthread_mutex_init(&_node_mutex, nullptr);
 
@@ -214,6 +219,10 @@ void UavcanNode::Run()
 
 		// update parameters from storage
 		updateParams();
+
+		for (auto &subscriber : _subscribers) {
+			subscriber.updateParam();
+		}
 	}
 
 	perf_begin(_cycle_perf);
@@ -318,6 +327,8 @@ void UavcanNode::Run()
 		}
 	}
 
+	/* Process received messages */
+
 	uint8_t data[64] {};
 	CanardFrame received_frame{};
 	received_frame.payload = &data;
@@ -350,6 +361,12 @@ void UavcanNode::Run()
 			} else if (receive.port_id == gps_port_id) {
 				result = handleUORBSensorGPS(receive);
 
+			} else if (receive.port_id > 0) {
+				for (auto &subscriber : _subscribers) {
+					if (receive.port_id == subscriber.id()) {
+						subscriber.callback(receive);
+					}
+				}
 			}
 
 			// Deallocate the dynamic memory afterwards.
diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp
index ec805920cc..3be1dd1fe3 100644
--- a/src/drivers/uavcan_v1/Uavcan.hpp
+++ b/src/drivers/uavcan_v1/Uavcan.hpp
@@ -74,6 +74,28 @@
 
 #include "CanardInterface.hpp"
 
+typedef struct {
+	const char *uavcan_name;
+	const char *px4_name;
+} UavcanParamBinder;
+
+class UavcanSubscription
+{
+public:
+	UavcanSubscription(const char *pname) : _uavcan_param(pname) {};
+
+	virtual void callback(const CanardTransfer &msg) { (void)msg; /* TODO virtual func; implement in subclass */ };
+
+	CanardPortID id() { return _port_id; };
+
+	void updateParam() { /* TODO: Set _port_id from _uavcan_param */ };
+
+private:
+	const char *_uavcan_param;
+
+	CanardPortID _port_id {0};
+};
+
 class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
 {
 	/*
@@ -170,6 +192,13 @@ private:
 	hrt_abstime _regulated_drone_sensor_bmsstatus_last{0};
 	CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0};
 
+	UavcanSubscription _gps0_sub;
+	UavcanSubscription _gps1_sub;
+	UavcanSubscription _bms0_sub;
+	UavcanSubscription _bms1_sub;
+
+	UavcanSubscription _subscribers[4];
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
 		(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
@@ -177,4 +206,16 @@ private:
 		(ParamInt<px4::params::UAVCAN_V1_BAT_MD>) _param_uavcan_v1_bat_md,
 		(ParamInt<px4::params::UAVCAN_V1_BAT_ID>) _param_uavcan_v1_bat_id
 	)
+
+	/// TODO:
+	/// use qsort() to order alphabetically by UAVCAN name
+	/// copy over Ken's parameter find/get/set code
+	const UavcanParamBinder _uavcan_params[6] {
+		{"uavcan.pub.esc.0.id",   "UAVCANV1_ESC0_PID"},
+		{"uavcan.pub.servo.0.id", "UAVCANV1_SERVO0_PID"},
+		{"uavcan.sub.gps.0.id",   "UAVCANV1_GPS0_PID"},
+		{"uavcan.sub.gps.1.id",   "UAVCANV1_GPS1_PID"},
+		{"uavcan.sub.bms.0.id",   "UAVCANV1_BMS0_PID"},
+		{"uavcan.sub.bms.1.id",   "UAVCANV1_BMS1_PID"},
+	};
 };