Browse Source
- NuttX stm32f4/stm32f7 uses character device driver - NuttX kinetis and s32k uses socketcanrelease/1.12
Daniel Agar
5 years ago
committed by
Lorenz Meier
27 changed files with 2126 additions and 3 deletions
@ -0,0 +1,91 @@
@@ -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 |
||||
) |
||||
@ -0,0 +1,60 @@
@@ -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 <canard.h> |
||||
|
||||
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: |
||||
|
||||
}; |
||||
@ -0,0 +1,151 @@
@@ -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 <fcntl.h> |
||||
#include <poll.h> |
||||
|
||||
#include <nuttx/can/can.h> |
||||
#include <arch/board/board.h> |
||||
|
||||
#include "stm32_can.h" |
||||
|
||||
#include <px4_platform_common/log.h> |
||||
|
||||
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; |
||||
} |
||||
@ -0,0 +1,67 @@
@@ -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 <px4_platform_common/px4_config.h> |
||||
|
||||
#include <canard.h> |
||||
|
||||
#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}; |
||||
}; |
||||
@ -0,0 +1,199 @@
@@ -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 <net/if.h> |
||||
#include <sys/ioctl.h> |
||||
#include <string.h> |
||||
|
||||
#include <px4_platform_common/log.h> |
||||
|
||||
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; |
||||
} |
||||
@ -0,0 +1,96 @@
@@ -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 <px4_platform_common/px4_config.h> |
||||
|
||||
#include <stdbool.h> |
||||
#include <stddef.h> |
||||
#include <stdint.h> |
||||
|
||||
#include <sys/time.h> |
||||
#include <sys/socket.h> |
||||
|
||||
#include <nuttx/can.h> |
||||
#include <netpacket/can.h> |
||||
|
||||
#include <canard.h> |
||||
|
||||
#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)] {}; |
||||
}; |
||||
@ -0,0 +1,407 @@
@@ -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 <lib/ecl/geo/geo.h> |
||||
#include <lib/version/version.h> |
||||
|
||||
#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<CanardPortID>(_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<CanardPortID>(_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<CanardPortID>(_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; |
||||
} |
||||
@ -0,0 +1,136 @@
@@ -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 <px4_platform_common/px4_config.h> |
||||
#include <px4_platform_common/atomic.h> |
||||
#include <px4_platform_common/defines.h> |
||||
#include <px4_platform_common/module.h> |
||||
#include <px4_platform_common/module_params.h> |
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp> |
||||
|
||||
#include <lib/parameters/param.h> |
||||
#include <lib/perf/perf_counter.h> |
||||
#include <uORB/Publication.hpp> |
||||
#include <uORB/Subscription.hpp> |
||||
#include <uORB/SubscriptionCallback.hpp> |
||||
#include <uORB/topics/battery_status.h> |
||||
#include <uORB/topics/parameter_update.h> |
||||
|
||||
#include "o1heap/o1heap.h" |
||||
|
||||
#include <canard.h> |
||||
#include <canard_dsdl.h> |
||||
|
||||
#include <regulated/drone/sensor/BMSStatus_1_0.hpp> |
||||
#include <uavcan/node/Heartbeat_1_0.hpp> |
||||
|
||||
#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_s> _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<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable, |
||||
(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id, |
||||
(ParamInt<px4::params::UAVCAN_V1_BAUD>) _param_uavcan_v1_baud, |
||||
(ParamInt<px4::params::UAVCAN_V1_BAT_MD>) _param_uavcan_v1_bat_md, |
||||
(ParamInt<px4::params::UAVCAN_V1_BAT_ID>) _param_uavcan_v1_bat_id |
||||
) |
||||
}; |
||||
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
Subproject commit bec890304a2888bc516416e4ebf252f761558b92 |
||||
@ -0,0 +1,498 @@
@@ -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 <pavel.kirienko@zubax.com>
|
||||
|
||||
#include "o1heap.h" |
||||
#include <assert.h> |
||||
|
||||
// ---------------------------------------- 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; |
||||
} |
||||
@ -0,0 +1,142 @@
@@ -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 <pavel.kirienko@zubax.com>
|
||||
//
|
||||
// READ THE DOCUMENTATION IN README.md.
|
||||
|
||||
#ifndef O1HEAP_H_INCLUDED |
||||
#define O1HEAP_H_INCLUDED |
||||
|
||||
#include <stdbool.h> |
||||
#include <stddef.h> |
||||
#include <stdint.h> |
||||
|
||||
#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
|
||||
@ -0,0 +1,87 @@
@@ -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); |
||||
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
Subproject commit e378c3a4c5eb38ad30866059b838d4099a85f2ea |
||||
@ -0,0 +1,32 @@
@@ -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 <canard_dsdl.h> |
||||
{{T.full_namespace | open_namespace}} |
||||
|
||||
{%- block object -%}{%- endblock -%} |
||||
|
||||
{{T.full_namespace | close_namespace}} |
||||
#endif // {{T.full_name | c.macrofy}} |
||||
@ -0,0 +1,10 @@
@@ -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 -%} |
||||
@ -0,0 +1,5 @@
@@ -0,0 +1,5 @@
|
||||
{% extends "Header.j2" %} |
||||
{%- block object -%} |
||||
{% set composite_type = T -%} |
||||
{% include '_composite_type.j2' %} |
||||
{% endblock -%} |
||||
@ -0,0 +1,5 @@
@@ -0,0 +1,5 @@
|
||||
{% extends "Header.j2" %} |
||||
{%- block object -%} |
||||
{% set composite_type = T -%} |
||||
{% include '_composite_type.j2' %} |
||||
{% endblock -%} |
||||
@ -0,0 +1,115 @@
@@ -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 }} |
||||
Loading…
Reference in new issue