fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 13:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan_v1: Refactor Pub/Sub class constrctors

Browse Source 
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
This commit is contained in:
JacobCrabill
2021-02-23 17:57:03 -08:00
committed by Lorenz Meier
parent c8e66a2f5c
commit b5ac6f1eb8
8 changed files with 38 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Actuators/EscClient.hpp
+2 -2
View File
@@ -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() {};
src/drivers/uavcan_v1/Publishers/Gnss.hpp
+2 -2
View File
@@ -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)
{
};
src/drivers/uavcan_v1/Publishers/Publisher.hpp
+11 -7
View File
@@ -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};
src/drivers/uavcan_v1/Subscribers/Battery.hpp
+2 -2
View File
@@ -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
{
src/drivers/uavcan_v1/Subscribers/Esc.hpp
+2 -2
View File
@@ -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
{
src/drivers/uavcan_v1/Subscribers/Gnss.hpp
+2 -2
View File
@@ -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
{
src/drivers/uavcan_v1/Subscribers/Subscriber.hpp
+10 -6
View File
@@ -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};
src/drivers/uavcan_v1/Uavcan.hpp
+7 -7
View File
@@ -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<UavcanSubscription*>