diff --git a/.gitmodules b/.gitmodules index f45a9fc86e..22ef494793 100644 --- a/.gitmodules +++ b/.gitmodules @@ -36,14 +36,14 @@ [submodule "Tools/jsbsim_bridge"] path = Tools/jsbsim_bridge url = https://github.com/PX4/px4-jsbsim-bridge.git -[submodule "src/drivers/uavcan_v1/libcanard"] - path = src/drivers/uavcan_v1/libcanard - 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/UAVCAN/public_regulated_data_types.git -[submodule "src/drivers/uavcan_v1/legacy_data_types"] - path = src/drivers/uavcan_v1/legacy_data_types +[submodule "src/drivers/cyphal/libcanard"] + path = src/drivers/cyphal/libcanard + url = https://github.com/opencyphal/libcanard.git +[submodule "src/drivers/cyphal/public_regulated_data_types"] + path = src/drivers/cyphal/public_regulated_data_types + url = https://github.com/opencyphal/public_regulated_data_types.git +[submodule "src/drivers/cyphal/legacy_data_types"] + path = src/drivers/cyphal/legacy_data_types url = https://github.com/PX4/public_regulated_data_types.git branch = legacy [submodule "src/lib/crypto/monocypher"] diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cc114cc635..187a2ff7f6 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -298,9 +298,9 @@ else tune_control play error fi else - if param greater -s UAVCAN_V1_ENABLE 0 + if param greater -s CYPHAL_ENABLE 0 then - uavcan_v1 start + cyphal start fi fi diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index ce8b51c7c4..6f77a514ab 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -13,8 +13,7 @@ exec find boards msg src platforms test \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ - -path src/drivers/uavcan_v1/libcanard -prune -o \ - -path src/drivers/uavcannode_gps_demo/libcanard -prune -o \ + -path src/drivers/cyphal/libcanard -prune -o \ -path src/lib/crypto/monocypher -prune -o \ -path src/lib/events/libevents -prune -o \ -path src/lib/parameters/uthash -prune -o \ diff --git a/boards/nxp/fmuk66-e/socketcan.px4board b/boards/nxp/fmuk66-e/socketcan.px4board index 3a4f62ea6a..7463620eb9 100644 --- a/boards/nxp/fmuk66-e/socketcan.px4board +++ b/boards/nxp/fmuk66-e/socketcan.px4board @@ -1,4 +1,4 @@ CONFIG_DRIVERS_UAVCAN=n -CONFIG_DRIVERS_UAVCAN_V1=y -CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y -CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/boards/nxp/fmuk66-v3/socketcan.px4board b/boards/nxp/fmuk66-v3/socketcan.px4board index abb97157ed..058d8efd67 100644 --- a/boards/nxp/fmuk66-v3/socketcan.px4board +++ b/boards/nxp/fmuk66-v3/socketcan.px4board @@ -1,9 +1,9 @@ CONFIG_DRIVERS_UAVCAN=n -CONFIG_DRIVERS_UAVCAN_V1=y -CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y -CONFIG_UAVCAN_V1_ESC_SUBSCRIBER=y -CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0=y -CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1=y -CONFIG_UAVCAN_V1_READINESS_PUBLISHER=y -CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y -CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_ESC_SUBSCRIBER=y +CONFIG_CYPHAL_GNSS_SUBSCRIBER_0=y +CONFIG_CYPHAL_GNSS_SUBSCRIBER_1=y +CONFIG_CYPHAL_READINESS_PUBLISHER=y +CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index 87d91630e7..50b749dafb 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -7,10 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_DRIVERS_PWM_OUT=y -CONFIG_DRIVERS_UAVCAN_V1=y -CONFIG_UAVCAN_V1_CLIENT=y -CONFIG_UAVCAN_V1_APP_DESCRIPTOR=y -CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER=y +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_CLIENT=y +CONFIG_CYPHAL_APP_DESCRIPTOR=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y CONFIG_MODULES_CONTROL_ALLOCATOR=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index fadf85f18a..f9775ee814 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -6,4 +6,4 @@ pwm_out mode_pwm1 start ifup can0 -uavcan_v1 start +cyphal start diff --git a/boards/px4/fmu-v5/uavcanv1.px4board b/boards/px4/fmu-v5/cyphal.px4board similarity index 52% rename from boards/px4/fmu-v5/uavcanv1.px4board rename to boards/px4/fmu-v5/cyphal.px4board index f9b60dd45f..b8e6b0b8c9 100644 --- a/boards/px4/fmu-v5/uavcanv1.px4board +++ b/boards/px4/fmu-v5/cyphal.px4board @@ -2,6 +2,6 @@ CONFIG_DRIVERS_HEATER=n CONFIG_DRIVERS_OSD=n CONFIG_DRIVERS_UAVCAN=n -CONFIG_DRIVERS_UAVCAN_V1=y -CONFIG_UAVCAN_V1_BMS_SUBSCRIBER=y -CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER=y +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_BMS_SUBSCRIBER=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER=y diff --git a/src/drivers/uavcan_v1/Actuators/EscClient.hpp b/src/drivers/cyphal/Actuators/EscClient.hpp similarity index 74% rename from src/drivers/uavcan_v1/Actuators/EscClient.hpp rename to src/drivers/cyphal/Actuators/EscClient.hpp index f22e0c983a..c0c7a32cc9 100644 --- a/src/drivers/uavcan_v1/Actuators/EscClient.hpp +++ b/src/drivers/cyphal/Actuators/EscClient.hpp @@ -34,13 +34,13 @@ /** * @file EscClient.hpp * - * Client-side implementation of DS-15 specification ESC service + * Client-side implementation of UDRAL specification ESC service * - * Publishes the following UAVCAN v1 messages: + * Publishes the following Cyphal messages: * reg.drone.service.actuator.common.sp.Value8.0.1 * reg.drone.service.common.Readiness.0.1 * - * Subscribes to the following UAVCAN v1 messages: + * Subscribes to the following Cyphal messages: * reg.drone.service.actuator.common.Feedback.0.1 * reg.drone.service.actuator.common.Status.0.1 * @@ -57,9 +57,9 @@ #include #include -// DS-15 Specification Messages -#include -#include +// UDRAL Specification Messages +#include +#include /// TODO: Allow derived class of Subscription at same time, to handle ESC Feedback/Status class UavcanEscController : public UavcanPublisher @@ -67,8 +67,8 @@ class UavcanEscController : public UavcanPublisher public: static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS; - UavcanEscController(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanPublisher(ins, pmgr, "ds_015", "esc") { }; + UavcanEscController(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanPublisher(handle, pmgr, "udral", "esc") { }; ~UavcanEscController() {}; @@ -80,45 +80,47 @@ public: if (new_arming.armed != _armed.armed) { _armed = new_arming; + size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; // Only publish if we have a valid publication ID set if (_port_id == 0) { return; } - reg_drone_service_common_Readiness_0_1 msg_arming {}; + reg_udral_service_common_Readiness_0_1 msg_arming {}; if (_armed.armed) { - msg_arming.value = reg_drone_service_common_Readiness_0_1_ENGAGED; + msg_arming.value = reg_udral_service_common_Readiness_0_1_ENGAGED; } else if (_armed.prearmed) { - msg_arming.value = reg_drone_service_common_Readiness_0_1_STANDBY; + msg_arming.value = reg_udral_service_common_Readiness_0_1_STANDBY; } else { - msg_arming.value = reg_drone_service_common_Readiness_0_1_SLEEP; + msg_arming.value = reg_udral_service_common_Readiness_0_1_SLEEP; } - uint8_t arming_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t arming_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; CanardPortID arming_pid = static_cast(static_cast(_port_id) + 1); - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = arming_pid, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = _arming_transfer_id, - .payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &arming_payload_buffer, }; - int result = reg_drone_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer, - &transfer.payload_size); + int result = reg_udral_service_common_Readiness_0_1_serialize_(&msg_arming, arming_payload_buffer, + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed ++_arming_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &arming_payload_buffer + ); } } } @@ -127,7 +129,8 @@ public: void update_outputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs) { if (_port_id > 0) { - reg_drone_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; + reg_udral_service_actuator_common_sp_Vector31_0_1 msg_sp {0}; + size_t payload_size = reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; for (uint8_t i = 0; i < MAX_ACTUATORS; i++) { if (i < num_outputs) { @@ -139,29 +142,30 @@ public: } } + PX4_INFO("Publish %d values %f, %f, %f, %f", num_outputs, (double)msg_sp.value[0], (double)msg_sp.value[1], (double)msg_sp.value[2], (double)msg_sp.value[3]); - uint8_t esc_sp_payload_buffer[reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t esc_sp_payload_buffer[reg_udral_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = _transfer_id, - .payload_size = reg_drone_service_actuator_common_sp_Vector31_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &esc_sp_payload_buffer, }; - int result = reg_drone_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, - &transfer.payload_size); + int result = reg_udral_service_actuator_common_sp_Vector31_0_1_serialize_(&msg_sp, esc_sp_payload_buffer, + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &esc_sp_payload_buffer); } } }; @@ -175,7 +179,7 @@ private: /** * ESC status message reception will be reported via this callback. */ - void esc_status_sub_cb(const CanardTransfer &msg); + void esc_status_sub_cb(const CanardRxTransfer &msg); uint8_t _rotor_count {0}; diff --git a/src/drivers/uavcan_v1/Actuators/EscServer.hpp b/src/drivers/cyphal/Actuators/EscServer.hpp similarity index 93% rename from src/drivers/uavcan_v1/Actuators/EscServer.hpp rename to src/drivers/cyphal/Actuators/EscServer.hpp index af2ba1338c..962e21be0d 100644 --- a/src/drivers/uavcan_v1/Actuators/EscServer.hpp +++ b/src/drivers/cyphal/Actuators/EscServer.hpp @@ -34,14 +34,14 @@ /** * @file EscServer.hpp * - * Server-side implementation of DS-15 specification ESC service + * Server-side implementation of UDRAL specification ESC service * (Used for CANNode control of an ESC via PWM output) * - * Subscribes to the following UAVCAN v1 messages: + * Subscribes to the following Cyphal messages: * reg.drone.service.actuator.common.sp.Value8.0.1 * reg.drone.service.common.Readiness.0.1 * - * Publishes to the following UAVCAN v1 messages: + * Publishes to the following Cyphal messages: * reg.drone.service.actuator.common.Feedback.0.1 * reg.drone.service.actuator.common.Status.0.1 * @@ -62,7 +62,7 @@ #include #include -// DS-15 Specification Messages +// UDRAL Specification Messages #include #include diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/cyphal/CMakeLists.txt similarity index 84% rename from src/drivers/uavcan_v1/CMakeLists.txt rename to src/drivers/cyphal/CMakeLists.txt index b07540af96..dc3c82b7bd 100644 --- a/src/drivers/uavcan_v1/CMakeLists.txt +++ b/src/drivers/cyphal/CMakeLists.txt @@ -41,7 +41,7 @@ px4_add_git_submodule(TARGET git_legacy_data_types PATH ${LEGACY_DSDL_DIR}) find_program(NNVG_PATH nnvg) if(NNVG_PATH) - message("Generating UAVCANv1 DSDL headers using Nunavut") + message("Generating Cyphal DSDL headers using Nunavut") 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 -I ${LEGACY_DSDL_DIR}/uavcan ${LEGACY_DSDL_DIR}/legacy) execute_process(COMMAND ${NNVG_PATH} --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language c ${DSDL_DIR}/uavcan) @@ -66,14 +66,14 @@ if(${PX4_PLATFORM} MATCHES "nuttx") endif() endif() -if(CONFIG_UAVCAN_V1_NODE_MANAGER) +if(CONFIG_CYPHAL_NODE_MANAGER) list(APPEND SRCS NodeManager.hpp NodeManager.cpp ) endif() -if(CONFIG_UAVCAN_V1_NODE_CLIENT) +if(CONFIG_CYPHAL_NODE_CLIENT) list(APPEND SRCS NodeClient.hpp NodeClient.cpp @@ -86,29 +86,31 @@ endif() # Temporary measure to get Kconfig option as defines into this application # Ideally we want nicely decouple and define this in kconfig.cmake # But then we need to overhaul the src modules naming convention -set(DRIVERS_UAVCAN_V1_OPTIONS) +set(DRIVERS_CYPHAL_OPTIONS) get_cmake_property(_variableNames VARIABLES) foreach (_variableName ${_variableNames}) - string(REGEX MATCH "^CONFIG_UAVCAN_V1_" UAVCAN_V1_OPTION ${_variableName}) + string(REGEX MATCH "^CONFIG_CYPHAL_" CYPHAL_OPTION ${_variableName}) - if(UAVCAN_V1_OPTION) + if(CYPHAL_OPTION) if(${${_variableName}} STREQUAL "y") - list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=1") + list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=1") else() - list(APPEND DRIVERS_UAVCAN_V1_OPTIONS "-D${_variableName}=${${_variableName}}") + list(APPEND DRIVERS_CYPHAL_OPTIONS "-D${_variableName}=${${_variableName}}") endif() endif() endforeach() +remove_definitions(-Werror) + px4_add_module( - MODULE drivers__uavcan_v1 - MAIN uavcan_v1 + MODULE drivers__cyphal + MAIN cyphal STACK_MAIN 4096 COMPILE_FLAGS #-DCANARD_ASSERT -DUINT32_C\(x\)=__UINT32_C\(x\) -O0 - ${DRIVERS_UAVCAN_V1_OPTIONS} + ${DRIVERS_CYPHAL_OPTIONS} INCLUDES ${LIBCANARD_DIR}/libcanard/ ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated @@ -119,8 +121,9 @@ px4_add_module( SubscriptionManager.hpp ParamManager.hpp ParamManager.cpp - Uavcan.cpp - Uavcan.hpp + Cyphal.cpp + Cyphal.hpp + CanardHandle.cpp Publishers/uORB/uorb_publisher.cpp Subscribers/uORB/uorb_subscriber.cpp ${SRCS} @@ -137,3 +140,6 @@ px4_add_module( version ${DPNDS} ) + +# libcanard 3.0 introduces this warning, for now no intention to fix it thus we ignore this warning +set_source_files_properties(${LIBCANARD_DIR}/libcanard/canard.c PROPERTIES COMPILE_FLAGS -Wno-cast-align) diff --git a/src/drivers/cyphal/CanardHandle.cpp b/src/drivers/cyphal/CanardHandle.cpp new file mode 100644 index 0000000000..d27d34c1d2 --- /dev/null +++ b/src/drivers/cyphal/CanardHandle.cpp @@ -0,0 +1,216 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "CanardHandle.hpp" + +#include +#include +#include + +#include + +#include "o1heap/o1heap.h" + +#include "Subscribers/BaseSubscriber.hpp" + +#if defined(__PX4_NUTTX) +# if defined(CONFIG_NET_CAN) +# include "CanardSocketCAN.hpp" +# elif defined(CONFIG_CAN) +# include "CanardNuttXCDev.hpp" +# endif // CONFIG_CAN +#endif // NuttX + + +O1HeapInstance *uavcan_allocator{nullptr}; + +static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); } +static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); } + + +CanardHandle::CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes) +{ + _uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize); + uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr); + + if (uavcan_allocator == nullptr) { + PX4_ERR("o1heapInit failed with size %u", HeapSize); + } + + _canard_instance = canardInit(&memAllocate, &memFree); + + _canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point. + + _queue = canardTxInit(capacity, mtu_bytes); + +#if defined(__PX4_NUTTX) +# if defined(CONFIG_NET_CAN) + _can_interface = new CanardSocketCAN(); +# elif defined(CONFIG_CAN) + _can_interface = new CanardNuttXCDev(); +# endif // CONFIG_CAN +#endif // NuttX + +} + +CanardHandle::~CanardHandle() +{ + _can_interface->close(); + delete _can_interface; + _can_interface = nullptr; + + delete static_cast(_uavcan_heap); + _uavcan_heap = nullptr; + +} + + +bool CanardHandle::init() +{ + if (_can_interface) { + if (_can_interface->init() == PX4_OK) { + return true; + } + } + + return false; +} + +void CanardHandle::receive() +{ + /* Process received messages */ + + uint8_t data[64] {}; + CanardRxFrame received_frame{}; + received_frame.frame.payload = &data; + + while (_can_interface->receive(&received_frame) > 0) { + CanardRxTransfer receive{}; + CanardRxSubscription *subscription = nullptr; + int32_t result = canardRxAccept(&_canard_instance, received_frame.timestamp_usec, &received_frame.frame, 0, &receive, + &subscription); + + 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. + // Reception of an invalid frame is NOT an error. + PX4_ERR("Receive error %" PRId32" \n", result); + + } else if (result == 1) { + // A transfer has been received, process it. + // PX4_INFO("received Port ID: %d", receive.port_id); + + if (subscription != nullptr) { + UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference; + sub_instance->callback(receive); + + } else { + PX4_ERR("No matching sub for %d", receive.metadata.port_id); + } + + // Deallocate the dynamic memory afterwards. + _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); + + } else { + //PX4_INFO("RX canard %d", result); + } + } + +} + +void CanardHandle::transmit() +{ + // Look at the top of the TX queue. + for (const CanardTxQueueItem *ti = NULL; (ti = canardTxPeek(&_queue)) != NULL;) { // Peek at the top of the queue. + if ((0U == ti->tx_deadline_usec) || (ti->tx_deadline_usec > hrt_absolute_time())) { // Check the deadline. + // Send the frame. Redundant interfaces may be used here. + const int tx_res = _can_interface->transmit(*ti); + + if (tx_res < 0) { + PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno)); + + } else if (tx_res == 0) { + // Timeout - just exit and try again later + break; + } + + } + + // After the frame is transmitted or if it has timed out while waiting, pop it from the queue and deallocate: + _canard_instance.memory_free(&_canard_instance, canardTxPop(&_queue, ti)); + } +} + +int32_t CanardHandle::TxPush(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *const metadata, + const size_t payload_size, + const void *const payload) +{ + return canardTxPush(&_queue, &_canard_instance, tx_deadline_usec, metadata, payload_size, payload); +} + +int8_t CanardHandle::RxSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t extent, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription *const out_subscription) +{ + return canardRxSubscribe(&_canard_instance, transfer_kind, port_id, extent, transfer_id_timeout_usec, out_subscription); +} + +int8_t CanardHandle::RxUnsubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id) +{ + return canardRxUnsubscribe(&_canard_instance, transfer_kind, port_id); +} + +CanardTreeNode *CanardHandle::getRxSubscriptions(CanardTransferKind kind) +{ + return _canard_instance.rx_subscriptions[kind]; +} + +int32_t CanardHandle::mtu() +{ + return _queue.mtu_bytes; +} + +CanardNodeID CanardHandle::node_id() +{ + return _canard_instance.node_id; +} + +void CanardHandle::set_node_id(CanardNodeID id) +{ + _canard_instance.node_id = id; +} diff --git a/src/drivers/cyphal/CanardHandle.hpp b/src/drivers/cyphal/CanardHandle.hpp new file mode 100644 index 0000000000..6e99def8e6 --- /dev/null +++ b/src/drivers/cyphal/CanardHandle.hpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2022 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include "CanardInterface.hpp" + +class CanardHandle +{ + /* + * This memory is allocated for the 01Heap allocator used by + * libcanard to store incoming/outcoming data + * Current size of 8192 bytes is arbitrary, should be optimized further + * when more nodes and messages are on the CAN bus + */ + static constexpr unsigned HeapSize = 8192; + +public: + CanardHandle(uint32_t node_id, const size_t capacity, const size_t mtu_bytes); + ~CanardHandle(); + + bool init(); + + void receive(); + void transmit(); + + int32_t TxPush(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *const metadata, + const size_t payload_size, + const void *const payload); + + int8_t RxSubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id, + const size_t extent, + const CanardMicrosecond transfer_id_timeout_usec, + CanardRxSubscription *const out_subscription); + int8_t RxUnsubscribe(const CanardTransferKind transfer_kind, + const CanardPortID port_id); + CanardTreeNode *getRxSubscriptions(CanardTransferKind kind); + + int32_t mtu(); + CanardNodeID node_id(); + void set_node_id(CanardNodeID id); + +private: + CanardInterface *_can_interface; + + CanardInstance _canard_instance; + + CanardTxQueue _queue; + + void *_uavcan_heap{nullptr}; + +}; diff --git a/src/drivers/uavcan_v1/CanardInterface.hpp b/src/drivers/cyphal/CanardInterface.hpp similarity index 87% rename from src/drivers/uavcan_v1/CanardInterface.hpp rename to src/drivers/cyphal/CanardInterface.hpp index 088600a18a..4e8aa4b8aa 100644 --- a/src/drivers/uavcan_v1/CanardInterface.hpp +++ b/src/drivers/cyphal/CanardInterface.hpp @@ -35,6 +35,14 @@ #include +/// One frame stored in the transmission queue along with its metadata. +struct CanardRxFrame { + CanardMicrosecond timestamp_usec; + + /// The actual CAN frame data. + CanardFrame frame; +}; + class CanardInterface { public: @@ -48,12 +56,12 @@ public: /// Send a CanardFrame /// This function is blocking /// The return value is number of bytes transferred, negative value on error. - virtual int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0) = 0; + virtual int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0) = 0; /// Receive a CanardFrame /// This function is blocking /// The return value is number of bytes received, negative value on error. - virtual int16_t receive(CanardFrame *rxf) = 0; + virtual int16_t receive(CanardRxFrame *rxf) = 0; private: diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp b/src/drivers/cyphal/CanardNuttXCDev.cpp similarity index 86% rename from src/drivers/uavcan_v1/CanardNuttXCDev.cpp rename to src/drivers/cyphal/CanardNuttXCDev.cpp index f9949c58ab..bbb4dbaec8 100644 --- a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp +++ b/src/drivers/cyphal/CanardNuttXCDev.cpp @@ -65,7 +65,7 @@ int CanardNuttXCDev::init() return 0; } -int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) +int16_t CanardNuttXCDev::transmit(const CanardTxQueueItem &txf, int timeout_ms) { if (_fd < 0) { return -1; @@ -93,13 +93,13 @@ int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) struct can_msg_s transmit_msg {}; - transmit_msg.cm_hdr.ch_id = txf.extended_can_id; + transmit_msg.cm_hdr.ch_id = txf.frame.extended_can_id; - transmit_msg.cm_hdr.ch_dlc = txf.payload_size; + transmit_msg.cm_hdr.ch_dlc = txf.frame.payload_size; transmit_msg.cm_hdr.ch_extid = 1; - memcpy(transmit_msg.cm_data, txf.payload, txf.payload_size); + memcpy(transmit_msg.cm_data, txf.frame.payload, txf.frame.payload_size); const size_t msg_len = CAN_MSGLEN(transmit_msg.cm_hdr.ch_dlc); @@ -112,7 +112,7 @@ int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) return 1; } -int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) +int16_t CanardNuttXCDev::receive(CanardRxFrame *received_frame) { if ((_fd < 0) || (received_frame == nullptr)) { return -1; @@ -140,9 +140,9 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) return -1; } else { - received_frame->extended_can_id = receive_msg.cm_hdr.ch_id; - received_frame->payload_size = receive_msg.cm_hdr.ch_dlc; - memcpy((void *)received_frame->payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc); + received_frame->frame.extended_can_id = receive_msg.cm_hdr.ch_id; + received_frame->frame.payload_size = receive_msg.cm_hdr.ch_dlc; + memcpy((void *)received_frame->frame.payload, receive_msg.cm_data, receive_msg.cm_hdr.ch_dlc); return nbytes; } } diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.hpp b/src/drivers/cyphal/CanardNuttXCDev.hpp similarity index 91% rename from src/drivers/uavcan_v1/CanardNuttXCDev.hpp rename to src/drivers/cyphal/CanardNuttXCDev.hpp index 6f0b79a4e5..e9d345afd0 100644 --- a/src/drivers/uavcan_v1/CanardNuttXCDev.hpp +++ b/src/drivers/cyphal/CanardNuttXCDev.hpp @@ -51,15 +51,15 @@ public: /// The return value is 0 on succes and -1 on error int init(); - /// Send a CanardFrame to the CanardSocketInstance socket + /// Send a CanardTxQueueItem to the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes transferred, negative value on error. - int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0); + int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0); - /// Receive a CanardFrame from the CanardSocketInstance socket + /// Receive a CanardRxFrame from the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes received, negative value on error. - int16_t receive(CanardFrame *rxf); + int16_t receive(CanardRxFrame *rxf); private: int _fd{-1}; diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/cyphal/CanardSocketCAN.cpp similarity index 85% rename from src/drivers/uavcan_v1/CanardSocketCAN.cpp rename to src/drivers/cyphal/CanardSocketCAN.cpp index 81e78735c4..ef1c3afc8b 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.cpp +++ b/src/drivers/cyphal/CanardSocketCAN.cpp @@ -152,22 +152,22 @@ int CanardSocketCAN::init() return 0; } -int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms) +int16_t CanardSocketCAN::transmit(const CanardTxQueueItem &txf, int timeout_ms) { /* Copy CanardFrame to can_frame/canfd_frame */ if (_can_fd) { - _send_frame.can_id = txf.extended_can_id | CAN_EFF_FLAG; - _send_frame.len = txf.payload_size; - memcpy(&_send_frame.data, txf.payload, txf.payload_size); + _send_frame.can_id = txf.frame.extended_can_id | CAN_EFF_FLAG; + _send_frame.len = txf.frame.payload_size; + memcpy(&_send_frame.data, txf.frame.payload, txf.frame.payload_size); } else { struct can_frame *frame = (struct can_frame *)&_send_frame; - frame->can_id = txf.extended_can_id | CAN_EFF_FLAG; - frame->can_dlc = txf.payload_size; - memcpy(&frame->data, txf.payload, txf.payload_size); + frame->can_id = txf.frame.extended_can_id | CAN_EFF_FLAG; + frame->can_dlc = txf.frame.payload_size; + memcpy(&frame->data, txf.frame.payload, txf.frame.payload_size); } - uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.timestamp_usec - hrt_absolute_time()) + + uint64_t deadline_systick = getMonotonicTimestampUSec() + (txf.tx_deadline_usec - hrt_absolute_time()) + CONFIG_USEC_PER_TICK; // Compensate for precision loss when converting hrt to systick /* Set CAN_RAW_TX_DEADLINE timestamp */ @@ -177,7 +177,7 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms) return sendmsg(_fd, &_send_msg, 0); } -int16_t CanardSocketCAN::receive(CanardFrame *rxf) +int16_t CanardSocketCAN::receive(CanardRxFrame *rxf) { int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT); @@ -189,15 +189,15 @@ int16_t CanardSocketCAN::receive(CanardFrame *rxf) if (_can_fd) { struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame; - rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK; - rxf->payload_size = recv_frame->len; - rxf->payload = &recv_frame->data; + rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->frame.payload_size = recv_frame->len; + rxf->frame.payload = &recv_frame->data; } else { struct can_frame *recv_frame = (struct can_frame *)&_recv_frame; - rxf->extended_can_id = recv_frame->can_id & CAN_EFF_MASK; - rxf->payload_size = recv_frame->can_dlc; - rxf->payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference + rxf->frame.extended_can_id = recv_frame->can_id & CAN_EFF_MASK; + rxf->frame.payload_size = recv_frame->can_dlc; + rxf->frame.payload = &recv_frame->data; //FIXME either copy or clearly state the pointer reference } /* Read SO_TIMESTAMP value */ diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.hpp b/src/drivers/cyphal/CanardSocketCAN.hpp similarity index 97% rename from src/drivers/uavcan_v1/CanardSocketCAN.hpp rename to src/drivers/cyphal/CanardSocketCAN.hpp index f27785e2ba..ba93b48dbe 100644 --- a/src/drivers/uavcan_v1/CanardSocketCAN.hpp +++ b/src/drivers/cyphal/CanardSocketCAN.hpp @@ -71,12 +71,12 @@ public: /// Send a CanardFrame to the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes transferred, negative value on error. - int16_t transmit(const CanardFrame &txframe, int timeout_ms = 0); + int16_t transmit(const CanardTxQueueItem &txframe, int timeout_ms = 0); /// Receive a CanardFrame from the CanardSocketInstance socket /// This function is blocking /// The return value is number of bytes received, negative value on error. - int16_t receive(CanardFrame *rxf); + int16_t receive(CanardRxFrame *rxf); // TODO implement ioctl for CAN filter //int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters); diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/cyphal/Cyphal.cpp similarity index 53% rename from src/drivers/uavcan_v1/Uavcan.cpp rename to src/drivers/cyphal/Cyphal.cpp index ce2a27480b..2dd977f06f 100644 --- a/src/drivers/uavcan_v1/Uavcan.cpp +++ b/src/drivers/cyphal/Cyphal.cpp @@ -31,13 +31,13 @@ * ****************************************************************************/ -#include "Uavcan.hpp" +#include "Cyphal.hpp" #include #include -#ifdef CONFIG_UAVCAN_V1_APP_DESCRIPTOR +#ifdef CONFIG_CYPHAL_APP_DESCRIPTOR #include "boot_app_shared.h" /* * This is the AppImageDescriptor used @@ -60,54 +60,22 @@ boot_app_shared_section app_descriptor_t AppDescriptor = { using namespace time_literals; -UavcanNode *UavcanNode::_instance; +CyphalNode *CyphalNode::_instance; -O1HeapInstance *uavcan_allocator{nullptr}; - -static void *memAllocate(CanardInstance *const ins, const size_t amount) { return o1heapAllocate(uavcan_allocator, amount); } -static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree(uavcan_allocator, pointer); } - -#if defined(__PX4_NUTTX) -# if defined(CONFIG_NET_CAN) -# include "CanardSocketCAN.hpp" -# elif defined(CONFIG_CAN) -# include "CanardNuttXCDev.hpp" -# endif // CONFIG_CAN -#endif // NuttX - -UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : +CyphalNode::CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes) : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), - _can_interface(interface) + _canard_handle(node_id, capacity, mtu_bytes) { pthread_mutex_init(&_node_mutex, nullptr); - _uavcan_heap = memalign(O1HEAP_ALIGNMENT, HeapSize); - uavcan_allocator = o1heapInit(_uavcan_heap, HeapSize, nullptr, nullptr); - if (uavcan_allocator == nullptr) { - PX4_ERR("o1heapInit failed with size %u", HeapSize); - } - - _canard_instance = canardInit(&memAllocate, &memFree); - - _canard_instance.node_id = node_id; // Defaults to anonymous; can be set up later at any point. - - bool can_fd = false; - - if (can_fd) { - _canard_instance.mtu_bytes = CANARD_MTU_CAN_FD; - - } else { - _canard_instance.mtu_bytes = CANARD_MTU_CAN_CLASSIC; - } - -#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER +#ifdef CONFIG_CYPHAL_NODE_MANAGER _node_manager.subscribe(); #endif -#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT - _node_client = new NodeClient(_canard_instance, _param_manager); +#ifdef CONFIG_CYPHAL_NODE_CLIENT + _node_client = new NodeClient(_canard_handle, _param_manager); _node_client->subscribe(); #endif @@ -117,7 +85,7 @@ UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) : _sub_manager.subscribe(); } -UavcanNode::~UavcanNode() +CyphalNode::~CyphalNode() { if (_instance) { /* tell the task we want it to go away */ @@ -138,32 +106,25 @@ UavcanNode::~UavcanNode() } while (_instance); } - delete _can_interface; - _can_interface = nullptr; - perf_free(_cycle_perf); perf_free(_interval_perf); - - delete static_cast(_uavcan_heap); - _uavcan_heap = nullptr; } -int UavcanNode::start(uint32_t node_id, uint32_t bitrate) +int CyphalNode::start(uint32_t node_id, uint32_t bitrate) { if (_instance != nullptr) { PX4_WARN("Already started"); return -1; } -#if defined(__PX4_NUTTX) -# if defined(CONFIG_NET_CAN) - CanardInterface *interface = new CanardSocketCAN(); -# elif defined(CONFIG_CAN) - CanardInterface *interface = new CanardNuttXCDev(); -# endif // CONFIG_CAN -#endif // NuttX + bool can_fd = false; - _instance = new UavcanNode(interface, node_id); + if (can_fd) { + _instance = new CyphalNode(node_id, 8, CANARD_MTU_CAN_FD); + + } else { + _instance = new CyphalNode(node_id, 64, CANARD_MTU_CAN_CLASSIC); + } if (_instance == nullptr) { PX4_ERR("Out of memory"); @@ -178,17 +139,16 @@ int UavcanNode::start(uint32_t node_id, uint32_t bitrate) return PX4_OK; } -void UavcanNode::init() +void CyphalNode::init() { // interface init - if (_can_interface) { - if (_can_interface->init() == PX4_OK) { - _initialized = true; - } + if (_canard_handle.init()) { + _initialized = true; } + } -void UavcanNode::Run() +void CyphalNode::Run() { pthread_mutex_lock(&_node_mutex); @@ -221,22 +181,22 @@ void UavcanNode::Run() perf_begin(_cycle_perf); perf_count(_interval_perf); - if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) { + if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) { // send uavcan::node::Heartbeat_1_0 @ 1 Hz sendHeartbeat(); // Check all publishers _pub_manager.update(); -#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER +#ifdef CONFIG_CYPHAL_NODE_MANAGER _node_manager.update(); #endif } -#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT +#ifdef CONFIG_CYPHAL_NODE_CLIENT else if (_node_client != nullptr) { - if (_canard_instance.node_id == CANARD_NODE_ID_UNSET) { + if (_canard_handle.node_id() == CANARD_NODE_ID_UNSET) { _node_client->update(); } else { @@ -246,56 +206,19 @@ void UavcanNode::Run() #endif - transmit(); + _canard_handle.transmit(); - /* Process received messages */ + _canard_handle.receive(); - uint8_t data[64] {}; - CanardFrame received_frame{}; - received_frame.payload = &data; - - while (!_task_should_exit.load() && _can_interface->receive(&received_frame) > 0) { - CanardTransfer receive{}; - CanardRxSubscription *subscription = nullptr; - 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. - // 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. - // Reception of an invalid frame is NOT an error. - PX4_ERR("Receive error %" PRId32" \n", result); - - } else if (result == 1) { - // A transfer has been received, process it. - // PX4_INFO("received Port ID: %d", receive.port_id); - - if (subscription != nullptr) { - UavcanBaseSubscriber *sub_instance = (UavcanBaseSubscriber *)subscription->user_reference; - sub_instance->callback(receive); - - } else { - PX4_ERR("No matching sub for %d", receive.port_id); - } - - // Deallocate the dynamic memory afterwards. - _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); - - } else { - //PX4_INFO("RX canard %d", result); - } - } - - // Pop canardTx queue to send out responses to requets - transmit(); + // Pop canardTx queue to send out responses to requests + _canard_handle.transmit(); perf_end(_cycle_perf); if (_instance && _task_should_exit.load()) { ScheduleClear(); - if (_initialized && _can_interface != nullptr) { - _can_interface->close(); + if (_initialized) { _initialized = false; } @@ -305,101 +228,62 @@ void UavcanNode::Run() pthread_mutex_unlock(&_node_mutex); } -void UavcanNode::transmit() +template +static void traverseTree(const CanardTreeNode *const root, const F &op) // NOLINT this recursion is tightly bounded { - // Look at the top of the TX queue. - for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { - // Attempt transmission only if the frame is not yet timed out while waiting in the TX queue. - // Otherwise just drop it and move on to the next one. - const hrt_abstime now = hrt_absolute_time(); - - if (txf->timestamp_usec == 0 || txf->timestamp_usec > now) { - // Send the frame. Redundant interfaces may be used here. - const int tx_res = _can_interface->transmit(*txf); - - if (tx_res < 0) { - // Failure - drop the frame and report - canardTxPop(&_canard_instance); - - // Deallocate the dynamic memory afterwards. - _canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf); - PX4_ERR("Transmit error %d, frame dropped, errno '%s'", tx_res, strerror(errno)); - - } else if (tx_res > 0) { - // Success - just drop the frame - canardTxPop(&_canard_instance); - - // Deallocate the dynamic memory afterwards. - _canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf); - - } else { - // Timeout - just exit and try again later - break; - } - - } else if (txf->timestamp_usec <= now) { - // Transmission timed out -- remove from queue and deallocate its memory - canardTxPop(&_canard_instance); - - _canard_instance.memory_free(&_canard_instance, (CanardFrame *)txf); - } + if (root != nullptr) { + traverseTree(root->lr[0], op); + op(static_cast(static_cast(root))); + traverseTree(root->lr[1], op); } } -void UavcanNode::print_info() +void CyphalNode::print_info() { pthread_mutex_lock(&_node_mutex); perf_print_counter(_cycle_perf); perf_print_counter(_interval_perf); - O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator); + /*O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator); PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64, heap_diagnostics.allocated, heap_diagnostics.capacity, heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size, - heap_diagnostics.oom_count); + heap_diagnostics.oom_count);*/ _pub_manager.printInfo(); - CanardRxSubscription *rxs = _canard_instance.rx_subscriptions[CanardTransferKindMessage]; - - while (rxs != nullptr) { - if (rxs->user_reference == nullptr) { - PX4_INFO("Message port id %d", rxs->port_id); + traverseTree(_canard_handle.getRxSubscriptions(CanardTransferKindMessage), + [&](const CanardRxSubscription * const sub) { + if (sub->user_reference == nullptr) { + PX4_INFO("Message port id %d", sub->port_id); } else { - ((UavcanBaseSubscriber *)rxs->user_reference)->printInfo(); + ((UavcanBaseSubscriber *)sub->user_reference)->printInfo(); } + }); - rxs = rxs->next; - } - rxs = _canard_instance.rx_subscriptions[CanardTransferKindRequest]; - - while (rxs != nullptr) { - if (rxs->user_reference == nullptr) { - PX4_INFO("Service response port id %d", rxs->port_id); + traverseTree(_canard_handle.getRxSubscriptions(CanardTransferKindRequest), + [&](const CanardRxSubscription * const sub) { + if (sub->user_reference == nullptr) { + PX4_INFO("Service response port id %d", sub->port_id); } else { - ((UavcanBaseSubscriber *)rxs->user_reference)->printInfo(); + ((UavcanBaseSubscriber *)sub->user_reference)->printInfo(); } + }); - rxs = rxs->next; - } - - rxs = _canard_instance.rx_subscriptions[CanardTransferKindResponse]; - - while (rxs != nullptr) { - if (rxs->user_reference == nullptr) { - PX4_INFO("Service request port id %d", rxs->port_id); + traverseTree(_canard_handle.getRxSubscriptions(CanardTransferKindResponse), + [&](const CanardRxSubscription * const sub) { + if (sub->user_reference == nullptr) { + PX4_INFO("Service request port id %d", sub->port_id); } else { - ((UavcanBaseSubscriber *)rxs->user_reference)->printInfo(); + ((UavcanBaseSubscriber *)sub->user_reference)->printInfo(); } - - rxs = rxs->next; - } + }); _mixing_output.printInfo(); @@ -412,7 +296,7 @@ static void print_usage() "\tuavcannode {start|status|stop}"); } -extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) +extern "C" __EXPORT int cyphal_main(int argc, char *argv[]) { if (argc < 2) { print_usage(); @@ -420,18 +304,18 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) } if (!strcmp(argv[1], "start")) { - if (UavcanNode::instance()) { + if (CyphalNode::instance()) { PX4_ERR("already started"); return 1; } // CAN bitrate int32_t bitrate = 0; - param_get(param_find("UAVCAN_V1_BAUD"), &bitrate); + param_get(param_find("CYPHAL_BAUD"), &bitrate); // Node ID int32_t node_id = 0; - param_get(param_find("UAVCAN_V1_ID"), &node_id); + param_get(param_find("CYPHAL_ID"), &node_id); if (node_id == -1) { node_id = CANARD_NODE_ID_UNSET; @@ -439,11 +323,11 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) // Start PX4_INFO("Node ID %" PRIu32 ", bitrate %" PRIu32, node_id, bitrate); - return UavcanNode::start(node_id, bitrate); + return CyphalNode::start(node_id, bitrate); } /* commands below require the app to be started */ - UavcanNode *const inst = UavcanNode::instance(); + CyphalNode *const inst = CyphalNode::instance(); if (!inst) { PX4_ERR("application not running"); @@ -464,7 +348,7 @@ extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) return 1; } -void UavcanNode::sendHeartbeat() +void CyphalNode::sendHeartbeat() { if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { @@ -473,21 +357,23 @@ void UavcanNode::sendHeartbeat() heartbeat.health.value = uavcan_node_Health_1_0_NOMINAL; heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL; const hrt_abstime now = hrt_absolute_time(); + size_t payload_size = uavcan_node_Heartbeat_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; - CanardTransfer transfer = { - .timestamp_usec = now + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .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_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &_uavcan_node_heartbeat_buffer, + .transfer_id = _uavcan_node_heartbeat_transfer_id++ }; - uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &transfer.payload_size); + uavcan_node_Heartbeat_1_0_serialize_(&heartbeat, _uavcan_node_heartbeat_buffer, &payload_size); - int32_t result = canardTxPush(&_canard_instance, &transfer); + int32_t result = _canard_handle.TxPush(now + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &_uavcan_node_heartbeat_buffer + ); if (result < 0) { // An error has occurred: either an argument is invalid or we've ran out of memory. diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/cyphal/Cyphal.hpp similarity index 79% rename from src/drivers/uavcan_v1/Uavcan.hpp rename to src/drivers/cyphal/Cyphal.hpp index 500a5e9845..203f11b601 100644 --- a/src/drivers/uavcan_v1/Uavcan.hpp +++ b/src/drivers/cyphal/Cyphal.hpp @@ -51,21 +51,18 @@ #include #include -#include "o1heap/o1heap.h" - #include -#include #include "CanardInterface.hpp" #include "Publishers/Publisher.hpp" #include "Publishers/uORB/uorb_publisher.hpp" -#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER +#ifdef CONFIG_CYPHAL_NODE_MANAGER #include "NodeManager.hpp" #endif -#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT +#ifdef CONFIG_CYPHAL_NODE_CLIENT #include "NodeClient.hpp" #endif @@ -76,8 +73,8 @@ /** * UAVCAN mixing class. - * It is separate from UavcanNode to have 2 WorkItems and therefore allowing independent scheduling - * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas UavcanNode runs at + * It is separate from CyphalNode to have 2 WorkItems and therefore allowing independent scheduling + * (I.e. UavcanMixingInterface runs upon actuator_control updates, whereas CyphalNode runs at * a fixed rate or upon bus updates). * Both work items are expected to run on the same work queue. */ @@ -106,22 +103,15 @@ public: protected: void Run() override; private: - friend class UavcanNode; + friend class CyphalNode; pthread_mutex_t &_node_mutex; UavcanEscController &_esc_controller; // UavcanServoController &_servo_controller; MixingOutput _mixing_output{"UCAN1_ESC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; }; -class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem +class CyphalNode : public ModuleParams, public px4::ScheduledWorkItem { - /* - * This memory is allocated for the 01Heap allocator used by - * libcanard to store incoming/outcoming data - * Current size of 8192 bytes is arbitrary, should be optimized further - * when more nodes and messages are on the CAN bus - */ - static constexpr unsigned HeapSize = 8192; /* * Base interval, has to be complemented with events from the CAN driver @@ -131,14 +121,14 @@ class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem public: - UavcanNode(CanardInterface *interface, uint32_t node_id); - ~UavcanNode() override; + CyphalNode(uint32_t node_id, size_t capacity, size_t mtu_bytes); + ~CyphalNode() override; static int start(uint32_t node_id, uint32_t bitrate); void print_info(); - static UavcanNode *instance() { return _instance; } + static CyphalNode *instance() { return _instance; } /* The bit rate that can be passed back to the bootloader */ int32_t active_bitrate{0}; @@ -148,22 +138,16 @@ private: void Run() override; void fill_node_info(); - void transmit(); - // Sends a heartbeat at 1s intervals void sendHeartbeat(); - void *_uavcan_heap{nullptr}; - - CanardInterface *_can_interface; - - CanardInstance _canard_instance; - px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver bool _initialized{false}; ///< number of actuators currently available - static UavcanNode *_instance; + static CyphalNode *_instance; + + CanardHandle _canard_handle; pthread_mutex_t _node_mutex; @@ -178,26 +162,26 @@ private: CanardTransferID _uavcan_node_heartbeat_transfer_id{0}; DEFINE_PARAMETERS( - (ParamInt) _param_uavcan_v1_enable, - (ParamInt) _param_uavcan_v1_id, - (ParamInt) _param_uavcan_v1_baud + (ParamInt) _param_uavcan_v1_enable, + (ParamInt) _param_uavcan_v1_id, + (ParamInt) _param_uavcan_v1_baud ) UavcanParamManager _param_manager; -#ifdef CONFIG_UAVCAN_V1_NODE_MANAGER - NodeManager _node_manager {_canard_instance, _param_manager}; +#ifdef CONFIG_CYPHAL_NODE_MANAGER + NodeManager _node_manager {_canard_handle, _param_manager}; #endif -#ifdef CONFIG_UAVCAN_V1_NODE_CLIENT +#ifdef CONFIG_CYPHAL_NODE_CLIENT NodeClient *_node_client {nullptr}; #endif - PublicationManager _pub_manager {_canard_instance, _param_manager}; - SubscriptionManager _sub_manager {_canard_instance, _param_manager}; + PublicationManager _pub_manager {_canard_handle, _param_manager}; + SubscriptionManager _sub_manager {_canard_handle, _param_manager}; /// TODO: Integrate with PublicationManager - UavcanEscController _esc_controller {_canard_instance, _param_manager}; + UavcanEscController _esc_controller {_canard_handle, _param_manager}; UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller}; diff --git a/src/drivers/uavcan_v1/Kconfig b/src/drivers/cyphal/Kconfig similarity index 61% rename from src/drivers/uavcan_v1/Kconfig rename to src/drivers/cyphal/Kconfig index eca9ae0536..050cd891f8 100644 --- a/src/drivers/uavcan_v1/Kconfig +++ b/src/drivers/cyphal/Kconfig @@ -2,65 +2,65 @@ # For a description of the syntax of this configuration file, # see the file kconfig-language.txt in the NuttX tools repository. # -menuconfig DRIVERS_UAVCAN_V1 - bool "UAVCANv1" +menuconfig DRIVERS_CYPHAL + bool "Cyphal" default n ---help--- - Enable support for UAVCANv1 + Enable support for Cyphal -if DRIVERS_UAVCAN_V1 +if DRIVERS_CYPHAL choice - prompt "UAVCANv1 Mode" + prompt "Cyphal Mode" - config UAVCAN_V1_FMU + config CYPHAL_FMU bool "Server (FMU)" - config UAVCAN_V1_CLIENT + config CYPHAL_CLIENT bool "Client (Peripheral)" endchoice - config UAVCAN_V1_NODE_MANAGER + config CYPHAL_NODE_MANAGER bool "Node manager" default y - depends on UAVCAN_V1_FMU + depends on CYPHAL_FMU help - Implement UAVCAN v1 PNP server functionality and manages discovered nodes + Implement Cyphal PNP server functionality and manages discovered nodes - config UAVCAN_V1_NODE_CLIENT + config CYPHAL_NODE_CLIENT bool "Node client" default y - depends on UAVCAN_V1_CLIENT + depends on CYPHAL_CLIENT help - Implement UAVCAN v1 PNP client functionality + Implement Cyphal PNP client functionality - config UAVCAN_V1_APP_DESCRIPTOR + config CYPHAL_APP_DESCRIPTOR bool "UAVCAN v0 bootloader app descriptor" default n - depends on UAVCAN_V1_CLIENT && DRIVERS_BOOTLOADERS + depends on CYPHAL_CLIENT && DRIVERS_BOOTLOADERS help When the board uses the UAVCANv0 bootloader functionality you need a AppImageDescriptor defined menu "Publisher support" - config UAVCAN_V1_GNSS_PUBLISHER + config CYPHAL_GNSS_PUBLISHER bool "GNSS Publisher" default n - config UAVCAN_V1_ESC_CONTROLLER + config CYPHAL_ESC_CONTROLLER bool "ESC Controller" default n - config UAVCAN_V1_READINESS_PUBLISHER + config CYPHAL_READINESS_PUBLISHER bool "Readiness Publisher" default n - config UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER + config CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER bool "uORB actuator_outputs publisher" default n - config UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER + config CYPHAL_UORB_SENSOR_GPS_PUBLISHER bool "uORB sensor_gps publisher" default n @@ -68,36 +68,36 @@ if DRIVERS_UAVCAN_V1 menu "Subscriber support" - config UAVCAN_V1_ESC_SUBSCRIBER + config CYPHAL_ESC_SUBSCRIBER bool "ESC Subscriber" default n - config UAVCAN_V1_GNSS_SUBSCRIBER_0 + config CYPHAL_GNSS_SUBSCRIBER_0 bool "GNSS Subscriber 0" default n - config UAVCAN_V1_GNSS_SUBSCRIBER_1 + config CYPHAL_GNSS_SUBSCRIBER_1 bool "GNSS Subscriber 1" default n - config UAVCAN_V1_BMS_SUBSCRIBER + config CYPHAL_BMS_SUBSCRIBER bool "BMS Subscriber" default n - config UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER + config CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER bool "uORB sensor_gps Subscriber" default n endmenu menu "Advertised Services" - config UAVCAN_V1_GETINFO_RESPONDER + config CYPHAL_GETINFO_RESPONDER bool "GetInfo1.0 responder" default y help Responds to uavcan.node.GetInfo.1.0 request sending over node information See https://github.com/UAVCAN/public_regulated_data_types/blob/master/uavcan/node/430.GetInfo.1.0.uavcan for full response - config UAVCAN_V1_EXECUTECOMMAND_RESPONDER + config CYPHAL_EXECUTECOMMAND_RESPONDER bool "ExecuteCommand1.0 responder" default n help @@ -107,4 +107,4 @@ if DRIVERS_UAVCAN_V1 menu "Service invokers" endmenu -endif #DRIVERS_UAVCAN_V1 +endif #DRIVERS_CYPHAL diff --git a/src/drivers/uavcan_v1/NodeClient.cpp b/src/drivers/cyphal/NodeClient.cpp similarity index 83% rename from src/drivers/uavcan_v1/NodeClient.cpp rename to src/drivers/cyphal/NodeClient.cpp index ca42646486..0f2ae41076 100644 --- a/src/drivers/uavcan_v1/NodeClient.cpp +++ b/src/drivers/cyphal/NodeClient.cpp @@ -44,15 +44,15 @@ #include #include "NodeClient.hpp" -void NodeClient::callback(const CanardTransfer &receive) +void NodeClient::callback(const CanardRxTransfer &receive) { - if (receive.remote_node_id != CANARD_NODE_ID_UNSET && _canard_instance.node_id == CANARD_NODE_ID_UNSET) { + if (receive.metadata.remote_node_id != CANARD_NODE_ID_UNSET && _canard_handle.node_id() == CANARD_NODE_ID_UNSET) { int32_t allocated = CANARD_NODE_ID_UNSET; px4_guid_t px4_guid; board_get_px4_guid(px4_guid); - if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) { + if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) { uavcan_pnp_NodeIDAllocationData_2_0 msg; size_t msg_size_in_bytes = receive.payload_size; @@ -87,9 +87,9 @@ void NodeClient::callback(const CanardTransfer &receive) return; } - _canard_instance.node_id = allocated; + _canard_handle.set_node_id(allocated); - PX4_INFO("Allocated Node ID %d", _canard_instance.node_id); + PX4_INFO("Allocated Node ID %d", _canard_handle.node_id()); } } @@ -103,38 +103,39 @@ void NodeClient::update() int32_t result; // Allocation already done, nothing to do - if (_canard_instance.node_id != CANARD_NODE_ID_UNSET) { + if (_canard_handle.node_id() != CANARD_NODE_ID_UNSET) { return; } - if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) { + if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) { // NodeIDAllocationData message uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg; uint8_t node_id_alloc_payload_buffer[PNP2_PAYLOAD_SIZE]; + size_t payload_size = PNP2_PAYLOAD_SIZE; px4_guid_t px4_guid; board_get_px4_guid(px4_guid); memcpy(node_id_alloc_msg.unique_id, px4_guid, sizeof(node_id_alloc_msg.unique_id)); //node_id_alloc_msg.node_id.value = preffered_node_id; //FIXME preffered ID PX4 Param - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, // Zero if transmission deadline is not limited. + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = PNP2_PORT_ID, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = _node_id_alloc_transfer_id, - .payload_size = PNP2_PAYLOAD_SIZE, - .payload = &node_id_alloc_payload_buffer, }; result = uavcan_pnp_NodeIDAllocationData_2_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer, - &transfer.payload_size); + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed ++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - canardTxPush(&_canard_instance, &transfer); + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &node_id_alloc_payload_buffer); } } else { @@ -142,29 +143,30 @@ void NodeClient::update() uavcan_pnp_NodeIDAllocationData_1_0 node_id_alloc_msg; uavcan_pnp_NodeIDAllocationData_1_0_initialize_(&node_id_alloc_msg); uint8_t node_id_alloc_payload_buffer[PNP1_PAYLOAD_SIZE]; + size_t payload_size = PNP1_PAYLOAD_SIZE; px4_guid_t px4_guid; board_get_px4_guid(px4_guid); node_id_alloc_msg.unique_id_hash = (crc64(px4_guid, PNP_UNIQUE_ID_SIZE) & 0xFFFFFFFFFFFF); - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, // Zero if transmission deadline is not limited. + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = PNP1_PORT_ID, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, // Messages cannot be unicast, so use UNSET. .transfer_id = _node_id_alloc_transfer_id, - .payload_size = PNP1_PAYLOAD_SIZE, - .payload = &node_id_alloc_payload_buffer, }; result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&node_id_alloc_msg, (uint8_t *)&node_id_alloc_payload_buffer, - &transfer.payload_size); + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed ++_node_id_alloc_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - canardTxPush(&_canard_instance, &transfer); + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &node_id_alloc_payload_buffer); } } diff --git a/src/drivers/uavcan_v1/NodeClient.hpp b/src/drivers/cyphal/NodeClient.hpp similarity index 83% rename from src/drivers/uavcan_v1/NodeClient.hpp rename to src/drivers/cyphal/NodeClient.hpp index a0d82cef95..f8a5a6ae19 100644 --- a/src/drivers/uavcan_v1/NodeClient.hpp +++ b/src/drivers/cyphal/NodeClient.hpp @@ -62,30 +62,30 @@ class NodeClient : public UavcanBaseSubscriber { public: - NodeClient(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0), - _canard_instance(ins) { }; + NodeClient(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData", + 0), + _canard_handle(handle) { }; void subscribe() override { - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID - (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); } bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg); bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg); - void callback(const CanardTransfer &receive); // NodeIDAllocation callback + void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback void update(); private: - CanardInstance &_canard_instance; + CanardHandle &_canard_handle; CanardTransferID _node_id_alloc_transfer_id{0}; hrt_abstime _nodealloc_request_last{0}; diff --git a/src/drivers/uavcan_v1/NodeManager.cpp b/src/drivers/cyphal/NodeManager.cpp similarity index 88% rename from src/drivers/uavcan_v1/NodeManager.cpp rename to src/drivers/cyphal/NodeManager.cpp index ab584cfce3..87596df3af 100644 --- a/src/drivers/uavcan_v1/NodeManager.cpp +++ b/src/drivers/cyphal/NodeManager.cpp @@ -43,9 +43,9 @@ #include "NodeManager.hpp" -void NodeManager::callback(const CanardTransfer &receive) +void NodeManager::callback(const CanardRxTransfer &receive) { - if (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD) { + if (_canard_handle.mtu() == CANARD_MTU_CAN_FD) { uavcan_pnp_NodeIDAllocationData_2_0 node_id_alloc_msg {}; size_t msg_size_in_bytes = receive.payload_size; uavcan_pnp_NodeIDAllocationData_2_0_deserialize_(&node_id_alloc_msg, (const uint8_t *)receive.payload, @@ -73,7 +73,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) /* Search for an available NodeID to assign */ for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { - if (i == _canard_instance.node_id) { + if (i == _canard_handle.node_id()) { continue; // Don't give our NodeID to a node } else if (nodeid_registry[i].node_id == 0) { // Unused @@ -96,24 +96,25 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg) 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_]; + size_t payload_size = uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .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); + int result = uavcan_pnp_NodeIDAllocationData_1_0_serialize_(&msg, node_id_alloc_payload_buffer, &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); + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &node_id_alloc_payload_buffer); } _register_request_last = hrt_absolute_time(); @@ -132,7 +133,7 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg) } -void NodeManager::HandleListResponse(const CanardTransfer &receive) +void NodeManager::HandleListResponse(const CanardRxTransfer &receive) { uavcan_register_List_Response_1_0 msg; @@ -142,21 +143,21 @@ void NodeManager::HandleListResponse(const CanardTransfer &receive) if (msg.name.name.count == 0) { // Index doesn't exist, we've parsed through all registers for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { - if (nodeid_registry[i].node_id == receive.remote_node_id) { + if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) { nodeid_registry[i].register_setup = true; // Don't update anymore } } } else { for (uint32_t i = 0; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) { - if (nodeid_registry[i].node_id == receive.remote_node_id) { + if (nodeid_registry[i].node_id == receive.metadata.remote_node_id) { nodeid_registry[i].register_index++; // Increment index counter for next update() nodeid_registry[i].register_setup = false; nodeid_registry[i].retry_count = 0; } } - if (_access_request.setPortId(receive.remote_node_id, msg.name, NULL)) { //FIXME confirm handler + if (_access_request.setPortId(receive.metadata.remote_node_id, msg.name, NULL)) { //FIXME confirm handler PX4_INFO("Set portID succesfull"); } else { diff --git a/src/drivers/uavcan_v1/NodeManager.hpp b/src/drivers/cyphal/NodeManager.hpp similarity index 81% rename from src/drivers/uavcan_v1/NodeManager.hpp rename to src/drivers/cyphal/NodeManager.hpp index c44678a234..af2fb8b62f 100644 --- a/src/drivers/uavcan_v1/NodeManager.hpp +++ b/src/drivers/cyphal/NodeManager.hpp @@ -70,37 +70,37 @@ typedef struct { class NodeManager : public UavcanBaseSubscriber, public UavcanServiceRequestInterface { public: - NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "", "NodeIDAllocationData", 0), - _canard_instance(ins), _access_request(ins, pmgr), _list_request(ins) { }; + NodeManager(CanardHandle &handle, UavcanParamManager &pmgr) : UavcanBaseSubscriber(handle, "", "NodeIDAllocationData", + 0), + _canard_handle(handle), _access_request(handle, pmgr), _list_request(handle) { }; void subscribe() override { _access_request.subscribe(); _list_request.subscribe(); - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID - (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID), // The fixed Subject-ID + (_canard_handle.mtu() == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); } bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg); bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg); - void response_callback(const CanardTransfer &receive) override + void response_callback(const CanardRxTransfer &receive) override { HandleListResponse(receive); } - void callback(const CanardTransfer &receive); // NodeIDAllocation callback + void callback(const CanardRxTransfer &receive); // NodeIDAllocation callback - void HandleListResponse(const CanardTransfer &receive); + void HandleListResponse(const CanardRxTransfer &receive); void update(); private: - CanardInstance &_canard_instance; + CanardHandle &_canard_handle; CanardTransferID _uavcan_pnp_nodeidallocation_v1_transfer_id{0}; UavcanNodeEntry nodeid_registry[16] {0}; //TODO configurable or just rewrite diff --git a/src/drivers/uavcan_v1/ParamManager.cpp b/src/drivers/cyphal/ParamManager.cpp similarity index 100% rename from src/drivers/uavcan_v1/ParamManager.cpp rename to src/drivers/cyphal/ParamManager.cpp diff --git a/src/drivers/uavcan_v1/ParamManager.hpp b/src/drivers/cyphal/ParamManager.hpp similarity index 100% rename from src/drivers/uavcan_v1/ParamManager.hpp rename to src/drivers/cyphal/ParamManager.hpp diff --git a/src/drivers/uavcan_v1/PublicationManager.cpp b/src/drivers/cyphal/PublicationManager.cpp similarity index 91% rename from src/drivers/uavcan_v1/PublicationManager.cpp rename to src/drivers/cyphal/PublicationManager.cpp index ff69f687e7..2fd3f745aa 100644 --- a/src/drivers/uavcan_v1/PublicationManager.cpp +++ b/src/drivers/cyphal/PublicationManager.cpp @@ -56,10 +56,11 @@ void PublicationManager::updateDynamicPublications() for (auto &dynpub : _dynpublishers) { // Check if subscriber has already been created - const char *subj_name = dynpub->getSubjectName(); + char full_subj_name[200]; + snprintf(full_subj_name, sizeof(full_subj_name), "%s%s", dynpub->getPrefixName(), dynpub->getSubjectName()); const uint8_t instance = dynpub->getInstance(); - if (strcmp(subj_name, sub.subject_name) == 0 && instance == sub.instance) { + if (strcmp(full_subj_name, sub.subject_name) == 0 && instance == sub.instance) { found_publisher = true; break; } @@ -77,7 +78,7 @@ void PublicationManager::updateDynamicPublications() uint16_t port_id = value.natural16.value.elements[0]; if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber - UavcanPublisher *dynpub = sub.create_pub(_canard_instance, _param_manager); + UavcanPublisher *dynpub = sub.create_pub(_canard_handle, _param_manager); if (dynpub == nullptr) { PX4_ERR("Out of memory"); diff --git a/src/drivers/uavcan_v1/PublicationManager.hpp b/src/drivers/cyphal/PublicationManager.hpp similarity index 60% rename from src/drivers/uavcan_v1/PublicationManager.hpp rename to src/drivers/cyphal/PublicationManager.hpp index 1be1b433bc..b3d39ffbac 100644 --- a/src/drivers/uavcan_v1/PublicationManager.hpp +++ b/src/drivers/cyphal/PublicationManager.hpp @@ -44,33 +44,33 @@ #include -#ifndef CONFIG_UAVCAN_V1_GNSS_PUBLISHER -#define CONFIG_UAVCAN_V1_GNSS_PUBLISHER 0 +#ifndef CONFIG_CYPHAL_GNSS_PUBLISHER +#define CONFIG_CYPHAL_GNSS_PUBLISHER 0 #endif -#ifndef CONFIG_UAVCAN_V1_ESC_CONTROLLER -#define CONFIG_UAVCAN_V1_ESC_CONTROLLER 0 +#ifndef CONFIG_CYPHAL_ESC_CONTROLLER +#define CONFIG_CYPHAL_ESC_CONTROLLER 0 #endif -#ifndef CONFIG_UAVCAN_V1_READINESS_PUBLISHER -#define CONFIG_UAVCAN_V1_READINESS_PUBLISHER 0 +#ifndef CONFIG_CYPHAL_READINESS_PUBLISHER +#define CONFIG_CYPHAL_READINESS_PUBLISHER 0 #endif -#ifndef CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER -#define CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0 +#ifndef CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER +#define CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER 0 #endif -#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER -#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER 0 +#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER +#define CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER 0 #endif /* Preprocessor calculation of publisher count */ -#define UAVCAN_PUB_COUNT CONFIG_UAVCAN_V1_GNSS_PUBLISHER + \ - CONFIG_UAVCAN_V1_ESC_CONTROLLER + \ - CONFIG_UAVCAN_V1_READINESS_PUBLISHER + \ - CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ - CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER +#define UAVCAN_PUB_COUNT CONFIG_CYPHAL_GNSS_PUBLISHER + \ + CONFIG_CYPHAL_ESC_CONTROLLER + \ + CONFIG_CYPHAL_READINESS_PUBLISHER + \ + CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER + \ + CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER #include #include @@ -81,12 +81,12 @@ #include #include "Actuators/EscClient.hpp" -#include "Publishers/DS-015/Readiness.hpp" -#include "Publishers/DS-015/Gnss.hpp" +#include "Publishers/udral/Readiness.hpp" +#include "Publishers/udral/Gnss.hpp" #include "Publishers/uORB/uorb_publisher.hpp" typedef struct { - UavcanPublisher *(*create_pub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; + UavcanPublisher *(*create_pub)(CanardHandle &handle, UavcanParamManager &pmgr) {}; const char *subject_name; const uint8_t instance; } UavcanDynPubBinder; @@ -94,7 +94,7 @@ typedef struct { class PublicationManager { public: - PublicationManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {} + PublicationManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {} ~PublicationManager(); void update(); @@ -104,57 +104,57 @@ public: private: void updateDynamicPublications(); - CanardInstance &_canard_instance; + CanardHandle &_canard_handle; UavcanParamManager &_param_manager; List _dynpublishers; const UavcanDynPubBinder _uavcan_pubs[UAVCAN_PUB_COUNT] { -#if CONFIG_UAVCAN_V1_GNSS_PUBLISHER +#if CONFIG_CYPHAL_GNSS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * { - return new UavcanGnssPublisher(ins, pmgr, 0); + return new UavcanGnssPublisher(handle, pmgr, 0); }, "gps", 0 }, #endif -#if CONFIG_UAVCAN_V1_ESC_CONTROLLER +#if CONFIG_CYPHAL_ESC_CONTROLLER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * { - return new UavcanEscController(ins, pmgr); + return new UavcanEscController(handle, pmgr); }, "esc", 0 }, #endif -#if CONFIG_UAVCAN_V1_READINESS_PUBLISHER +#if CONFIG_CYPHAL_READINESS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * { - return new UavcanReadinessPublisher(ins, pmgr, 0); + return new UavcanReadinessPublisher(handle, pmgr, 0); }, "readiness", 0 }, #endif -#if CONFIG_UAVCAN_V1_UORB_ACTUATOR_OUTPUTS_PUBLISHER +#if CONFIG_CYPHAL_UORB_ACTUATOR_OUTPUTS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * { - return new uORB_over_UAVCAN_Publisher(ins, pmgr, ORB_ID(actuator_outputs)); + return new uORB_over_UAVCAN_Publisher(handle, pmgr, ORB_ID(actuator_outputs)); }, "uorb.actuator_outputs", 0 }, #endif -#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_PUBLISHER +#if CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanPublisher * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanPublisher * { - return new uORB_over_UAVCAN_Publisher(ins, pmgr, ORB_ID(sensor_gps)); + return new uORB_over_UAVCAN_Publisher(handle, pmgr, ORB_ID(sensor_gps)); }, "uorb.sensor_gps", 0 diff --git a/src/drivers/uavcan_v1/Publishers/Publisher.hpp b/src/drivers/cyphal/Publishers/Publisher.hpp similarity index 92% rename from src/drivers/uavcan_v1/Publishers/Publisher.hpp rename to src/drivers/cyphal/Publishers/Publisher.hpp index f13c176b68..cb04ef5fdd 100644 --- a/src/drivers/uavcan_v1/Publishers/Publisher.hpp +++ b/src/drivers/cyphal/Publishers/Publisher.hpp @@ -34,7 +34,7 @@ /** * @file Publisher.hpp * - * Defines basic functionality of UAVCAN v1 publisher class + * Defines basic functionality of Cyphal publisher class * * @author Jacob Crabill */ @@ -48,6 +48,7 @@ #include +#include "../CanardHandle.hpp" #include "../CanardInterface.hpp" #include "../ParamManager.hpp" @@ -60,9 +61,9 @@ class UavcanPublisher : public ListNode { public: - UavcanPublisher(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name, + UavcanPublisher(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name, uint8_t instance = 0) : - _canard_instance(ins), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name), + _canard_handle(handle), _param_manager(pmgr), _prefix_name(prefix_name), _subject_name(subject_name), _instance(instance) { }; virtual ~UavcanPublisher() = default; @@ -132,7 +133,7 @@ public: } protected: - CanardInstance &_canard_instance; + CanardHandle &_canard_handle; UavcanParamManager &_param_manager; const char *_prefix_name; const char *_subject_name; diff --git a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.cpp b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp similarity index 100% rename from src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.cpp rename to src/drivers/cyphal/Publishers/uORB/uorb_publisher.cpp diff --git a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp similarity index 88% rename from src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp rename to src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp index 518915da12..3843737e35 100644 --- a/src/drivers/uavcan_v1/Publishers/uORB/uorb_publisher.hpp +++ b/src/drivers/cyphal/Publishers/uORB/uorb_publisher.hpp @@ -50,9 +50,9 @@ template class uORB_over_UAVCAN_Publisher : public UavcanPublisher { public: - uORB_over_UAVCAN_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta, + uORB_over_UAVCAN_Publisher(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "uorb.", meta->o_name, instance), + UavcanPublisher(handle, pmgr, "uorb.", meta->o_name, instance), _uorb_meta{meta}, _uorb_sub(meta) {}; @@ -67,20 +67,20 @@ public: T data {}; _uorb_sub.update(&data); - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = _transfer_id, - .payload_size = get_payload_size(&data), - .payload = &data, + .transfer_id = _transfer_id }; // set the data ready in the buffer and chop if needed ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - canardTxPush(&_canard_instance, &transfer); + _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + get_payload_size(&data), + &data); } }; diff --git a/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp b/src/drivers/cyphal/Publishers/udral/Gnss.hpp similarity index 57% rename from src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp rename to src/drivers/cyphal/Publishers/udral/Gnss.hpp index 1447e42ae2..168b1e437f 100644 --- a/src/drivers/uavcan_v1/Publishers/DS-015/Gnss.hpp +++ b/src/drivers/cyphal/Publishers/udral/Gnss.hpp @@ -34,24 +34,23 @@ /** * @file Gnss.hpp * - * Defines basic functionality of UAVCAN v1 GNSS publisher + * Defines basic functionality of Cyphal GNSS publisher * * @author Jacob Crabill */ #pragma once -// DS-15 Specification Messages -#include -#include +// UDRAL Specification Messages +#include #include "../Publisher.hpp" class UavcanGnssPublisher : public UavcanPublisher { public: - UavcanGnssPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "ds_015", "gps", instance) + UavcanGnssPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(handle, pmgr, "udral", "gps", instance) { }; @@ -64,66 +63,33 @@ public: if (_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { sensor_gps_s gps {}; _gps_sub.update(&gps); + size_t payload_size = reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_; - reg_drone_physics_kinematics_geodetic_Point_0_1 geo {}; + reg_udral_physics_kinematics_geodetic_Point_0_1 geo {}; geo.latitude = gps.lat; geo.longitude = gps.lon; geo.altitude = uavcan_si_unit_length_WideScalar_1_0 { .meter = static_cast(gps.alt) }; - uint8_t geo_payload_buffer[reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t geo_payload_buffer[reg_udral_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _transfer_id, - .payload_size = reg_drone_physics_kinematics_geodetic_Point_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &geo_payload_buffer, }; - int32_t result = reg_drone_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer, - &transfer.payload_size); + int32_t result = reg_udral_physics_kinematics_geodetic_Point_0_1_serialize_(&geo, geo_payload_buffer, + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); - } - - /// TODO: Also publish DilutionOfPrecision, ...? - reg_drone_service_gnss_DilutionOfPrecision_0_1 dop { - .geometric = NAN, - .position = NAN, - .horizontal = gps.hdop, - .vertical = gps.vdop, - .time = NAN, - .northing = NAN, - .easting = NAN, - }; - - uint8_t dop_payload_buffer[reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - - CanardPortID _port_id_2 = static_cast((uint16_t)_port_id + 1U); - - CanardTransfer transfer2 = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, - .priority = CanardPriorityNominal, - .transfer_kind = CanardTransferKindMessage, - .port_id = _port_id_2, // This is the subject-ID. - .remote_node_id = CANARD_NODE_ID_UNSET, - .transfer_id = _transfer_id_2, - .payload_size = reg_drone_service_gnss_DilutionOfPrecision_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &dop_payload_buffer, - }; - - result = reg_drone_service_gnss_DilutionOfPrecision_0_1_serialize_(&dop, dop_payload_buffer, &transfer2.payload_size); - - if (result == 0) { - // set the data ready in the buffer and chop if needed - ++_transfer_id_2; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer2); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &geo_payload_buffer); } } }; diff --git a/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp b/src/drivers/cyphal/Publishers/udral/Readiness.hpp similarity index 76% rename from src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp rename to src/drivers/cyphal/Publishers/udral/Readiness.hpp index 98b5cd4837..8afe59f34a 100644 --- a/src/drivers/uavcan_v1/Publishers/DS-015/Readiness.hpp +++ b/src/drivers/cyphal/Publishers/udral/Readiness.hpp @@ -34,7 +34,7 @@ /** * @file Readiness.hpp * - * Defines the UAVCAN v1 readiness publisher + * Defines the Cyphal readiness publisher * readiness state is used to command or report the availability status * * @author Peter van der Perk @@ -42,16 +42,16 @@ #pragma once -// DS-15 Specification Messages -#include +// UDRAL Specification Messages +#include #include "../Publisher.hpp" class UavcanReadinessPublisher : public UavcanPublisher { public: - UavcanReadinessPublisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanPublisher(ins, pmgr, "ds_015", "readiness", instance) + UavcanReadinessPublisher(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanPublisher(handle, pmgr, "udral", "readiness", instance) { }; @@ -65,36 +65,37 @@ public: if (_actuator_armed_sub.updated() && _port_id != CANARD_PORT_ID_UNSET) { actuator_armed_s armed {}; _actuator_armed_sub.update(&armed); + size_t payload_size; - reg_drone_service_common_Readiness_0_1 readiness {}; + reg_udral_service_common_Readiness_0_1 readiness {}; if (armed.armed) { - readiness.value = reg_drone_service_common_Readiness_0_1_ENGAGED; + readiness.value = reg_udral_service_common_Readiness_0_1_ENGAGED; } else { - readiness.value = reg_drone_service_common_Readiness_0_1_STANDBY; + readiness.value = reg_udral_service_common_Readiness_0_1_STANDBY; } - uint8_t readiness_payload_buffer[reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; + uint8_t readiness_payload_buffer[reg_udral_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindMessage, .port_id = _port_id, // This is the subject-ID. .remote_node_id = CANARD_NODE_ID_UNSET, .transfer_id = _transfer_id, - .payload_size = reg_drone_service_common_Readiness_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &readiness_payload_buffer, }; - int32_t result = reg_drone_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer, - &transfer.payload_size); + int32_t result = reg_udral_service_common_Readiness_0_1_serialize_(&readiness, readiness_payload_buffer, + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed ++_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - result = canardTxPush(&_canard_instance, &transfer); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &readiness_payload_buffer); } } }; diff --git a/src/drivers/uavcan_v1/ServiceClients/Access.hpp b/src/drivers/cyphal/ServiceClients/Access.hpp similarity index 78% rename from src/drivers/uavcan_v1/ServiceClients/Access.hpp rename to src/drivers/cyphal/ServiceClients/Access.hpp index 128912b067..824c051407 100644 --- a/src/drivers/uavcan_v1/ServiceClients/Access.hpp +++ b/src/drivers/cyphal/ServiceClients/Access.hpp @@ -55,25 +55,26 @@ class UavcanAccessResponse : public UavcanBaseSubscriber { public: - UavcanAccessResponse(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanBaseSubscriber(ins, "", "Access", 0), _param_manager(pmgr) { }; + UavcanAccessResponse(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanBaseSubscriber(handle, "", "Access", 0), _param_manager(pmgr) { }; void subscribe() override { // Subscribe to requests uavcan.pnp.NodeIDAllocationData - canardRxSubscribe(&_canard_instance, - CanardTransferKindRequest, - uavcan_register_Access_1_0_FIXED_PORT_ID_, - uavcan_register_Access_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindRequest, + uavcan_register_Access_1_0_FIXED_PORT_ID_, + uavcan_register_Access_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { PX4_INFO("Access request"); + size_t payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + uavcan_register_Access_Request_1_0 msg; uavcan_register_Access_Request_1_0_initialize_(&msg); @@ -102,22 +103,22 @@ public: uint8_t response_payload_buffer[uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .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 = receive.transfer_id, - .payload_size = uavcan_register_Access_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &response_payload_buffer, + .remote_node_id = receive.metadata.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = receive.metadata.transfer_id, }; - result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size); + result = uavcan_register_Access_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed - result = canardTxPush(&_canard_instance, &transfer); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &response_payload_buffer); } //return result; diff --git a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp b/src/drivers/cyphal/ServiceClients/GetInfo.hpp similarity index 82% rename from src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp rename to src/drivers/cyphal/ServiceClients/GetInfo.hpp index 4b752c205a..803128cf05 100644 --- a/src/drivers/uavcan_v1/ServiceClients/GetInfo.hpp +++ b/src/drivers/cyphal/ServiceClients/GetInfo.hpp @@ -54,28 +54,29 @@ class UavcanGetInfoResponse : public UavcanBaseSubscriber { public: - UavcanGetInfoResponse(CanardInstance &ins) : - UavcanBaseSubscriber(ins, "", "GetInfo", 0) { }; + UavcanGetInfoResponse(CanardHandle &handle) : + UavcanBaseSubscriber(handle, "", "GetInfo", 0) { }; void subscribe() override { // Subscribe to requests uavcan.pnp.NodeIDAllocationData - canardRxSubscribe(&_canard_instance, - CanardTransferKindRequest, - uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, - uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe( + CanardTransferKindRequest, + uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, + uavcan_node_GetInfo_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { PX4_INFO("GetInfo request"); // Setup node.GetInfo response uavcan_node_GetInfo_Response_1_0 node_info; + size_t payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; uavcan_node_GetInfo_Response_1_0_initialize_(&node_info); @@ -106,23 +107,23 @@ public: uint8_t response_payload_buffer[uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer response = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindResponse, .port_id = uavcan_node_GetInfo_1_0_FIXED_PORT_ID_, // This is the subject-ID. - .remote_node_id = receive.remote_node_id, // Send back to request Node - .transfer_id = receive.transfer_id, - .payload_size = uavcan_node_GetInfo_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &response_payload_buffer, + .remote_node_id = receive.metadata.remote_node_id, // Send back to request Node + .transfer_id = receive.metadata.transfer_id }; int32_t result = uavcan_node_GetInfo_Response_1_0_serialize_(&node_info, (uint8_t *)&response_payload_buffer, - &response.payload_size); + &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed - result = canardTxPush(&_canard_instance, &response); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &response_payload_buffer); } //TODO proper error handling diff --git a/src/drivers/uavcan_v1/ServiceClients/List.hpp b/src/drivers/cyphal/ServiceClients/List.hpp similarity index 77% rename from src/drivers/uavcan_v1/ServiceClients/List.hpp rename to src/drivers/cyphal/ServiceClients/List.hpp index 8259fbdaf3..1040067391 100644 --- a/src/drivers/uavcan_v1/ServiceClients/List.hpp +++ b/src/drivers/cyphal/ServiceClients/List.hpp @@ -54,25 +54,27 @@ class UavcanListResponse : public UavcanBaseSubscriber { public: - UavcanListResponse(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanBaseSubscriber(ins, "", "List", 0), _param_manager(pmgr) { }; + UavcanListResponse(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanBaseSubscriber(handle, "", "List", 0), _param_manager(pmgr) { }; void subscribe() override { // Subscribe to requests uavcan.pnp.NodeIDAllocationData - canardRxSubscribe(&_canard_instance, - CanardTransferKindRequest, - uavcan_register_List_1_0_FIXED_PORT_ID_, - uavcan_register_List_Response_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + + _canard_handle.RxSubscribe(CanardTransferKindRequest, + uavcan_register_List_1_0_FIXED_PORT_ID_, + uavcan_register_List_Response_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { PX4_INFO("List request"); + size_t payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; + uavcan_register_List_Request_1_0 msg; uavcan_register_List_Response_1_0 response; @@ -90,22 +92,22 @@ public: uint8_t response_payload_buffer[uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindResponse, .port_id = uavcan_register_List_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 = receive.transfer_id, - .payload_size = uavcan_register_List_Response_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &response_payload_buffer, + .remote_node_id = receive.metadata.remote_node_id, // Messages cannot be unicast, so use UNSET. + .transfer_id = receive.metadata.transfer_id }; - result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &transfer.payload_size); + result = uavcan_register_List_Response_1_0_serialize_(&response, response_payload_buffer, &payload_size); if (result == 0) { // set the data ready in the buffer and chop if needed - result = canardTxPush(&_canard_instance, &transfer); + result = _canard_handle.TxPush(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &response_payload_buffer); } }; diff --git a/src/drivers/uavcan_v1/Services/AccessRequest.hpp b/src/drivers/cyphal/Services/AccessRequest.hpp similarity index 85% rename from src/drivers/uavcan_v1/Services/AccessRequest.hpp rename to src/drivers/cyphal/Services/AccessRequest.hpp index 7d5941c951..f162d24e68 100644 --- a/src/drivers/uavcan_v1/Services/AccessRequest.hpp +++ b/src/drivers/cyphal/Services/AccessRequest.hpp @@ -54,8 +54,8 @@ class UavcanAccessServiceRequest : public UavcanServiceRequest { public: - UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) : - UavcanServiceRequest(ins, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_, + UavcanAccessServiceRequest(CanardHandle &handle, UavcanParamManager &pmgr) : + UavcanServiceRequest(handle, "", "Access", uavcan_register_Access_1_0_FIXED_PORT_ID_, uavcan_register_Access_Response_1_0_EXTENT_BYTES_), _param_manager(pmgr) { }; bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name, UavcanServiceRequestInterface *handler) @@ -70,26 +70,28 @@ public: name.name.elements[7] = 's'; //HACK Change pub into sub if (_param_manager.GetParamByName(name, request_msg.value)) { + size_t payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; name.name.elements[7] = 'p'; //HACK Change sub into pub memcpy(&request_msg.name, &name, sizeof(request_msg.name)); uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindRequest, .port_id = _portID, // This is the subject-ID. .remote_node_id = node_id, // Messages cannot be unicast, so use UNSET. - .transfer_id = request_transfer_id, - .payload_size = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, + .transfer_id = request_transfer_id }; - result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size); + result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &payload_size); if (result == 0) { - return request(&transfer, handler); + return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &request_payload_buffer, + handler); } else { return false; diff --git a/src/drivers/uavcan_v1/Services/ListRequest.hpp b/src/drivers/cyphal/Services/ListRequest.hpp similarity index 84% rename from src/drivers/uavcan_v1/Services/ListRequest.hpp rename to src/drivers/cyphal/Services/ListRequest.hpp index c6f8f6a4fd..f5bad790d0 100644 --- a/src/drivers/uavcan_v1/Services/ListRequest.hpp +++ b/src/drivers/cyphal/Services/ListRequest.hpp @@ -54,31 +54,33 @@ class UavcanListServiceRequest : public UavcanServiceRequest { public: - UavcanListServiceRequest(CanardInstance &ins) : - UavcanServiceRequest(ins, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_, + UavcanListServiceRequest(CanardHandle &handle) : + UavcanServiceRequest(handle, "", "List", uavcan_register_List_1_0_FIXED_PORT_ID_, uavcan_register_List_Response_1_0_EXTENT_BYTES_) { }; bool getIndex(CanardNodeID node_id, uint16_t index, UavcanServiceRequestInterface *handler) { uavcan_register_List_Request_1_0 msg; + size_t payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_; msg.index = index; uint8_t request_payload_buffer[uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_]; - CanardTransfer transfer = { - .timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + const CanardTransferMetadata transfer_metadata = { .priority = CanardPriorityNominal, .transfer_kind = CanardTransferKindRequest, .port_id = uavcan_register_List_1_0_FIXED_PORT_ID_, // This is the subject-ID. .remote_node_id = node_id, - .transfer_id = request_transfer_id, - .payload_size = uavcan_register_List_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_, - .payload = &request_payload_buffer, + .transfer_id = request_transfer_id }; - if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &transfer.payload_size) == 0) { - return request(&transfer, handler); + if (uavcan_register_List_Request_1_0_serialize_(&msg, request_payload_buffer, &payload_size) == 0) { + return request(hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC, + &transfer_metadata, + payload_size, + &request_payload_buffer, + handler); } else { return false; diff --git a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp b/src/drivers/cyphal/Services/ServiceRequest.hpp similarity index 71% rename from src/drivers/uavcan_v1/Services/ServiceRequest.hpp rename to src/drivers/cyphal/Services/ServiceRequest.hpp index 0dabb65fe2..b636ab75df 100644 --- a/src/drivers/uavcan_v1/Services/ServiceRequest.hpp +++ b/src/drivers/cyphal/Services/ServiceRequest.hpp @@ -53,43 +53,49 @@ class UavcanServiceRequestInterface { public: - virtual void response_callback(const CanardTransfer &receive) = 0; + virtual void response_callback(const CanardRxTransfer &receive) = 0; }; class UavcanServiceRequest : public UavcanBaseSubscriber { public: - UavcanServiceRequest(CanardInstance &ins, const char *prefix_name, const char *subject_name, CanardPortID portID, + UavcanServiceRequest(CanardHandle &handle, const char *prefix_name, const char *subject_name, CanardPortID portID, size_t extent) : - UavcanBaseSubscriber(ins, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { }; + UavcanBaseSubscriber(handle, prefix_name, subject_name, 0), _portID(portID), _extent(extent) { }; void subscribe() override { // Subscribe to requests response - canardRxSubscribe(&_canard_instance, - CanardTransferKindResponse, - _portID, - _extent, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindResponse, + _portID, + _extent, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); }; - bool request(CanardTransfer *transfer, UavcanServiceRequestInterface *handler) + bool request(const CanardMicrosecond tx_deadline_usec, + const CanardTransferMetadata *transfer_metadata, + const size_t payload_size, + const void *const payload, + UavcanServiceRequestInterface *handler) { _response_callback = handler; - remote_node_id = transfer->remote_node_id; + remote_node_id = transfer_metadata->remote_node_id; ++request_transfer_id; // The transfer-ID shall be incremented after every transmission on this subject. - return canardTxPush(&_canard_instance, transfer) > 0; + return _canard_handle.TxPush(tx_deadline_usec, + transfer_metadata, + payload_size, + payload) > 0; } - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { PX4_INFO("Response"); if (_response_callback != nullptr && - receive.transfer_id == (request_transfer_id - 1) && - receive.remote_node_id == remote_node_id) { + receive.metadata.transfer_id == (request_transfer_id - 1) && + receive.metadata.remote_node_id == remote_node_id) { _response_callback->response_callback(receive); } }; diff --git a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp b/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp similarity index 89% rename from src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp rename to src/drivers/cyphal/Subscribers/BaseSubscriber.hpp index 06160b3667..a673bd046b 100644 --- a/src/drivers/uavcan_v1/Subscribers/BaseSubscriber.hpp +++ b/src/drivers/cyphal/Subscribers/BaseSubscriber.hpp @@ -34,7 +34,7 @@ /** * @file BaseSubscriber.hpp * - * Defines basic functionality of UAVCAN v1 subscriber class + * Defines basic functionality of Cyphal subscriber class * * @author Jacob Crabill */ @@ -45,14 +45,15 @@ #include +#include "../CanardHandle.hpp" #include "../CanardInterface.hpp" #include "../ParamManager.hpp" class UavcanBaseSubscriber { public: - UavcanBaseSubscriber(CanardInstance &ins, const char *prefix_name, const char *subject_name, uint8_t instance = 0) : - _canard_instance(ins), _prefix_name(prefix_name), _instance(instance) + UavcanBaseSubscriber(CanardHandle &handle, const char *prefix_name, const char *subject_name, uint8_t instance = 0) : + _canard_handle(handle), _prefix_name(prefix_name), _instance(instance) { _subj_sub._subject_name = subject_name; _subj_sub._canard_sub.user_reference = this; @@ -72,12 +73,12 @@ public: SubjectSubscription *curSubj = &_subj_sub; while (curSubj != nullptr) { - canardRxUnsubscribe(&_canard_instance, CanardTransferKindMessage, curSubj->_canard_sub.port_id); + _canard_handle.RxUnsubscribe(CanardTransferKindMessage, curSubj->_canard_sub.port_id); curSubj = curSubj->next; } }; - virtual void callback(const CanardTransfer &msg) = 0; + virtual void callback(const CanardRxTransfer &msg) = 0; CanardPortID id(uint32_t instance = 0) { @@ -145,7 +146,7 @@ protected: struct SubjectSubscription *next {nullptr}; }; - CanardInstance &_canard_instance; + CanardHandle &_canard_handle; const char *_prefix_name; SubjectSubscription _subj_sub; uint8_t _instance {0}; diff --git a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp b/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp similarity index 92% rename from src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp rename to src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp index 7a18f19184..dd417c9967 100644 --- a/src/drivers/uavcan_v1/Subscribers/DynamicPortSubscriber.hpp +++ b/src/drivers/cyphal/Subscribers/DynamicPortSubscriber.hpp @@ -34,7 +34,7 @@ /** * @file DynamicPortSubscriber.hpp * - * Defines basic functionality of UAVCAN v1 subscriber class with Non-fixed unregulated port identifier + * Defines basic functionality of Cyphal subscriber class with Non-fixed unregulated port identifier * * @author Jacob Crabill */ @@ -54,9 +54,9 @@ class UavcanDynamicPortSubscriber : public UavcanBaseSubscriber { public: - UavcanDynamicPortSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, const char *prefix_name, + UavcanDynamicPortSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, const char *prefix_name, const char *subject_name, uint8_t instance = 0) : - UavcanBaseSubscriber(ins, prefix_name, subject_name, instance), _param_manager(pmgr) { }; + UavcanBaseSubscriber(handle, prefix_name, subject_name, instance), _param_manager(pmgr) { }; void updateParam() { diff --git a/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp b/src/drivers/cyphal/Subscribers/Heartbeat.hpp similarity index 83% rename from src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp rename to src/drivers/cyphal/Subscribers/Heartbeat.hpp index acd3667344..9001b93861 100644 --- a/src/drivers/uavcan_v1/Subscribers/Heartbeat.hpp +++ b/src/drivers/cyphal/Subscribers/Heartbeat.hpp @@ -50,22 +50,21 @@ class UavcanHeartbeatSubscriber : public UavcanBaseSubscriber { public: - UavcanHeartbeatSubscriber(CanardInstance &ins) : - UavcanBaseSubscriber(ins, "", "Heartbeat", 0) { }; + UavcanHeartbeatSubscriber(CanardHandle &handle) : + UavcanBaseSubscriber(handle, "", "Heartbeat", 0) { }; void subscribe() override { // Subscribe to heartbeat messages - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, // The fixed Subject-ID - uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_, // The fixed Subject-ID + uavcan_node_Heartbeat_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { //TODO heartbeat management }; diff --git a/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp similarity index 88% rename from src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp rename to src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp index 3273c25ba8..4a4d316ee0 100644 --- a/src/drivers/uavcan_v1/Subscribers/legacy/LegacyBatteryInfo.hpp +++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp @@ -52,21 +52,20 @@ class UavcanLegacyBatteryInfoSubscriber : public UavcanDynamicPortSubscriber { public: - UavcanLegacyBatteryInfoSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "legacy.", "legacy_bms", instance) { }; + UavcanLegacyBatteryInfoSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "legacy.", "legacy_bms", instance) { }; void subscribe() override { // Subscribe to messages reg.drone.service.battery.Status.0.1 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _subj_sub._canard_sub.port_id, - legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + legacy_equipment_power_BatteryInfo_1_0_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 100, //FIXME timeout caused by scheduler + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { PX4_INFO("Legacy BmsCallback"); diff --git a/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp similarity index 100% rename from src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.cpp rename to src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.cpp diff --git a/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp similarity index 87% rename from src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp rename to src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp index 98a46524f4..a80edf292d 100644 --- a/src/drivers/uavcan_v1/Subscribers/uORB/uorb_subscriber.hpp +++ b/src/drivers/cyphal/Subscribers/uORB/uorb_subscriber.hpp @@ -52,9 +52,9 @@ template class uORB_over_UAVCAN_Subscriber : public UavcanDynamicPortSubscriber { public: - uORB_over_UAVCAN_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, const orb_metadata *meta, + uORB_over_UAVCAN_Subscriber(CanardHandle &handle, UavcanParamManager &pmgr, const orb_metadata *meta, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "uorb.", meta->o_name, instance), + UavcanDynamicPortSubscriber(handle, pmgr, "uorb.", meta->o_name, instance), _uorb_meta{meta}, _uorb_pub(meta) {}; @@ -66,15 +66,14 @@ public: T *data = NULL; // Subscribe to messages uORB sensor_gps payload over UAVCAN - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _subj_sub._canard_sub.port_id, - get_payload_size(data), - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + get_payload_size(data), + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000, + &_subj_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { T *data = (T *)receive.payload; diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp b/src/drivers/cyphal/Subscribers/udral/Battery.hpp similarity index 75% rename from src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp rename to src/drivers/cyphal/Subscribers/udral/Battery.hpp index 767ec10dfc..732c3dc2e8 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Battery.hpp +++ b/src/drivers/cyphal/Subscribers/udral/Battery.hpp @@ -34,7 +34,7 @@ /** * @file Battery.hpp * - * Defines basic functionality of UAVCAN v1 Battery subscription + * Defines basic functionality of Cyphal Battery subscription * * @author Jacob Crabill */ @@ -44,10 +44,10 @@ #include #include -// DS-15 Specification Messages -#include -#include -#include +// UDRAL Specification Messages +#include +#include +#include #include "../DynamicPortSubscriber.hpp" @@ -57,8 +57,8 @@ class UavcanBmsSubscriber : public UavcanDynamicPortSubscriber { public: - UavcanBmsSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "energy_source", instance) + UavcanBmsSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "energy_source", instance) { _subj_sub.next = &_status_sub; @@ -74,36 +74,33 @@ public: void subscribe() override { // Subscribe to messages reg.drone.physics.electricity.SourceTs.0.1 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _subj_sub._canard_sub.port_id, - reg_drone_physics_electricity_SourceTs_0_1_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_physics_electricity_SourceTs_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); // Subscribe to messages reg.drone.service.battery.Status.0.2 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _status_sub._canard_sub.port_id, - reg_drone_service_battery_Status_0_2_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_status_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _status_sub._canard_sub.port_id, + reg_udral_service_battery_Status_0_2_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_status_sub._canard_sub); // Subscribe to messages reg.drone.service.battery.Parameters.0.3 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _parameters_sub._canard_sub.port_id, - reg_drone_service_battery_Parameters_0_3_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_parameters_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _parameters_sub._canard_sub.port_id, + reg_udral_service_battery_Parameters_0_3_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_parameters_sub._canard_sub); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { - if (receive.port_id == _subj_sub._canard_sub.port_id) { - reg_drone_physics_electricity_SourceTs_0_1 source_ts {}; + if (receive.metadata.port_id == _subj_sub._canard_sub.port_id) { + reg_udral_physics_electricity_SourceTs_0_1 source_ts {}; size_t source_ts_size_in_bytes = receive.payload_size; - reg_drone_physics_electricity_SourceTs_0_1_deserialize_(&source_ts, + reg_udral_physics_electricity_SourceTs_0_1_deserialize_(&source_ts, (const uint8_t *)receive.payload, &source_ts_size_in_bytes); bat_status.timestamp = hrt_absolute_time(); //FIXME timesyncronization source_ts.timestamp.microsecond; @@ -123,10 +120,10 @@ public: // TODO uORB publication rate limiting _battery_status_pub.publish(bat_status); - } else if (receive.port_id == _status_sub._canard_sub.port_id) { - reg_drone_service_battery_Status_0_2 bat {}; + } else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) { + reg_udral_service_battery_Status_0_2 bat {}; size_t bat_size_in_bytes = receive.payload_size; - reg_drone_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes); + reg_udral_service_battery_Status_0_2_deserialize_(&bat, (const uint8_t *)receive.payload, &bat_size_in_bytes); bat_status.scale = -1; // What does the mean? @@ -157,10 +154,10 @@ public: bat_status.max_cell_voltage_delta = voltage_cell_max - voltage_cell_min; // Current delta or max delta over time? - } else if (receive.port_id == _parameters_sub._canard_sub.port_id) { - reg_drone_service_battery_Parameters_0_3 parameters {}; + } else if (receive.metadata.port_id == _parameters_sub._canard_sub.port_id) { + reg_udral_service_battery_Parameters_0_3 parameters {}; size_t parameters_size_in_bytes = receive.payload_size; - reg_drone_service_battery_Parameters_0_3_deserialize_(¶meters, + reg_udral_service_battery_Parameters_0_3_deserialize_(¶meters, (const uint8_t *)receive.payload, ¶meters_size_in_bytes); diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp b/src/drivers/cyphal/Subscribers/udral/Esc.hpp similarity index 75% rename from src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp rename to src/drivers/cyphal/Subscribers/udral/Esc.hpp index 90f26e5dbe..7ccfea448a 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Esc.hpp +++ b/src/drivers/cyphal/Subscribers/udral/Esc.hpp @@ -34,7 +34,7 @@ /** * @file Esc.hpp * - * Defines basic functionality of UAVCAN v1 ESC setpoint subscription + * Defines basic functionality of Cyphal ESC setpoint subscription * (For use on a CAN-->PWM node) * * @author Jacob Crabill @@ -45,47 +45,45 @@ /// For use with PR-16808 once merged // #include -// DS-15 Specification Messages -#include -#include +// UDRAL Specification Messages +#include +#include #include "../DynamicPortSubscriber.hpp" class UavcanEscSubscriber : public UavcanDynamicPortSubscriber { public: - UavcanEscSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "esc", instance) { }; + UavcanEscSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "esc", instance) { }; void subscribe() override { // Subscribe to messages reg.drone.service.actuator.common.sp.Vector8.0.1 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _subj_sub._canard_sub.port_id, - reg_drone_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_service_actuator_common_sp_Vector8_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); // Subscribe to messages reg.drone.service.common.Readiness.0.1 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - static_cast(static_cast(_subj_sub._canard_sub.port_id) + 1), - reg_drone_service_common_Readiness_0_1_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_canard_sub_readiness); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + static_cast(static_cast(_subj_sub._canard_sub.port_id) + 1), + reg_udral_service_common_Readiness_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_canard_sub_readiness); }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { // Test with Yakut: // export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)" // yakut pub 22.reg.drone.service.actuator.common.sp.Vector8.0.1 '{value: [1000, 2000, 3000, 4000, 0, 0, 0, 0]}' PX4_INFO("EscCallback"); - reg_drone_service_actuator_common_sp_Vector8_0_1 esc {}; + reg_udral_service_actuator_common_sp_Vector8_0_1 esc {}; size_t esc_size_in_bits = receive.payload_size; - reg_drone_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload, + reg_udral_service_actuator_common_sp_Vector8_0_1_deserialize_(&esc, (const uint8_t *)receive.payload, &esc_size_in_bits); double val1 = static_cast(esc.value[0]); diff --git a/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp b/src/drivers/cyphal/Subscribers/udral/Gnss.hpp similarity index 83% rename from src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp rename to src/drivers/cyphal/Subscribers/udral/Gnss.hpp index d768f01ea8..589b967a84 100644 --- a/src/drivers/uavcan_v1/Subscribers/DS-015/Gnss.hpp +++ b/src/drivers/cyphal/Subscribers/udral/Gnss.hpp @@ -34,33 +34,32 @@ /** * @file Gnss.hpp * - * Defines basic functionality of UAVCAN v1 GNSS subscription + * Defines basic functionality of Cyphal GNSS subscription * * @author Jacob Crabill */ #pragma once -// DS-15 Specification Messages -#include +// UDRAL Specification Messages +#include #include "../DynamicPortSubscriber.hpp" class UavcanGnssSubscriber : public UavcanDynamicPortSubscriber { public: - UavcanGnssSubscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) : - UavcanDynamicPortSubscriber(ins, pmgr, "ds_015.", "gps", instance) { }; + UavcanGnssSubscriber(CanardHandle &handle, UavcanParamManager &pmgr, uint8_t instance = 0) : + UavcanDynamicPortSubscriber(handle, pmgr, "udral.", "gps", instance) { }; void subscribe() override { // Subscribe to messages reg.drone.physics.kinematics.geodetic.Point.0.1 - canardRxSubscribe(&_canard_instance, - CanardTransferKindMessage, - _subj_sub._canard_sub.port_id, - reg_drone_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_, - CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, - &_subj_sub._canard_sub); + _canard_handle.RxSubscribe(CanardTransferKindMessage, + _subj_sub._canard_sub.port_id, + reg_udral_physics_kinematics_geodetic_Point_0_1_EXTENT_BYTES_, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_subj_sub._canard_sub); /** TODO: Add additional GPS-data messages: (reg.drone.service.gnss._.0.1.uavcan): * # A compliant implementation of this service should publish the following subjects: @@ -78,16 +77,16 @@ public: */ }; - void callback(const CanardTransfer &receive) override + void callback(const CanardRxTransfer &receive) override { // Test with Yakut: // export YAKUT_TRANSPORT="pyuavcan.transport.can.CANTransport(pyuavcan.transport.can.media.slcan.SLCANMedia('/dev/serial/by-id/usb-Zubax_Robotics_Zubax_Babel_23002B000E514E413431302000000000-if00', 8, 115200), 42)" // yakut pub 1500.reg.drone.physics.kinematics.geodetic.Point.0.1 '{latitude: 1.234, longitude: 2.34, altitude: {meter: 0.5}}' PX4_INFO("GpsCallback"); - reg_drone_physics_kinematics_geodetic_Point_0_1 geo {}; + reg_udral_physics_kinematics_geodetic_Point_0_1 geo {}; size_t geo_size_in_bits = receive.payload_size; - reg_drone_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits); + reg_udral_physics_kinematics_geodetic_Point_0_1_deserialize_(&geo, (const uint8_t *)receive.payload, &geo_size_in_bits); double lat = geo.latitude; double lon = geo.longitude; diff --git a/src/drivers/uavcan_v1/SubscriptionManager.cpp b/src/drivers/cyphal/SubscriptionManager.cpp similarity index 97% rename from src/drivers/uavcan_v1/SubscriptionManager.cpp rename to src/drivers/cyphal/SubscriptionManager.cpp index 2801383a80..3f50850f0f 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.cpp +++ b/src/drivers/cyphal/SubscriptionManager.cpp @@ -59,7 +59,7 @@ void SubscriptionManager::subscribe() { _heartbeat_sub.subscribe(); -#if CONFIG_UAVCAN_V1_GETINFO_RESPONDER +#if CONFIG_CYPHAL_GETINFO_RESPONDER _getinfo_rsp.subscribe(); #endif @@ -101,7 +101,7 @@ void SubscriptionManager::updateDynamicSubscriptions() uint16_t port_id = value.natural16.value.elements[0]; if (port_id <= CANARD_PORT_ID_MAX) { // PortID is set, create a subscriber - dynsub = sub.create_sub(_canard_instance, _param_manager); + dynsub = sub.create_sub(_canard_handle, _param_manager); if (dynsub == nullptr) { PX4_ERR("Out of memory"); diff --git a/src/drivers/uavcan_v1/SubscriptionManager.hpp b/src/drivers/cyphal/SubscriptionManager.hpp similarity index 56% rename from src/drivers/uavcan_v1/SubscriptionManager.hpp rename to src/drivers/cyphal/SubscriptionManager.hpp index 519d5d12be..1e25a65d52 100644 --- a/src/drivers/uavcan_v1/SubscriptionManager.hpp +++ b/src/drivers/cyphal/SubscriptionManager.hpp @@ -41,33 +41,33 @@ #pragma once -#ifndef CONFIG_UAVCAN_V1_ESC_SUBSCRIBER -#define CONFIG_UAVCAN_V1_ESC_SUBSCRIBER 0 +#ifndef CONFIG_CYPHAL_ESC_SUBSCRIBER +#define CONFIG_CYPHAL_ESC_SUBSCRIBER 0 #endif -#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 -#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 0 +#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 +#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 0 #endif -#ifndef CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 -#define CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 0 +#ifndef CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 +#define CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 0 #endif -#ifndef CONFIG_UAVCAN_V1_BMS_SUBSCRIBER -#define CONFIG_UAVCAN_V1_BMS_SUBSCRIBER 0 +#ifndef CONFIG_CYPHAL_BMS_SUBSCRIBER +#define CONFIG_CYPHAL_BMS_SUBSCRIBER 0 #endif -#ifndef CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER -#define CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER 0 +#ifndef CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER +#define CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER 0 #endif /* Preprocessor calculation of Subscribers count */ -#define UAVCAN_SUB_COUNT CONFIG_UAVCAN_V1_ESC_SUBSCRIBER + \ - CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 + \ - CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 + \ - CONFIG_UAVCAN_V1_BMS_SUBSCRIBER + \ - CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER +#define UAVCAN_SUB_COUNT CONFIG_CYPHAL_ESC_SUBSCRIBER + \ + CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 + \ + CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 + \ + CONFIG_CYPHAL_BMS_SUBSCRIBER + \ + CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER #include #include @@ -79,14 +79,14 @@ #include "ServiceClients/List.hpp" #include "Subscribers/BaseSubscriber.hpp" #include "Subscribers/Heartbeat.hpp" -#include "Subscribers/DS-015/Battery.hpp" -#include "Subscribers/DS-015/Esc.hpp" -#include "Subscribers/DS-015/Gnss.hpp" +#include "Subscribers/udral/Battery.hpp" +#include "Subscribers/udral/Esc.hpp" +#include "Subscribers/udral/Gnss.hpp" #include "Subscribers/legacy/LegacyBatteryInfo.hpp" #include "Subscribers/uORB/uorb_subscriber.hpp" typedef struct { - UavcanDynamicPortSubscriber *(*create_sub)(CanardInstance &ins, UavcanParamManager &pmgr) {}; + UavcanDynamicPortSubscriber *(*create_sub)(CanardHandle &handle, UavcanParamManager &pmgr) {}; const char *subject_name; const uint8_t instance; } UavcanDynSubBinder; @@ -94,7 +94,7 @@ typedef struct { class SubscriptionManager { public: - SubscriptionManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _param_manager(pmgr) {} + SubscriptionManager(CanardHandle &handle, UavcanParamManager &pmgr) : _canard_handle(handle), _param_manager(pmgr) {} ~SubscriptionManager(); void subscribe(); @@ -104,57 +104,57 @@ public: private: void updateDynamicSubscriptions(); - CanardInstance &_canard_instance; + CanardHandle &_canard_handle; UavcanParamManager &_param_manager; UavcanDynamicPortSubscriber *_dynsubscribers {nullptr}; - UavcanHeartbeatSubscriber _heartbeat_sub {_canard_instance}; + UavcanHeartbeatSubscriber _heartbeat_sub {_canard_handle}; -#if CONFIG_UAVCAN_V1_GETINFO_RESPONDER +#if CONFIG_CYPHAL_GETINFO_RESPONDER // GetInfo response - UavcanGetInfoResponse _getinfo_rsp {_canard_instance}; + UavcanGetInfoResponse _getinfo_rsp {_canard_handle}; #endif // Process register requests - UavcanAccessResponse _access_rsp {_canard_instance, _param_manager}; - UavcanListResponse _list_rsp {_canard_instance, _param_manager}; + UavcanAccessResponse _access_rsp {_canard_handle, _param_manager}; + UavcanListResponse _list_rsp {_canard_handle, _param_manager}; const UavcanDynSubBinder _uavcan_subs[UAVCAN_SUB_COUNT] { -#if CONFIG_UAVCAN_V1_ESC_SUBSCRIBER +#if CONFIG_CYPHAL_ESC_SUBSCRIBER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanEscSubscriber(ins, pmgr, 0); + return new UavcanEscSubscriber(handle, pmgr, 0); }, "esc", 0 }, #endif -#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_0 +#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_0 { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanGnssSubscriber(ins, pmgr, 0); + return new UavcanGnssSubscriber(handle, pmgr, 0); }, "gps", 0 }, #endif -#if CONFIG_UAVCAN_V1_GNSS_SUBSCRIBER_1 //FIXME decouple instanceing +#if CONFIG_CYPHAL_GNSS_SUBSCRIBER_1 //FIXME decouple handletanceing { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanGnssSubscriber(ins, pmgr, 1); + return new UavcanGnssSubscriber(handle, pmgr, 1); }, "gps", 1 }, #endif -#if CONFIG_UAVCAN_V1_BMS_SUBSCRIBER +#if CONFIG_CYPHAL_BMS_SUBSCRIBER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanBmsSubscriber(ins, pmgr, 0); + return new UavcanBmsSubscriber(handle, pmgr, 0); }, "energy_source", 0 @@ -162,19 +162,19 @@ private: #endif #if 0 //Obsolete to be removed { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new UavcanLegacyBatteryInfoSubscriber(ins, pmgr, 0); + return new UavcanLegacyBatteryInfoSubscriber(handle, pmgr, 0); }, "legacy_bms", 0 }, #endif -#if CONFIG_UAVCAN_V1_UORB_SENSOR_GPS_SUBSCRIBER +#if CONFIG_CYPHAL_UORB_SENSOR_GPS_SUBSCRIBER { - [](CanardInstance & ins, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * + [](CanardHandle & handle, UavcanParamManager & pmgr) -> UavcanDynamicPortSubscriber * { - return new uORB_over_UAVCAN_Subscriber(ins, pmgr, ORB_ID(sensor_gps)); + return new uORB_over_UAVCAN_Subscriber(handle, pmgr, ORB_ID(sensor_gps)); }, "uorb.sensor_gps", 0 diff --git a/src/drivers/uavcan_v1/legacy_data_types b/src/drivers/cyphal/legacy_data_types similarity index 100% rename from src/drivers/uavcan_v1/legacy_data_types rename to src/drivers/cyphal/legacy_data_types diff --git a/src/drivers/cyphal/libcanard b/src/drivers/cyphal/libcanard new file mode 160000 index 0000000000..db87ea32aa --- /dev/null +++ b/src/drivers/cyphal/libcanard @@ -0,0 +1 @@ +Subproject commit db87ea32aa092c48ea103963138b6346dd3e9008 diff --git a/src/drivers/uavcan_v1/module.yaml b/src/drivers/cyphal/module.yaml similarity index 99% rename from src/drivers/uavcan_v1/module.yaml rename to src/drivers/cyphal/module.yaml index abd941df63..0b4f60e602 100644 --- a/src/drivers/uavcan_v1/module.yaml +++ b/src/drivers/cyphal/module.yaml @@ -8,4 +8,3 @@ actuator_output: max: { min: 0, max: 8191, default: 8191 } failsafe: { min: 0, max: 8191 } num_channels: 16 - diff --git a/src/drivers/uavcan_v1/o1heap/o1heap.c b/src/drivers/cyphal/o1heap/o1heap.c similarity index 100% rename from src/drivers/uavcan_v1/o1heap/o1heap.c rename to src/drivers/cyphal/o1heap/o1heap.c diff --git a/src/drivers/uavcan_v1/o1heap/o1heap.h b/src/drivers/cyphal/o1heap/o1heap.h similarity index 100% rename from src/drivers/uavcan_v1/o1heap/o1heap.h rename to src/drivers/cyphal/o1heap/o1heap.h diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/cyphal/parameters.c similarity index 76% rename from src/drivers/uavcan_v1/parameters.c rename to src/drivers/cyphal/parameters.c index 67b5741b0a..52d3db5b35 100644 --- a/src/drivers/uavcan_v1/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -32,28 +32,28 @@ ****************************************************************************/ /** - * UAVCAN v1 + * Cyphal * - * 0 - UAVCAN disabled. - * 1 - Enables UAVCANv1 + * 0 - Cyphal disabled. + * 1 - Enables Cyphal * * @boolean * @reboot_required true - * @group UAVCAN v1 + * @group Cyphal */ -PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0); +PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0); /** - * UAVCAN v1 Node ID. + * Cyphal Node ID. * * Read the specs at http://uavcan.org to learn more about Node ID. * * @min -1 * @max 125 * @reboot_required true - * @group UAVCANv1 + * @group Cyphal */ -PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1); +PARAM_DEFINE_INT32(CYPHAL_ID, 1); /** * UAVCAN/CAN v1 bus bitrate. @@ -62,9 +62,9 @@ PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1); * @min 20000 * @max 1000000 * @reboot_required true - * @group UAVCAN v1 + * @group Cyphal */ -PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000); +PARAM_DEFINE_INT32(CYPHAL_BAUD, 1000000); /* Subscription port ID, -1 will be treated as unset */ @@ -73,7 +73,7 @@ PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000); * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_ESC0_SUB, -1); @@ -82,7 +82,7 @@ PARAM_DEFINE_INT32(UCAN1_ESC0_SUB, -1); * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_GPS0_SUB, -1); @@ -91,100 +91,100 @@ PARAM_DEFINE_INT32(UCAN1_GPS0_SUB, -1); * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_GPS1_SUB, -1); /** - * DS-015 battery energy source subscription port ID. + * UDRAL battery energy source subscription port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_BMS_ES_SUB, -1); /** - * DS-015 battery status subscription port ID. + * UDRAL battery status subscription port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_BMS_BS_SUB, -1); /** - * DS-015 battery parameters subscription port ID. + * UDRAL battery parameters subscription port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_BMS_BP_SUB, -1); /** - * UAVCAN v1 leagcy battery port ID. + * Cyphal leagcy battery port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_LG_BMS_SUB, -1); /** - * sensor_gps uORB over UAVCAN v1 subscription port ID. + * sensor_gps uORB over Cyphal subscription port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_UORB_GPS, -1); /** - * sensor_gps uORB over UAVCAN v1 publication port ID. + * sensor_gps uORB over Cyphal publication port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_UORB_GPS_P, -1); // Publication Port IDs /** - * UAVCAN v1 ESC publication port ID. + * Cyphal ESC publication port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_ESC_PUB, -1); /** - * UAVCAN v1 GPS publication port ID. + * Cyphal GPS publication port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_GPS_PUB, -1); /** - * UAVCAN v1 Servo publication port ID. + * Cyphal Servo publication port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, -1); /** - * actuator_outputs uORB over UAVCAN v1 publication port ID. + * actuator_outputs uORB over Cyphal publication port ID. * * @min -1 * @max 6143 - * @group UAVCAN v1 + * @group Cyphal */ PARAM_DEFINE_INT32(UCAN1_ACTR_PUB, -1); diff --git a/src/drivers/cyphal/public_regulated_data_types b/src/drivers/cyphal/public_regulated_data_types new file mode 160000 index 0000000000..d0bd6516da --- /dev/null +++ b/src/drivers/cyphal/public_regulated_data_types @@ -0,0 +1 @@ +Subproject commit d0bd6516dac8ff61287fe49a9f2c75e7d4dc1b8e diff --git a/src/drivers/uavcan_v1/libcanard b/src/drivers/uavcan_v1/libcanard deleted file mode 160000 index 2d449453fc..0000000000 --- a/src/drivers/uavcan_v1/libcanard +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 2d449453fc8c4060f276c6dc585d4e1e5bf4fd52 diff --git a/src/drivers/uavcan_v1/public_regulated_data_types b/src/drivers/uavcan_v1/public_regulated_data_types deleted file mode 160000 index 0a773b93ce..0000000000 --- a/src/drivers/uavcan_v1/public_regulated_data_types +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 0a773b93ce5c94e1d2791b180058cb9897fab7e1