uavcan_v1: More work on Subscriber class

src/drivers/uavcan_v1/Uavcan.cpp

@@ -53,15 +53,72 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
 # endif // CONFIG_CAN
 #endif // NuttX
 
+/**                Begin UavcanSubscription Class Definitions                **/
+/// TODO: Reorganize into separate files
+
+void UavcanGpsSubscription::subscribe()
+{
+	// Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1
+	canardRxSubscribe(_canard_instance,
+			  CanardTransferKindMessage,
+			  _port_id,
+			  reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_,
+			  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+			  &_canard_sub);
+}
+
+void UavcanGpsSubscription::callback(const CanardTransfer &receive)
+{
+	PX4_INFO("GpsCallback");
+
+	reg_drone_physics_kinematics_geodetic_Point_0_1 geo {};
+	size_t geo_size_in_bits = receive.payload_size;
+	reg_drone_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits);
+
+	double lat = geo.latitude;
+	double lon = geo.longitude;
+	PX4_INFO("Latitude: %f, Longitude: %f", lat, lon);
+	/// do something with the data
+}
+
+void UavcanBmsSubscription::subscribe()
+{
+	// Subscribe to messages reg.drone.service.battery.Status.0.1
+	canardRxSubscribe(_canard_instance,
+			  CanardTransferKindMessage,
+			  _port_id,
+			  reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
+			  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
+			  &_canard_sub);
+}
+
+void UavcanBmsSubscription::callback(const CanardTransfer &receive)
+{
+	PX4_INFO("BmsCallback");
+
+	reg_drone_service_battery_Status_0_1 bat {};
+	size_t bat_size_in_bits = receive.payload_size;
+	reg_drone_service_battery_Status_0_1_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bits);
+
+	uavcan_si_unit_voltage_Scalar_1_0 V_Min = bat.cell_voltage_min_max[0];
+	uavcan_si_unit_voltage_Scalar_1_0 V_Max = bat.cell_voltage_min_max[1];
+	double vmin = static_cast<double>(V_Min.volt);
+	double vmax = static_cast<double>(V_Max.volt);
+	PX4_INFO("Min voltage: %f, Max Voltage: %f", vmin, vmax);
+	/// do something with the data
+}
+
+/**                 End UavcanSubscription Class Definitions                 **/
+
 UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
 	_can_interface(interface),
-	_gps0_sub("uavcan.sub.gps.0.id"),
+	_gps0_sub(&_canard_instance, "uavcan.sub.gps.0.id"),
-	_gps1_sub("uavcan.sub.gps.1.id"),
+	_gps1_sub(&_canard_instance, "uavcan.sub.gps.1.id"),
-	_bms0_sub("uavcan.sub.bms.0.id"),
+	_bms0_sub(&_canard_instance, "uavcan.sub.bms.0.id"),
-	_bms1_sub("uavcan.sub.bms.1.id"),
+	_bms1_sub(&_canard_instance, "uavcan.sub.bms.1.id"),
-	_subscribers({_gps0_sub, _gps1_sub, _bms0_sub, _bms1_sub})
+	_subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
 {
 	pthread_mutex_init(&_node_mutex, nullptr);
 
@@ -153,6 +210,8 @@ void UavcanNode::Run()
 		if (_can_interface) {
 			if (_can_interface->init() == PX4_OK) {
 
+				// We can't accept just any message; we must fist subscribe to specific IDs
+
 				// Subscribe to messages uavcan.node.Heartbeat.
 				canardRxSubscribe(&_canard_instance,
 						  CanardTransferKindMessage,
@@ -188,7 +247,7 @@ void UavcanNode::Run()
 				canardRxSubscribe(&_canard_instance,
 						  CanardTransferKindMessage,
 						  bms_port_id,
-						  reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_,
+						  reg_drone_service_battery_Status_0_1_EXTENT_BYTES_,
 						  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 						  &_drone_srv_battery_subscription);
 
@@ -221,7 +280,10 @@ void UavcanNode::Run()
 		updateParams();
 
 		for (auto &subscriber : _subscribers) {
-			subscriber.updateParam();
+			// Have the subscriber update its associated port-id parameter
+			// If the port-id changes, (re)start the subscription
+			// Setting the port-id to 0 disables the subscription
+			subscriber->updateParam();
 		}
 	}
 
@@ -363,8 +425,8 @@ void UavcanNode::Run()
 
 			} else if (receive.port_id > 0) {
 				for (auto &subscriber : _subscribers) {
-					if (receive.port_id == subscriber.id()) {
+					if (receive.port_id == subscriber->id()) {
-						subscriber.callback(receive);
+						subscriber->callback(receive);
 					}
 				}
 			}

src/drivers/uavcan_v1/Uavcan.hpp

@@ -54,10 +54,14 @@
 #include <canard.h>
 #include <canard_dsdl.h>
 
-#include <reg/drone/srv/battery/Status_0_1.h>
+#include <reg/drone/service/battery/Status_0_1.h>
-#include <reg/drone/srv/battery/Parameters_0_1.h>
+#include <reg/drone/service/battery/Parameters_0_1.h>
 #include <uavcan/node/Heartbeat_1_0.h>
 
+/// DSDL UPDATE WIP
+#include <reg/drone/physics/kinematics/geodetic/Point_0_1.h>
+/// ---------------
+
 //Quick and Dirty PNP imlementation only V1 for now as well
 #include <uavcan/node/ID_1_0.h>
 #include <uavcan/pnp/NodeIDAllocationData_1_0.h>
@@ -82,20 +86,74 @@ typedef struct {
 class UavcanSubscription
 {
 public:
-	UavcanSubscription(const char *pname) : _uavcan_param(pname) {};
+	UavcanSubscription(CanardInstance *ins, const char *pname) : _canard_instance(ins), _uavcan_param(pname) { };
+
+	virtual void subscribe() = 0;
+	virtual void unsubscribe() { canardRxUnsubscribe(_canard_instance, CanardTransferKindMessage, _port_id); };
 
-	virtual void callback(const CanardTransfer &msg) { (void)msg; /* TODO virtual func; implement in subclass */ };
+	virtual void callback(const CanardTransfer &msg) = 0;
 
 	CanardPortID id() { return _port_id; };
 
-	void updateParam() { /* TODO: Set _port_id from _uavcan_param */ };
+	void updateParam()
+	{
+		// Set _port_id from _uavcan_param
+
+		CanardPortID new_id {0};
+		/// TODO: --- update parameter here: new_id = uavcan_param_get(_uavcan_param);
+
+		if (_port_id != new_id) {
+			if (new_id == 0) {
+				// Cancel subscription
+				unsubscribe();
+
+			} else {
+				if (_port_id > 0) {
+					// Already active; unsubscribe first
+					unsubscribe();
+				}
+
+				// Subscribe on the new port ID
+				_port_id = new_id;
+				subscribe();
+			}
+		}
+	};
 
-private:
+protected:
+	CanardInstance *_canard_instance;
+	CanardRxSubscription _canard_sub;
 	const char *_uavcan_param;
 
 	CanardPortID _port_id {0};
 };
 
+class UavcanGpsSubscription : public UavcanSubscription
+{
+public:
+	UavcanGpsSubscription(CanardInstance *ins, const char *pname) : UavcanSubscription(ins, pname) { };
+
+	void subscribe() override;
+
+	void callback(const CanardTransfer &msg) override;
+
+private:
+
+};
+
+class UavcanBmsSubscription : public UavcanSubscription
+{
+public:
+	UavcanBmsSubscription(CanardInstance *ins, const char *pname) : UavcanSubscription(ins, pname) { };
+
+	void subscribe() override;
+
+	void callback(const CanardTransfer &msg) override;
+
+private:
+
+};
+
 class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem
 {
 	/*
@@ -156,6 +214,11 @@ private:
 	CanardRxSubscription _register_access_subscription;
 	CanardRxSubscription _register_list_subscription;
 
+	CanardRxSubscription _canard_gps0_sub;
+	CanardRxSubscription _canard_gps1_sub;
+	CanardRxSubscription _canard_bms0_sub;
+	CanardRxSubscription _canard_bms1_sub;
+
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 
@@ -188,16 +251,16 @@ private:
 	int32_t _node_register_last_received_index = -1;
 
 	// regulated::drone::sensor::BMSStatus_1_0
-	uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_];
+	uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_service_battery_Status_0_1_EXTENT_BYTES_];
 	hrt_abstime _regulated_drone_sensor_bmsstatus_last{0};
 	CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0};
 
-	UavcanSubscription _gps0_sub;
+	UavcanGpsSubscription _gps0_sub;
-	UavcanSubscription _gps1_sub;
+	UavcanGpsSubscription _gps1_sub;
-	UavcanSubscription _bms0_sub;
+	UavcanBmsSubscription _bms0_sub;
-	UavcanSubscription _bms1_sub;
+	UavcanBmsSubscription _bms1_sub;
 
-	UavcanSubscription _subscribers[4];
+	UavcanSubscription *_subscribers[4]; /// TODO: turn into List<UavcanSubscription*>
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,