diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index c9c0cd7b62..f43507aa76 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -128,7 +128,7 @@ private: {"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.sub.uorb.sensor_gps.0.id", "UCAN1_UORB_GPS_S", 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/uavcan_v1/PublicationManager.cpp b/src/drivers/uavcan_v1/PublicationManager.cpp index ff69f687e7..b592c0dc0d 100644 --- a/src/drivers/uavcan_v1/PublicationManager.cpp +++ b/src/drivers/uavcan_v1/PublicationManager.cpp @@ -77,7 +77,7 @@ void PublicationManager::updateDynamicPublications() uint16_t port_id = value.natural16.value.elements[0]; if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber - UavcanPublisher *dynpub = sub.create_pub(_canard_instance, _param_manager); + UavcanPublisher *dynpub = sub.create_pub(_canard_instance, _param_manager, _work_item); if (dynpub == nullptr) { PX4_ERR("Out of memory"); diff --git a/src/drivers/uavcan_v1/PublicationManager.hpp b/src/drivers/uavcan_v1/PublicationManager.hpp index 1be1b433bc..ee7bda051b 100644 --- a/src/drivers/uavcan_v1/PublicationManager.hpp +++ b/src/drivers/uavcan_v1/PublicationManager.hpp @@ -48,10 +48,6 @@ #define CONFIG_UAVCAN_V1_GNSS_PUBLISHER 0 #endif -#ifndef CONFIG_UAVCAN_V1_ESC_CONTROLLER -#define CONFIG_UAVCAN_V1_ESC_CONTROLLER 0 -#endif - #ifndef CONFIG_UAVCAN_V1_READINESS_PUBLISHER #define CONFIG_UAVCAN_V1_READINESS_PUBLISHER 0 #endif @@ -67,7 +63,6 @@ /* Preprocessor calculation of publisher count */ #define UAVCAN_PUB_COUNT CONFIG_UAVCAN_V1_GNSS_PUBLISHER + \ - CONFIG_UAVCAN_V1_ESC_CONTROLLER + \ CONFIG_UAVCAN_V1_READINESS_PUBLISHER + \ CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER @@ -86,7 +81,7 @@ #include "Publishers/uORB/uorb_publisher.hpp" typedef struct { - UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; + UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *wrk) {}; const char *subject_name; const uint8_t instance; } UavcanDynPubBinder; @@ -94,7 +89,8 @@ typedef struct { class PublicationManager { public: - PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {} + PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *wrk) : + _canard_instance(ins), _param_manager(pmgr), _work_item(wrk) {} ~PublicationManager(); void update(); @@ -106,35 +102,26 @@ private: CanardInstance &_canard_instance; UavcanParamManager &_param_manager; + px4::WorkItem *_work_item; List _dynpublishers; const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] { #if CONFIG_UAVCAN_V1_GNSS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher * { - return new UavcanGnssPublisher(ins, pmgr, 0); + return new UavcanGnssPublisher(ins, pmgr, wrk, 0); }, "gps", 0 }, #endif -#if CONFIG_UAVCAN_V1_ESC_CONTROLLER - { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * - { - return new UavcanEscController(ins, pmgr); - }, - "esc", - 0 - }, -#endif #if CONFIG_UAVCAN_V1_READINESS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher * { - return new UavcanReadinessPublisher(ins, pmgr, 0); + return new UavcanReadinessPublisher(ins, pmgr, wrk, 0); }, "readiness", 0 @@ -142,9 +129,9 @@ private: #endif #if CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher * { - return new uORB_over_UAVCAN_Publisher(ins, pmgr, ORB_ID(actuator_outputs)); + return new uORB_over_UAVCAN_Publisher(ins, pmgr, wrk, ORB_ID(actuator_outputs)); }, "uorb.actuator_outputs", 0 @@ -152,9 +139,9 @@ private: #endif #if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardInstance & ins, UavcanParamManager & pmgr, px4::WorkItem * wrk) -> UavcanPublisher * { - return new uORB_over_UAVCAN_Publisher(ins, pmgr, ORB_ID(sensor_gps)); + return new uORB_over_UAVCAN_Publisher(ins, pmgr, wrk, ORB_ID(sensor_gps)); }, "uorb.sensor_gps", 0 diff --git a/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp index f9babd4da4..1c60575114 100644 --- a/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp @@ -41,6 +41,8 @@ #pragma once +#include + // DS-15 Specification Messages #include #include @@ -49,11 +51,12 @@ class UavcanNode; #include "Publisher.hpp" -class UavcanGnssPublisher : public UavcanPublisher +class UavcanGnssPublisher : public UavcanPublisher, public uORB::SubscriptionCallbackWorkItem { public: - UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "ds_015", "gps", instance) + UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *work_item, uint8_t instance = 0) : + UavcanPublisher(ins, pmgr, "ds_015", "gps", instance), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_gps), instance) { }; @@ -63,9 +66,9 @@ public: // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() override { - if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + if (uORB::SubscriptionCallbackWorkItem::updated() && _port_id != CANARD_PORT_ID_UNSET) { sensor_gps_s gps {}; - _gps_sub.update(&gps); + uORB::SubscriptionCallbackWorkItem::update(&gps); reg_drone_physics_kinematics_geodetic_Point_0_1 geo {}; geo.latitude = gps.lat; @@ -134,7 +137,5 @@ public: private: - /// TODO: Allow >1 instance - uORB::Subscription _gps_sub{ORB_ID(sensor_gps)}; CanardTransferID _transfer_id_2 {0}; }; diff --git a/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp b/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp index dc41bd479d..cdcfd96dba 100644 --- a/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp +++ b/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp @@ -42,16 +42,21 @@ #pragma once +#include +#include + // DS-15 Specification Messages #include #include "../Publisher.hpp" -class UavcanReadinessPublisher : public UavcanPublisher +class UavcanReadinessPublisher : public UavcanPublisher, public uORB::SubscriptionCallbackWorkItem { public: - UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance) + UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *work_item, + uint8_t instance = 0) : + UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(actuator_armed), instance) { }; @@ -62,10 +67,11 @@ public: virtual void update() override { // Not sure if actuator_armed is a good indication of readiness but seems close to it - if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + if (uORB::SubscriptionCallbackWorkItem::updated() && _port_id != CANARD_PORT_ID_UNSET) { actuator_armed_s armed {}; - _actuator_armed_sub.update(&armed); + uORB::SubscriptionCallbackWorkItem::update(&armed); + /// TODO: Replace this with Readiness publisher... reg_drone_service_common_Readiness_0_1 readiness {}; if (armed.armed) { diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/uavcan_v1/Publishers/Publisher.hpp index 6de4d8fa49..7f40f33536 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/Publisher.hpp @@ -43,6 +43,7 @@ #include +#include #include #include diff --git a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp index a422b4cac0..7a1b66f3de 100644 --- a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp +++ b/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp @@ -43,18 +43,17 @@ #include "../Publisher.hpp" -#include +#include #include template -class uORB_over_UAVCAN_Publisher : public UavcanPublisher +class uORB_over_UAVCAN_Publisher : public UavcanPublisher, public uORB::SubscriptionCallbackWorkItem { public: - uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta, - uint8_t instance = 0) : + uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, px4::WorkItem *work_item, + const orb_metadata *meta, uint8_t instance = 0) : UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance), - _uorb_meta{meta}, - _uorb_sub(meta) + uORB::SubscriptionCallbackWorkItem(work_item, meta, instance) {}; ~uORB_over_UAVCAN_Publisher() override = default; @@ -63,9 +62,9 @@ public: virtual void update() override { // Not sure if actuator_armed is a good indication of readiness but seems close to it - if (_uorb_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { + if (uORB::SubscriptionCallbackWorkItem::updated() && _port_id != CANARD_PORT_ID_UNSET) { T data {}; - _uorb_sub.update(&data); + uORB::SubscriptionCallbackWorkItem::update(&data); CanardTransfer transfer = { .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, @@ -82,6 +81,9 @@ public: ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. canardTxPush(&_canard_instance, &transfer); transmit(); + + // ensure callback is registered + uORB::SubscriptionCallbackWorkItem::registerCallback(); } }; @@ -94,8 +96,8 @@ protected: } private: - const orb_metadata *_uorb_meta; - uORB::Subscription _uorb_sub; + // const orb_metadata *_uorb_meta; + // uORB::Subscription _uorb_sub; }; /* ---- Specializations of get_payload_size() to reduce wasted bandwidth where possible ---- */ diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index ce2a27480b..35ae20f888 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -78,7 +78,8 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), - _can_interface(interface) + _can_interface(interface), + _pub_manager(_canard_instance, _param_manager, this) { pthread_mutex_init(&_node_mutex, nullptr); @@ -248,8 +249,30 @@ void UavcanNode::Run() transmit(); - /* Process received messages */ + // Process received messages + receive(); + // Pop canardTx queue to send out responses to requets + transmit(); + + perf_end(_cycle_perf); + + if (_instance && _task_should_exit.load()) { + ScheduleClear(); + + if (_initialized && _can_interface != nullptr) { + _can_interface->close(); + _initialized = false; + } + + _instance = nullptr; + } + + pthread_mutex_unlock(&_node_mutex); +} + +void UavcanNode::receive() +{ uint8_t data[64] {}; CanardFrame received_frame{}; received_frame.payload = &data; @@ -285,24 +308,6 @@ void UavcanNode::Run() //PX4_INFO("RX canard %d", result); } } - - // Pop canardTx queue to send out responses to requets - transmit(); - - perf_end(_cycle_perf); - - if (_instance && _task_should_exit.load()) { - ScheduleClear(); - - if (_initialized && _can_interface != nullptr) { - _can_interface->close(); - _initialized = false; - } - - _instance = nullptr; - } - - pthread_mutex_unlock(&_node_mutex); } void UavcanNode::transmit() diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index 74f40026a3..6646e502dc 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -140,6 +140,7 @@ public: static UavcanNode *instance() { return _instance; } + static void do_receive() { if (_instance) _instance->receive(); }; static void do_transmit() { if (_instance) _instance->transmit(); }; /* The bit rate that can be passed back to the bootloader */ @@ -150,6 +151,7 @@ private: void Run() override; void fill_node_info(); + void receive(); void transmit(); // Sends a heartbeat at 1s intervals @@ -195,12 +197,15 @@ private: NodeClient *_node_client {nullptr}; #endif - PublicationManager _pub_manager {_canard_instance, _param_manager}; + PublicationManager _pub_manager;// {_canard_instance, _param_manager, this}; SubscriptionManager _sub_manager {_canard_instance, _param_manager}; - /// TODO: Integrate with PublicationManager +#if CONFIG_UAVCAN_V1_ESC_CONTROLLER + /// NOTE: This needs to be part of the OutputModuleInterface to run as a ScheduledWorkItem, + /// so **DON'T** use it within the PublicationManager!! UavcanEscController _esc_controller {_canard_instance, _param_manager}; UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; +#endif }; diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c index 67b5741b0a..1761634c32 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/uavcan_v1/parameters.c @@ -139,7 +139,7 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1); * @max 6143 * @group UAVCAN v1 */ -PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1); +PARAM_DEFINE_INT32(UCAN1_UORB_GPS_S, -1); /**