drivers/cyphal: incremental fixes for fmu-v5 (#20671)

* Cyphal: fix comparing floating-point issue

* Cyphal: fix setpoint serialization

* Cyphal: fix bug with wrong comparasion of param name and pub/sub name: remove prefix from UavcanPublisher::updateParam and UavcanDynamicPortSubscriber::updateParam and PublicationManager::updateDynamicPublications

* Cyphal: integrate UavcanEscController with PublicationManager, remove second instance of UavcanEscController from CyphalNode

* Cyphal: publish readiness with minimal frequency because according to UDRAL The drive shall enter STANDBY state automatically if the readiness subject is not updated for CONTROL_TIMEOUT

* Cyphal: increase setpoint publish rate from ~75 to 200 by removing PX4_INFO (it really significantly react on the the output rate) and changing the mixing output rate and the shedule interval

* Cyphal: restore prefix because we need it for uorb over uavcan/cyphal and add udral prefix for non uorb pub/sub

* Cyphal: fix DynamicPortSubscriber subscription: if it has multiple subscribers, it should call subscription only after updating of all port subscribers port identifiers

* Cyphal: fix SubscriptionManager: we should take care about prefix

* Cyphal: fix readiness for test motor mode

* [Cyphal] Fix dynamicsubscription, improve printinfo, enable MR-CANHUBK3 config

---------

Co-authored-by: Peter van der Perk <peter.vanderperk@nxp.com>

boards/nxp/mr-canhubk3/fmu.px4board

@@ -4,6 +4,8 @@ CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_UAVCAN_INTERFACES=1
 CONFIG_COMMON_LIGHT=y
+CONFIG_CYPHAL_BMS_SUBSCRIBER=y
+CONFIG_DRIVERS_CYPHAL=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y

boards/nxp/mr-canhubk3/init/rc.board_defaults

@@ -15,6 +15,7 @@ param set-default MAV_1_REMOTE_PRT 14550
 param set-default MAV_1_UDP_PRT 14550
 
 param set-default SENS_EXT_I2C_PRB 0
+param set-default CYPHAL_ENABLE 0
 
 if param greater -s UAVCAN_ENABLE 0
 then
@@ -23,3 +24,10 @@ then
 	ifup can2
 	ifup can3
 fi
+if param greater -s CYPHAL_ENABLE 0
+then
+	ifup can0
+	ifup can1
+	ifup can2
+	ifup can3
+fi

boards/px4/fmu-v5/cyphal.px4board

@@ -4,3 +4,4 @@ CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
 CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y
 CONFIG_DRIVERS_CYPHAL=y
+CONFIG_CYPHAL_ESC_CONTROLLER=y

src/drivers/cyphal/Actuators/EscClient.hpp

@@ -53,6 +53,7 @@
 #include <lib/perf/perf_counter.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/actuator_test.h>
 #include <uORB/topics/esc_status.h>
 #include <drivers/drv_hrt.h>
 #include <lib/mixer_module/mixer_module.hpp>
@@ -69,69 +70,38 @@ public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
 	UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) :
-		UavcanPublisher(handle, pmgr, "udral", "esc") { };
+		UavcanPublisher(handle, pmgr, "udral.", "esc") { };
 
 	~UavcanEscController() {};
 
 	void update() override
 	{
+		actuator_test_s actuator_test;
+
+		if (_actuator_test_sub.update(&actuator_test)) {
+			_actuator_test_timestamp = actuator_test.timestamp;
+		}
+
 		if (_armed_sub.updated()) {
 			actuator_armed_s new_arming;
 			_armed_sub.update(&new_arming);
 
 			if (new_arming.armed != _armed.armed) {
 				_armed = new_arming;
-				size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+				publish_readiness();
-
-				// Only publish if we have a valid publication ID set
-				if (_port_id == 0) {
-					return;
-				}
-
-				reg_udral_service_common_Readiness_0_1 msg_arming {};
-
-				if (_armed.armed) {
-					msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
-
-				} else if (_armed.prearmed) {
-					msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
-
-				} else {
-					msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
-				}
-
-				uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
-
-				CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
-				const CanardTransferMetadata transfer_metadata = {
-					.priority       = CanardPriorityNominal,
-					.transfer_kind  = CanardTransferKindMessage,
-					.port_id        = arming_pid,                // This is the subject-ID.
-					.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
-					.transfer_id    = _arming_transfer_id,
-				};
-
-				int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
-						&payload_size);
-
-				if (result == 0) {
-					// set the data ready in the buffer and chop if needed
-					++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-					result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-								       &transfer_metadata,
-								       payload_size,
-								       &arming_payload_buffer
-								      );
-				}
 			}
 		}
+
+		if (hrt_absolute_time() > _previous_pub_time + READINESS_PUBLISH_PERIOD) {
+			publish_readiness();
+		}
 	};
 
 	void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs)
 	{
 		if (_port_id > 0) {
 			reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0};
-			size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+			size_t payload_size = reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
 
 			for (uint8_t i = 0; i < MAX_ACTUATORS; i++) {
 				if (i < num_outputs) {
@@ -143,10 +113,6 @@ public:
 				}
 			}
 
-
-			PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1],
-				 (double)msg_sp.value[2], (double)msg_sp.value[3]);
-
 			uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
 
 			const CanardTransferMetadata transfer_metadata = {
@@ -182,10 +148,64 @@ private:
 	 */
 	void esc_status_sub_cb(const CanardRxTransfer &msg);
 
+	void publish_readiness()
+	{
+		const hrt_abstime now = hrt_absolute_time();
+		_previous_pub_time = now;
+
+		size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
+
+		// Only publish if we have a valid publication ID set
+		if (_port_id == 0) {
+			return;
+		}
+
+		reg_udral_service_common_Readiness_0_1 msg_arming {};
+
+		if (_armed.armed || _actuator_test_timestamp + 100_ms > now) {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED;
+
+		} else if (_armed.prearmed) {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY;
+
+		} else {
+			msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP;
+		}
+
+		uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_];
+
+		CanardPortID arming_pid = static_cast<CanardPortID>(static_cast<uint32_t>(_port_id) + 1);
+		const CanardTransferMetadata transfer_metadata = {
+			.priority       = CanardPriorityNominal,
+			.transfer_kind  = CanardTransferKindMessage,
+			.port_id        = arming_pid,                // This is the subject-ID.
+			.remote_node_id = CANARD_NODE_ID_UNSET,      // Messages cannot be unicast, so use UNSET.
+			.transfer_id    = _arming_transfer_id,
+		};
+
+		int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer,
+				&payload_size);
+
+		if (result == 0) {
+			// set the data ready in the buffer and chop if needed
+			++_arming_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+			result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+						       &transfer_metadata,
+						       payload_size,
+						       &arming_payload_buffer);
+		}
+	};
+
 	uint8_t _rotor_count {0};
 
+	static constexpr hrt_abstime READINESS_PUBLISH_PERIOD = 100000;
+	hrt_abstime _previous_pub_time = 0;
+
 	uORB::Subscription _armed_sub{ORB_ID(actuator_armed)};
 	actuator_armed_s _armed {};
 
+	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uint64_t _actuator_test_timestamp{0};
+
 	CanardTransferID _arming_transfer_id;
 };

src/drivers/cyphal/CanardHandle.cpp

@@ -131,7 +131,7 @@ void CanardHandle::receive()
 
 		} else if (result == 1) {
 			// A transfer has been received, process it.
-			// PX4_INFO("received Port ID: %d", receive.port_id);
+			// PX4_INFO("received Port ID: %d", receive.metadata.port_id);
 
 			if (subscription != nullptr) {
 				UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
@@ -145,7 +145,7 @@ void CanardHandle::receive()
 			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
 
 		} else {
-			//PX4_INFO("RX canard %d", result);
+			// PX4_INFO("RX canard %li\n", result);
 		}
 	}
 

src/drivers/cyphal/Cyphal.cpp

@@ -83,6 +83,8 @@ CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) :
 	_pub_manager.updateParams();
 
 	_sub_manager.subscribe();
+
+	_mixing_output.mixingOutput().setMaxTopicUpdateRate(1000000 / 200);
 }
 
 CyphalNode::~CyphalNode()
@@ -135,6 +137,7 @@ int CyphalNode::start(uint32_t node_id, uint32_t bitrate)
 	_instance->active_bitrate = bitrate;
 
 	_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
+	_instance->_mixing_output.ScheduleNow();
 
 	return PX4_OK;
 }
@@ -254,17 +257,19 @@ void CyphalNode::print_info()
 
 	_pub_manager.printInfo();
 
+	PX4_INFO("Message subscriptions:");
 	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindMessage),
 	[&](const CanardRxSubscription * const sub) {
 		if (sub->user_reference == nullptr) {
 			PX4_INFO("Message port id %d", sub->port_id);
 
 		} else {
-			((UavcanBaseSubscriber *)sub->user_reference)->printInfo();
+			((UavcanBaseSubscriber *)sub->user_reference)->printInfo(sub->port_id);
 		}
 	});
 
 
+	PX4_INFO("Service response subscriptions:");
 	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindRequest),
 	[&](const CanardRxSubscription * const sub) {
 		if (sub->user_reference == nullptr) {
@@ -275,6 +280,7 @@ void CyphalNode::print_info()
 		}
 	});
 
+	PX4_INFO("Service request subscriptions:");
 	traverseTree<CanardRxSubscription>(_canard_handle.getRxSubscriptions(CanardTransferKindResponse),
 	[&](const CanardRxSubscription * const sub) {
 		if (sub->user_reference == nullptr) {
@@ -393,8 +399,11 @@ bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX
 	// Hence, the mutex lock in UavcanMixingInterface::Run() is in effect
 
 	/// TODO: Need esc/servo metadata / bitmask(s)
-	_esc_controller.update_outputs(stop_motors, outputs, num_outputs);
+	auto publisher = static_cast<UavcanEscController *>(_pub_manager.getPublisher("esc"));
-	// _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
+
+	if (publisher) {
+		publisher->update_outputs(stop_motors, outputs, num_outputs);
+	}
 
 	return true;
 }

src/drivers/cyphal/Cyphal.hpp

@@ -82,11 +82,10 @@ class UavcanMixingInterface : public OutputModuleInterface
 {
 public:
 	UavcanMixingInterface(pthread_mutex_t &node_mutex,
-			      UavcanEscController &esc_controller) //, UavcanServoController &servo_controller)
+			      PublicationManager &pub_manager)
 		: OutputModuleInterface(MODULE_NAME "-actuators", px4::wq_configurations::uavcan),
 		  _node_mutex(node_mutex),
-		  _esc_controller(esc_controller)/*,
+		  _pub_manager(pub_manager) {}
-		  _servo_controller(servo_controller)*/ {}
 
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
@@ -103,8 +102,7 @@ protected:
 private:
 	friend class CyphalNode;
 	pthread_mutex_t &_node_mutex;
-	UavcanEscController &_esc_controller;
+	PublicationManager &_pub_manager;
-	// UavcanServoController &_servo_controller;
 	MixingOutput _mixing_output{"UCAN1_ESC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 };
 
@@ -115,7 +113,7 @@ class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem
 	 * Base interval, has to be complemented with events from the CAN driver
 	 * and uORB topics sending data, to decrease response time.
 	 */
-	static constexpr unsigned ScheduleIntervalMs = 10;
+	static constexpr unsigned ScheduleIntervalMs = 3;
 
 public:
 
@@ -178,9 +176,6 @@ private:
 	PublicationManager _pub_manager {_canard_handle, _param_manager};
 	SubscriptionManager _sub_manager {_canard_handle, _param_manager};
 
-	/// TODO: Integrate with PublicationManager
+	UavcanMixingInterface _mixing_output {_node_mutex, _pub_manager};
-	UavcanEscController _esc_controller {_canard_handle, _param_manager};
-
-	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
 
 };

src/drivers/cyphal/ParamManager.hpp

@@ -117,19 +117,19 @@ private:
 
 
 	const UavcanParamBinder _uavcan_params[13] {
-		{"uavcan.pub.esc.0.id",                "UCAN1_ESC_PUB",		    px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.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.udral.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},
+		{"uavcan.pub.udral.gps.0.id",                	"UCAN1_GPS_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.pub.actuator_outputs.0.id",   "UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.pub.udral.actuator_outputs.0.id",   	"UCAN1_ACTR_PUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.esc.0.id",                "UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.esc.0.id",                	"UCAN1_ESC0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.gps.0.id",                "UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.gps.0.id",                	"UCAN1_GPS0_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.gps.1.id",                "UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.gps.1.id",                	"UCAN1_GPS1_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.energy_source.0.id",      "UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.energy_source.0.id",      	"UCAN1_BMS_ES_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.battery_status.0.id",     	"UCAN1_BMS_BS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.battery_parameters.0.id",	"UCAN1_BMS_BP_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
-		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_SUB",		px4_param_to_uavcan_port_id, uavcan_port_id_to_px4_param},
+		{"uavcan.sub.udral.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.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.pub.uorb.sensor_gps.0.id",    		"UCAN1_UORB_GPS_P",		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"},
 	};

src/drivers/cyphal/PublicationManager.cpp

@@ -114,6 +114,18 @@ void PublicationManager::updateParams()
 	updateDynamicPublications();
 }
 
+UavcanPublisher *PublicationManager::getPublisher(const char *subject_name)
+{
+	for (auto &dynpub : _dynpublishers) {
+		if (strcmp(dynpub->getSubjectName(), subject_name) == 0) {
+			return dynpub;
+		}
+	}
+
+	return NULL;
+}
+
+
 void PublicationManager::update()
 {
 	for (auto &dynpub : _dynpublishers) {

src/drivers/cyphal/PublicationManager.hpp

@@ -101,6 +101,8 @@ public:
 	void printInfo();
 	void updateParams();
 
+	UavcanPublisher *getPublisher(const char *subject_name);
+
 private:
 	void updateDynamicPublications();
 
@@ -116,7 +118,7 @@ private:
 			{
 				return new UavcanGnssPublisher(handle, pmgr, 0);
 			},
-			"gps",
+			"udral.gps",
 			0
 		},
 #endif
@@ -126,7 +128,7 @@ private:
 			{
 				return new UavcanEscController(handle, pmgr);
 			},
-			"esc",
+			"udral.esc",
 			0
 		},
 #endif
@@ -136,7 +138,7 @@ private:
 			{
 				return new UavcanReadinessPublisher(handle, pmgr, 0);
 			},
-			"readiness",
+			"udral.readiness",
 			0
 		},
 #endif

src/drivers/cyphal/Subscribers/BaseSubscriber.hpp

@@ -122,18 +122,26 @@ public:
 		return _subj_sub._subject_name;
 	}
 
+	const char *getSubjectPrefix()
+	{
+		return _prefix_name;
+	}
+
 	uint8_t getInstance()
 	{
 		return _instance;
 	}
 
-	void printInfo()
+	void printInfo(CanardPortID port_id = CANARD_PORT_ID_UNSET)
 	{
 		SubjectSubscription *curSubj = &_subj_sub;
 
 		while (curSubj != nullptr) {
 			if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
-				PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+				if (port_id == CANARD_PORT_ID_UNSET ||
+				    port_id == curSubj->_canard_sub.port_id) {
+					PX4_INFO("Subscribed %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub.port_id);
+				}
 			}
 
 			curSubj = curSubj->next;

src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp

@@ -61,6 +61,8 @@ public:
 	void updateParam()
 	{
 		SubjectSubscription *curSubj = &_subj_sub;
+		bool unsubscribeRequired = false;
+		bool subscribeRequired = false;
 
 		while (curSubj != nullptr) {
 			char uavcan_param[90];
@@ -76,29 +78,37 @@ public:
 				if (curSubj->_canard_sub.port_id != new_id) {
 					if (new_id == CANARD_PORT_ID_UNSET) {
 						// Cancel subscription
-						unsubscribe();
+						unsubscribeRequired = true;
 
 					} else {
 						if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) {
 							// Already active; unsubscribe first
-							unsubscribe();
+							unsubscribeRequired = true;
 						}
 
 						// Subscribe on the new port ID
 						curSubj->_canard_sub.port_id = (CanardPortID)new_id;
 						PX4_INFO("Subscribing %s%s.%d on port %d", _prefix_name, curSubj->_subject_name, _instance,
 							 curSubj->_canard_sub.port_id);
-						subscribe();
+						subscribeRequired = true;
 					}
 				}
 
 			} else if (curSubj->_canard_sub.port_id != CANARD_PORT_ID_UNSET) { // No valid sub id unsubscribe when neccesary
 				// Already active; unsubscribe first
-				unsubscribe();
+				unsubscribeRequired = true;
 			}
 
 			curSubj = curSubj->next;
 		}
+
+		if (unsubscribeRequired) {
+			unsubscribe();
+		}
+
+		if (subscribeRequired) {
+			subscribe();
+		}
 	};
 
 	UavcanDynamicPortSubscriber *next()

src/drivers/cyphal/Subscribers/udral/Battery.hpp

@@ -110,7 +110,7 @@ public:
 			bat_status.connected = true; // Based on some threshold or an error??
 
 			// Estimate discharged mah from Joule
-			if (_nominal_voltage != NAN) {
+			if (PX4_ISFINITE(_nominal_voltage)) {
 				bat_status.discharged_mah = (source_ts.value.full_energy.joule - source_ts.value.energy.joule)
 							    / (_nominal_voltage * WH_TO_JOULE);
 			}

src/drivers/cyphal/SubscriptionManager.cpp

@@ -78,10 +78,13 @@ void SubscriptionManager::updateDynamicSubscriptions()
 
 		while (dynsub != nullptr) {
 			// Check if subscriber has already been created
+			const char *subj_prefix = dynsub->getSubjectPrefix();
 			const char *subj_name = dynsub->getSubjectName();
 			const uint8_t instance = dynsub->getInstance();
+			char subject_name[90];
+			snprintf(subject_name, sizeof(subject_name), "%s%s", subj_prefix, subj_name);
 
-			if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) {
+			if (strcmp(subject_name, sub.subject_name) == 0 && instance == sub.instance) {
 				found_subscriber = true;
 				break;
 			}

src/drivers/cyphal/SubscriptionManager.hpp

@@ -126,7 +126,7 @@ private:
 			{
 				return new UavcanEscSubscriber(handle, pmgr, 0);
 			},
-			"esc",
+			"udral.esc",
 			0
 		},
 #endif
@@ -136,7 +136,7 @@ private:
 			{
 				return new UavcanGnssSubscriber(handle, pmgr, 0);
 			},
-			"gps",
+			"udral.gps",
 			0
 		},
 #endif
@@ -146,7 +146,7 @@ private:
 			{
 				return new UavcanGnssSubscriber(handle, pmgr, 1);
 			},
-			"gps",
+			"udral.gps",
 			1
 		},
 #endif
@@ -156,7 +156,7 @@ private:
 			{
 				return new UavcanBmsSubscriber(handle, pmgr, 0);
 			},
-			"energy_source",
+			"udral.energy_source",
 			0
 		},
 #endif
@@ -166,7 +166,7 @@ private:
 			{
 				return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0);
 			},
-			"legacy_bms",
+			"udral.legacy_bms",
 			0
 		},
 #endif