diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index e38e438573..3bb25d62b7 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -4,6 +4,7 @@ CONFIG_BOARD_ROMFSROOT="cannode" CONFIG_BOARD_CONSTRAINED_MEMORY=y CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS1" CONFIG_DRIVERS_BOOTLOADERS=y +CONFIG_DRIVERS_DISTANCE_SENSOR_TFMINI=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_DRIVERS_PWM_OUT=y diff --git a/boards/nxp/ucans32k146/init/rc.board_defaults b/boards/nxp/ucans32k146/init/rc.board_defaults index f9775ee814..2e52bc7b72 100644 --- a/boards/nxp/ucans32k146/init/rc.board_defaults +++ b/boards/nxp/ucans32k146/init/rc.board_defaults @@ -6,4 +6,9 @@ pwm_out mode_pwm1 start ifup can0 -cyphal start + +# Start Cyphal when enabled +if param compare -s CYPHAL_ENABLE 1 +then + cyphal start +fi diff --git a/src/drivers/cyphal/parameters.c b/src/drivers/cyphal/parameters.c index 52d3db5b35..d9f8ea9b38 100644 --- a/src/drivers/cyphal/parameters.c +++ b/src/drivers/cyphal/parameters.c @@ -41,7 +41,7 @@ * @reboot_required true * @group Cyphal */ -PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0); +PARAM_DEFINE_INT32(CYPHAL_ENABLE, 1); /** * Cyphal Node ID. diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/README.md b/src/drivers/uavcan/uavcan_drivers/socketcan/README.md index 2edad1a833..474231575e 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/README.md +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/README.md @@ -2,44 +2,13 @@ Libuavcan platform driver for NuttX SocketCAN ================================================ This document describes the Libuavcan v0 driver for NuttX SocketCAN. -The libuavcan driver for NuttX is a header-only C++11 library that implements a fully functional platform interface +The libuavcan driver for NuttX is a C++11 library that implements a fully functional platform interface for libuavcan and also adds a few convenient wrappers. It's built on the following Linux components: * [SocketCAN](http://en.wikipedia.org/wiki/SocketCAN) - A generic CAN bus stack for Linux. - -## Installation - - - -## Using without installation - -It is possible to include Libuavcan into another CMake project as a CMake subproject. -In order to do so, extend your `CMakeLists.txt` with the following lines: - -```cmake -# Include the Libuavcan CMake subproject -add_subdirectory( - libuavcan # Path to the Libuavcan repository, modify accordingly - ${CMAKE_BINARY_DIR}/libuavcan # This path can be changed arbitrarily -) - -# Add Libuavcan include directories -include_directories( # Or use target_include_directories() instead - libuavcan/libuavcan/include - libuavcan/libuavcan/include/dsdlc_generated - libuavcan_linux/include -) - -# Link your targets with Libuavcan -target_link_libraries( - your_target # This is the name of your target - uavcan -) -``` - ## Usage Documentation for each feature is provided in the Doxygen comments in the header files. diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp index 29c9bb6604..562fe054ec 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp @@ -1,6 +1,36 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp index 90a7e423d5..75b9045489 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp @@ -1,6 +1,36 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 @@ -8,32 +38,85 @@ #include #include +#include +#include + +#include +#include + namespace uavcan_socketcan { +class CanIface : public uavcan::ICanIface + , uavcan::Noncopyable +{ + 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)] {}; + + SystemClock clock; + +public: + uavcan::uint32_t socketInit(const char *can_iface_name); + + uavcan::int16_t send(const uavcan::CanFrame &frame, + uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) override; + + uavcan::int16_t receive(uavcan::CanFrame &out_frame, + uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, + uavcan::CanIOFlags &out_flags) override; + + uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs, + uavcan::uint16_t num_configs) override; + + uavcan::uint64_t getErrorCount() const override; + + uavcan::uint16_t getNumFilters() const override; + + int getFD(); +}; + /** * This class implements CAN driver interface for libuavcan. * No configuration needed other than CAN baudrate. - * This class is a singleton. */ class CanDriver : public uavcan::ICanDriver - , public uavcan::ICanIface , uavcan::Noncopyable { BusEvent update_event_; - static CanDriver self; + CanIface if_[UAVCAN_SOCKETCAN_NUM_IFACES]; SystemClock clock; + struct pollfd pfds[UAVCAN_SOCKETCAN_NUM_IFACES]; public: CanDriver() : update_event_(*this) {} - /** - * Returns the singleton reference. - * No other copies can be created. - */ - static CanDriver &instance() { return self; } + uavcan::int32_t initIface(uint32_t index, const char *name) + { + if (index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) { + return -1; + } + + return if_[index].socketInit(name); + } /** * Attempts to detect bit rate of the CAN bus. @@ -50,15 +133,6 @@ public: */ int init(uavcan::uint32_t bitrate); - bool hasReadyRx() const; - bool hasEmptyTx() const; - - /** - * This method will return true only if there was any CAN bus activity since previous call of this method. - * This is intended to be used for LED iface activity indicators. - */ - bool hadActivity(); - /** * Returns the number of times the RX queue was overrun. */ @@ -71,26 +145,10 @@ public: */ bool isInBusOffState() const; - uavcan::int16_t send(const uavcan::CanFrame &frame, - uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) override; - - uavcan::int16_t receive(uavcan::CanFrame &out_frame, - uavcan::MonotonicTime &out_ts_monotonic, - uavcan::UtcTime &out_ts_utc, - uavcan::CanIOFlags &out_flags) override; - uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks, const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override; - uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs, - uavcan::uint16_t num_configs) override; - - uavcan::uint64_t getErrorCount() const override; - - uavcan::uint16_t getNumFilters() const override; - uavcan::ICanIface *getIface(uavcan::uint8_t iface_index) override; uavcan::uint8_t getNumIfaces() const override; @@ -122,6 +180,10 @@ public: */ int init(uavcan::uint32_t bitrate) { + driver.initIface(0, "can0"); +#if UAVCAN_SOCKETCAN_NUM_IFACES > 1 + driver.initIface(1, "can1"); +#endif return driver.init(bitrate); } diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp index 64a6355bd6..671d60286e 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp @@ -1,7 +1,37 @@ -/* - * Copyright (C) 2014, 2018 Pavel Kirienko - * Kinetis Port Author David Sidrane - */ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * Kinetis Port Author David Sidrane + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp index e6aebeab1f..32a0cbc5a7 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp @@ -1,6 +1,36 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp index 9c2260d583..422722b6ca 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp @@ -1,136 +1,64 @@ /* * Copyright (C) 2014 Pavel Kirienko + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * + * */ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 #include #include -#define UAVCAN_SOCKETCAN_RX_QUEUE_LEN 64 +#include +#include +#include -struct CriticalSectionLocker { - const irqstate_t flags_; +#include +#include - CriticalSectionLocker() - : flags_(enter_critical_section()) - { - } +#define MODULE_NAME "UAVCAN_SOCKETCAN" - ~CriticalSectionLocker() - { - leave_critical_section(flags_); - } -}; +#include namespace uavcan_socketcan { namespace { -/** - * Hardware message objects are allocated as follows: - * - 1 - Single TX object - * - 2..32 - RX objects - * TX priority is defined by the message object number, not by the CAN ID (chapter 16.7.3.5 of the user manual), - * hence we can't use more than one object because that would cause priority inversion on long transfers. - */ -constexpr unsigned NumberOfMessageObjects = 32; -constexpr unsigned NumberOfTxMessageObjects = 1; -constexpr unsigned NumberOfRxMessageObjects = NumberOfMessageObjects - NumberOfTxMessageObjects; -constexpr unsigned TxMessageObjectNumber = 1; - -/** - * Total number of CAN errors. - * Does not overflow. - */ -volatile std::uint32_t error_cnt = 0; - -/** - * False if there's no pending TX frame, i.e. write is possible. - */ -volatile bool tx_pending = false; - -/** - * Currently pending frame must be aborted on first error. - */ -volatile bool tx_abort_on_error = false; - -/** - * Gets updated every time the CAN IRQ handler is being called. - */ -volatile std::uint64_t last_irq_utc_timestamp = 0; - -/** - * Set by the driver on every successful TX or RX; reset by the application. - */ -volatile bool had_activity = false; - -/** - * After a received message gets extracted from C_CAN, it will be stored in the RX queue until libuavcan - * reads it via select()/receive() calls. - */ -class RxQueue -{ - struct Item { - std::uint64_t utc_usec = 0; - uavcan::CanFrame frame; - }; - - Item buf_[UAVCAN_SOCKETCAN_RX_QUEUE_LEN]; - std::uint32_t overflow_cnt_ = 0; - std::uint8_t in_ = 0; - std::uint8_t out_ = 0; - std::uint8_t len_ = 0; - -public: - void push(const uavcan::CanFrame &frame, const volatile std::uint64_t &utc_usec) - { - buf_[in_].frame = frame; - buf_[in_].utc_usec = utc_usec; - in_++; - - if (in_ >= UAVCAN_SOCKETCAN_RX_QUEUE_LEN) { - in_ = 0; - } - - len_++; - - if (len_ > UAVCAN_SOCKETCAN_RX_QUEUE_LEN) { - len_ = UAVCAN_SOCKETCAN_RX_QUEUE_LEN; - - if (overflow_cnt_ < 0xFFFFFFFF) { - overflow_cnt_++; - } - - out_++; - - if (out_ >= UAVCAN_SOCKETCAN_RX_QUEUE_LEN) { - out_ = 0; - } - } - } - - void pop(uavcan::CanFrame &out_frame, std::uint64_t &out_utc_usec) - { - if (len_ > 0) { - out_frame = buf_[out_].frame; - out_utc_usec = buf_[out_].utc_usec; - out_++; - - if (out_ >= UAVCAN_SOCKETCAN_RX_QUEUE_LEN) { - out_ = 0; - } - - len_--; - } - } - - unsigned getLength() const { return len_; } - - std::uint32_t getOverflowCount() const { return overflow_cnt_; } -}; - -RxQueue rx_queue; - struct BitTimingSettings { std::uint32_t canclkdiv; @@ -141,7 +69,199 @@ struct BitTimingSettings { } // namespace -CanDriver CanDriver::self; +uavcan::uint32_t CanIface::socketInit(const char *can_iface_name) +{ + + struct sockaddr_can addr; + struct ifreq ifr; + + //FIXME Change this when we update to DroneCAN with CAN FD support + 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; +} + +uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) +{ + int res = -1; + + /* Copy CanardFrame to can_frame/canfd_frame */ + if (_can_fd) { + _send_frame.can_id = frame.id | CAN_EFF_FLAG; + _send_frame.len = frame.dlc; + memcpy(&_send_frame.data, frame.data, frame.dlc); + + } else { + struct can_frame *net_frame = (struct can_frame *)&_send_frame; + net_frame->can_id = frame.id | CAN_EFF_FLAG; + net_frame->can_dlc = frame.dlc; + memcpy(&net_frame->data, frame.data, frame.dlc); + } + + /* Set CAN_RAW_TX_DEADLINE timestamp */ + _send_tv->tv_usec = tx_deadline.toUSec() % 1000000ULL; + _send_tv->tv_sec = (tx_deadline.toUSec() - _send_tv->tv_usec) / 1000000ULL; + + res = sendmsg(_fd, &_send_msg, 0); + + if (res > 0) { + return 1; + + } else { + return res; + } +} + +uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, + uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) +{ + int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT); + + if (result < 0) { + return result; + } + + /* Copy SocketCAN frame to CanardFrame */ + + if (_can_fd) { + struct canfd_frame *recv_frame = (struct canfd_frame *)&_recv_frame; + out_frame.id = recv_frame->can_id; + out_frame.dlc = recv_frame->len; + memcpy(out_frame.data, &recv_frame->data, recv_frame->len); + + } else { + struct can_frame *recv_frame = (struct can_frame *)&_recv_frame; + out_frame.id = recv_frame->can_id; + out_frame.dlc = recv_frame->can_dlc; + memcpy(out_frame.data, &recv_frame->data, recv_frame->can_dlc); + } + + /* 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); + out_ts_monotonic = uavcan::MonotonicTime::fromUSec(tv->tv_sec * 1000000ULL + tv->tv_usec); + } + + return result; +} + + +uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs, + uavcan::uint16_t num_configs) +{ + //FIXME + return 0; +} + +uavcan::uint64_t CanIface::getErrorCount() const +{ + //FIXME query SocketCAN network stack + return 0; +} + +uavcan::uint16_t CanIface::getNumFilters() const +{ + //FIXME + return 0; +} + +int CanIface::getFD() +{ + return _fd; +} uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) { @@ -151,126 +271,74 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) int CanDriver::init(uavcan::uint32_t bitrate) { - { - //FIXME - } + pfds[0].fd = if_[0].getFD(); + pfds[0].events = POLLIN | POLLOUT; +#if UAVCAN_SOCKETCAN_NUM_IFACES > 1 + pfds[1].fd = if_[1].getFD(); + pfds[1].events = POLLIN | POLLOUT; +#endif /* - * Applying default filter configuration (accept all) + * TODO add filter configuration ioctl */ - if (configureFilters(nullptr, 0) < 0) { - return -1; - } return 0; } -bool CanDriver::hasReadyRx() const -{ - CriticalSectionLocker locker; - return rx_queue.getLength() > 0; -} - -bool CanDriver::hasEmptyTx() const -{ - CriticalSectionLocker locker; - return !tx_pending; -} - -bool CanDriver::hadActivity() -{ - CriticalSectionLocker locker; - const bool ret = had_activity; - had_activity = false; - return ret; -} - uavcan::uint32_t CanDriver::getRxQueueOverflowCount() const { - CriticalSectionLocker locker; - return rx_queue.getOverflowCount(); + //FIXME query SocketCAN network stack + return 0; } bool CanDriver::isInBusOffState() const { - //FIXME + //FIXME no interface available yet, maybe make a NuttX ioctl return false; } -uavcan::int16_t CanDriver::send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) -{ - //FIXME - return 1; -} - -uavcan::int16_t CanDriver::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, - uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags) -{ - out_ts_monotonic = clock.getMonotonic(); - out_flags = 0; // We don't support any IO flags - - CriticalSectionLocker locker; - - if (rx_queue.getLength() == 0) { - return 0; - } - - std::uint64_t ts_utc = 0; - rx_queue.pop(out_frame, ts_utc); - out_ts_utc = uavcan::UtcTime::fromUSec(ts_utc); - return 1; -} - uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks, const uavcan::CanFrame * (&)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) { + std::int64_t timeout_usec = (blocking_deadline - clock.getMonotonic()).toUSec(); - - const bool noblock = ((inout_masks.read == 1) && hasReadyRx()) || - ((inout_masks.write == 1) && hasEmptyTx()); - - if (!noblock && (clock.getMonotonic() > blocking_deadline)) { - + if (timeout_usec < 0) { + timeout_usec = 0; } - inout_masks.read = hasReadyRx() ? 1 : 0; + inout_masks.read = 0; + //FIXME NuttX SocketCAN implement POLLOUT + inout_masks.write = 0x3; - inout_masks.write = (hasEmptyTx()) ? 1 : 0; // Disable write while in bus-off + if (poll(pfds, UAVCAN_SOCKETCAN_NUM_IFACES, timeout_usec / 1000) > 0) { + for (int i = 0; i < UAVCAN_SOCKETCAN_NUM_IFACES; i++) { + if (pfds[i].revents & POLLIN) { + inout_masks.read |= 1U << i; + } + + if (pfds[i].revents & POLLOUT) { + inout_masks.write |= 1U << i; + } + } + } return 0; // Return value doesn't matter as long as it is non-negative } -uavcan::int16_t CanDriver::configureFilters(const uavcan::CanFilterConfig *filter_configs, - uavcan::uint16_t num_configs) -{ - CriticalSectionLocker locker; - - //FIXME - - return 0; -} - -uavcan::uint64_t CanDriver::getErrorCount() const -{ - CriticalSectionLocker locker; - return std::uint64_t(error_cnt) + std::uint64_t(rx_queue.getOverflowCount()); -} - -uavcan::uint16_t CanDriver::getNumFilters() const -{ - return NumberOfRxMessageObjects; -} uavcan::ICanIface *CanDriver::getIface(uavcan::uint8_t iface_index) { - return (iface_index == 0) ? this : nullptr; + if (iface_index > (UAVCAN_SOCKETCAN_NUM_IFACES - 1)) { + return nullptr; + } + + return &if_[iface_index]; } uavcan::uint8_t CanDriver::getNumIfaces() const { - return 1; + return UAVCAN_SOCKETCAN_NUM_IFACES; } } diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp index 165873b698..1060c611f2 100644 --- a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp @@ -1,7 +1,37 @@ -/* - * Copyright (C) 2014, 2018 Pavel Kirienko - * Kinetis Port Author David Sidrane - */ +/**************************************************************************** + * + * Copyright (C) 2014 Pavel Kirienko + * Kinetis Port Author David Sidrane + * NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors + * + * 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 #include diff --git a/src/drivers/uavcannode/UavcanNode.cpp b/src/drivers/uavcannode/UavcanNode.cpp index a70058e885..b42b46e9d6 100644 --- a/src/drivers/uavcannode/UavcanNode.cpp +++ b/src/drivers/uavcannode/UavcanNode.cpp @@ -61,6 +61,8 @@ using namespace time_literals; namespace uavcannode { + + /** * @file uavcan_main.cpp * @@ -92,7 +94,8 @@ boot_app_shared_section app_descriptor_t AppDescriptor = { UavcanNode *UavcanNode::_instance; -UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : +UavcanNode::UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDriver &can_driver, + uavcan::ISystemClock &system_clock) : ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan), _node(can_driver, system_clock, _pool_allocator), _time_sync_slave(_node), @@ -102,6 +105,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys { int res = pthread_mutex_init(&_node_mutex, nullptr); + _can = can_init; + _bitrate = bitrate; + if (res < 0) { std::abort(); } @@ -158,39 +164,27 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) return -1; } - /* - * CAN driver init - * Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver - * shipped with libuavcan does not support deinitialization. - */ - static CanInitHelper *can = nullptr; + static CanInitHelper *_can = nullptr; - if (can == nullptr) { + if (_can == nullptr) { - can = new CanInitHelper(); + _can = new CanInitHelper(); - if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown + if (_can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown PX4_ERR("Out of memory"); return -1; } - - const int can_init_res = can->init(bitrate); - - if (can_init_res < 0) { - PX4_ERR("CAN driver init failed %i", can_init_res); - return can_init_res; - } } // Node init - _instance = new UavcanNode(can->driver, UAVCAN_DRIVER::SystemClock::instance()); + _instance = new UavcanNode(_can, bitrate, _can->driver, UAVCAN_DRIVER::SystemClock::instance()); if (_instance == nullptr) { PX4_ERR("Out of memory"); return -1; } - const int node_init_res = _instance->init(node_id, can->driver.updateEvent()); + const int node_init_res = _instance->init(node_id, _can->driver.updateEvent()); if (node_init_res < 0) { delete _instance; @@ -334,45 +328,7 @@ int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline); - int rv = _node.start(); - - if (rv < 0) { - return rv; - } - - // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation - - if (node_id == 0) { - - uavcan::DynamicNodeIDClient client(_node); - - int client_start_res = client.start(_node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE - node_id); - - if (client_start_res < 0) { - PX4_ERR("Failed to start the dynamic node ID client"); - return client_start_res; - } - - watchdog_pet(); // If allocation takes too long reboot - - /* - * Waiting for the client to obtain a node ID. - * This may take a few seconds. - */ - - while (!client.isAllocationComplete()) { - const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter - - if (res < 0) { - PX4_ERR("Transient failure: %d", res); - } - } - - _node.setNodeID(client.getAllocatedNodeID()); - } - - return rv; + return 1; } // Restart handler @@ -397,6 +353,51 @@ void UavcanNode::Run() watchdog_pet(); if (!_initialized) { + + + const int can_init_res = _can->init(_bitrate); + + if (can_init_res < 0) { + PX4_ERR("CAN driver init failed %i", can_init_res); + } + + int rv = _node.start(); + + if (rv < 0) { + PX4_ERR("Failed to start the node"); + } + + // If the node_id was not supplied by the bootloader do Dynamic Node ID allocation + + if (_node.getNodeID() == 0) { + + uavcan::DynamicNodeIDClient client(_node); + + int client_start_res = client.start(_node.getHardwareVersion().unique_id, // USING THE SAME UNIQUE ID AS ABOVE + _node.getNodeID()); + + if (client_start_res < 0) { + PX4_ERR("Failed to start the dynamic node ID client"); + } + + watchdog_pet(); // If allocation takes too long reboot + + /* + * Waiting for the client to obtain a node ID. + * This may take a few seconds. + */ + + while (!client.isAllocationComplete()) { + const int res = _node.spin(uavcan::MonotonicDuration::fromMSec(200)); // Spin duration doesn't matter + + if (res < 0) { + PX4_ERR("Transient failure: %d", res); + } + } + + _node.setNodeID(client.getAllocatedNodeID()); + } + up_time = hrt_absolute_time(); get_node().setRestartRequestHandler(&restart_request_handler); _param_server.start(&_param_manager); diff --git a/src/drivers/uavcannode/UavcanNode.hpp b/src/drivers/uavcannode/UavcanNode.hpp index e6452445fb..f7a3a25374 100644 --- a/src/drivers/uavcannode/UavcanNode.hpp +++ b/src/drivers/uavcannode/UavcanNode.hpp @@ -120,7 +120,8 @@ public: typedef UAVCAN_DRIVER::CanInitHelper CanInitHelper; typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate; - UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock); + UavcanNode(CanInitHelper *can_init, uint32_t bitrate, uavcan::ICanDriver &can_driver, + uavcan::ISystemClock &system_clock); virtual ~UavcanNode(); @@ -139,6 +140,9 @@ public: /* The bit rate that can be passed back to the bootloader */ int32_t active_bitrate{0}; + uint32_t _bitrate; + + CanInitHelper *_can; private: void Run() override;