From b5ac6f1eb8dcfe0a80fa6945ff14ad4f9773a0a2 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Tue, 23 Feb 2021 17:57:03 -0800 Subject: [PATCH] uavcan_v1: Refactor Pub/Sub class constrctors Each class now sets the 'subject name', and the base class looks for a parameter following the format uavcan.pub/sub.SUBJECT_NAME.INSTANCE.id --- src/drivers/uavcan_v1/Actuators/EscClient.hpp | 4 ++-- src/drivers/uavcan_v1/Publishers/Gnss.hpp | 4 ++-- src/drivers/uavcan_v1/Publishers/Publisher.hpp | 18 +++++++++++------- src/drivers/uavcan_v1/Subscribers/Battery.hpp | 4 ++-- src/drivers/uavcan_v1/Subscribers/Esc.hpp | 4 ++-- src/drivers/uavcan_v1/Subscribers/Gnss.hpp | 4 ++-- .../uavcan_v1/Subscribers/Subscriber.hpp | 16 ++++++++++------ src/drivers/uavcan_v1/Uavcan.hpp | 14 +++++++------- 8 files changed, 38 insertions(+), 30 deletions(-) diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/uavcan_v1/Actuators/EscClient.hpp index 57bb180789..e680d9ee68 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/uavcan_v1/Actuators/EscClient.hpp @@ -67,8 +67,8 @@ class UavcanEscController : public UavcanPublisher public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - UavcanPublisher(ins, pmgr, uavcan_pname) { }; + UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr) : + UavcanPublisher(ins, pmgr, "esc") { }; ~UavcanEscController() {}; diff --git a/src/drivers/uavcan_v1/Publishers/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/Gnss.hpp index 29e86be45c..9cac6502fd 100644 --- a/src/drivers/uavcan_v1/Publishers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/Gnss.hpp @@ -50,8 +50,8 @@ class UavcanGnssPublisher : public UavcanPublisher { public: - UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - UavcanPublisher(ins, pmgr, uavcan_pname) + UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(ins, pmgr, "gps", instance) { }; diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 275f76ad13..05245fbf35 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -55,8 +55,8 @@ class UavcanPublisher public: static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; - UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - _canard_instance(ins), _param_manager(pmgr), _uavcan_param(uavcan_pname) { }; + UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) : + _canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { }; // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() = 0; @@ -65,18 +65,21 @@ public: void updateParam() { + char uavcan_param[90]; + sprintf(uavcan_param, "uavcan.pub.%s.%d.id", _subject_name, _instance); + // Set _port_id from _uavcan_param uavcan_register_Value_1_0 value; - _param_manager.GetParamByName(_uavcan_param, value); + _param_manager.GetParamByName(uavcan_param, value); int32_t new_id = value.integer32.value.elements[0]; if (_port_id != new_id) { if (new_id == CANARD_PORT_ID_UNSET) { - PX4_INFO("Disabling publication of %s", _uavcan_param); + PX4_INFO("Disabling publication of subject %s.%d", _subject_name, _instance); } else { _port_id = (CanardPortID)new_id; - PX4_INFO("Enabling %s on port %d", _uavcan_param, _port_id); + PX4_INFO("Enabling subject %s.%d on port %d", _subject_name, _instance, _port_id); } } }; @@ -84,7 +87,7 @@ public: void printInfo() { if (_port_id != CANARD_PORT_ID_UNSET) { - PX4_INFO("Enabled %s on port %d", _uavcan_param, _port_id); + PX4_INFO("Enabled subject %s.%d on port %d", _subject_name, _instance, _port_id); } } @@ -92,7 +95,8 @@ protected: CanardInstance &_canard_instance; UavcanParamManager &_param_manager; CanardRxSubscription _canard_sub; - const char *_uavcan_param; // Port ID parameter + const char *_subject_name; + uint8_t _instance {0}; CanardPortID _port_id {CANARD_PORT_ID_UNSET}; CanardTransferID _transfer_id {0}; diff --git a/src/drivers/uavcan_v1/Subscribers/Battery.hpp b/src/drivers/uavcan_v1/Subscribers/Battery.hpp index c790351614..804599af2e 100644 --- a/src/drivers/uavcan_v1/Subscribers/Battery.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Battery.hpp @@ -50,8 +50,8 @@ class UavcanBmsSubscription : public UavcanSubscription { public: - UavcanBmsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - UavcanSubscription(ins, pmgr, uavcan_pname) { }; + UavcanBmsSubscription(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanSubscription(ins, pmgr, "bms", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/Esc.hpp b/src/drivers/uavcan_v1/Subscribers/Esc.hpp index 36e4dafa64..4e10ac4a26 100644 --- a/src/drivers/uavcan_v1/Subscribers/Esc.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Esc.hpp @@ -54,8 +54,8 @@ class UavcanEscSubscription : public UavcanSubscription { public: - UavcanEscSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - UavcanSubscription(ins, pmgr, uavcan_pname) { }; + UavcanEscSubscription(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanSubscription(ins, pmgr, "esc", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp index 10d0be33d6..6659ef747e 100644 --- a/src/drivers/uavcan_v1/Subscribers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Gnss.hpp @@ -49,8 +49,8 @@ class UavcanGnssSubscription : public UavcanSubscription { public: - UavcanGnssSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - UavcanSubscription(ins, pmgr, uavcan_pname) { }; + UavcanGnssSubscription(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanSubscription(ins, pmgr, "gps", instance) { }; void subscribe() override { diff --git a/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp b/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp index 25c69dd519..3754d6917a 100644 --- a/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp +++ b/src/drivers/uavcan_v1/Subscribers/Subscriber.hpp @@ -55,8 +55,8 @@ class UavcanSubscription public: static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; - UavcanSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *uavcan_pname) : - _canard_instance(ins), _param_manager(pmgr), _uavcan_param(uavcan_pname) { }; + UavcanSubscription(CanardInstance &ins, UavcanParamManager &pmgr, const char *subject_name, uint8_t instance = 0) : + _canard_instance(ins), _param_manager(pmgr), _subject_name(subject_name), _instance(instance) { }; virtual void subscribe() = 0; virtual void unsubscribe() { canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, _port_id); }; @@ -67,9 +67,12 @@ public: void updateParam() { + char uavcan_param[90]; + sprintf(uavcan_param, "uavcan.sub.%s.%d.id", _subject_name, _instance); + // Set _port_id from _uavcan_param uavcan_register_Value_1_0 value; - _param_manager.GetParamByName(_uavcan_param, value); + _param_manager.GetParamByName(uavcan_param, value); int32_t new_id = value.integer32.value.elements[0]; if (_port_id != new_id) { @@ -85,7 +88,7 @@ public: // Subscribe on the new port ID _port_id = (CanardPortID)new_id; - PX4_INFO("Subscribing %s on port %d", _uavcan_param, _port_id); + PX4_INFO("Subscribing %s.%d on port %d", _subject_name, _instance, _port_id); subscribe(); } } @@ -94,7 +97,7 @@ public: void printInfo() { if (_port_id != CANARD_PORT_ID_UNSET) { - PX4_INFO("Subscribed %s on port %d", _uavcan_param, _port_id); + PX4_INFO("Subscribed %s.%d on port %d", _subject_name, _instance, _port_id); } } @@ -102,7 +105,8 @@ protected: CanardInstance &_canard_instance; UavcanParamManager &_param_manager; CanardRxSubscription _canard_sub; - const char *_uavcan_param; // Port ID parameter + const char *_subject_name; + uint8_t _instance {0}; /// TODO: 'type' parameter? uavcan.pub.PORT_NAME.type (see 384.Access.1.0.uavcan) CanardPortID _port_id {CANARD_PORT_ID_UNSET}; diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index b5d202c02f..3ba0f15410 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -238,19 +238,19 @@ private: UavcanParamManager _param_manager; - UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager, "uavcan.pub.gps.0.id"}; + UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager}; - UavcanEscController _esc_controller {_canard_instance, _param_manager, "uavcan.pub.esc.0.id"}; + UavcanEscController _esc_controller {_canard_instance, _param_manager}; // Publication objects: Any object used to bridge a uORB message to a UAVCAN message /// TODO: For some service implementations, it makes sense to have them be both Publishers and Subscribers UavcanPublisher *_publishers[2] {&_gps_pub, &_esc_controller}; - UavcanGnssSubscription _gps0_sub {_canard_instance, _param_manager, "uavcan.sub.gps.0.id"}; - UavcanGnssSubscription _gps1_sub {_canard_instance, _param_manager, "uavcan.sub.gps.1.id"}; - UavcanBmsSubscription _bms0_sub {_canard_instance, _param_manager, "uavcan.sub.bms.0.id"}; - UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, "uavcan.sub.bms.1.id"}; - UavcanEscSubscription _esc_sub {_canard_instance, _param_manager, "uavcan.sub.esc.0.id"}; + UavcanGnssSubscription _gps0_sub {_canard_instance, _param_manager, 0}; + UavcanGnssSubscription _gps1_sub {_canard_instance, _param_manager, 1}; + UavcanBmsSubscription _bms0_sub {_canard_instance, _param_manager, 0}; + UavcanBmsSubscription _bms1_sub {_canard_instance, _param_manager, 1}; + UavcanEscSubscription _esc_sub {_canard_instance, _param_manager, 0}; // Subscription objects: Any object used to bridge a UAVCAN message to a uORB message UavcanSubscription *_subscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub, &_esc_sub}; /// TODO: turn into List