fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_v1: Cleanup _uavcan_subs in SubMgr

Browse Source 
Remove hard-coding of PX4 param names - use subject name and instance
instead

Also cleanup duplicates of CANARD_PORT_ID_UNSET/MAX vars
This commit is contained in:
JacobCrabill 2021-07-02 10:17:46 -07:00 committed by Daniel Agar
parent 8f55c960b6
commit 3143756d97
5 changed files with 14 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/drivers/uavcan_v1/Publishers/Publisher.hpp
View File

@ -59,9 +59,6 @@
class UavcanPublisher
{
public:
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U;
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) { };

4
src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp
View File

@ -46,13 +46,11 @@
#include <lib/parameters/param.h>
#include "../CanardInterface.hpp"
#include "../ParamManager.hpp"
class UavcanBaseSubscriber
{
public:
static constexpr uint16_t CANARD_PORT_ID_UNSET = 65535U;
static constexpr uint16_t CANARD_PORT_ID_MAX = 32767U;
UavcanBaseSubscriber(CanardInstance &ins, const char *subject_name, uint8_t instance = 0) :
_canard_instance(ins), _instance(instance)
{

23
src/drivers/uavcan_v1/SubscriptionManager.cpp
View File

@ -42,6 +42,8 @@
#include "SubscriptionManager.hpp"
#include "ParamManager.hpp"
SubscriptionManager::~SubscriptionManager()
{
UavcanDynamicPortSubscriber *dynsub;
@ -66,12 +68,6 @@ void SubscriptionManager::subscribe()
void SubscriptionManager::updateDynamicSubscriptions()
{
for (auto &sub : _uavcan_subs) {
param_t param_handle = param_find(sub.px4_name);
if (param_handle == PARAM_INVALID) {
PX4_ERR("Param %s not found", sub.px4_name);
break;
}
bool found_subscriber = false;
UavcanDynamicPortSubscriber *dynsub = _dynsubscribers;
@ -93,11 +89,14 @@ void SubscriptionManager::updateDynamicSubscriptions()
continue;
}
if ((param_type(param_handle) == PARAM_TYPE_INT32)) {
int32_t port_id {};
param_get(param_handle, &port_id);
char uavcan_param[90];
snprintf(uavcan_param, sizeof(uavcan_param), "uavcan.sub.%s.%d.id", sub.subject_name, sub.instance);
uavcan_register_Value_1_0 value;
if (port_id >= 0) { // PortID is set create a subscriber
if (_param_manager.GetParamByName(uavcan_param, value)) {
uint16_t port_id = value.natural16.value.elements[0];
if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber
dynsub = sub.create_sub(_canard_instance, _param_manager);
if (dynsub == NULL) {
@ -122,6 +121,10 @@ void SubscriptionManager::updateDynamicSubscriptions()
dynsub->updateParam();
}
} else {
PX4_ERR("Port ID param for subscriber %s.%u not found", sub.subject_name, sub.instance);
return;
}
}
}

7
src/drivers/uavcan_v1/SubscriptionManager.hpp
View File

@ -58,7 +58,6 @@
#include "Subscribers/uORB/sensor_gps.hpp"
typedef struct {
const char *px4_name;
UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {};
const char *subject_name;
const uint8_t instance;
@ -91,7 +90,6 @@ private:
const UavcanDynSubBinder _uavcan_subs[6] {
{
"UCAN1_ESC0_SUB",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanEscSubscriber(ins, pmgr, 0);
@ -100,7 +98,6 @@ private:
0
},
{
"UCAN1_GPS0_SUB",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanGnssSubscriber(ins, pmgr, 0);
@ -109,7 +106,6 @@ private:
0
},
{
"UCAN1_GPS1_SUB",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanGnssSubscriber(ins, pmgr, 1);
@ -118,7 +114,6 @@ private:
1
},
{
"UCAN1_BMS_ES_SUB",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanBmsSubscriber(ins, pmgr, 0);
@ -127,7 +122,6 @@ private:
0
},
{
"UCAN1_LG_BMS_SUB",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0);
@ -136,7 +130,6 @@ private:
0
},
{
"UCAN1_UORB_GPS",
[](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber *
{
return new UORB_over_UAVCAN_sensor_gps_Subscriber(ins, pmgr, 0);

2
src/drivers/uavcan_v1/Uavcan.cpp
View File

@ -80,8 +80,6 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
_canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC;
}
PX4_INFO("main _canard_instance = %p", &_canard_instance);
_node_manager.subscribe();
for (auto &publisher : _publishers) {