From 58ca575871e732305e06a1243a255f0542bc9f25 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 5 May 2020 19:18:59 -0400 Subject: [PATCH] UAVCAN v1 bridge - NuttX stm32f4/stm32f7 uses character device driver - NuttX kinetis and s32k uses socketcan --- .gitmodules | 6 + Tools/astyle/files_to_check_code_style.sh | 1 + Tools/setup/requirements.txt | 1 + boards/nxp/fmuk66-v3/socketcan.cmake | 2 +- boards/nxp/ucans32k146/default.cmake | 1 + boards/px4/fmu-v4/default.cmake | 3 +- boards/px4/fmu-v4/nuttx-config/nsh/defconfig | 3 + boards/px4/fmu-v5/default.cmake | 3 +- boards/px4/fmu-v5/nuttx-config/nsh/defconfig | 6 + src/drivers/uavcan_v1/CMakeLists.txt | 91 ++++ src/drivers/uavcan_v1/CanardInterface.hpp | 60 +++ src/drivers/uavcan_v1/CanardNuttXCDev.cpp | 151 ++++++ src/drivers/uavcan_v1/CanardNuttXCDev.hpp | 67 +++ src/drivers/uavcan_v1/CanardSocketCAN.cpp | 199 +++++++ src/drivers/uavcan_v1/CanardSocketCAN.hpp | 96 ++++ src/drivers/uavcan_v1/Uavcan.cpp | 407 ++++++++++++++ src/drivers/uavcan_v1/Uavcan.hpp | 136 +++++ src/drivers/uavcan_v1/libcanard | 1 + src/drivers/uavcan_v1/o1heap/o1heap.c | 498 ++++++++++++++++++ src/drivers/uavcan_v1/o1heap/o1heap.h | 142 +++++ src/drivers/uavcan_v1/parameters.c | 87 +++ .../uavcan_v1/public_regulated_data_types | 1 + src/drivers/uavcan_v1/templates/Header.j2 | 32 ++ .../uavcan_v1/templates/ServiceType.j2 | 10 + .../uavcan_v1/templates/StructureType.j2 | 5 + src/drivers/uavcan_v1/templates/UnionType.j2 | 5 + .../uavcan_v1/templates/_composite_type.j2 | 115 ++++ 27 files changed, 2126 insertions(+), 3 deletions(-) create mode 100644 src/drivers/uavcan_v1/CMakeLists.txt create mode 100644 src/drivers/uavcan_v1/CanardInterface.hpp create mode 100644 src/drivers/uavcan_v1/CanardNuttXCDev.cpp create mode 100644 src/drivers/uavcan_v1/CanardNuttXCDev.hpp create mode 100644 src/drivers/uavcan_v1/CanardSocketCAN.cpp create mode 100644 src/drivers/uavcan_v1/CanardSocketCAN.hpp create mode 100644 src/drivers/uavcan_v1/Uavcan.cpp create mode 100644 src/drivers/uavcan_v1/Uavcan.hpp create mode 160000 src/drivers/uavcan_v1/libcanard create mode 100644 src/drivers/uavcan_v1/o1heap/o1heap.c create mode 100644 src/drivers/uavcan_v1/o1heap/o1heap.h create mode 100644 src/drivers/uavcan_v1/parameters.c create mode 160000 src/drivers/uavcan_v1/public_regulated_data_types create mode 100644 src/drivers/uavcan_v1/templates/Header.j2 create mode 100644 src/drivers/uavcan_v1/templates/ServiceType.j2 create mode 100644 src/drivers/uavcan_v1/templates/StructureType.j2 create mode 100644 src/drivers/uavcan_v1/templates/UnionType.j2 create mode 100644 src/drivers/uavcan_v1/templates/_composite_type.j2 diff --git a/.gitmodules b/.gitmodules index 1ac587bcd7..899a3ee641 100644 --- a/.gitmodules +++ b/.gitmodules @@ -30,6 +30,12 @@ path = src/drivers/gps/devices url = https://github.com/PX4/PX4-GPSDrivers.git branch = master +[submodule "src/drivers/uavcan_v1/libcanard"] + path = src/drivers/uavcan_v1/libcanard + url = https://github.com/PX4/libcanard.git +[submodule "src/drivers/uavcan_v1/public_regulated_data_types"] + path = src/drivers/uavcan_v1/public_regulated_data_types + url = https://github.com/PX4/public_regulated_data_types.git [submodule "src/modules/micrortps_bridge/micro-CDR"] path = src/modules/micrortps_bridge/micro-CDR url = https://github.com/PX4/Micro-CDR.git diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index 1418e7450b..9032d8495a 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -12,6 +12,7 @@ exec find boards msg src platforms test \ -path platforms/nuttx/NuttX -prune -o \ -path platforms/qurt/dspal -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ + -path src/drivers/uavcan_v1/libcanard -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ -path src/lib/ecl -prune -o \ -path src/lib/matrix -prune -o \ diff --git a/Tools/setup/requirements.txt b/Tools/setup/requirements.txt index d264e06a3f..f7e322d14f 100644 --- a/Tools/setup/requirements.txt +++ b/Tools/setup/requirements.txt @@ -6,6 +6,7 @@ empy>=3.3 jinja2>=2.8 matplotlib>=3.0.* numpy>=1.13 +nunavut packaging pandas>=0.21 pkgconfig diff --git a/boards/nxp/fmuk66-v3/socketcan.cmake b/boards/nxp/fmuk66-v3/socketcan.cmake index 76d1068ba9..d8cd18d5a6 100644 --- a/boards/nxp/fmuk66-v3/socketcan.cmake +++ b/boards/nxp/fmuk66-v3/socketcan.cmake @@ -50,7 +50,7 @@ px4_add_board( telemetry # all available telemetry drivers #test_ppm # NOT Portable YET tone_alarm - #uavcannode_v1 + uavcan_v1 MODULES airspeed_selector attitude_estimator_q diff --git a/boards/nxp/ucans32k146/default.cmake b/boards/nxp/ucans32k146/default.cmake index faf3826e59..0a4f02a3ae 100644 --- a/boards/nxp/ucans32k146/default.cmake +++ b/boards/nxp/ucans32k146/default.cmake @@ -45,6 +45,7 @@ px4_add_board( #safety_button #tone_alarm #uavcannode # TODO: CAN driver needed + uavcan_v1 MODULES #ekf2 #load_mon diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake index 3d657b650b..2e0d5531ae 100644 --- a/boards/px4/fmu-v4/default.cmake +++ b/boards/px4/fmu-v4/default.cmake @@ -58,7 +58,8 @@ px4_add_board( telemetry # all available telemetry drivers test_ppm tone_alarm - uavcan + #uavcan + uavcan_v1 MODULES airspeed_selector attitude_estimator_q diff --git a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig index d135e4dd34..c7f276eeb7 100644 --- a/boards/px4/fmu-v4/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v4/nuttx-config/nsh/defconfig @@ -40,6 +40,7 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y +CONFIG_CAN_EXTID=y CONFIG_CDCACM=y CONFIG_CDCACM_PRODUCTID=0x0012 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v4.x" @@ -150,6 +151,8 @@ CONFIG_STM32_ADC1=y CONFIG_STM32_BBSRAM=y CONFIG_STM32_BBSRAM_FILES=5 CONFIG_STM32_BKPSRAM=y +CONFIG_STM32_CAN1=y +CONFIG_STM32_CAN1_BAUD=1000000 CONFIG_STM32_CCMDATARAM=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y CONFIG_STM32_DMA1=y diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake index 8c9f8519be..e860c3f90b 100644 --- a/boards/px4/fmu-v5/default.cmake +++ b/boards/px4/fmu-v5/default.cmake @@ -60,7 +60,8 @@ px4_add_board( telemetry # all available telemetry drivers test_ppm tone_alarm - uavcan + #uavcan # legacy v0 + uavcan_v1 MODULES airspeed_selector attitude_estimator_q diff --git a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig index 202dcbbff1..9670132e73 100644 --- a/boards/px4/fmu-v5/nuttx-config/nsh/defconfig +++ b/boards/px4/fmu-v5/nuttx-config/nsh/defconfig @@ -44,6 +44,8 @@ CONFIG_BOARD_LOOPSPERMSEC=22114 CONFIG_BOARD_RESET_ON_ASSERT=2 CONFIG_BUILTIN=y CONFIG_C99_BOOL8=y +CONFIG_CAN=y +CONFIG_CAN_EXTID=y CONFIG_CDCACM=y CONFIG_CDCACM_PRODUCTID=0x0032 CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5.x" @@ -156,6 +158,10 @@ CONFIG_STM32F7_ADC1=y CONFIG_STM32F7_BBSRAM=y CONFIG_STM32F7_BBSRAM_FILES=5 CONFIG_STM32F7_BKPSRAM=y +CONFIG_STM32F7_CAN1=y +CONFIG_STM32F7_CAN1_BAUD=1000000 +CONFIG_STM32F7_CAN_TSEG1=7 +CONFIG_STM32F7_CAN_TSEG2=1 CONFIG_STM32F7_DMA1=y CONFIG_STM32F7_DMA2=y CONFIG_STM32F7_DMACAPABLE=y diff --git a/src/drivers/uavcan_v1/CMakeLists.txt b/src/drivers/uavcan_v1/CMakeLists.txt new file mode 100644 index 0000000000..8f0e956381 --- /dev/null +++ b/src/drivers/uavcan_v1/CMakeLists.txt @@ -0,0 +1,91 @@ +############################################################################ +# +# 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. +# +############################################################################ + +set(LIBCANARD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/libcanard) +set(DSDL_DIR ${CMAKE_CURRENT_SOURCE_DIR}/public_regulated_data_types) + +px4_add_git_submodule(TARGET git_libcanard PATH ${LIBCANARD_DIR}) +px4_add_git_submodule(TARGET git_public_regulated_data_types PATH ${DSDL_DIR}) + +find_program(NNVG_PATH nnvg) +if(NNVG_PATH) + execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp -I ${DSDL_DIR}/uavcan ${DSDL_DIR}/regulated) + execute_process(COMMAND ${NNVG_PATH} --templates ${CMAKE_CURRENT_SOURCE_DIR}/templates --outdir ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated --target-language cpp ${DSDL_DIR}/uavcan) +else() + message(FATAL_ERROR "UAVCAN Nunavut nnvg not found") +endif() + +add_definitions( + -DCANARD_DSDL_CONFIG_LITTLE_ENDIAN=1 +) + +set(SRCS) +if(${PX4_PLATFORM} MATCHES "nuttx") + if(CONFIG_NET_CAN) + list(APPEND SRCS + CanardSocketCAN.cpp + CanardSocketCAN.hpp + ) + elseif(CONFIG_CAN_EXTID) + list(APPEND SRCS + CanardNuttXCDev.cpp + CanardNuttXCDev.hpp + ) + endif() +endif() + +px4_add_module( + MODULE drivers__uavcan_v1 + MAIN uavcan_v1 + COMPILE_FLAGS + #-DCANARD_ASSERT + -DUINT32_C\(x\)=__UINT32_C\(x\) + -O0 + INCLUDES + ${LIBCANARD_DIR}/libcanard/ + ${CMAKE_CURRENT_BINARY_DIR}/dsdlc_generated + SRCS + Uavcan.cpp + Uavcan.hpp + ${SRCS} + o1heap/o1heap.c + o1heap/o1heap.h + ${LIBCANARD_DIR}/libcanard/canard_dsdl.c + ${LIBCANARD_DIR}/libcanard/canard_dsdl.h + ${LIBCANARD_DIR}/libcanard/canard.c + ${LIBCANARD_DIR}/libcanard/canard.h + DEPENDS + git_libcanard + git_public_regulated_data_types + version + ) diff --git a/src/drivers/uavcan_v1/CanardInterface.hpp b/src/drivers/uavcan_v1/CanardInterface.hpp new file mode 100644 index 0000000000..088600a18a --- /dev/null +++ b/src/drivers/uavcan_v1/CanardInterface.hpp @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +class CanardInterface +{ +public: + CanardInterface() = default; + virtual ~CanardInterface() = default; + + virtual int init() { return 0; }; + + virtual int close() { return 0; }; + + /// 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; + + /// 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; + +private: + +}; diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.cpp b/src/drivers/uavcan_v1/CanardNuttXCDev.cpp new file mode 100644 index 0000000000..f854cbf60e --- /dev/null +++ b/src/drivers/uavcan_v1/CanardNuttXCDev.cpp @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 "CanardNuttXCDev.hpp" + +#include +#include + +#include +#include + +#include "stm32_can.h" + +#include + +int CanardNuttXCDev::init() +{ + struct can_dev_s *can = stm32_caninitialize(1); + + if (can == nullptr) { + PX4_ERR("Failed to get CAN interface"); + + } else { + /* Register the CAN driver at "/dev/can0" */ + int ret = can_register("/dev/can0", can); + + if (ret < 0) { + PX4_ERR("can_register failed: %d", ret); + + } else { + _fd = ::open("/dev/can0", O_RDWR | O_NONBLOCK); + } + } + + return 0; +} + +int16_t CanardNuttXCDev::transmit(const CanardFrame &txf, int timeout_ms) +{ + if (_fd < 0) { + return -1; + } + + struct pollfd fds {}; + + fds.fd = _fd; + + fds.events |= POLLOUT; + + const int poll_result = poll(&fds, 1, timeout_ms); + + if (poll_result < 0) { + return -1; + } + + if (poll_result == 0) { + return 0; + } + + if ((fds.revents & POLLOUT) == 0) { + return -1; + } + + struct can_msg_s transmit_msg {}; + + transmit_msg.cm_hdr.ch_id = txf.extended_can_id; + + transmit_msg.cm_hdr.ch_dlc = txf.payload_size; + + transmit_msg.cm_hdr.ch_extid = 1; + + memcpy(transmit_msg.cm_data, txf.payload, txf.payload_size); + + const size_t msg_len = CAN_MSGLEN(transmit_msg.cm_hdr.ch_dlc); + + const ssize_t nbytes = ::write(_fd, &transmit_msg, msg_len); + + if (nbytes < 0 || (size_t)nbytes != msg_len) { + return -1; + } + + return 1; +} + +int16_t CanardNuttXCDev::receive(CanardFrame *received_frame) +{ + if ((_fd < 0) || (received_frame == nullptr)) { + return -1; + } + + // File desriptor for CAN. + struct pollfd fds {}; + fds.fd = _fd; + fds.events = POLLIN; + + // Any recieved CAN messages will cause the poll statement to unblock and run + // This way CAN read runs with minimal latency. + // Note that multiple messages may be received in a short time, so this will try to read any availible in a loop + ::poll(&fds, 1, 10); + + // Only execute this part if can0 is changed. + if (fds.revents & POLLIN) { + + // Try to read. + struct can_msg_s receive_msg; + const ssize_t nbytes = ::read(fds.fd, &receive_msg, sizeof(receive_msg)); + + if (nbytes < 0 || (size_t)nbytes < CAN_MSGLEN(0) || (size_t)nbytes > sizeof(receive_msg)) { + // error + 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); + return nbytes; + } + } + + return 0; +} diff --git a/src/drivers/uavcan_v1/CanardNuttXCDev.hpp b/src/drivers/uavcan_v1/CanardNuttXCDev.hpp new file mode 100644 index 0000000000..6f0b79a4e5 --- /dev/null +++ b/src/drivers/uavcan_v1/CanardNuttXCDev.hpp @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#include "CanardInterface.hpp" + +class CanardNuttXCDev : public CanardInterface +{ +public: + CanardNuttXCDev() = default; + ~CanardNuttXCDev() override = default; + + /// Creates a SocketCAN socket for corresponding iface can_iface_name + /// Also sets up the message structures required for socketcanTransmit & socketcanReceive + /// can_fd determines to use CAN FD frame when is 1, and classical CAN frame when is 0 + /// The return value is 0 on succes and -1 on error + int init(); + + /// 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); + + /// 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); + +private: + int _fd{-1}; + bool _can_fd{false}; +}; diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.cpp b/src/drivers/uavcan_v1/CanardSocketCAN.cpp new file mode 100644 index 0000000000..b85e510c8b --- /dev/null +++ b/src/drivers/uavcan_v1/CanardSocketCAN.cpp @@ -0,0 +1,199 @@ +/**************************************************************************** + * + * 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 "CanardSocketCAN.hpp" + +#include +#include +#include + +#include + +int CanardSocketCAN::init() +{ + const char *const can_iface_name = "can0"; + + struct sockaddr_can addr; + struct ifreq ifr; + + //FIXME HOTFIX to make this code compile + bool can_fd = 0; + + _can_fd = can_fd; + + /* open socket */ + if ((_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) { + PX4_ERR("socket"); + return -1; + } + + strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + ifr.ifr_ifindex = if_nametoindex(ifr.ifr_name); + + if (!ifr.ifr_ifindex) { + PX4_ERR("if_nametoindex"); + return -1; + } + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + const int on = 1; + /* RX Timestamping */ + + if (setsockopt(_fd, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { + PX4_ERR("SO_TIMESTAMP is disabled"); + return -1; + } + + /* NuttX Feature: Enable TX deadline when sending CAN frames + * When a deadline occurs the driver will remove the CAN frame + */ + + if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_TX_DEADLINE, &on, sizeof(on)) < 0) { + PX4_ERR("CAN_RAW_TX_DEADLINE is disabled"); + return -1; + } + + if (can_fd) { + if (setsockopt(_fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &on, sizeof(on)) < 0) { + PX4_ERR("no CAN FD support"); + return -1; + } + } + + if (bind(_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { + PX4_ERR("bind"); + return -1; + } + + // Setup TX msg + _send_iov.iov_base = &_send_frame; + + if (_can_fd) { + _send_iov.iov_len = sizeof(struct canfd_frame); + + } else { + _send_iov.iov_len = sizeof(struct can_frame); + } + + memset(&_send_control, 0x00, sizeof(_send_control)); + + _send_msg.msg_iov = &_send_iov; + _send_msg.msg_iovlen = 1; + _send_msg.msg_control = &_send_control; + _send_msg.msg_controllen = sizeof(_send_control); + + _send_cmsg = CMSG_FIRSTHDR(&_send_msg); + _send_cmsg->cmsg_level = SOL_CAN_RAW; + _send_cmsg->cmsg_type = CAN_RAW_TX_DEADLINE; + _send_cmsg->cmsg_len = sizeof(struct timeval); + _send_tv = (struct timeval *)CMSG_DATA(&_send_cmsg); + + // Setup RX msg + _recv_iov.iov_base = &_recv_frame; + + if (can_fd) { + _recv_iov.iov_len = sizeof(struct canfd_frame); + + } else { + _recv_iov.iov_len = sizeof(struct can_frame); + } + + memset(_recv_control, 0x00, sizeof(_recv_control)); + + _recv_msg.msg_iov = &_recv_iov; + _recv_msg.msg_iovlen = 1; + _recv_msg.msg_control = &_recv_control; + _recv_msg.msg_controllen = sizeof(_recv_control); + _recv_cmsg = CMSG_FIRSTHDR(&_recv_msg); + + return 0; +} + +int16_t CanardSocketCAN::transmit(const CanardFrame &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); + + } 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); + } + + /* Set CAN_RAW_TX_DEADLINE timestamp */ + _send_tv->tv_usec = txf.timestamp_usec % 1000000ULL; + _send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL; + + return sendmsg(_fd, &_send_msg, 0); +} + +int16_t CanardSocketCAN::receive(CanardFrame *rxf) +{ + int32_t result = recvmsg(_fd, &_recv_msg, 0); + + if (result < 0) { + return result; + } + + /* Copy CAN frame to CanardFrame */ + + 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; + + } 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 + } + + /* Read SO_TIMESTAMP value */ + + if (_recv_cmsg->cmsg_level == SOL_SOCKET && _recv_cmsg->cmsg_type == SO_TIMESTAMP) { + struct timeval *tv = (struct timeval *)CMSG_DATA(_recv_cmsg); + rxf->timestamp_usec = tv->tv_sec * 1000000ULL + tv->tv_usec; + } + + return result; +} diff --git a/src/drivers/uavcan_v1/CanardSocketCAN.hpp b/src/drivers/uavcan_v1/CanardSocketCAN.hpp new file mode 100644 index 0000000000..58f2e19001 --- /dev/null +++ b/src/drivers/uavcan_v1/CanardSocketCAN.hpp @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include + +#include "CanardInterface.hpp" + +class CanardSocketCAN : public CanardInterface +{ +public: + CanardSocketCAN() = default; + ~CanardSocketCAN() override = default; + + /// Creates a SocketCAN socket for corresponding iface can_iface_name + /// Also sets up the message structures required for socketcanTransmit & socketcanReceive + /// can_fd determines to use CAN FD frame when is 1, and classical CAN frame when is 0 + /// The return value is 0 on succes and -1 on error + int init(); + + /// 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); + + /// 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); + + // TODO implement ioctl for CAN filter + //int16_t socketcanConfigureFilter(const fd_t fd, const size_t num_filters, const struct can_filter *filters); + +private: + + int _fd{-1}; + bool _can_fd{false}; + + //// Send msg structure + struct iovec _send_iov {}; + struct canfd_frame _send_frame {}; + struct msghdr _send_msg {}; + struct cmsghdr *_send_cmsg {}; + struct timeval *_send_tv {}; /* TX deadline timestamp */ + uint8_t _send_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {}; + + //// Receive msg structure + struct iovec _recv_iov {}; + struct canfd_frame _recv_frame {}; + struct msghdr _recv_msg {}; + struct cmsghdr *_recv_cmsg {}; + uint8_t _recv_control[sizeof(struct cmsghdr) + sizeof(struct timeval)] {}; +}; diff --git a/src/drivers/uavcan_v1/Uavcan.cpp b/src/drivers/uavcan_v1/Uavcan.cpp new file mode 100644 index 0000000000..af245c6e8d --- /dev/null +++ b/src/drivers/uavcan_v1/Uavcan.cpp @@ -0,0 +1,407 @@ +/**************************************************************************** + * + * 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 "Uavcan.hpp" + +#include +#include + +#define REGULATED_DRONE_SENSOR_BMSSTATUS_ID 32080 + +using namespace time_literals; + +UavcanNode *UavcanNode::_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) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), + _can_interface(interface) +{ + 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 %d", 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; + } +} + +UavcanNode::~UavcanNode() +{ + delete _can_interface; + + if (_instance) { + /* tell the task we want it to go away */ + _task_should_exit.store(true); + ScheduleNow(); + + unsigned i = 10; + + do { + /* wait 5ms - it should wake every 10ms or so worst-case */ + usleep(5000); + + if (--i == 0) { + break; + } + + } while (_instance); + } + + perf_free(_cycle_perf); + perf_free(_interval_perf); + + //delete _uavcan_heap; +} + +int UavcanNode::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 + + _instance = new UavcanNode(interface, node_id); + + if (_instance == nullptr) { + PX4_ERR("Out of memory"); + return -1; + } + + // Keep the bit rate for reboots on BenginFirmware updates + _instance->active_bitrate = bitrate; + + _instance->ScheduleOnInterval(ScheduleIntervalMs * 1000); + + return PX4_OK; +} + +void UavcanNode::Run() +{ + pthread_mutex_lock(&_node_mutex); + + if (!_initialized) { + // interface init + if (_can_interface) { + if (_can_interface->init() == PX4_OK) { + + // Subscribe to messages uavcan.node.Heartbeat. + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + uavcan::node::Heartbeat_1_0::PORT_ID, + uavcan::node::Heartbeat_1_0::SIZE, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_heartbeat_subscription); + + if (_param_uavcan_v1_bat_md.get() == 1) { + canardRxSubscribe(&_canard_instance, + CanardTransferKindMessage, + static_cast(_param_uavcan_v1_bat_id.get()), + regulated::drone::sensor::BMSStatus_1_0::SIZE, + CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC, + &_drone_sensor_BMSStatus_subscription); + } + + _initialized = true; + } + } + + // return early if still not initialized + if (!_initialized) { + pthread_mutex_unlock(&_node_mutex); + return; + } + } + + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); + + // update parameters from storage + updateParams(); + } + + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + + + // send uavcan::node::Heartbeat_1_0 @ 1 Hz + if (hrt_elapsed_time(&_uavcan_node_heartbeat_last) >= 1_s) { + + uavcan::node::Heartbeat_1_0 heartbeat{}; + heartbeat.uptime = _uavcan_node_heartbeat_transfer_id; // TODO: use real uptime + heartbeat.health = uavcan::node::Heartbeat_1_0::HEALTH_NOMINAL; + heartbeat.mode = uavcan::node::Heartbeat_1_0::MODE_OPERATIONAL; + + heartbeat.serializeToBuffer(_uavcan_node_heartbeat_buffer); + + const CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = uavcan::node::Heartbeat_1_0::PORT_ID, + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _uavcan_node_heartbeat_transfer_id++, + .payload_size = uavcan::node::Heartbeat_1_0::SIZE, + .payload = &_uavcan_node_heartbeat_buffer, + }; + + int32_t result = canardTxPush(&_canard_instance, &transfer); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + PX4_ERR("Heartbeat transmit error %d", result); + } + + _uavcan_node_heartbeat_last = transfer.timestamp_usec; + } + + // send regulated::drone::sensor::BMSStatus_1_0 @ 1 Hz + if (_param_uavcan_v1_bat_md.get() == 2) { + if (hrt_elapsed_time(&_regulated_drone_sensor_bmsstatus_last) >= 1_s) { + + battery_status_s battery_status; + + if (_battery_status_sub.update(&battery_status)) { + regulated::drone::sensor::BMSStatus_1_0 bmsstatus{}; + //bmsstatus.timestamp = battery_status.timestamp; + bmsstatus.remaining_capacity = battery_status.remaining; + + bmsstatus.serializeToBuffer(_regulated_drone_sensor_bmsstatus_buffer); + + const CanardTransfer transfer = { + .timestamp_usec = hrt_absolute_time(), + .priority = CanardPriorityNominal, + .transfer_kind = CanardTransferKindMessage, + .port_id = static_cast(_param_uavcan_v1_bat_id.get()), + .remote_node_id = CANARD_NODE_ID_UNSET, + .transfer_id = _regulated_drone_sensor_bmsstatus_transfer_id++, + .payload_size = regulated::drone::sensor::BMSStatus_1_0::SIZE, + .payload = &_regulated_drone_sensor_bmsstatus_buffer, + }; + + int32_t result = canardTxPush(&_canard_instance, &transfer); + + if (result < 0) { + // An error has occurred: either an argument is invalid or we've ran out of memory. + // It is possible to statically prove that an out-of-memory will never occur for a given application if the + // heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap. + PX4_ERR("Battery transmit error %d", result); + } + + _regulated_drone_sensor_bmsstatus_last = transfer.timestamp_usec; + } + } + } + + // Transmitting + // Look at the top of the TX queue. + for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) { + // Check if the frame has timed out. + if (hrt_absolute_time() > txf->timestamp_usec) { + // 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; + } + } + } + + uint8_t data[64] {}; + CanardFrame received_frame{}; + received_frame.payload = &data; + + if (_can_interface->receive(&received_frame) > 0) { + + CanardTransfer receive{}; + int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive); + + 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 %d\n", result); + + } else if (result == 1) { + // A transfer has been received, process it. + PX4_DEBUG("received Port ID: %d", receive.port_id); + + if (receive.port_id == static_cast(_param_uavcan_v1_bat_id.get())) { + auto bms_status = regulated::drone::sensor::BMSStatus_1_0::deserializeFromBuffer((const uint8_t *)receive.payload, + receive.payload_size); + + battery_status_s battery_status{}; + battery_status.id = bms_status.battery_id; + battery_status.remaining = bms_status.remaining_capacity; + battery_status.timestamp = hrt_absolute_time(); + _battery_status_pub.publish(battery_status); + } + + // Deallocate the dynamic memory afterwards. + _canard_instance.memory_free(&_canard_instance, (void *)receive.payload); + } + } + + perf_end(_cycle_perf); + + if (_task_should_exit.load()) { + _can_interface->close(); + + ScheduleClear(); + _instance = nullptr; + } + + pthread_mutex_unlock(&_node_mutex); +} + +void UavcanNode::print_info() +{ + pthread_mutex_lock(&_node_mutex); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + + pthread_mutex_unlock(&_node_mutex); +} + +static void print_usage() +{ + PX4_INFO("usage: \n" + "\tuavcannode {start|status|stop}"); +} + +extern "C" __EXPORT int uavcan_v1_main(int argc, char *argv[]) +{ + if (argc < 2) { + print_usage(); + return 1; + } + + if (!strcmp(argv[1], "start")) { + if (UavcanNode::instance()) { + PX4_ERR("already started"); + return 1; + } + + // CAN bitrate + int32_t bitrate = 0; + param_get(param_find("UAVCAN_V1_BAUD"), &bitrate); + + // Node ID + int32_t node_id = 0; + param_get(param_find("UAVCAN_V1_ID"), &node_id); + + // Start + PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate); + return UavcanNode::start(node_id, bitrate); + } + + /* commands below require the app to be started */ + UavcanNode *const inst = UavcanNode::instance(); + + if (!inst) { + PX4_ERR("application not running"); + return 1; + } + + if (!strcmp(argv[1], "status") || !strcmp(argv[1], "info")) { + inst->print_info(); + return 0; + } + + if (!strcmp(argv[1], "stop")) { + delete inst; + return 0; + } + + print_usage(); + return 1; +} diff --git a/src/drivers/uavcan_v1/Uavcan.hpp b/src/drivers/uavcan_v1/Uavcan.hpp new file mode 100644 index 0000000000..c4761b908c --- /dev/null +++ b/src/drivers/uavcan_v1/Uavcan.hpp @@ -0,0 +1,136 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "o1heap/o1heap.h" + +#include +#include + +#include +#include + +#include "CanardInterface.hpp" + +class UavcanNode : public ModuleParams, public px4::ScheduledWorkItem +{ + /* + * This memory is reserved for uavcan to use as over flow for message + * Coming from multiple sources that my not be considered at development + * time. + * + * The call to getNumFreeBlocks will tell how many blocks there are + * free -and multiply it times getBlockSize to get the number of bytes + * + */ + static constexpr unsigned HeapSize = 8192; + + static constexpr unsigned ScheduleIntervalMs = 10; + +public: + + UavcanNode(CanardInterface *interface, uint32_t node_id); + ~UavcanNode() override; + + static int start(uint32_t node_id, uint32_t bitrate); + + void print_info(); + + static UavcanNode *instance() { return _instance; } + + /* The bit rate that can be passed back to the bootloader */ + int32_t active_bitrate{0}; + +private: + void Run() override; + void fill_node_info(); + + void *_uavcan_heap{nullptr}; + + CanardInterface *const _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; + + pthread_mutex_t _node_mutex; + + CanardRxSubscription _heartbeat_subscription; + CanardRxSubscription _drone_sensor_BMSStatus_subscription; + + uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + + uORB::Publication _battery_status_pub{ORB_ID(battery_status)}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")}; + perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")}; + + // uavcan::node::Heartbeat_1_0 + uint8_t _uavcan_node_heartbeat_buffer[uavcan::node::Heartbeat_1_0::SIZE]; + hrt_abstime _uavcan_node_heartbeat_last{0}; + CanardTransferID _uavcan_node_heartbeat_transfer_id{0}; + + // regulated::drone::sensor::BMSStatus_1_0 + uint8_t _regulated_drone_sensor_bmsstatus_buffer[regulated::drone::sensor::BMSStatus_1_0::SIZE]; + hrt_abstime _regulated_drone_sensor_bmsstatus_last{0}; + CanardTransferID _regulated_drone_sensor_bmsstatus_transfer_id{0}; + + DEFINE_PARAMETERS( + (ParamInt) _param_uavcan_v1_enable, + (ParamInt) _param_uavcan_v1_id, + (ParamInt) _param_uavcan_v1_baud, + (ParamInt) _param_uavcan_v1_bat_md, + (ParamInt) _param_uavcan_v1_bat_id + ) +}; diff --git a/src/drivers/uavcan_v1/libcanard b/src/drivers/uavcan_v1/libcanard new file mode 160000 index 0000000000..bec890304a --- /dev/null +++ b/src/drivers/uavcan_v1/libcanard @@ -0,0 +1 @@ +Subproject commit bec890304a2888bc516416e4ebf252f761558b92 diff --git a/src/drivers/uavcan_v1/o1heap/o1heap.c b/src/drivers/uavcan_v1/o1heap/o1heap.c new file mode 100644 index 0000000000..0766745f09 --- /dev/null +++ b/src/drivers/uavcan_v1/o1heap/o1heap.c @@ -0,0 +1,498 @@ +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, +// and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions +// of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +// +// Copyright (c) 2020 Pavel Kirienko +// Authors: Pavel Kirienko + +#include "o1heap.h" +#include + +// ---------------------------------------- BUILD CONFIGURATION OPTIONS ---------------------------------------- + +/// The assertion macro defaults to the standard assert(). +/// It can be overridden to manually suppress assertion checks or use a different error handling policy. +#ifndef O1HEAP_ASSERT +// Intentional violation of MISRA: the assertion check macro cannot be replaced with a function definition. +# define O1HEAP_ASSERT(x) assert(x) // NOSONAR +#endif + +/// Branch probability annotations are used to improve the worst case execution time (WCET). They are entirely optional. +/// A stock implementation is provided for some well-known compilers; for other compilers it defaults to nothing. +/// If you are using a different compiler, consider overriding this value. +#ifndef O1HEAP_LIKELY +# if defined(__GNUC__) || defined(__clang__) || defined(__CC_ARM) +// Intentional violation of MISRA: branch hinting macro cannot be replaced with a function definition. +# define O1HEAP_LIKELY(x) __builtin_expect((x), 1) // NOSONAR +# else +# define O1HEAP_LIKELY(x) x +# endif +#endif + +/// This option is used for testing only. Do not use in production. +#if defined(O1HEAP_EXPOSE_INTERNALS) && O1HEAP_EXPOSE_INTERNALS +# define O1HEAP_PRIVATE +#else +# define O1HEAP_PRIVATE static inline +#endif + +// ---------------------------------------- INTERNAL DEFINITIONS ---------------------------------------- + +#if !defined(__STDC_VERSION__) || (__STDC_VERSION__ < 199901L) +# error "Unsupported language: ISO C99 or a newer version is required." +#endif + +#if __STDC_VERSION__ < 201112L +// Intentional violation of MISRA: static assertion macro cannot be replaced with a function definition. +# define static_assert(x, ...) typedef char _static_assert_gl(_static_assertion_, __LINE__)[(x) ? 1 : -1] // NOSONAR +# define _static_assert_gl(a, b) _static_assert_gl_impl(a, b) // NOSONAR +// Intentional violation of MISRA: the paste operator ## cannot be avoided in this context. +# define _static_assert_gl_impl(a, b) a##b // NOSONAR +#endif + +/// The overhead is at most O1HEAP_ALIGNMENT bytes large, +/// then follows the user data which shall keep the next fragment aligned. +#define FRAGMENT_SIZE_MIN (O1HEAP_ALIGNMENT * 2U) + +/// This is risky, handle with care: if the allocation amount plus per-fragment overhead exceeds 2**(b-1), +/// where b is the pointer bit width, then ceil(log2(amount)) yields b; then 2**b causes an integer overflow. +/// To avoid this, we put a hard limit on fragment size (which is amount + per-fragment overhead): 2**(b-1) +#define FRAGMENT_SIZE_MAX ((SIZE_MAX >> 1U) + 1U) + +/// Normally we should subtract log2(FRAGMENT_SIZE_MIN) but log2 is bulky to compute using the preprocessor only. +/// We will certainly end up with unused bins this way, but it is cheap to ignore. +#define NUM_BINS_MAX (sizeof(size_t) * 8U) + +static_assert((O1HEAP_ALIGNMENT & (O1HEAP_ALIGNMENT - 1U)) == 0U, "Not a power of 2"); +static_assert((FRAGMENT_SIZE_MIN & (FRAGMENT_SIZE_MIN - 1U)) == 0U, "Not a power of 2"); +static_assert((FRAGMENT_SIZE_MAX & (FRAGMENT_SIZE_MAX - 1U)) == 0U, "Not a power of 2"); + +typedef struct Fragment Fragment; + +typedef struct FragmentHeader { + Fragment *next; + Fragment *prev; + size_t size; + bool used; +} FragmentHeader; +static_assert(sizeof(FragmentHeader) <= O1HEAP_ALIGNMENT, "Memory layout error"); + +struct Fragment { + FragmentHeader header; + // Everything past the header may spill over into the allocatable space. The header survives across alloc/free. + Fragment *next_free; // Next free fragment in the bin; NULL in the last one. + Fragment *prev_free; // Same but points back; NULL in the first one. +}; +static_assert(sizeof(Fragment) <= FRAGMENT_SIZE_MIN, "Memory layout error"); + +struct O1HeapInstance { + Fragment *bins[NUM_BINS_MAX]; ///< Smallest fragments are in the bin at index 0. + size_t nonempty_bin_mask; ///< Bit 1 represents a non-empty bin; bin at index 0 is for the smallest fragments. + + O1HeapHook critical_section_enter; + O1HeapHook critical_section_leave; + + O1HeapDiagnostics diagnostics; +}; + +/// The amount of space allocated for the heap instance. +/// Its size is padded up to O1HEAP_ALIGNMENT to ensure correct alignment of the allocation arena that follows. +#define INSTANCE_SIZE_PADDED ((sizeof(O1HeapInstance) + O1HEAP_ALIGNMENT - 1U) & ~(O1HEAP_ALIGNMENT - 1U)) + +static_assert(INSTANCE_SIZE_PADDED >= sizeof(O1HeapInstance), "Invalid instance footprint computation"); +static_assert((INSTANCE_SIZE_PADDED % O1HEAP_ALIGNMENT) == 0U, "Invalid instance footprint computation"); + +/// True if the argument is an integer power of two or zero. +O1HEAP_PRIVATE bool isPowerOf2(const size_t x); +O1HEAP_PRIVATE bool isPowerOf2(const size_t x) +{ + return (x & (x - 1U)) == 0U; +} + +/// Special case: if the argument is zero, returns zero. +O1HEAP_PRIVATE uint8_t log2Floor(const size_t x); +O1HEAP_PRIVATE uint8_t log2Floor(const size_t x) +{ + size_t tmp = x; + uint8_t y = 0; + + // This is currently the only exception to the statement "routines contain neither loops nor recursion". + // It is unclear if there is a better way to compute the binary logarithm than this. + while (tmp > 1U) { + tmp >>= 1U; + y++; + } + + return y; +} + +/// Special case: if the argument is zero, returns zero. +O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x); +O1HEAP_PRIVATE uint8_t log2Ceil(const size_t x) +{ + return (uint8_t)(log2Floor(x) + (isPowerOf2(x) ? 0U : 1U)); +} + +/// Raise 2 into the specified power. +/// You might be tempted to do something like (1U << power). WRONG! We humans are prone to forgetting things. +/// If you forget to cast your 1U to size_t or ULL, you may end up with undefined behavior. +O1HEAP_PRIVATE size_t pow2(const uint8_t power); +O1HEAP_PRIVATE size_t pow2(const uint8_t power) +{ + return ((size_t) 1U) << power; +} + +O1HEAP_PRIVATE void invoke(const O1HeapHook hook); +O1HEAP_PRIVATE void invoke(const O1HeapHook hook) +{ + if (hook != NULL) { + hook(); + } +} + +/// Links two fragments so that their next/prev pointers point to each other; left goes before right. +O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right); +O1HEAP_PRIVATE void interlink(Fragment *const left, Fragment *const right) +{ + if (O1HEAP_LIKELY(left != NULL)) { + left->header.next = right; + } + + if (O1HEAP_LIKELY(right != NULL)) { + right->header.prev = left; + } +} + +/// Adds a new block into the appropriate bin and updates the lookup mask. +O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment); +O1HEAP_PRIVATE void rebin(O1HeapInstance *const handle, Fragment *const fragment) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(fragment != NULL); + O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U); + const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN); // Round DOWN when inserting. + O1HEAP_ASSERT(idx < NUM_BINS_MAX); + // Add the new fragment to the beginning of the bin list. + // I.e., each allocation will be returning the least-recently-used fragment -- good for caching. + fragment->next_free = handle->bins[idx]; + fragment->prev_free = NULL; + + if (O1HEAP_LIKELY(handle->bins[idx] != NULL)) { + handle->bins[idx]->prev_free = fragment; + } + + handle->bins[idx] = fragment; + handle->nonempty_bin_mask |= pow2(idx); +} + +/// Removes the specified block from its bin. +O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment); +O1HEAP_PRIVATE void unbin(O1HeapInstance *const handle, const Fragment *const fragment) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(fragment != NULL); + O1HEAP_ASSERT(fragment->header.size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT((fragment->header.size % FRAGMENT_SIZE_MIN) == 0U); + const uint8_t idx = log2Floor(fragment->header.size / FRAGMENT_SIZE_MIN); // Round DOWN when removing. + O1HEAP_ASSERT(idx < NUM_BINS_MAX); + + // Remove the bin from the free fragment list. + if (O1HEAP_LIKELY(fragment->next_free != NULL)) { + fragment->next_free->prev_free = fragment->prev_free; + } + + if (O1HEAP_LIKELY(fragment->prev_free != NULL)) { + fragment->prev_free->next_free = fragment->next_free; + } + + // Update the bin header. + if (O1HEAP_LIKELY(handle->bins[idx] == fragment)) { + O1HEAP_ASSERT(fragment->prev_free == NULL); + handle->bins[idx] = fragment->next_free; + + if (O1HEAP_LIKELY(handle->bins[idx] == NULL)) { + handle->nonempty_bin_mask &= ~pow2(idx); + } + } +} + +// ---------------------------------------- PUBLIC API IMPLEMENTATION ---------------------------------------- + +O1HeapInstance *o1heapInit(void *const base, + const size_t size, + const O1HeapHook critical_section_enter, + const O1HeapHook critical_section_leave) +{ + O1HeapInstance *out = NULL; + + if ((base != NULL) && ((((size_t) base) % O1HEAP_ALIGNMENT) == 0U) && + (size >= (INSTANCE_SIZE_PADDED + FRAGMENT_SIZE_MIN))) { + // Allocate the core heap metadata structure in the beginning of the arena. + O1HEAP_ASSERT(((size_t) base) % sizeof(O1HeapInstance *) == 0U); + out = (O1HeapInstance *) base; + out->nonempty_bin_mask = 0U; + out->critical_section_enter = critical_section_enter; + out->critical_section_leave = critical_section_leave; + + for (size_t i = 0; i < NUM_BINS_MAX; i++) { + out->bins[i] = NULL; + } + + // Limit and align the capacity. + size_t capacity = size - INSTANCE_SIZE_PADDED; + + if (capacity > FRAGMENT_SIZE_MAX) { + capacity = FRAGMENT_SIZE_MAX; + } + + while ((capacity % FRAGMENT_SIZE_MIN) != 0) { + O1HEAP_ASSERT(capacity > 0U); + capacity--; + } + + O1HEAP_ASSERT((capacity % FRAGMENT_SIZE_MIN) == 0); + O1HEAP_ASSERT((capacity >= FRAGMENT_SIZE_MIN) && (capacity <= FRAGMENT_SIZE_MAX)); + + // Initialize the root fragment. + Fragment *const frag = (Fragment *)(void *)(((uint8_t *) base) + INSTANCE_SIZE_PADDED); + O1HEAP_ASSERT((((size_t) frag) % O1HEAP_ALIGNMENT) == 0U); + frag->header.next = NULL; + frag->header.prev = NULL; + frag->header.size = capacity; + frag->header.used = false; + frag->next_free = NULL; + frag->prev_free = NULL; + rebin(out, frag); + O1HEAP_ASSERT(out->nonempty_bin_mask != 0U); + + // Initialize the diagnostics. + out->diagnostics.capacity = capacity; + out->diagnostics.allocated = 0U; + out->diagnostics.peak_allocated = 0U; + out->diagnostics.peak_request_size = 0U; + out->diagnostics.oom_count = 0U; + } + + return out; +} + +void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX); + void *out = NULL; + + // If the amount approaches approx. SIZE_MAX/2, an undetected integer overflow may occur. + // To avoid that, we do not attempt allocation if the amount exceeds the hard limit. + // We perform multiple redundant checks to account for a possible unaccounted overflow. + if (O1HEAP_LIKELY((amount > 0U) && (amount <= (handle->diagnostics.capacity - O1HEAP_ALIGNMENT)))) { + // Add the header size and align the allocation size to the power of 2. + // See "Timing-Predictable Memory Allocation In Hard Real-Time Systems", Herter, page 27. + const size_t fragment_size = pow2(log2Ceil(amount + O1HEAP_ALIGNMENT)); + O1HEAP_ASSERT(fragment_size <= FRAGMENT_SIZE_MAX); + O1HEAP_ASSERT(fragment_size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT(fragment_size >= amount + O1HEAP_ALIGNMENT); + O1HEAP_ASSERT(isPowerOf2(fragment_size)); + + const uint8_t optimal_bin_index = log2Ceil(fragment_size / FRAGMENT_SIZE_MIN); // Use CEIL when fetching. + O1HEAP_ASSERT(optimal_bin_index < NUM_BINS_MAX); + const size_t candidate_bin_mask = ~(pow2(optimal_bin_index) - 1U); + + invoke(handle->critical_section_enter); + + // Find the smallest non-empty bin we can use. + const size_t suitable_bins = handle->nonempty_bin_mask & candidate_bin_mask; + const size_t smallest_bin_mask = suitable_bins & ~(suitable_bins - 1U); // Clear all bits but the lowest. + + if (O1HEAP_LIKELY(smallest_bin_mask != 0)) { + O1HEAP_ASSERT(isPowerOf2(smallest_bin_mask)); + const uint8_t bin_index = log2Floor(smallest_bin_mask); + O1HEAP_ASSERT(bin_index >= optimal_bin_index); + O1HEAP_ASSERT(bin_index < NUM_BINS_MAX); + + // The bin we found shall not be empty, otherwise it's a state divergence (memory corruption?). + Fragment *const frag = handle->bins[bin_index]; + O1HEAP_ASSERT(frag != NULL); + O1HEAP_ASSERT(frag->header.size >= fragment_size); + O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U); + O1HEAP_ASSERT(!frag->header.used); + unbin(handle, frag); + + // Split the fragment if it is too large. + const size_t leftover = frag->header.size - fragment_size; + frag->header.size = fragment_size; + O1HEAP_ASSERT(leftover < handle->diagnostics.capacity); // Overflow check. + O1HEAP_ASSERT(leftover % FRAGMENT_SIZE_MIN == 0U); // Alignment check. + + if (O1HEAP_LIKELY(leftover >= FRAGMENT_SIZE_MIN)) { + Fragment *const new_frag = (Fragment *)(void *)(((uint8_t *) frag) + fragment_size); + O1HEAP_ASSERT(((size_t) new_frag) % O1HEAP_ALIGNMENT == 0U); + new_frag->header.size = leftover; + new_frag->header.used = false; + interlink(new_frag, frag->header.next); + interlink(frag, new_frag); + rebin(handle, new_frag); + } + + // Update the diagnostics. + O1HEAP_ASSERT((handle->diagnostics.allocated % FRAGMENT_SIZE_MIN) == 0U); + handle->diagnostics.allocated += fragment_size; + O1HEAP_ASSERT(handle->diagnostics.allocated <= handle->diagnostics.capacity); + + if (O1HEAP_LIKELY(handle->diagnostics.peak_allocated < handle->diagnostics.allocated)) { + handle->diagnostics.peak_allocated = handle->diagnostics.allocated; + } + + // Finalize the fragment we just allocated. + O1HEAP_ASSERT(frag->header.size >= amount + O1HEAP_ALIGNMENT); + frag->header.used = true; + + out = ((uint8_t *) frag) + O1HEAP_ALIGNMENT; + } + + } else { + invoke(handle->critical_section_enter); + } + + // Update the diagnostics. + if (O1HEAP_LIKELY(handle->diagnostics.peak_request_size < amount)) { + handle->diagnostics.peak_request_size = amount; + } + + if (O1HEAP_LIKELY((out == NULL) && (amount > 0U))) { + handle->diagnostics.oom_count++; + } + + invoke(handle->critical_section_leave); + return out; +} + +void o1heapFree(O1HeapInstance *const handle, void *const pointer) +{ + O1HEAP_ASSERT(handle != NULL); + O1HEAP_ASSERT(handle->diagnostics.capacity <= FRAGMENT_SIZE_MAX); + + if (O1HEAP_LIKELY(pointer != NULL)) { // NULL pointer is a no-op. + Fragment *const frag = (Fragment *)(void *)(((uint8_t *) pointer) - O1HEAP_ALIGNMENT); + + // Check for heap corruption in debug builds. + O1HEAP_ASSERT(((size_t) frag) % sizeof(Fragment *) == 0U); + O1HEAP_ASSERT(((size_t) frag) >= (((size_t) handle) + INSTANCE_SIZE_PADDED)); + O1HEAP_ASSERT(((size_t) frag) <= + (((size_t) handle) + INSTANCE_SIZE_PADDED + handle->diagnostics.capacity - FRAGMENT_SIZE_MIN)); + O1HEAP_ASSERT(frag->header.used); // Catch double-free + O1HEAP_ASSERT(((size_t) frag->header.next) % sizeof(Fragment *) == 0U); + O1HEAP_ASSERT(((size_t) frag->header.prev) % sizeof(Fragment *) == 0U); + O1HEAP_ASSERT(frag->header.size >= FRAGMENT_SIZE_MIN); + O1HEAP_ASSERT(frag->header.size <= handle->diagnostics.capacity); + O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U); + + invoke(handle->critical_section_enter); + + // Even if we're going to drop the fragment later, mark it free anyway to prevent double-free. + frag->header.used = false; + + // Update the diagnostics. It must be done before merging because it invalidates the fragment size information. + O1HEAP_ASSERT(handle->diagnostics.allocated >= frag->header.size); // Heap corruption check. + handle->diagnostics.allocated -= frag->header.size; + + // Merge with siblings and insert the returned fragment into the appropriate bin and update metadata. + Fragment *const prev = frag->header.prev; + Fragment *const next = frag->header.next; + const bool join_left = (prev != NULL) && (!prev->header.used); + const bool join_right = (next != NULL) && (!next->header.used); + + if (join_left && join_right) { // [ prev ][ this ][ next ] => [ ------- prev ------- ] + unbin(handle, prev); + unbin(handle, next); + prev->header.size += frag->header.size + next->header.size; + frag->header.size = 0; // Invalidate the dropped fragment headers to prevent double-free. + next->header.size = 0; + O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U); + interlink(prev, next->header.next); + rebin(handle, prev); + + } else if (join_left) { // [ prev ][ this ][ next ] => [ --- prev --- ][ next ] + unbin(handle, prev); + prev->header.size += frag->header.size; + frag->header.size = 0; + O1HEAP_ASSERT((prev->header.size % FRAGMENT_SIZE_MIN) == 0U); + interlink(prev, next); + rebin(handle, prev); + + } else if (join_right) { // [ prev ][ this ][ next ] => [ prev ][ --- this --- ] + unbin(handle, next); + frag->header.size += next->header.size; + next->header.size = 0; + O1HEAP_ASSERT((frag->header.size % FRAGMENT_SIZE_MIN) == 0U); + interlink(frag, next->header.next); + rebin(handle, frag); + + } else { + rebin(handle, frag); + } + + invoke(handle->critical_section_leave); + } +} + +bool o1heapDoInvariantsHold(const O1HeapInstance *const handle) +{ + O1HEAP_ASSERT(handle != NULL); + bool valid = true; + + invoke(handle->critical_section_enter); + + // Check the bin mask consistency. + for (size_t i = 0; i < NUM_BINS_MAX; i++) { // Dear compiler, feel free to unroll this loop. + const bool mask_bit_set = (handle->nonempty_bin_mask & pow2((uint8_t) i)) != 0U; + const bool bin_nonempty = handle->bins[i] != NULL; + valid = valid && (mask_bit_set == bin_nonempty); + } + + // Create a local copy of the diagnostics struct to check later and release the critical section early. + const O1HeapDiagnostics diag = handle->diagnostics; + + invoke(handle->critical_section_leave); + + // Capacity check. + valid = valid && (diag.capacity <= FRAGMENT_SIZE_MAX) && (diag.capacity >= FRAGMENT_SIZE_MIN) && + ((diag.capacity % FRAGMENT_SIZE_MIN) == 0U); + + // Allocation info check. + valid = valid && (diag.allocated <= diag.capacity) && ((diag.allocated % FRAGMENT_SIZE_MIN) == 0U) && + (diag.peak_allocated <= diag.capacity) && (diag.peak_allocated >= diag.allocated) && + ((diag.peak_allocated % FRAGMENT_SIZE_MIN) == 0U); + + // Peak request check + valid = valid && ((diag.peak_request_size < diag.capacity) || (diag.oom_count > 0U)); + + if (diag.peak_request_size == 0U) { + valid = valid && (diag.peak_allocated == 0U) && (diag.allocated == 0U) && (diag.oom_count == 0U); + + } else { + valid = valid && // Overflow on summation is possible but safe to ignore. + (((diag.peak_request_size + O1HEAP_ALIGNMENT) <= diag.peak_allocated) || (diag.oom_count > 0U)); + } + + return valid; +} + +O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle) +{ + O1HEAP_ASSERT(handle != NULL); + invoke(handle->critical_section_enter); + const O1HeapDiagnostics out = handle->diagnostics; + invoke(handle->critical_section_leave); + return out; +} diff --git a/src/drivers/uavcan_v1/o1heap/o1heap.h b/src/drivers/uavcan_v1/o1heap/o1heap.h new file mode 100644 index 0000000000..4f095495a0 --- /dev/null +++ b/src/drivers/uavcan_v1/o1heap/o1heap.h @@ -0,0 +1,142 @@ +// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +// documentation files (the "Software"), to deal in the Software without restriction, including without limitation +// the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, +// and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all copies or substantial portions +// of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE +// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS +// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR +// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +// +// Copyright (c) 2020 Pavel Kirienko +// Authors: Pavel Kirienko +// +// READ THE DOCUMENTATION IN README.md. + +#ifndef O1HEAP_H_INCLUDED +#define O1HEAP_H_INCLUDED + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/// The semantic version number of this distribution. +#define O1HEAP_VERSION_MAJOR 1 + +/// The guaranteed alignment depends on the platform pointer width. +#define O1HEAP_ALIGNMENT (sizeof(void*) * 4U) + +/// The definition is private, so the user code can only operate on pointers. This is done to enforce encapsulation. +typedef struct O1HeapInstance O1HeapInstance; + +/// A hook function invoked by the allocator. NULL hooks are silently not invoked (not an error). +typedef void (*O1HeapHook)(void); + +/// Runtime diagnostic information. This information can be used to facilitate runtime self-testing, +/// as required by certain safety-critical development guidelines. +/// If assertion checks are not disabled, the library will perform automatic runtime self-diagnostics that trigger +/// an assertion failure if a heap corruption is detected. +/// Health checks and validation can be done with @ref o1heapDoInvariantsHold(). +typedef struct { + /// The total amount of memory available for serving allocation requests (heap size). + /// The maximum allocation size is (capacity - O1HEAP_ALIGNMENT). + /// This parameter does not include the overhead used up by @ref O1HeapInstance and arena alignment. + /// This parameter is constant. + size_t capacity; + + /// The amount of memory that is currently allocated, including the per-fragment overhead and size alignment. + /// For example, if the application requested a fragment of size 1 byte, the value reported here may be 32 bytes. + size_t allocated; + + /// The maximum value of 'allocated' seen since initialization. This parameter is never decreased. + size_t peak_allocated; + + /// The largest amount of memory that the allocator has attempted to allocate (perhaps unsuccessfully) + /// since initialization (not including the rounding and the allocator's own per-fragment overhead, + /// so the total is larger). This parameter is never decreased. The initial value is zero. + size_t peak_request_size; + + /// The number of times an allocation request could not be completed due to the lack of memory or + /// excessive fragmentation. OOM stands for "out of memory". This parameter is never decreased. + uint64_t oom_count; +} O1HeapDiagnostics; + +/// The arena base pointer shall be aligned at @ref O1HEAP_ALIGNMENT, otherwise NULL is returned. +/// +/// The total heap capacity cannot exceed approx. (SIZE_MAX/2). If the arena size allows for a larger heap, +/// the excess will be silently truncated away (no error). This is not a realistic use case because a typical +/// application is unlikely to be able to dedicate that much of the address space for the heap. +/// +/// The critical section enter/leave callbacks will be invoked when the allocator performs an atomic transaction. +/// There is at most one atomic transaction per allocation/deallocation. +/// Either or both of the callbacks may be NULL if locking is not needed (i.e., the heap is not shared). +/// It is guaranteed that a critical section will never be entered recursively. +/// It is guaranteed that 'enter' is invoked the same number of times as 'leave', unless either of them are NULL. +/// It is guaranteed that 'enter' is invoked before 'leave', unless either of them are NULL. +/// The callbacks are never invoked from the initialization function itself. +/// +/// The function initializes a new heap instance allocated in the provided arena, taking some of its space for its +/// own needs (normally about 40..600 bytes depending on the architecture, but this parameter is not characterized). +/// A pointer to the newly initialized instance is returned. +/// +/// If the provided space is insufficient, NULL is returned. +/// +/// An initialized instance does not hold any resources. Therefore, if the instance is no longer needed, +/// it can be discarded without any de-initialization procedures. +/// +/// The time complexity is unspecified. +O1HeapInstance *o1heapInit(void *const base, + const size_t size, + const O1HeapHook critical_section_enter, + const O1HeapHook critical_section_leave); + +/// The semantics follows malloc() with additional guarantees the full list of which is provided below. +/// +/// If the allocation request is served successfully, a pointer to the newly allocated memory fragment is returned. +/// The returned pointer is guaranteed to be aligned at @ref O1HEAP_ALIGNMENT. +/// +/// If the allocation request cannot be served due to the lack of memory or its excessive fragmentation, +/// a NULL pointer is returned. +/// +/// The function is executed in constant time (unless the critical section management hooks are used and are not +/// constant-time). The allocated memory is NOT zero-filled (because zero-filling is a variable-complexity operation). +/// +/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored). +void *o1heapAllocate(O1HeapInstance *const handle, const size_t amount); + +/// The semantics follows free() with additional guarantees the full list of which is provided below. +/// +/// If the pointer does not point to a previously allocated block and is not NULL, the behavior is undefined. +/// Builds where assertion checks are enabled may trigger an assertion failure for some invalid inputs. +/// +/// The function is executed in constant time (unless the critical section management hooks are used and are not +/// constant-time). +/// +/// The function may invoke critical_section_enter and critical_section_leave at most once each (NULL hooks ignored). +void o1heapFree(O1HeapInstance *const handle, void *const pointer); + +/// Performs a basic sanity check on the heap. +/// This function can be used as a weak but fast method of heap corruption detection. +/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL). +/// If the handle pointer is NULL, the behavior is undefined. +/// The time complexity is constant. +/// The return value is truth if the heap looks valid, falsity otherwise. +bool o1heapDoInvariantsHold(const O1HeapInstance *const handle); + +/// Samples and returns a copy of the diagnostic information, see @ref O1HeapDiagnostics. +/// This function merely copies the structure from an internal storage, so it is fast to return. +/// It invokes critical_section_enter once (unless NULL) and then critical_section_leave once (unless NULL). +/// If the handle pointer is NULL, the behavior is undefined. +O1HeapDiagnostics o1heapGetDiagnostics(const O1HeapInstance *const handle); + +#ifdef __cplusplus +} +#endif +#endif // O1HEAP_H_INCLUDED diff --git a/src/drivers/uavcan_v1/parameters.c b/src/drivers/uavcan_v1/parameters.c new file mode 100644 index 0000000000..dc4e39858e --- /dev/null +++ b/src/drivers/uavcan_v1/parameters.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * UAVCAN v1 + * + * 0 - UAVCAN disabled. + * 1 - Enables UAVCANv1 + * + * @boolean + * @reboot_required true + * @group UAVCAN v1 + */ +PARAM_DEFINE_INT32(UAVCAN_V1_ENABLE, 0); + +/** + * UAVCAN v1 Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min 1 + * @max 125 + * @reboot_required true + * @group UAVCANv1 + */ +PARAM_DEFINE_INT32(UAVCAN_V1_ID, 1); + +/** + * UAVCAN/CAN v1 bus bitrate. + * + * @unit bit/s + * @min 20000 + * @max 1000000 + * @reboot_required true + * @group UAVCAN v1 + */ +PARAM_DEFINE_INT32(UAVCAN_V1_BAUD, 1000000); + +/** + * UAVCAN v1 battery mode. + * + * @value 0 Disable + * @value 1 Receive BMSStatus + * @value 2 Send BMSStatus + * @reboot_required true + * @group UAVCAN v1 + */ +PARAM_DEFINE_INT32(UAVCAN_V1_BAT_MD, 0); + +/** + * UAVCAN v1 battery port ID. + * + * @min 1 + * @max 32767 + * @group UAVCAN v1 + */ +PARAM_DEFINE_INT32(UAVCAN_V1_BAT_ID, 4242); diff --git a/src/drivers/uavcan_v1/public_regulated_data_types b/src/drivers/uavcan_v1/public_regulated_data_types new file mode 160000 index 0000000000..e378c3a4c5 --- /dev/null +++ b/src/drivers/uavcan_v1/public_regulated_data_types @@ -0,0 +1 @@ +Subproject commit e378c3a4c5eb38ad30866059b838d4099a85f2ea diff --git a/src/drivers/uavcan_v1/templates/Header.j2 b/src/drivers/uavcan_v1/templates/Header.j2 new file mode 100644 index 0000000000..f1474317fa --- /dev/null +++ b/src/drivers/uavcan_v1/templates/Header.j2 @@ -0,0 +1,32 @@ +/* + * + * UAVCAN data structure definition. + * + * AUTOGENERATED, DO NOT EDIT. + * + * Source File: + * {{ T.source_file_path }} + * + * Template: + * {{ self._TemplateReference__context.name }} + * + * Generated at: {{ now_utc }} UTC + * Is deprecated: {{ T.deprecated and 'yes' or 'no' }} + * Fixed port ID: {{ T.fixed_port_id }} + * Full name: {{ T.full_name }} + * Version: {{ T.version.major }}.{{ T.version.minor }} + * + */ + +#ifndef {{T.full_name | c.macrofy}} +#define {{T.full_name | c.macrofy}} +{% for n in T | includes -%} +#include {{ n }} +{% endfor %} +#include +{{T.full_namespace | open_namespace}} + +{%- block object -%}{%- endblock -%} + +{{T.full_namespace | close_namespace}} +#endif // {{T.full_name | c.macrofy}} diff --git a/src/drivers/uavcan_v1/templates/ServiceType.j2 b/src/drivers/uavcan_v1/templates/ServiceType.j2 new file mode 100644 index 0000000000..6f4d9a4f44 --- /dev/null +++ b/src/drivers/uavcan_v1/templates/ServiceType.j2 @@ -0,0 +1,10 @@ +{% extends "Header.j2" %} +{%- block object -%} +{{ T | definition_begin }} +{ +{% set composite_type = T.request_type -%} +{% include '_composite_type.j2' %} +{% set composite_type = T.response_type -%} +{% include '_composite_type.j2' %} +}{{ T | definition_end }} +{% endblock -%} \ No newline at end of file diff --git a/src/drivers/uavcan_v1/templates/StructureType.j2 b/src/drivers/uavcan_v1/templates/StructureType.j2 new file mode 100644 index 0000000000..87137b3291 --- /dev/null +++ b/src/drivers/uavcan_v1/templates/StructureType.j2 @@ -0,0 +1,5 @@ +{% extends "Header.j2" %} +{%- block object -%} +{% set composite_type = T -%} +{% include '_composite_type.j2' %} +{% endblock -%} diff --git a/src/drivers/uavcan_v1/templates/UnionType.j2 b/src/drivers/uavcan_v1/templates/UnionType.j2 new file mode 100644 index 0000000000..d763941e58 --- /dev/null +++ b/src/drivers/uavcan_v1/templates/UnionType.j2 @@ -0,0 +1,5 @@ +{% extends "Header.j2" %} +{%- block object -%} +{% set composite_type = T -%} +{% include '_composite_type.j2' %} +{% endblock -%} \ No newline at end of file diff --git a/src/drivers/uavcan_v1/templates/_composite_type.j2 b/src/drivers/uavcan_v1/templates/_composite_type.j2 new file mode 100644 index 0000000000..07dec0846d --- /dev/null +++ b/src/drivers/uavcan_v1/templates/_composite_type.j2 @@ -0,0 +1,115 @@ +{{ composite_type | definition_begin }} +{ +{%- if T.fixed_port_id is not None %} + static constexpr CanardPortID PORT_ID = {{T.fixed_port_id}}; +{%- endif %} + +{%- set total_size = namespace(value=0) -%} +{%- for field in composite_type.fields -%} + {% set total_size.value = total_size.value + field.data_type.bit_length_set|max %} +{%- endfor %} + static constexpr size_t SIZE = {{total_size.value}}; + + static constexpr auto getDataTypeFullName() { return "{{T}}"; } + +{% for constant in composite_type.constants %} + static constexpr {{ constant.data_type | declaration }} {{ constant.name | id }} = {{ constant.value.native_value.numerator }} / {{ constant.value.native_value.denominator }}; +{%- endfor -%} + +{% if composite_type is UnionType %} +#error "TODO: UnionType +{% else %} + +{% for field in composite_type.fields -%} + {%- if field is not padding %} + {%- if field.data_type is FloatType %} + {%- if field.data_type.bit_length_set|max <= 32 %} + float {{ field | id }}{NAN}; + {%- else %} + double {{ field | id }}{NAN}; + {%- endif %} + {%- elif field.data_type is BooleanType %} + bool {{ field | id }}{false}; + {%- else %} + {{ field.data_type | declaration }} {{ field | id }}{}; + {%- endif -%} + {%- endif -%} +{%- endfor %} +{% endif %} + + void serializeToBuffer(uint8_t* const buffer, const size_t starting_bit = 0) + { +{%- set bit_offset = namespace(value=0) -%} +{%- for field in composite_type.fields -%} + {%- if field is not padding %} + {%- if field.data_type is SerializableType %} + {%- if field.data_type is IntegerType %} + canardDSDLSetUxx(buffer, starting_bit + {{ bit_offset.value }}, {{ field.name }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type is BooleanType %} + canardDSDLSetBit(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); + {%- elif field.data_type is FloatType %} + {%- if field.data_type.bit_length_set == 16 %} + canardDSDLSetF16(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); + {%- elif field.data_type.bit_length_set == 32 %} + canardDSDLSetF32(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); + {%- elif field.data_type.bit_length_set == 64 %} + canardDSDLSetF64(buffer, starting_bit + {{ field.data_type.bit_length_set | max }}, {{ field.name }}); + {%- endif %} + {%- else %} + {{ field.name }}.serializeToBuffer(buffer, starting_bit + {{ bit_offset.value }}); + {%- endif %} + {%- endif -%} + {%- endif -%} + {% set bit_offset.value = bit_offset.value + field.data_type.bit_length_set|max %} +{%- endfor %} + } + + static {{T | full_reference_name}} deserializeFromBuffer(const uint8_t* const buffer, const size_t buf_size, const size_t starting_bit = 0) + { + {{T | full_reference_name}} msg; +{% set bit_offset = namespace(value=0) %} +{%- for field in composite_type.fields -%} + {%- if field is not padding %} + {%- if field.data_type is SerializableType %} + {%- if field.data_type is UnsignedIntegerType %} + {%- if field.data_type.bit_length_set|max<= 8 %} + msg.{{ field.name }} = canardDSDLGetU8(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type.bit_length_set|max <= 16 %} + msg.{{ field.name }} = canardDSDLGetU16(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type.bit_length_set|max <= 32 %} + msg.{{ field.name }} = canardDSDLGetU32(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type.bit_length_set|max <= 64 %} + msg.{{ field.name }} = canardDSDLGetU64(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- endif %} + {%- elif field.data_type is SignedIntegerType %} + {%- if field.data_type.bit_length_set|max<= 8 %} + msg.{{ field.name }} = canardDSDLGetI8(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type.bit_length_set|max <= 16 %} + msg.{{ field.name }} = canardDSDLGetI16(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type.bit_length_set|max <= 32 %} + msg.{{ field.name }} = canardDSDLGetI32(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- elif field.data_type.bit_length_set|max <= 64 %} + msg.{{ field.name }} = canardDSDLGetI64(buffer, buf_size, starting_bit + {{ bit_offset.value }}, {{ field.data_type.bit_length_set | max }}); + {%- endif %} + {%- elif field.data_type is BooleanType %} + msg.{{ field.name }} = canardDSDLGetBit(buffer, buf_size, starting_bit + {{ bit_offset.value }}); + {%- elif field.data_type is FloatType %} + {%- if field.data_type.bit_length_set == 16 %} + msg.{{ field.name }} = canardDSDLGetF16(buffer, buf_size, starting_bit + {{ bit_offset.value }}); + {%- elif field.data_type.bit_length_set == 32 %} + msg.{{ field.name }} = canardDSDLGetF32(buffer, buf_size, starting_bit + {{ bit_offset.value }}); + {%- elif field.data_type.bit_length_set == 64 %} + msg.{{ field.name }} = canardDSDLGetF64(buffer, buf_size, starting_bit + {{ bit_offset.value }}); + {%- endif %} + {%- else %} + msg.{{ field.name }} = {{ field.data_type | declaration }}::deserializeFromBuffer(buffer, buf_size, starting_bit + {{ bit_offset.value }}); + {%- endif %} + {%- endif -%} + {%- endif -%} + {% set bit_offset.value = bit_offset.value + field.data_type.bit_length_set|max %} +{%- endfor %} + + return msg; + } + +}{{ composite_type | definition_end }}