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

Added the notion of BaseSubscriber which allows to

Browse Source 
subsscription for services responses and request and helps the usage of fixed
port subscribers Furthermore move register autconfigure logic from Uavcan.cpp
to NodeManager
This commit is contained in:
Peter van der Perk
2021-03-09 14:35:50 +01:00
committed by Lorenz Meier
parent 9f9b01504d
commit c4ab2797eb
16 changed files with 843 additions and 289 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan_v1/Uavcan.cpp
+14 -227
View File
@@ -87,7 +87,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
publisher->updateParam();
}
for (auto &subscriber : _subscribers) {
for (auto &subscriber : _dynsubscribers) {
subscriber->updateParam();
}
@@ -168,20 +168,6 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_heartbeat_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_Access_1_0_FIXED_PORT_ID_,
uavcan_register_Access_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_access_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindResponse,
uavcan_register_List_1_0_FIXED_PORT_ID_,
uavcan_register_List_Response_1_0_EXTENT_BYTES_,
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_register_list_subscription);
canardRxSubscribe(&_canard_instance,
CanardTransferKindMessage,
bms_port_id,
@@ -196,6 +182,9 @@ void UavcanNode::init()
CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
&_drone_srv_gps_subscription);
for (auto &subscriber : _subscribers) {
subscriber->subscribe();
}
_initialized = true;
}
@@ -231,7 +220,7 @@ void UavcanNode::Run()
publisher->updateParam();
}
for (auto &subscriber : _subscribers) {
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
@@ -246,40 +235,6 @@ void UavcanNode::Run()
perf_begin(_cycle_perf);
perf_count(_interval_perf);
if (hrt_elapsed_time(&_node_manager._uavcan_pnp_nodeidallocation_last) >= 1_s &&
_node_manager._node_register_setup != CANARD_NODE_ID_UNSET &&
_node_manager._node_register_request_index == _node_manager._node_register_last_received_index + 1) {
PX4_INFO("NodeID %i request register %i", _node_manager._node_register_setup,
_node_manager._node_register_request_index);
uavcan_register_List_Request_1_0 msg;
msg.index = _node_manager._node_register_request_index;
uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer request = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = _node_manager._node_register_setup,
.transfer_id = _uavcan_register_list_request_transfer_id,
.payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
int32_t result = uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &request.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_list_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &request);
++_node_manager._node_register_request_index;
}
}
// send uavcan::node::Heartbeat_1_0 @ 1 Hz
sendHeartbeat();
@@ -288,6 +243,8 @@ void UavcanNode::Run()
publisher->update();
}
_node_manager.update();
// Transmitting
// Look at the top of the TX queue.
for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
@@ -340,13 +297,7 @@ void UavcanNode::Run()
// A transfer has been received, process it.
// PX4_INFO("received Port ID: %d", receive.port_id);
if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) {
result = handleRegisterList(receive);
} else if (receive.port_id == uavcan_register_Access_1_0_FIXED_PORT_ID_) {
result = handleRegisterAccess(receive);
} else if (receive.port_id == bms_port_id) {
if (receive.port_id == bms_port_id) {
result = handleBMSStatus(receive);
} else if (receive.port_id == gps_port_id) {
@@ -359,6 +310,12 @@ void UavcanNode::Run()
subscriber->callback(receive);
}
}
for (auto &subscriber : _dynsubscribers) {
if (receive.port_id == subscriber->id()) {
subscriber->callback(receive);
}
}
}
// Deallocate the dynamic memory afterwards.
@@ -492,176 +449,6 @@ void UavcanNode::sendHeartbeat()
}
}
int UavcanNode::handleRegisterList(const CanardTransfer &receive)
{
uavcan_register_List_Response_1_0 msg;
size_t register_in_size_bits = receive.payload_size;
uavcan_register_List_Response_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
int result;
if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id",
msg.name.name.count) == 0) { //Demo GPS status publisher
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id);
_node_manager._node_register_last_received_index++;
uavcan_register_Access_Request_1_0 request_msg;
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = gps_port_id;
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
} else if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id",
msg.name.name.count) == 0) { //Battery status publisher
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
PX4_INFO("NodeID %i battery_status publisher set PortID to %i", receive.remote_node_id, bms_port_id);
_node_manager._node_register_last_received_index++;
uavcan_register_Access_Request_1_0 request_msg;
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = bms_port_id;
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
} else {
uavcan_register_Value_1_0 out_value;
_node_manager._node_register_last_received_index++;
_node_manager._uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); // Reset timer for next request
if (_param_manager.GetParamByName(msg.name, out_value)) {
_node_manager._node_register_setup = CANARD_NODE_ID_UNSET;
uavcan_register_Access_Request_1_0 request_msg;
memcpy(&request_msg.name, &msg.name, sizeof(uavcan_register_Name_1_0));
uavcan_register_Value_1_0_select_natural16_(&request_msg.value); /// TODO don't select; see which it is
request_msg.value.natural16.value.count = 1;
request_msg.value.natural16.value.elements[0] = out_value.natural16.value.elements[0]; /// TODO
uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindRequest,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id,
.payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &request_payload_buffer,
};
result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
}
}
return result;
}
int UavcanNode::handleRegisterAccess(const CanardTransfer &receive)
{
uavcan_register_Access_Request_1_0 msg {};
size_t register_in_size_bits = receive.payload_size;
uavcan_register_Access_Request_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &register_in_size_bits);
int result {0};
uavcan_register_Value_1_0 value = msg.value;
uavcan_register_Name_1_0 name = msg.name;
/// TODO: get/set parameter based on whether empty or not
if (uavcan_register_Value_1_0_is_empty_(&value)) { // Tag Type: uavcan_primitive_Empty_1_0
// Value is empty -- 'Get' only
result = _param_manager.GetParamByName(name, value) ? 0 : -1;
} else {
// Set value
result = _param_manager.SetParamByName(name, value) ? 0 : -1;
}
/// TODO: Access_Response
uavcan_register_Access_Response_1_0 response {};
response.value = value;
uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
CanardTransfer transfer = {
.timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited.
.priority = CanardPriorityNominal,
.transfer_kind = CanardTransferKindResponse,
.port_id = uavcan_register_Access_1_0_FIXED_PORT_ID_, // This is the subject-ID.
.remote_node_id = receive.remote_node_id, // Messages cannot be unicast, so use UNSET.
.transfer_id = _uavcan_register_access_request_transfer_id, /// TODO: track register Access _response_ separately?
.payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
.payload = &response_payload_buffer,
};
result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size);
if (result == 0) {
// set the data ready in the buffer and chop if needed
++_uavcan_register_access_request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject.
result = canardTxPush(&_canard_instance, &transfer);
}
return result;
}
int UavcanNode::handleBMSStatus(const CanardTransfer &receive)
{
PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id);