From e5cf92f20d0372b5399d8473cf920a4dfc49d6f6 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Sat, 13 Feb 2021 08:54:33 -0800 Subject: [PATCH] uavcan_v1: Refactor RX handling into functions Cleans up the main transfer-handling loop --- src/drivers/uavcan_v1/Uavcan.cpp | 287 +++++++++++++++++-------------- src/drivers/uavcan_v1/Uavcan.hpp | 5 + 2 files changed, 161 insertions(+), 131 deletions(-) diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index d097fd49c7..cf59af44a6 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -339,143 +339,17 @@ void UavcanNode::Run() PX4_INFO("received Port ID: %d", receive.port_id); if (receive.port_id == uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_) { - uavcan_pnp_NodeIDAllocationData_1_0 msg; - - size_t pnp_in_size_bits = receive.payload_size; - uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits); - - //TODO internal database with unique id to node ip mappings now we give an hardcoded ID back - - msg.allocated_node_id.count = 1; - msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID - - _uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); - _node_register_request_index = 0; - _node_register_last_received_index = -1; - _node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured - - PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value); - - uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. - .transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id, - .payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &node_id_alloc_payload_buffer, - }; - - result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size); - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - - } if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { - - 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, ®ister_in_size_bits); - - - if (strncmp((char *)msg.name.name.elements, "uavcan.pub.gnss_uorb.id", - msg.name.name.count) == 0) { //Demo GPS status publisher - _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_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); - } - } - - if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id", - msg.name.name.count) == 0) { //Battery status publisher - _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_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); - } - } + result = handlePnpNodeIDAllocationData(receive); + } else if (receive.port_id == uavcan_register_List_1_0_FIXED_PORT_ID_) { + result = handleRegisterList(receive); } else if (receive.port_id == bms_port_id) { - PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id); - //TODO deserialize - /* - - battery_status_s battery_status{}; - battery_status.id = bms_status.battery_id; - battery_status.voltage_v = bms_status.voltage; - //battery_status.remaining = bms_status.remaining_capacity; - battery_status.timestamp = hrt_absolute_time(); - _battery_status_pub.publish(battery_status);*/ + result = handleBMSStatus(receive); } else if (receive.port_id == gps_port_id) { - PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id); - - sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload; + result = handleUORBSensorGPS(receive); - _sensor_gps_pub.publish(*gps_msg); } // Deallocate the dynamic memory afterwards. @@ -561,3 +435,154 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) print_usage(); return 1; } + +int UavcanNode::handlePnpNodeIDAllocationData(const CanardTransfer &receive) +{ + uavcan_pnp_NodeIDAllocationData_1_0 msg; + + size_t pnp_in_size_bits = receive.payload_size; + uavcan_pnp_NodeIDAllocationData_1_0_deserialize_(&msg, (const uint8_t *)receive.payload, &pnp_in_size_bits); + + //TODO internal database with unique id to node ip mappings now we give an hardcoded ID back + msg.allocated_node_id.count = 1; + msg.allocated_node_id.elements[0].value = 15; // HACK hardcoded ID + + _uavcan_pnp_nodeidallocation_last = hrt_absolute_time(); + _node_register_request_index = 0; + _node_register_last_received_index = -1; + _node_register_setup = msg.allocated_node_id.elements[0].value; // This nodeID has to be configured + + PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value); + + uint8_t node_id_alloc_payload_buffer[uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; + + CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), // Zero if transmission deadline is not limited. + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, // This is the subject-ID. + .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. + .transfer_id = _uavcan_pnp_nodeidallocation_v1_transfer_id, + .payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + .payload = &node_id_alloc_payload_buffer, + }; + + int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &transfer.payload_size); + + if (result == 0) { + // set the data ready in the buffer and chop if needed + ++_uavcan_pnp_nodeidallocation_v1_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. + result = canardTxPush(&_canard_instance, &transfer); + } + + return result; +} + +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, ®ister_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_register_setup = CANARD_NODE_ID_UNSET; + PX4_INFO("NodeID %i GPS publisher set PortID to %i", receive.remote_node_id, gps_port_id); + _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); + } + } + + if (strncmp((char *)msg.name.name.elements, "uavcan.pub.battery_status.id", + msg.name.name.count) == 0) { //Battery status publisher + _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_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); + } + } + + return result; +} + +int UavcanNode::handleBMSStatus(const CanardTransfer &receive) +{ + PX4_INFO("NodeID %i Battery Status msg", receive.remote_node_id); + //TODO deserialize + /* + battery_status_s battery_status{}; + battery_status.id = bms_status.battery_id; + battery_status.voltage_v = bms_status.voltage; + //battery_status.remaining = bms_status.remaining_capacity; + battery_status.timestamp = hrt_absolute_time(); + _battery_status_pub.publish(battery_status); + */ + + return 0; +} + +int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive) +{ + PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id); + + sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload; + + return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1; +} diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index f5132043ba..ec805920cc 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -108,6 +108,11 @@ private: void Run() override; void fill_node_info(); + int handlePnpNodeIDAllocationData(const CanardTransfer &receive); + int handleRegisterList(const CanardTransfer &receive); + int handleBMSStatus(const CanardTransfer &receive); + int handleUORBSensorGPS(const CanardTransfer &receive); + void *_uavcan_heap{nullptr}; CanardInterface *const _can_interface;