dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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>
This commit is contained in:
Dmitry Ponomarev 2023-02-23 18:57:50 +03:00 committed by GitHub
parent 013365d6c8
commit a1efafc42b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
15 changed files with 163 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
boards/nxp/mr-canhubk3/fmu.px4board
View File

@ -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

10
boards/nxp/mr-canhubk3/init/rc.board_defaults
View File

@ -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
fi
if param greater -s CYPHAL_ENABLE 0
then
ifup can0
ifup can1
ifup can2
ifup can3
fi

1
boards/px4/fmu-v5/cyphal.px4board
View File

@ -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

116
src/drivers/cyphal/Actuators/EscClient.hpp
View File

@ -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_;
// 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
);
}
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<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;
};

4
src/drivers/cyphal/CanardHandle.cpp
View File

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

15
src/drivers/cyphal/Cyphal.cpp
View File

@ -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);
// _servo_controller.update_outputs(stop_motors, outputs, num_outputs);
auto publisher = static_cast<UavcanEscController *>(_pub_manager.getPublisher("esc"));
if (publisher) {
publisher->update_outputs(stop_motors, outputs, num_outputs);
}
return true;
}

15
src/drivers/cyphal/Cyphal.hpp
View File

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

26
src/drivers/cyphal/ParamManager.hpp
View File

@ -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"},
};

12
src/drivers/cyphal/PublicationManager.cpp
View File

@ -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) {

8
src/drivers/cyphal/PublicationManager.hpp
View File

@ -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

12
src/drivers/cyphal/Subscribers/BaseSubscriber.hpp
View File

@ -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;

18
src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp
View File

@ -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()

2
src/drivers/cyphal/Subscribers/udral/Battery.hpp
View File

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

5
src/drivers/cyphal/SubscriptionManager.cpp
View File

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

10
src/drivers/cyphal/SubscriptionManager.hpp
View File

@ -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