From a99b657f03ebfc2723ff13ddf44fac22dd23dbb4 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Tue, 29 Jun 2021 18:10:37 -0700 Subject: [PATCH] uavcan_v1: Fix SubscriptionManager dynamic update --- src/drivers/uavcan_v1/ParamManager.hpp | 18 +++++-- src/drivers/uavcan_v1/Publishers/Gnss.hpp | 3 +- .../uavcan_v1/Publishers/Readiness.hpp | 2 +- .../uavcan_v1/Publishers/uORB/sensor_gps.hpp | 2 +- src/drivers/uavcan_v1/SubscriptionManager.cpp | 51 +++++++++++++------ src/drivers/uavcan_v1/SubscriptionManager.hpp | 25 +++++---- src/drivers/uavcan_v1/Uavcan.cpp | 1 - 7 files changed, 68 insertions(+), 34 deletions(-) diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/uavcan_v1/ParamManager.hpp index 3bc56be4eb..a4b59407b1 100644 --- a/src/drivers/uavcan_v1/ParamManager.hpp +++ b/src/drivers/uavcan_v1/ParamManager.hpp @@ -47,23 +47,31 @@ #include #include +static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U; static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U; static bool px4_param_to_uavcan_port_id(param_t &in, uavcan_register_Value_1_0 &out) { if (param_type(in) == PARAM_TYPE_INT32) { int32_t out_val {}; - param_get(in, &out_val); + const int res = param_get(in, &out_val); + + if (res != PX4_OK) { + // Parameter not found / internal error + return false; + } if (out_val >= 0 && out_val <= CANARD_PORT_ID_MAX) { out.natural16.value.elements[0] = (uint16_t)out_val; - out.natural16.value.count = 1; - uavcan_register_Value_1_0_select_natural16_(&out); - return true; } else { - return false; + // "Invalid" value -- set to "UNSET" + out.natural16.value.elements[0] = CANARD_PORT_ID_UNSET; } + + out.natural16.value.count = 1; + uavcan_register_Value_1_0_select_natural16_(&out); + return true; } return false; diff --git a/src/drivers/uavcan_v1/Publishers/Gnss.hpp b/src/drivers/uavcan_v1/Publishers/Gnss.hpp index fe44f2ce34..ae8e078c3c 100644 --- a/src/drivers/uavcan_v1/Publishers/Gnss.hpp +++ b/src/drivers/uavcan_v1/Publishers/Gnss.hpp @@ -59,8 +59,7 @@ public: // Update the uORB Subscription and broadcast a UAVCAN message virtual void update() override { - if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET - && _port_id != 0) { //FIXME either make default param UNSET or handle 0 in base class + if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { sensor_gps_s gps {}; _gps_sub.update(&gps); diff --git a/src/drivers/uavcan_v1/Publishers/Readiness.hpp b/src/drivers/uavcan_v1/Publishers/Readiness.hpp index 11800d5eb6..eb2756ba22 100644 --- a/src/drivers/uavcan_v1/Publishers/Readiness.hpp +++ b/src/drivers/uavcan_v1/Publishers/Readiness.hpp @@ -60,7 +60,7 @@ 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 && _port_id != 0) { + if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { actuator_armed_s armed {}; _actuator_armed_sub.update(&armed); diff --git a/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp b/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp index 54e0e7de05..b8ac5a9ce1 100644 --- a/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp +++ b/src/drivers/uavcan_v1/Publishers/uORB/sensor_gps.hpp @@ -56,7 +56,7 @@ public: virtual void update() override { // Not sure if actuator_armed is a good indication of readiness but seems close to it - if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) { + if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { sensor_gps_s gps_msg {}; _sensor_gps_sub.update(&gps_msg); diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/uavcan_v1/SubscriptionManager.cpp index 136e08c191..4a93683281 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.cpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.cpp @@ -61,26 +61,44 @@ void SubscriptionManager::subscribe() _access_rsp.subscribe(); for (auto &sub : _uavcan_subs) { - param_t param_handle = param_find(sub.px4_name); + if (sub.instance == NULL) { + param_t param_handle = param_find(sub.px4_name); - if (param_handle == PARAM_INVALID) { - PX4_ERR("Param %s not found", sub.px4_name); - break; - } + if (param_handle == PARAM_INVALID) { + PX4_ERR("Param %s not found", sub.px4_name); + break; + } - if ((param_type(param_handle) == PARAM_TYPE_INT32)) { - int32_t port_id {}; - param_get(param_handle, &port_id); + if ((param_type(param_handle) == PARAM_TYPE_INT32)) { + int32_t port_id {}; + param_get(param_handle, &port_id); - if (port_id >= 0) { // PortID is set create a subscriber - UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager); + if (port_id >= 0) { // PortID is set create a subscriber + UavcanDynamicPortSubscriber *dynsub = sub.create_sub(_canard_instance, _param_manager); - if (_dynsubscribers != NULL) { - _dynsubscribers->setNext(dynsub); + if (dynsub == NULL) { + PX4_ERR("Out of memory"); + return; + } + + if (_dynsubscribers == NULL) { + // Set the head of our linked list + _dynsubscribers = dynsub; + + } else { + // Append the new subscriber to our linked list + UavcanDynamicPortSubscriber *tmp = _dynsubscribers; + + while (tmp->next() != NULL) { + tmp = tmp->next(); + } + + tmp->setNext(dynsub); + } + + sub.instance = dynsub; + dynsub->updateParam(); } - - _dynsubscribers = dynsub; - dynsub->updateParam(); } } } @@ -104,4 +122,7 @@ void SubscriptionManager::updateParams() dynsub->updateParam(); dynsub = dynsub->next(); } + + // Check for any newly-enabled subscriptions + subscribe(); } diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/uavcan_v1/SubscriptionManager.hpp index 498a8c7c91..c340899b82 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.hpp +++ b/src/drivers/uavcan_v1/SubscriptionManager.hpp @@ -60,6 +60,7 @@ typedef struct { const char *px4_name; UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; + UavcanDynamicPortSubscriber *instance {nullptr}; } UavcanDynSubBinder; class SubscriptionManager @@ -85,48 +86,54 @@ private: // Process register requests UavcanAccessResponse _access_rsp {_canard_instance, _param_manager}; - const UavcanDynSubBinder _uavcan_subs[6] { + UavcanDynSubBinder _uavcan_subs[6] { { "UCAN1_ESC0_PID", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanEscSubscriber(ins, pmgr, 0); - } + }, + nullptr }, { "UCAN1_GPS0_PID", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanGnssSubscriber(ins, pmgr, 1); - } + return new UavcanGnssSubscriber(ins, pmgr, 0); + }, + nullptr }, { "UCAN1_GPS1_PID", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanGnssSubscriber(ins, pmgr, 0); - } + return new UavcanGnssSubscriber(ins, pmgr, 1); + }, + nullptr }, { "UCAN1_BMS_ES_PID", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanBmsSubscriber(ins, pmgr, 0); - } + }, + nullptr }, { "UCAN1_LG_BMS_PID", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0); - } + }, + nullptr }, { "UCAN1_UORB_GPS", [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0); - } + }, + nullptr }, }; }; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 82696eaa01..c3b8291fb1 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -364,7 +364,6 @@ void UavcanNode::print_info() } _mixing_output.printInfo(); - _esc_controller.printInfo(); pthread_mutex_unlock(&_node_mutex); }