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>

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);
 			}