fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-12 18:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

UAVCANv1 Iteration, dynamic subscribers and decoupling (#17624)

Browse Source 
* Update to use libcanard callback
Reworked param system with lambda

* Use callback instead of lambda saves 500 bytes of flash

* [UAVCANv1] Added ServiceRequest class, subscribers decoupled

* PX4 UAVCANv1 portID param default CANARD_PORT_ID_UNSET (65535U)

* [UAVCANv1] uORB sensor_gps modify timestamp for now untill we've got timesync working

* UAVCANv1 update parameter definition to use -1 as unset
This commit is contained in:
Peter van der Perk
2021-06-04 09:30:17 -04:00
committed by GitHub
parent 470c3cfa6e
commit 9756c70491
24 changed files with 591 additions and 410 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Uavcan.cpp
+46 -41
View File
@@ -82,13 +82,13 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
PX4_INFO("main _canard_instance = %p", &_canard_instance);
_node_manager.subscribe();
for (auto &publisher : _publishers) {
publisher->updateParam();
}
for (auto &subscriber : _dynsubscribers) {
subscriber->updateParam();
}
_sub_manager.subscribe();
_mixing_output.mixingOutput().updateSubscriptions(false, false);
}
@@ -156,21 +156,6 @@ void UavcanNode::init()
// interface init
if (_can_interface) {
if (_can_interface->init() == PX4_OK) {
// We can't accept just any message; we must fist subscribe to specific IDs
// Subscribe to messages uavcan.node.Heartbeat.
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
uavcan_node_Heartbeat_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
for (auto &subscriber : _subscribers) {
subscriber->subscribe();
}
_initialized = true;
}
}
@@ -205,12 +190,7 @@ void UavcanNode::Run()
publisher->updateParam();
}
for (auto &subscriber : _dynsubscribers) {
// Have the subscriber update its associated port-id parameter
// If the port-id changes, (re)start the subscription
// Setting the port-id to 0 disables the subscription
subscriber->updateParam();
}
_sub_manager.updateParams();
_mixing_output.updateParams();
@@ -240,7 +220,8 @@ void UavcanNode::Run()
while (_can_interface->receive(&received_frame) > 0) {
CanardTransfer receive{};
int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
CanardRxSubscription *subscription = NULL;
int32_t result = canardRxAccept2(&_canard_instance, &received_frame, 0, &receive, &subscription);
if (result < 0) {
// An error has occurred: either an argument is invalid or we've ran out of memory.
@@ -253,19 +234,12 @@ void UavcanNode::Run()
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id > 0) {
// If not a fixed port ID, check any subscribers which may have registered it
for (auto &subscriber : _subscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
if (subscription != NULL) {
UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference;
sub_instance->callback(receive);
for (auto &subscriber : _dynsubscribers) {
if (subscriber->hasPortID(receive.port_id)) {
subscriber->callback(receive);
}
}
} else {
PX4_ERR("No matching sub for %d", receive.port_id);
}
// Deallocate the dynamic memory afterwards.
@@ -342,12 +316,43 @@ void UavcanNode::print_info()
publisher->printInfo();
}
for (auto &subscriber : _subscribers) {
subscriber->printInfo();
CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage];
while (rxs != NULL) {
if (rxs->user_reference == NULL) {
PX4_INFO("Message port id %d", rxs->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
}
rxs = rxs->next;
}
for (auto &subscriber : _dynsubscribers) {
subscriber->printInfo();
rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest];
while (rxs != NULL) {
if (rxs->user_reference == NULL) {
PX4_INFO("Service response port id %d", rxs->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
}
rxs = rxs->next;
}
rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse];
while (rxs != NULL) {
if (rxs->user_reference == NULL) {
PX4_INFO("Service request port id %d", rxs->port_id);
} else {
((UavcanBaseSubscriber *)rxs->user_reference)->printInfo();
}
rxs = rxs->next;
}
_mixing_output.printInfo();