diff --git a/boards/nxp/mr-canhubk3/fmu.px4board b/boards/nxp/mr-canhubk3/fmu.px4board index feafdf2b9a..e8f3d264cc 100644 --- a/boards/nxp/mr-canhubk3/fmu.px4board +++ b/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 diff --git a/boards/nxp/mr-canhubk3/init/rc.board_defaults b/boards/nxp/mr-canhubk3/init/rc.board_defaults index d9bd59a7fd..64dedb9fe8 100644 --- a/boards/nxp/mr-canhubk3/init/rc.board_defaults +++ b/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 @@ -22,4 +23,11 @@ then ifup can1 ifup can2 ifup can3 -fi \ No newline at end of file +fi +if param greater -s CYPHAL_ENABLE 0 +then + ifup can0 + ifup can1 + ifup can2 + ifup can3 +fi diff --git a/boards/px4/fmu-v5/cyphal.px4board b/boards/px4/fmu-v5/cyphal.px4board index 1f5bee721b..fd1d17bcfa 100644 --- a/boards/px4/fmu-v5/cyphal.px4board +++ b/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 diff --git a/src/drivers/cyphal/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp index c2bf03e6f2..8d3f75fba6 100644 --- a/src/drivers/cyphal/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -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_; - - // 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(static_cast(_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 - ); - } + publish_readiness(); } } + + 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(static_cast(_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; }; diff --git a/src/drivers/cyphal/CanardHandle.cpp b/src/drivers/cyphal/CanardHandle.cpp index 7a90863587..2632713193 100644 --- a/src/drivers/cyphal/CanardHandle.cpp +++ b/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); } } diff --git a/src/drivers/cyphal/Cyphal.cpp b/src/drivers/cyphal/Cyphal.cpp index dd6fd8b711..73de1992ff 100644 --- a/src/drivers/cyphal/Cyphal.cpp +++ b/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(_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(_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(_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); - // _servo_controller.update_outputs(stop_motors, outputs, num_outputs); + auto publisher = static_cast(_pub_manager.getPublisher("esc")); + + if (publisher) { + publisher->update_outputs(stop_motors, outputs, num_outputs); + } return true; } diff --git a/src/drivers/cyphal/Cyphal.hpp b/src/drivers/cyphal/Cyphal.hpp index b8550b74fb..004a509f40 100644 --- a/src/drivers/cyphal/Cyphal.hpp +++ b/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)/*, - _servo_controller(servo_controller)*/ {} + _pub_manager(pub_manager) {} 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; - // UavcanServoController &_servo_controller; + PublicationManager &_pub_manager; 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 - UavcanEscController _esc_controller {_canard_handle, _param_manager}; - - UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; + UavcanMixingInterface _mixing_output {_node_mutex, _pub_manager}; }; diff --git a/src/drivers/cyphal/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp index c9c0cd7b62..67c5dbc20b 100644 --- a/src/drivers/cyphal/ParamManager.hpp +++ b/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.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.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.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.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.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.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.udral.esc.0.id", "UCAN1_ESC_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.udral.gps.0.id", "UCAN1_GPS_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.udral.esc.0.id", "UCAN1_ESC0_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.udral.gps.1.id", "UCAN1_GPS1_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.udral.battery_status.0.id", "UCAN1_BMS_BS_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.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.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"}, }; diff --git a/src/drivers/cyphal/PublicationManager.cpp b/src/drivers/cyphal/PublicationManager.cpp index 2fd3f745aa..74ef3699c4 100644 --- a/src/drivers/cyphal/PublicationManager.cpp +++ b/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) { diff --git a/src/drivers/cyphal/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp index b3d39ffbac..2c3da87d9d 100644 --- a/src/drivers/cyphal/PublicationManager.hpp +++ b/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 diff --git a/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp b/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp index 33b8f0ea87..e1f86902d6 100644 --- a/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp +++ b/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; diff --git a/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp index dd417c9967..8d0fe45bc3 100644 --- a/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp +++ b/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() diff --git a/src/drivers/cyphal/Subscribers/udral/Battery.hpp b/src/drivers/cyphal/Subscribers/udral/Battery.hpp index 732c3dc2e8..0a8a6c798e 100644 --- a/src/drivers/cyphal/Subscribers/udral/Battery.hpp +++ b/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); } diff --git a/src/drivers/cyphal/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp index 3f50850f0f..e489357fc2 100644 --- a/src/drivers/cyphal/SubscriptionManager.cpp +++ b/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; } diff --git a/src/drivers/cyphal/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp index 1e25a65d52..bb56e1e057 100644 --- a/src/drivers/cyphal/SubscriptionManager.hpp +++ b/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