UAVCANv1 port unset fixes and deinitialization

src/drivers/uavcan_v1/CanardSocketCAN.hpp

@@ -38,6 +38,7 @@
 #include <stdbool.h>
 #include <stddef.h>
 #include <stdint.h>
+#include <unistd.h>
 
 #include <sys/time.h>
 #include <sys/socket.h>
@@ -61,6 +62,12 @@ public:
 	/// The return value is 0 on succes and -1 on error
 	int init();
 
+	/// Close socket connection
+	int close()
+	{
+		return ::close(_fd);
+	}
+
 	/// Send a CanardFrame to the CanardSocketInstance socket
 	/// This function is blocking
 	/// The return value is number of bytes transferred, negative value on error.

src/drivers/uavcan_v1/ParamManager.hpp

@@ -47,15 +47,23 @@
 #include <uavcan/_register/Name_1_0.h>
 #include <uavcan/_register/Value_1_0.h>
 
+static constexpr uint16_t CANARD_PORT_ID_MAX   = 32767U;
+
 static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out)
 {
 	if (param_type(in) == PARAM_TYPE_INT32) {
 		int32_t out_val {};
 		param_get(in, &out_val);
-		out.natural16.value.elements[0] = (uint16_t)out_val;
-		out.natural16.value.count = 1;
-		uavcan_register_Value_1_0_select_natural16_(&out);
-		return true;
+
+		if (out_val >= 0 && out_val <= CANARD_PORT_ID_MAX) {
+			out.natural16.value.elements[0] = (uint16_t)out_val;
+			out.natural16.value.count = 1;
+			uavcan_register_Value_1_0_select_natural16_(&out);
+			return true;
+
+		} else {
+			return false;
+		}
 	}
 
 	return false;

src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp

@@ -58,9 +58,10 @@ public:
 	{
 		_subj_sub._subject_name = subject_name;
 		_subj_sub._canard_sub.user_reference = this;
+		_subj_sub._canard_sub.port_id = CANARD_PORT_ID_UNSET;
 	}
 
-	~UavcanBaseSubscriber()
+	virtual ~UavcanBaseSubscriber()
 	{
 		unsubscribe();
 	}

src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp

@@ -100,8 +100,6 @@ public:
 
 	void callback(const CanardTransfer &receive) override
 	{
-		PX4_INFO("BmsCallback");
-
 		if (receive.port_id == _subj_sub._canard_sub.port_id) {
 			reg_drone_physics_electricity_SourceTs_0_1 source_ts {};
 			size_t source_ts_size_in_bytes = receive.payload_size;
@@ -124,8 +122,6 @@ public:
 
 			// TODO uORB publication rate limiting
 			_battery_status_pub.publish(bat_status);
-			print_message(bat_status);
-
 
 		} else if (receive.port_id == _status_sub._canard_sub.port_id) {
 			reg_drone_service_battery_Status_0_2 bat {};

src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp

@@ -70,13 +70,7 @@ public:
 			uavcan_register_Value_1_0 value;
 
 			if (_param_manager.GetParamByName(uavcan_param, value)) {
-				int32_t new_id = value.integer32.value.elements[0];
-
-				// Allow, for example, a default PX4 param value of '-1' to disable subscription
-				if (!isValidPortId(new_id)) {
-					// but always use the standard 'unset' value for comparison
-					new_id = CANARD_PORT_ID_UNSET;
-				}
+				uint16_t new_id = value.natural16.value.elements[0];
 
 				/* FIXME how about partial subscribing */
 				if (curSubj->_canard_sub.port_id != new_id) {
@@ -96,6 +90,10 @@ public:
 						subscribe();
 					}
 				}
+
+			} else if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { // No valid sub id unsubscribe when neccesary
+				// Already active; unsubscribe first
+				unsubscribe();
 			}
 
 			curSubj = curSubj->next;

src/drivers/uavcan_v1/SubscriptionManager.cpp

@@ -42,6 +42,16 @@
 
 #include "SubscriptionManager.hpp"
 
+SubscriptionManager::~SubscriptionManager()
+{
+	UavcanDynamicPortSubscriber *dynsub;
+
+	while (_dynsubscribers != NULL) {
+		dynsub = _dynsubscribers;
+		_dynsubscribers = dynsub->next();
+		delete dynsub;
+	}
+}
 
 
 void SubscriptionManager::subscribe()

src/drivers/uavcan_v1/SubscriptionManager.hpp

@@ -66,6 +66,7 @@ class SubscriptionManager
 {
 public:
 	SubscriptionManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {}
+	~SubscriptionManager();
 
 	void subscribe();
 	void printInfo();

src/drivers/uavcan_v1/Uavcan.cpp

@@ -95,8 +95,6 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 
 UavcanNode::~UavcanNode()
 {
-	delete _can_interface;
-
 	if (_instance) {
 		/* tell the task we want it to go away */
 		_task_should_exit.store(true);
@@ -115,6 +113,8 @@ UavcanNode::~UavcanNode()
 		} while (_instance);
 	}
 
+	delete _can_interface;
+
 	perf_free(_cycle_perf);
 	perf_free(_interval_perf);