From 8b2d20df347143ab96268140c07fdc29f9d8a8fe Mon Sep 17 00:00:00 2001 From: Peter van der Perk Date: Mon, 14 Dec 2020 21:06:20 +0100 Subject: [PATCH] "MAVCAN" PNP & Register example implementation --- .gitmodules | 4 +- src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp | Bin 0 -> 65 bytes src/drivers/uavcan_v1/CMakeLists.txt | 4 +- src/drivers/uavcan_v1/CanardSocketCAN.cpp | 4 +- src/drivers/uavcan_v1/Uavcan.cpp | 228 +++++++++++++----- src/drivers/uavcan_v1/Uavcan.hpp | 44 +++- src/drivers/uavcan_v1/libcanard | 2 +- .../uavcan_v1/public_regulated_data_types | 2 +- 8 files changed, 211 insertions(+), 77 deletions(-) create mode 100644 src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp diff --git a/.gitmodules b/.gitmodules index 899a3ee641..ea0f86aabb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -32,10 +32,10 @@ branch = master [submodule "src/drivers/uavcan_v1/libcanard"] path = src/drivers/uavcan_v1/libcanard - url = https://github.com/PX4/libcanard.git + url = https://github.com/UAVCAN/libcanard.git [submodule "src/drivers/uavcan_v1/public_regulated_data_types"] path = src/drivers/uavcan_v1/public_regulated_data_types - url = https://github.com/PX4/public_regulated_data_types.git + url = https://github.com/UAVCAN/public_regulated_data_types.git [submodule "src/modules/micrortps_bridge/micro-CDR"] path = src/modules/micrortps_bridge/micro-CDR url = https://github.com/PX4/Micro-CDR.git diff --git a/src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp b/src/drivers/uavcan_v1/.Uavcan.cpp.kate-swp new file mode 100644 index 0000000000000000000000000000000000000000..a383caf56a50ca80a357ae13bb1d482a32acf76a GIT binary patch literal 65 zcmZQzU=Z?7EJ;-eE>A2_aLdd|RWQ;sU|?Vn5py=Y&Qe{jXyj-5WV>!a;r_Q*rGq^g P7#PigvLL{s00gc8DM}Bf literal 0 HcmV?d00001 diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt index 8f0e956381..9e1f3b74a5 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -39,8 +39,8 @@ px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR}) find_program(NNVG_PATH nnvg) if(NNVG_PATH) - execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/regulated) - execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp ${DSDL_DIR}/uavcan) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/reg) + execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan) else() message(FATAL_ERROR "UAVCAN Nunavut nnvg not found") endif() diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/uavcan_v1/CanardSocketCAN.cpp index b85e510c8b..1d47e1f3cf 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.cpp +++ b/src/drivers/uavcan_v1/CanardSocketCAN.cpp @@ -162,12 +162,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms) _send_tv->tv_usec = txf.timestamp_usec % 1000000ULL; _send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL; - return sendmsg(_fd, &_send_msg, 0); + return sendmsg(_fd, &_send_msg, 1000); } int16_t CanardSocketCAN::receive(CanardFrame *rxf) { - int32_t result = recvmsg(_fd, &_recv_msg, 0); + int32_t result = recvmsg(_fd, &_recv_msg, 1000); if (result < 0) { return result; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp index 988ca334c8..164331d704 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -36,8 +36,6 @@ #include #include -#define REGULATED_DRONE_SENSOR_BMSSTATUS_ID 32080 - using namespace time_literals; UavcanNode *UavcanNode::_instance; @@ -153,19 +151,41 @@ void UavcanNode::Run() // Subscribe to messages uavcan.node.Heartbeat. canardRxSubscribe(&_canard_instance, CanardTransferKindMessage, - uavcan::node::Heartbeat_1_0::PORT_ID, - uavcan::node::Heartbeat_1_0::SIZE, + uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, + uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, &_heartbeat_subscription); + + // Subscribe to messages uavcan.node.NodeIDAllocationData_1_0 for PNP V1 + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_, + uavcan_pnp_NodeIDAllocationData_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_pnp_v1_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); - if (_param_uavcan_v1_bat_md.get() == 1) { - canardRxSubscribe(&_canard_instance, + 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, - static_cast(_param_uavcan_v1_bat_id.get()), - regulated::drone::sensor::BMSStatus_1_0::SIZE, + test_port_id, + reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_, CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_drone_sensor_BMSStatus_subscription); - } + &_drone_srv_battery_subscription); _initialized = true; } @@ -193,27 +213,61 @@ void UavcanNode::Run() + if (hrt_elapsed_time(&_uavcan_pnp_nodeidallocation_last) >= 1_s && + _node_register_setup != CANARD_NODE_ID_UNSET && + _node_register_request_index == _node_register_last_received_index+1){ + + PX4_INFO("NodeID %i request register %i", _node_register_setup, _node_register_request_index); + + uavcan_register_List_Request_1_0 msg; + msg.index = _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_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_register_request_index; + } + } + // send uavcan::node::Heartbeat_1_0 @ 1 Hz if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { - uavcan::node::Heartbeat_1_0 heartbeat{}; + uavcan_node_Heartbeat_1_0 heartbeat{}; heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime - heartbeat.health = uavcan::node::Heartbeat_1_0::HEALTH_NOMINAL; - heartbeat.mode = uavcan::node::Heartbeat_1_0::MODE_OPERATIONAL; + heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; + heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; - heartbeat.serializeToBuffer(_uavcan_node_heartbeat_buffer); - const CanardTransfer transfer = { + CanardTransfer transfer = { .timestamp_usec = hrt_absolute_time(), .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, - .port_id = uavcan::node::Heartbeat_1_0::PORT_ID, + .port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _uavcan_node_heartbeat_transfer_id++, - .payload_size = uavcan::node::Heartbeat_1_0::SIZE, + .payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, .payload = &_uavcan_node_heartbeat_buffer, }; + uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size); + int32_t result = canardTxPush(&_canard_instance, &transfer); if (result < 0) { @@ -226,49 +280,11 @@ void UavcanNode::Run() _uavcan_node_heartbeat_last = transfer.timestamp_usec; } - // send regulated::drone::sensor::BMSStatus_1_0 @ 1 Hz - if (_param_uavcan_v1_bat_md.get() == 2) { - if (hrt_elapsed_time(&_regulated_drone_sensor_bmsstatus_last) >= 1_s) { - - battery_status_s battery_status; - - if (_battery_status_sub.update(&battery_status)) { - regulated::drone::sensor::BMSStatus_1_0 bmsstatus{}; - //bmsstatus.timestamp = battery_status.timestamp; - //bmsstatus.remaining_capacity = battery_status.remaining; - - bmsstatus.serializeToBuffer(_regulated_drone_sensor_bmsstatus_buffer); - - const CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time(), - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = static_cast(_param_uavcan_v1_bat_id.get()), - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = _regulated_drone_sensor_bmsstatus_transfer_id++, - .payload_size = regulated::drone::sensor::BMSStatus_1_0::SIZE, - .payload = &_regulated_drone_sensor_bmsstatus_buffer, - }; - - int32_t result = canardTxPush(&_canard_instance, &transfer); - - if (result < 0) { - // An error has occurred: either an argument is invalid or we've ran out of memory. - // It is possible to statically prove that an out-of-memory will never occur for a given application if the - // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. - PX4_ERR("Battery transmit error %d", result); - } - - _regulated_drone_sensor_bmsstatus_last = transfer.timestamp_usec; - } - } - } - // Transmitting // Look at the top of the TX queue. for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { // Check if the frame has timed out. - if (hrt_absolute_time() > txf->timestamp_usec) { + if (hrt_absolute_time() > txf->timestamp_usec) { //FIXME wrong I think // Send the frame. Redundant interfaces may be used here. const int tx_res = _can_interface->transmit(*txf); @@ -312,23 +328,111 @@ void UavcanNode::Run() } else if (result == 1) { // A transfer has been received, process it. - PX4_DEBUG("received Port ID: %d", receive.port_id); + PX4_INFO("received Port ID: %d", receive.port_id); - if (receive.port_id == static_cast(_param_uavcan_v1_bat_id.get())) { - auto bms_status = regulated::drone::sensor::BMSStatus_1_0::deserializeFromBuffer((const uint8_t *)receive.payload, - receive.payload_size); + 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.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, test_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] = test_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 (receive.port_id == test_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); + _battery_status_pub.publish(battery_status);*/ } // Deallocate the dynamic memory afterwards. _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); - } + } else { + PX4_INFO("RX canard %d\r\n", result); + } } perf_end(_cycle_perf); diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp index c4761b908c..cb18578de9 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -53,8 +53,23 @@ #include #include -#include -#include +#include +#include +#include + +//Quick and Dirty PNP imlementation only V1 for now as well +#include +#include +#include + +//Quick and Dirty UAVCAN register implementation +#include +#include + +#define PNP1_PORT_ID uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_ +#define PNP1_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_ +#define PNP2_PORT_ID uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_ +#define PNP2_PAYLOAD_SIZE uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_ #include "CanardInterface.hpp" @@ -106,7 +121,10 @@ private: pthread_mutex_t _node_mutex; CanardRxSubscription _heartbeat_subscription; - CanardRxSubscription _drone_sensor_BMSStatus_subscription; + CanardRxSubscription _pnp_v1_subscription; + CanardRxSubscription _drone_srv_battery_subscription; + CanardRxSubscription _register_access_subscription; + CanardRxSubscription _register_list_subscription; uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; @@ -117,12 +135,24 @@ private: perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; // uavcan::node::Heartbeat_1_0 - uint8_t _uavcan_node_heartbeat_buffer[uavcan::node::Heartbeat_1_0::SIZE]; + uint8_t _uavcan_node_heartbeat_buffer[uavcan_node_Heartbeat_1_0_EXTENT_BYTES_]; hrt_abstime _uavcan_node_heartbeat_last{0}; CanardTransferID _uavcan_node_heartbeat_transfer_id{0}; - - // regulated::drone::sensor::BMSStatus_1_0 - uint8_t _regulated_drone_sensor_bmsstatus_buffer[regulated::drone::sensor::BMSStatus_1_0::SIZE]; + + const uint16_t test_port_id = 1234; + + CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0}; + hrt_abstime _uavcan_pnp_nodeidallocation_last{0}; + + CanardTransferID _uavcan_register_list_request_transfer_id{0}; + CanardTransferID _uavcan_register_access_request_transfer_id{0}; + //Register interface NodeID TODO MVP right have to make a queue + uint8_t _node_register_setup = CANARD_NODE_ID_UNSET; + int32_t _node_register_request_index = 0; + int32_t _node_register_last_received_index = -1; + + // regulated::drone::sensor::BMSStatus_1_0 + uint8_t _regulated_drone_sensor_bmsstatus_buffer[reg_drone_srv_battery_Status_0_1_EXTENT_BYTES_]; hrt_abstime _regulated_drone_sensor_bmsstatus_last{0}; CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0}; diff --git a/src/drivers/uavcan_v1/libcanard b/src/drivers/uavcan_v1/libcanard index bec890304a..cde6703474 160000 --- a/src/drivers/uavcan_v1/libcanard +++ b/src/drivers/uavcan_v1/libcanard @@ -1 +1 @@ -Subproject commit bec890304a2888bc516416e4ebf252f761558b92 +Subproject commit cde670347425023480a1417fcd603b27c8eb06c1 diff --git a/src/drivers/uavcan_v1/public_regulated_data_types b/src/drivers/uavcan_v1/public_regulated_data_types index 4ef80c77c7..d874225ea4 160000 --- a/src/drivers/uavcan_v1/public_regulated_data_types +++ b/src/drivers/uavcan_v1/public_regulated_data_types @@ -1 +1 @@ -Subproject commit 4ef80c77c7ea9a5ac74ec1fa58055e7609b7758e +Subproject commit d874225ea4b5a79a4639bcfcb9096ccd943138a9