From 7de444ec5392324ad67cc6cfc43ceb2d9e40ace8 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sun, 31 May 2020 17:58:24 +0530 Subject: [PATCH] HAL_Linux: add uavcan agnostic CANSocket Iface Driver --- libraries/AP_HAL_Linux/CAN.cpp | 595 -------------------- libraries/AP_HAL_Linux/CAN.h | 221 -------- libraries/AP_HAL_Linux/CANSocketIface.cpp | 604 +++++++++++++++++++++ libraries/AP_HAL_Linux/CANSocketIface.h | 197 +++++++ libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 12 +- libraries/AP_HAL_Linux/HAL_Linux_Class.h | 7 + 6 files changed, 819 insertions(+), 817 deletions(-) delete mode 100644 libraries/AP_HAL_Linux/CAN.cpp delete mode 100644 libraries/AP_HAL_Linux/CAN.h create mode 100644 libraries/AP_HAL_Linux/CANSocketIface.cpp create mode 100644 libraries/AP_HAL_Linux/CANSocketIface.h diff --git a/libraries/AP_HAL_Linux/CAN.cpp b/libraries/AP_HAL_Linux/CAN.cpp deleted file mode 100644 index 170b15521c..0000000000 --- a/libraries/AP_HAL_Linux/CAN.cpp +++ /dev/null @@ -1,595 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -/* - * Many thanks to members of the UAVCAN project: - * Pavel Kirienko - * Ilia Sheremet - * - * license info can be found in the uavcan submodule located: - * modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp - */ - -#include -#include - -#include - -#if HAL_WITH_UAVCAN - -#include "CAN.h" - -#include -#include - -#include -#include -#include -#include - -extern const AP_HAL::HAL& hal; - -using namespace Linux; - -uavcan::MonotonicTime getMonotonic() -{ - return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64()); -} - -static can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame) -{ - can_frame sockcan_frame { uavcan_frame.id& uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } }; - std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data); - if (uavcan_frame.isExtended()) { - sockcan_frame.can_id |= CAN_EFF_FLAG; - } - if (uavcan_frame.isErrorFrame()) { - sockcan_frame.can_id |= CAN_ERR_FLAG; - } - if (uavcan_frame.isRemoteTransmissionRequest()) { - sockcan_frame.can_id |= CAN_RTR_FLAG; - } - return sockcan_frame; -} - -static uavcan::CanFrame makeUavcanFrame(const can_frame& sockcan_frame) -{ - uavcan::CanFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); - if (sockcan_frame.can_id & CAN_EFF_FLAG) { - uavcan_frame.id |= uavcan::CanFrame::FlagEFF; - } - if (sockcan_frame.can_id & CAN_ERR_FLAG) { - uavcan_frame.id |= uavcan::CanFrame::FlagERR; - } - if (sockcan_frame.can_id & CAN_RTR_FLAG) { - uavcan_frame.id |= uavcan::CanFrame::FlagRTR; - } - return uavcan_frame; -} - -bool CAN::begin(uint32_t bitrate) -{ - if (_initialized) { - return _initialized; - } - - // TODO: Add possibility change bitrate - _fd = openSocket(HAL_BOARD_CAN_IFACE_NAME); - if (_fd > 0) { - _bitrate = bitrate; - _initialized = true; - } else { - _initialized = false; - } - return _initialized; -} - -void CAN::reset() -{ - if (_initialized && _bitrate != 0) { - close(_fd); - begin(_bitrate); - } -} - -void CAN::end() -{ - _initialized = false; - close(_fd); -} - -bool CAN::is_initialized() -{ - return _initialized; -} - -int32_t CAN::tx_pending() -{ - if (_initialized) { - return _tx_queue.size(); - } else { - return -1; - } -} - -int32_t CAN::available() -{ - if (_initialized) { - return _rx_queue.size(); - } else { - return -1; - } -} - -int CAN::openSocket(const std::string& iface_name) -{ - errno = 0; - - int s = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if (s < 0) { - return s; - } - - std::shared_ptr defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); }); - const int ret = s; - - // Detect the iface index - auto ifr = ifreq(); - if (iface_name.length() >= IFNAMSIZ) { - errno = ENAMETOOLONG; - return -1; - } - std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length()); - if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) { - return -1; - } - - // Bind to the specified CAN iface - { - auto addr = sockaddr_can(); - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - if (bind(s, reinterpret_cast(&addr), sizeof(addr)) < 0) { - return -1; - } - } - - // Configure - { - const int on = 1; - // Timestamping - if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { - return -1; - } - // Socket loopback - if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) { - return -1; - } - // Non-blocking - if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) { - return -1; - } - } - - // Validate the resulting socket - { - int socket_error = 0; - socklen_t errlen = sizeof(socket_error); - getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&socket_error), &errlen); - if (socket_error != 0) { - errno = socket_error; - return -1; - } - } - s = -1; - return ret; -} - -int16_t CAN::send(const uavcan::CanFrame& frame, const uavcan::MonotonicTime tx_deadline, - const uavcan::CanIOFlags flags) -{ - _tx_queue.emplace(frame, tx_deadline, flags, _tx_frame_counter); - _tx_frame_counter++; - _pollRead(); // Read poll is necessary because it can release the pending TX flag - _pollWrite(); - return 1; -} - -int16_t CAN::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) -{ - if (_rx_queue.empty()) { - _pollRead(); // This allows to use the socket not calling poll() explicitly. - if (_rx_queue.empty()) { - return 0; - } - } - { - const RxItem& rx = _rx_queue.front(); - out_frame = rx.frame; - out_ts_monotonic = rx.ts_mono; - out_ts_utc = rx.ts_utc; - out_flags = rx.flags; - } - _rx_queue.pop(); - return 1; -} - -bool CAN::hasReadyTx() const -{ - return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue); -} - -bool CAN::hasReadyRx() const -{ - return !_rx_queue.empty(); -} - -void CAN::poll(bool read, bool write) -{ - if (read) { - _pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue - } - if (write) { - _pollWrite(); - } -} - -int16_t CAN::configureFilters(const uavcan::CanFilterConfig* const filter_configs, - const std::uint16_t num_configs) -{ - if (filter_configs == nullptr) { - return -1; - } - _hw_filters_container.clear(); - _hw_filters_container.resize(num_configs); - - for (unsigned i = 0; i < num_configs; i++) { - const uavcan::CanFilterConfig& fc = filter_configs[i]; - _hw_filters_container[i].can_id = fc.id & uavcan::CanFrame::MaskExtID; - _hw_filters_container[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID; - if (fc.id & uavcan::CanFrame::FlagEFF) { - _hw_filters_container[i].can_id |= CAN_EFF_FLAG; - } - if (fc.id & uavcan::CanFrame::FlagRTR) { - _hw_filters_container[i].can_id |= CAN_RTR_FLAG; - } - if (fc.mask & uavcan::CanFrame::FlagEFF) { - _hw_filters_container[i].can_mask |= CAN_EFF_FLAG; - } - if (fc.mask & uavcan::CanFrame::FlagRTR) { - _hw_filters_container[i].can_mask |= CAN_RTR_FLAG; - } - } - - return 0; -} - -/** - * SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited. - * This method returns a constant value. - */ -static constexpr unsigned NumFilters = CAN_FILTER_NUMBER; -uint16_t CAN::getNumFilters() const { return NumFilters; } - -uint64_t CAN::getErrorCount() const -{ - uint64_t ec = 0; - for (auto& kv : _errors) { ec += kv.second; } - return ec; -} - -void CAN::_pollWrite() -{ - while (hasReadyTx()) { - const TxItem tx = _tx_queue.top(); - - if (tx.deadline >= getMonotonic()) { - const int res = _write(tx.frame); - if (res == 1) { // Transmitted successfully - _incrementNumFramesInSocketTxQueue(); - if (tx.flags & uavcan::CanIOFlagLoopback) { - _pending_loopback_ids.insert(tx.frame.id); - } - } else if (res == 0) { // Not transmitted, nor is it an error - break; // Leaving the loop, the frame remains enqueued for the next retry - } else { // Transmission error - _registerError(SocketCanError::SocketWriteFailure); - } - } else { - _registerError(SocketCanError::TxTimeout); - } - - // Removing the frame from the queue even if transmission failed - _tx_queue.pop(); - } -} - -void CAN::_pollRead() -{ - uint8_t iterations_count = 0; - while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT) - { - iterations_count++; - RxItem rx; - rx.ts_mono = getMonotonic(); // Monotonic timestamp is not required to be precise (unlike UTC) - bool loopback = false; - const int res = _read(rx.frame, rx.ts_utc, loopback); - if (res == 1) { - bool accept = true; - if (loopback) { // We receive loopback for all CAN frames - _confirmSentFrame(); - rx.flags |= uavcan::CanIOFlagLoopback; - accept = _wasInPendingLoopbackSet(rx.frame); - } - if (accept) { - _rx_queue.push(rx); - } - } else if (res == 0) { - break; - } else { - _registerError(SocketCanError::SocketReadFailure); - break; - } - } -} - -int CAN::_write(const uavcan::CanFrame& frame) const -{ - errno = 0; - - const can_frame sockcan_frame = makeSocketCanFrame(frame); - - const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); - if (res <= 0) { - if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error - return 0; - } - return res; - } - if (res != sizeof(sockcan_frame)) { - return -1; - } - return 1; -} - - -int CAN::_read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const -{ - auto iov = iovec(); - auto sockcan_frame = can_frame(); - iov.iov_base = &sockcan_frame; - iov.iov_len = sizeof(sockcan_frame); - union { - uint8_t data[CMSG_SPACE(sizeof(::timeval))]; - struct cmsghdr align; - } control; - - auto msg = msghdr(); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control.data; - msg.msg_controllen = sizeof(control.data); - - const int res = recvmsg(_fd, &msg, MSG_DONTWAIT); - if (res <= 0) { - return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; - } - /* - * Flags - */ - loopback = (msg.msg_flags & static_cast(MSG_CONFIRM)) != 0; - - if (!loopback && !_checkHWFilters(sockcan_frame)) { - return 0; - } - - frame = makeUavcanFrame(sockcan_frame); - /* - * Timestamp - */ - const cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg); - if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP) { - auto tv = timeval(); - std::memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); // Copy to avoid alignment problems - ts_utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * 1000000ULL + tv.tv_usec); - } else { - return -1; - } - return 1; -} - -void CAN::_incrementNumFramesInSocketTxQueue() -{ - _frames_in_socket_tx_queue++; -} - -void CAN::_confirmSentFrame() -{ - if (_frames_in_socket_tx_queue > 0) { - _frames_in_socket_tx_queue--; - } -} - -bool CAN::_wasInPendingLoopbackSet(const uavcan::CanFrame& frame) -{ - if (_pending_loopback_ids.count(frame.id) > 0) { - _pending_loopback_ids.erase(frame.id); - return true; - } - return false; -} - -bool CAN::_checkHWFilters(const can_frame& frame) const -{ - if (!_hw_filters_container.empty()) { - for (auto& f : _hw_filters_container) { - if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) { - return true; - } - } - return false; - } else { - return true; - } -} - -void CANManager::IfaceWrapper::updateDownStatusFromPollResult(const pollfd& pfd) -{ - if (!_down&& (pfd.revents & POLLERR)) { - int error = 0; - socklen_t errlen = sizeof(error); - getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); - - _down= error == ENETDOWN || error == ENODEV; - - hal.console->printf("Iface %d is dead; error %d", this->getFileDescriptor(), error); - } -} - -bool CANManager::begin(uint32_t bitrate, uint8_t can_number) -{ - if (init(can_number) >= 0) { - _initialized = true; - } - return _initialized; -} - -bool CANManager::is_initialized() -{ - return _initialized; -} - -void CANManager::initialized(bool val) -{ - _initialized = val; -} - -CAN* CANManager::getIface(uint8_t iface_index) -{ - return (iface_index >= _ifaces.size()) ? nullptr : _ifaces[iface_index].get(); -} - -int CANManager::init(uint8_t can_number) -{ - int res = -1; - char iface_name[16]; - sprintf(iface_name, "can%u", can_number); - - res = addIface(iface_name); - if (res < 0) { - hal.console->printf("CANManager: init %s failed\n", iface_name); - } - - return res; -} - -int16_t CANManager::select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces], - uavcan::MonotonicTime blocking_deadline) -{ - // Detecting whether we need to block at all - bool need_block = (inout_masks.write == 0); // Write queue is infinite - - for (unsigned i = 0; need_block && (i < _ifaces.size()); i++) { - const bool need_read = inout_masks.read & (1 << i); - if (need_read && _ifaces[i]->hasReadyRx()) { - need_block = false; - } - } - - if (need_block) { - // Poll FD set setup - pollfd pollfds[uavcan::MaxCanIfaces] = {}; - unsigned num_pollfds = 0; - IfaceWrapper* pollfd_index_to_iface[uavcan::MaxCanIfaces] = {}; - - for (unsigned i = 0; i < _ifaces.size(); i++) { - if (_ifaces[i]->isDown()) { - continue; - } - pollfds[num_pollfds].fd = _ifaces[i]->getFileDescriptor(); - pollfds[num_pollfds].events = POLLIN; - if (_ifaces[i]->hasReadyTx() || (inout_masks.write & (1U << i))) { - pollfds[num_pollfds].events |= POLLOUT; - } - pollfd_index_to_iface[num_pollfds] = _ifaces[i].get(); - num_pollfds++; - } - - if (num_pollfds == 0) { - return 0; - } - - // Timeout conversion - const std::int64_t timeout_usec = (blocking_deadline - getMonotonic()).toUSec(); - auto ts = timespec(); - if (timeout_usec > 0) { - ts.tv_sec = timeout_usec / 1000000LL; - ts.tv_nsec = (timeout_usec % 1000000LL) * 1000; - } - - // Blocking here - const int res = ppoll(pollfds, num_pollfds, &ts, nullptr); - if (res < 0) { - return res; - } - - // Handling poll output - for (unsigned i = 0; i < num_pollfds; i++) { - pollfd_index_to_iface[i]->updateDownStatusFromPollResult(pollfds[i]); - - const bool poll_read = pollfds[i].revents & POLLIN; - const bool poll_write = pollfds[i].revents & POLLOUT; - pollfd_index_to_iface[i]->poll(poll_read, poll_write); - } - } - - // Writing the output masks - inout_masks = uavcan::CanSelectMasks(); - for (unsigned i = 0; i < _ifaces.size(); i++) { - if (!_ifaces[i]->isDown()) { - inout_masks.write |= std::uint8_t(1U << i); // Always ready to write if not down - } - if (_ifaces[i]->hasReadyRx()) { - inout_masks.read |= std::uint8_t(1U << i); // Readability depends only on RX buf, even if down - } - } - - // Return value is irrelevant as long as it's non-negative - return _ifaces.size(); -} - -int CANManager::addIface(const std::string& iface_name) -{ - if (_ifaces.size() >= uavcan::MaxCanIfaces) { - return -1; - } - - // Open the socket - const int fd = CAN::openSocket(iface_name); - if (fd < 0) { - return fd; - } - - // Construct the iface - upon successful construction the iface will take ownership of the fd. - _ifaces.emplace_back(new IfaceWrapper(fd)); - - hal.console->printf("New iface '%s' fd %d\n", iface_name.c_str(), fd); - - return _ifaces.size() - 1; -} - -#endif diff --git a/libraries/AP_HAL_Linux/CAN.h b/libraries/AP_HAL_Linux/CAN.h deleted file mode 100644 index 221f111b80..0000000000 --- a/libraries/AP_HAL_Linux/CAN.h +++ /dev/null @@ -1,221 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -/* - * Many thanks to members of the UAVCAN project: - * Pavel Kirienko - * Ilia Sheremet - * - * license info can be found in the uavcan submodule located: - * modules/uavcan/LICENSE - * modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp - */ - - -#pragma once - -#include "AP_HAL_Linux.h" -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace Linux { - -enum class SocketCanError -{ - SocketReadFailure, - SocketWriteFailure, - TxTimeout -}; - -#define CAN_MAX_POLL_ITERATIONS_COUNT 100 -#define CAN_MAX_INIT_TRIES_COUNT 100 -#define CAN_FILTER_NUMBER 8 - -class CAN: public AP_HAL::CANHal { -public: - CAN(int socket_fd=0) - : _fd(socket_fd) - , _frames_in_socket_tx_queue(0) - , _max_frames_in_socket_tx_queue(2) - { } - ~CAN() { } - - bool begin(uint32_t bitrate) override; - - void end() override; - - void reset() override; - - bool is_initialized() override; - - int32_t tx_pending() override; - - int32_t available() override; - - static int openSocket(const std::string& iface_name); - - int getFileDescriptor() const { return _fd; } - - int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, - uavcan::CanIOFlags flags) override; - - int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, - uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override; - - bool hasReadyTx() const; - - bool hasReadyRx() const; - - void poll(bool read, bool write); - - int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs) override; - - uint16_t getNumFilters() const override; - - uint64_t getErrorCount() const override; - - -private: - struct TxItem - { - uavcan::CanFrame frame; - uavcan::MonotonicTime deadline; - uavcan::CanIOFlags flags = 0; - std::uint64_t order = 0; - - TxItem(const uavcan::CanFrame& arg_frame, uavcan::MonotonicTime arg_deadline, - uavcan::CanIOFlags arg_flags, std::uint64_t arg_order) - : frame(arg_frame) - , deadline(arg_deadline) - , flags(arg_flags) - , order(arg_order) - { } - - bool operator<(const TxItem& rhs) const - { - if (frame.priorityLowerThan(rhs.frame)) { - return true; - } - if (frame.priorityHigherThan(rhs.frame)) { - return false; - } - return order > rhs.order; - } - }; - - struct RxItem - { - uavcan::CanFrame frame; - uavcan::MonotonicTime ts_mono; - uavcan::UtcTime ts_utc; - uavcan::CanIOFlags flags; - - RxItem() - : flags(0) - { } - }; - - void _pollWrite(); - - void _pollRead(); - - int _write(const uavcan::CanFrame& frame) const; - - int _read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const; - - void _incrementNumFramesInSocketTxQueue(); - - void _confirmSentFrame(); - - bool _wasInPendingLoopbackSet(const uavcan::CanFrame& frame); - - bool _checkHWFilters(const can_frame& frame) const; - - void _registerError(SocketCanError e) { _errors[e]++; } - - uint32_t _bitrate; - - bool _initialized; - - int _fd; - - const unsigned _max_frames_in_socket_tx_queue; - unsigned _frames_in_socket_tx_queue; - uint64_t _tx_frame_counter; - - std::map _errors; - std::priority_queue _tx_queue; - std::queue _rx_queue; - std::unordered_multiset _pending_loopback_ids; - std::vector _hw_filters_container; -}; - -class CANManager: public AP_HAL::CANManager, public uavcan::ICanDriver { -public: - static CANManager *from(AP_HAL::CANManager *can) - { - return static_cast(can); - } - - CANManager() : AP_HAL::CANManager(this) { _ifaces.reserve(uavcan::MaxCanIfaces); } - ~CANManager() { } - - //These methods belong to AP_HAL::CANManager - - virtual bool begin(uint32_t bitrate, uint8_t can_number) override; - - virtual void initialized(bool val) override; - virtual bool is_initialized() override; - - //These methods belong to ICanDriver - - virtual CAN* getIface(uint8_t iface_index) override; - - virtual uint8_t getNumIfaces() const override { return _ifaces.size(); } - - virtual int16_t select(uavcan::CanSelectMasks& inout_masks, - const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override; - - int init(uint8_t can_number); - - int addIface(const std::string& iface_name); - -private: - class IfaceWrapper : public CAN - { - bool _down = false; - - public: - IfaceWrapper(int fd) : CAN(fd) { } - - void updateDownStatusFromPollResult(const pollfd& pfd); - - bool isDown() const { return _down; } - }; - - bool _initialized; - - std::vector> _ifaces; -}; - -} diff --git a/libraries/AP_HAL_Linux/CANSocketIface.cpp b/libraries/AP_HAL_Linux/CANSocketIface.cpp new file mode 100644 index 0000000000..e41bd954ba --- /dev/null +++ b/libraries/AP_HAL_Linux/CANSocketIface.cpp @@ -0,0 +1,604 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + * Many thanks to members of the UAVCAN project: + * Pavel Kirienko + * Ilia Sheremet + * + * license info can be found in the uavcan submodule located: + * modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp + */ + +#include +#include + +#if HAL_NUM_CAN_IFACES + +#include "CANSocketIface.h" + +#include +#include + +#include +#include +#include +#include +#include +#include "Scheduler.h" +#include +extern const AP_HAL::HAL& hal; + +using namespace Linux; + +#define Debug(fmt, args...) do { AP::can().log_text(AP_CANManager::LOG_DEBUG, "CANLinuxIface", fmt, ##args); } while (0) + +CANIface::CANSocketEventSource CANIface::evt_can_socket[HAL_NUM_CAN_IFACES]; + +static can_frame makeSocketCanFrame(const AP_HAL::CANFrame& uavcan_frame) +{ + can_frame sockcan_frame { uavcan_frame.id& AP_HAL::CANFrame::MaskExtID, uavcan_frame.dlc, { } }; + std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data); + if (uavcan_frame.isExtended()) { + sockcan_frame.can_id |= CAN_EFF_FLAG; + } + if (uavcan_frame.isErrorFrame()) { + sockcan_frame.can_id |= CAN_ERR_FLAG; + } + if (uavcan_frame.isRemoteTransmissionRequest()) { + sockcan_frame.can_id |= CAN_RTR_FLAG; + } + return sockcan_frame; +} + +static AP_HAL::CANFrame makeUavcanFrame(const can_frame& sockcan_frame) +{ + AP_HAL::CANFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); + if (sockcan_frame.can_id & CAN_EFF_FLAG) { + uavcan_frame.id |= AP_HAL::CANFrame::FlagEFF; + } + if (sockcan_frame.can_id & CAN_ERR_FLAG) { + uavcan_frame.id |= AP_HAL::CANFrame::FlagERR; + } + if (sockcan_frame.can_id & CAN_RTR_FLAG) { + uavcan_frame.id |= AP_HAL::CANFrame::FlagRTR; + } + return uavcan_frame; +} + +bool CANIface::is_initialized() const +{ + return _initialized; +} + +int CANIface::_openSocket(const std::string& iface_name) +{ + errno = 0; + + int s = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (s < 0) { + return s; + } + + std::shared_ptr defer(&s, [](int* fd) { if (*fd >= 0) close(*fd); }); + const int ret = s; + + // Detect the iface index + auto ifr = ifreq(); + if (iface_name.length() >= IFNAMSIZ) { + errno = ENAMETOOLONG; + return -1; + } + std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length()); + if (ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) { + return -1; + } + + // Bind to the specified CAN iface + { + auto addr = sockaddr_can(); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + if (bind(s, reinterpret_cast(&addr), sizeof(addr)) < 0) { + return -1; + } + } + + // Configure + { + const int on = 1; + // Timestamping + if (setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { + return -1; + } + // Socket loopback + if (setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) { + return -1; + } + // Non-blocking + if (fcntl(s, F_SETFL, O_NONBLOCK) < 0) { + return -1; + } + } + + // Validate the resulting socket + { + int socket_error = 0; + socklen_t errlen = sizeof(socket_error); + getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&socket_error), &errlen); + if (socket_error != 0) { + errno = socket_error; + return -1; + } + } + s = -1; + return ret; +} + +int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline, + const CANIface::CanIOFlags flags) +{ + CanTxItem tx_item {}; + tx_item.frame = frame; + if (flags & Loopback) { + tx_item.loopback = true; + } + if (flags & AbortOnError) { + tx_item.abort_on_error = true; + } + tx_item.setup = true; + tx_item.index = _tx_frame_counter; + tx_item.deadline = tx_deadline; + _tx_queue.emplace(tx_item); + _tx_frame_counter++; + stats.tx_requests++; + _pollRead(); // Read poll is necessary because it can release the pending TX flag + _pollWrite(); + return 1; +} + +int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, + CANIface::CanIOFlags& out_flags) +{ + if (_rx_queue.empty()) { + _pollRead(); // This allows to use the socket not calling poll() explicitly. + if (_rx_queue.empty()) { + return 0; + } + } + { + const CanRxItem& rx = _rx_queue.front(); + out_frame = rx.frame; + out_timestamp_us = rx.timestamp_us; + out_flags = rx.flags; + } + (void)_rx_queue.pop(); + return 1; +} + +bool CANIface::_hasReadyTx() const +{ + return !_tx_queue.empty() && (_frames_in_socket_tx_queue < _max_frames_in_socket_tx_queue); +} + +bool CANIface::_hasReadyRx() const +{ + return !_rx_queue.empty(); +} + +void CANIface::_poll(bool read, bool write) +{ + if (read) { + stats.num_poll_rx_events++; + _pollRead(); // Read poll must be executed first because it may decrement _frames_in_socket_tx_queue + } + if (write) { + stats.num_poll_tx_events++; + _pollWrite(); + } +} + +bool CANIface::configureFilters(const CanFilterConfig* const filter_configs, + const std::uint16_t num_configs) +{ + if (filter_configs == nullptr) { + return false; + } + _hw_filters_container.clear(); + _hw_filters_container.resize(num_configs); + + for (unsigned i = 0; i < num_configs; i++) { + const CanFilterConfig& fc = filter_configs[i]; + _hw_filters_container[i].can_id = fc.id & AP_HAL::CANFrame::MaskExtID; + _hw_filters_container[i].can_mask = fc.mask & AP_HAL::CANFrame::MaskExtID; + if (fc.id & AP_HAL::CANFrame::FlagEFF) { + _hw_filters_container[i].can_id |= CAN_EFF_FLAG; + } + if (fc.id & AP_HAL::CANFrame::FlagRTR) { + _hw_filters_container[i].can_id |= CAN_RTR_FLAG; + } + if (fc.mask & AP_HAL::CANFrame::FlagEFF) { + _hw_filters_container[i].can_mask |= CAN_EFF_FLAG; + } + if (fc.mask & AP_HAL::CANFrame::FlagRTR) { + _hw_filters_container[i].can_mask |= CAN_RTR_FLAG; + } + } + + return true; +} + +/** + * SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited. + * This method returns a constant value. + */ +static constexpr unsigned NumFilters = CAN_FILTER_NUMBER; +uint16_t CANIface::getNumFilters() const { return NumFilters; } + +uint32_t CANIface::getErrorCount() const +{ + uint32_t ec = 0; + for (auto& kv : _errors) { ec += kv.second; } + return ec; +} + +void CANIface::_pollWrite() +{ + while (_hasReadyTx()) { + const CanTxItem tx = _tx_queue.top(); + uint64_t curr_time = AP_HAL::native_micros64(); + if (tx.deadline >= curr_time) { + // hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time); + const int res = _write(tx.frame); + if (res == 1) { // Transmitted successfully + _incrementNumFramesInSocketTxQueue(); + if (tx.loopback) { + _pending_loopback_ids.insert(tx.frame.id); + } + stats.tx_success++; + } else if (res == 0) { // Not transmitted, nor is it an error + stats.tx_full++; + break; // Leaving the loop, the frame remains enqueued for the next retry + } else { // Transmission error + stats.tx_write_fail++; + } + } else { + // hal.console->printf("TDEAD: %lu CURRT: %lu DEL: %lu\n", tx.deadline, curr_time, curr_time-tx.deadline); + stats.tx_timedout++; + } + + // Removing the frame from the queue even if transmission failed + (void)_tx_queue.pop(); + } +} + +bool CANIface::_pollRead() +{ + uint8_t iterations_count = 0; + while (iterations_count < CAN_MAX_POLL_ITERATIONS_COUNT) + { + iterations_count++; + CanRxItem rx; + rx.timestamp_us = AP_HAL::native_micros64(); // Monotonic timestamp is not required to be precise (unlike UTC) + bool loopback = false; + const int res = _read(rx.frame, rx.timestamp_us, loopback); + if (res == 1) { + bool accept = true; + if (loopback) { // We receive loopback for all CAN frames + _confirmSentFrame(); + rx.flags |= Loopback; + accept = _wasInPendingLoopbackSet(rx.frame); + stats.tx_confirmed++; + } + if (accept) { + _rx_queue.push(rx); + stats.rx_received++; + return true; + } + } else if (res == 0) { + break; + } else { + stats.rx_errors++; + break; + } + } + return false; +} + +int CANIface::_write(const AP_HAL::CANFrame& frame) const +{ + errno = 0; + + const can_frame sockcan_frame = makeSocketCanFrame(frame); + + const int res = write(_fd, &sockcan_frame, sizeof(sockcan_frame)); + if (res <= 0) { + if (errno == ENOBUFS || errno == EAGAIN) { // Writing is not possible atm, not an error + return 0; + } + return res; + } + if (res != sizeof(sockcan_frame)) { + return -1; + } + return 1; +} + + +int CANIface::_read(AP_HAL::CANFrame& frame, uint64_t& timestamp_us, bool& loopback) const +{ + auto iov = iovec(); + auto sockcan_frame = can_frame(); + iov.iov_base = &sockcan_frame; + iov.iov_len = sizeof(sockcan_frame); + union { + uint8_t data[CMSG_SPACE(sizeof(::timeval))]; + struct cmsghdr align; + } control; + + auto msg = msghdr(); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control.data; + msg.msg_controllen = sizeof(control.data); + + const int res = recvmsg(_fd, &msg, MSG_DONTWAIT); + if (res <= 0) { + return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; + } + /* + * Flags + */ + loopback = (msg.msg_flags & static_cast(MSG_CONFIRM)) != 0; + + if (!loopback && !_checkHWFilters(sockcan_frame)) { + return 0; + } + + frame = makeUavcanFrame(sockcan_frame); + /* + * Timestamp + */ + timestamp_us = AP_HAL::native_micros64(); + return 1; +} + +// Might block forever, only to be used for testing +void CANIface::flush_tx() +{ + do { + _updateDownStatusFromPollResult(_pollfd); + _poll(true, true); + } while(!_tx_queue.empty() && !_down); +} + +void CANIface::clear_rx() +{ + // Clean Rx Queue + std::queue empty; + std::swap( _rx_queue, empty ); +} + +void CANIface::_incrementNumFramesInSocketTxQueue() +{ + _frames_in_socket_tx_queue++; +} + +void CANIface::_confirmSentFrame() +{ + if (_frames_in_socket_tx_queue > 0) { + _frames_in_socket_tx_queue--; + } +} + +bool CANIface::_wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame) +{ + if (_pending_loopback_ids.count(frame.id) > 0) { + _pending_loopback_ids.erase(frame.id); + return true; + } + return false; +} + +bool CANIface::_checkHWFilters(const can_frame& frame) const +{ + if (!_hw_filters_container.empty()) { + for (auto& f : _hw_filters_container) { + if (((frame.can_id & f.can_mask) ^ f.can_id) == 0) { + return true; + } + } + return false; + } else { + return true; + } +} + +void CANIface::_updateDownStatusFromPollResult(const pollfd& pfd) +{ + if (!_down && (pfd.revents & POLLERR)) { + int error = 0; + socklen_t errlen = sizeof(error); + getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); + + _down= error == ENETDOWN || error == ENODEV; + stats.num_downs++; + Debug("Iface %d is dead; error %d", _fd, error); + } +} + +bool CANIface::init(const uint32_t bitrate, const OperatingMode mode) +{ + char iface_name[16]; + sprintf(iface_name, "can%u", _self_index); + + if (_initialized) { + return _initialized; + } + + // TODO: Add possibility change bitrate + _fd = _openSocket(iface_name); + Debug("Socket opened iface_name: %s fd: %d", iface_name, _fd); + if (_fd > 0) { + _bitrate = bitrate; + _initialized = true; + } else { + _initialized = false; + } + return _initialized; +} + +bool CANIface::select(bool &read_select, bool &write_select, + const AP_HAL::CANFrame* const pending_tx, uint64_t blocking_deadline) +{ + // Detecting whether we need to block at all + bool need_block = !write_select; // Write queue is infinite + + if (read_select && _hasReadyRx()) { + need_block = false; + } + + if (need_block) { + if (_down) { + return false; + } else { + _pollfd.fd = _fd; + _pollfd.events |= POLLIN; + stats.num_rx_poll_req++; + if (_hasReadyTx() && write_select) { + _pollfd.events |= POLLOUT; + stats.num_tx_poll_req++; + } + } + if (_evt_handle != nullptr && blocking_deadline > AP_HAL::native_micros64()) { + _evt_handle->wait(blocking_deadline - AP_HAL::native_micros64()); + } + } + + // Writing the output masks + if (!_down) { + write_select = true; // Always ready to write if not down + } else { + write_select = false; + } + if (_hasReadyRx()) { + read_select = true; // Readability depends only on RX buf, even if down + } else { + read_select = false; + } + + // Return value is irrelevant as long as it's non-negative + return true; +} + +bool CANIface::set_event_handle(AP_HAL::EventHandle* handle) { + _evt_handle = handle; + evt_can_socket[_self_index]._ifaces[_self_index] = this; + _evt_handle->set_source(&evt_can_socket[_self_index]); + return true; +} + + +bool CANIface::CANSocketEventSource::wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) +{ + if (evt_handle == nullptr) { + return false; + } + pollfd pollfds[HAL_NUM_CAN_IFACES] {}; + uint8_t pollfd_iface_map[HAL_NUM_CAN_IFACES] {}; + unsigned long int num_pollfds = 0; + + // Poll FD set setup + for (unsigned i = 0; i < HAL_NUM_CAN_IFACES; i++) { + if (_ifaces[i] == nullptr) { + continue; + } + if (_ifaces[i]->_down) { + continue; + } + pollfds[num_pollfds] = _ifaces[i]->_pollfd; + pollfd_iface_map[num_pollfds] = i; + num_pollfds++; + _ifaces[i]->stats.num_poll_waits++; + } + + if (num_pollfds == 0) { + return true; + } + + // Timeout conversion + auto ts = timespec(); + ts.tv_sec = duration / 1000000LL; + ts.tv_nsec = (duration % 1000000LL) * 1000; + + // Blocking here + const int res = ppoll(pollfds, num_pollfds, &ts, nullptr); + + if (res < 0) { + return false; + } + + // Handling poll output + for (unsigned i = 0; i < num_pollfds; i++) { + if (_ifaces[pollfd_iface_map[i]] == nullptr) { + continue; + } + _ifaces[pollfd_iface_map[i]]->_updateDownStatusFromPollResult(pollfds[i]); + + const bool poll_read = pollfds[i].revents & POLLIN; + const bool poll_write = pollfds[i].revents & POLLOUT; + _ifaces[pollfd_iface_map[i]]->_poll(poll_read, poll_write); + } + return true; +} + +uint32_t CANIface::get_stats(char* data, uint32_t max_size) +{ + if (data == nullptr) { + return 0; + } + uint32_t ret = snprintf(data, max_size, + "tx_requests: %u\n" + "tx_write_fail: %u\n" + "tx_full: %u\n" + "tx_confirmed: %u\n" + "tx_success: %u\n" + "tx_timedout: %u\n" + "rx_received: %u\n" + "rx_errors: %u\n" + "num_downs: %u\n" + "num_rx_poll_req: %u\n" + "num_tx_poll_req: %u\n" + "num_poll_waits: %u\n" + "num_poll_tx_events: %u\n" + "num_poll_rx_events: %u\n", + stats.tx_requests, + stats.tx_write_fail, + stats.tx_full, + stats.tx_confirmed, + stats.tx_success, + stats.tx_timedout, + stats.rx_received, + stats.rx_errors, + stats.num_downs, + stats.num_rx_poll_req, + stats.num_tx_poll_req, + stats.num_poll_waits, + stats.num_poll_tx_events, + stats.num_poll_rx_events); + return ret; +} + +#endif diff --git a/libraries/AP_HAL_Linux/CANSocketIface.h b/libraries/AP_HAL_Linux/CANSocketIface.h new file mode 100644 index 0000000000..84e2c14a5f --- /dev/null +++ b/libraries/AP_HAL_Linux/CANSocketIface.h @@ -0,0 +1,197 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ + +/* + * Many thanks to members of the UAVCAN project: + * Pavel Kirienko + * Ilia Sheremet + * + * license info can be found in the uavcan submodule located: + * modules/uavcan/LICENSE + * modules/uavcan/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp + */ + + +#pragma once + +#include "AP_HAL_Linux.h" + +#if HAL_NUM_CAN_IFACES + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace Linux { + +enum class SocketCanError +{ + SocketReadFailure, + SocketWriteFailure, + TxTimeout +}; + +#define CAN_MAX_POLL_ITERATIONS_COUNT 100 +#define CAN_MAX_INIT_TRIES_COUNT 100 +#define CAN_FILTER_NUMBER 8 + +class CANIface: public AP_HAL::CANIface { +public: + CANIface(int index) + : _self_index(index) + , _frames_in_socket_tx_queue(0) + , _max_frames_in_socket_tx_queue(2) + { } + + ~CANIface() { } + + // Initialise CAN Peripheral + bool init(const uint32_t bitrate, const OperatingMode mode) override; + + // Put frame into Tx FIFO returns negative on error, 0 on buffer full, + // 1 on successfully pushing a frame into FIFO + int16_t send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline, + CanIOFlags flags) override; + + // Receive frame from Rx Buffer, returns negative on error, 0 on nothing available, + // 1 on successfully poping a frame + int16_t receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, + CanIOFlags& out_flags) override; + + // Set Filters to ignore frames not to be handled by us + bool configureFilters(const CanFilterConfig* filter_configs, + uint16_t num_configs) override; + + // Always return false, there's no busoff condition in Linux CAN + bool is_busoff() const override + { + return false; + } + + void flush_tx() override; + + void clear_rx() override; + + // Get number of Filter configurations + uint16_t getNumFilters() const override; + + // Get total number of Errors discovered + uint32_t getErrorCount() const override; + + // returns true if init was successfully called + bool is_initialized() const override; + + /****************************************** + * Select Method * + * ****************************************/ + // wait until selected event is available, false when timed out waiting else true + bool select(bool &read, bool &write, + const AP_HAL::CANFrame* const pending_tx, + uint64_t blocking_deadline) override; + + // setup event handle for waiting on events + bool set_event_handle(AP_HAL::EventHandle* handle) override; + + // fetch stats text and return the size of the same, + // results available via @SYS/can0_stats.txt or @SYS/can1_stats.txt + uint32_t get_stats(char* data, uint32_t max_size) override; + + class CANSocketEventSource : public AP_HAL::EventSource { + friend class CANIface; + CANIface *_ifaces[HAL_NUM_CAN_IFACES]; + + public: + // we just poll fd, no signaling is done + void signal(uint32_t evt_mask) override { return; } + bool wait(uint64_t duration, AP_HAL::EventHandle* evt_handle) override; + }; + +private: + void _pollWrite(); + + bool _pollRead(); + + int _write(const AP_HAL::CANFrame& frame) const; + + int _read(AP_HAL::CANFrame& frame, uint64_t& ts_usec, bool& loopback) const; + + void _incrementNumFramesInSocketTxQueue(); + + void _confirmSentFrame(); + + bool _wasInPendingLoopbackSet(const AP_HAL::CANFrame& frame); + + bool _checkHWFilters(const can_frame& frame) const; + + bool _hasReadyTx() const; + + bool _hasReadyRx() const; + + void _poll(bool read, bool write); + + int _openSocket(const std::string& iface_name); + + void _updateDownStatusFromPollResult(const pollfd& pfd); + + uint32_t _bitrate; + + bool _down; + bool _initialized; + + int _fd; + + const uint8_t _self_index; + + const unsigned _max_frames_in_socket_tx_queue; + unsigned _frames_in_socket_tx_queue; + uint32_t _tx_frame_counter; + AP_HAL::EventHandle *_evt_handle; + static CANSocketEventSource evt_can_socket[HAL_NUM_CAN_IFACES]; + + pollfd _pollfd; + std::map _errors; + std::priority_queue _tx_queue; + std::queue _rx_queue; + std::unordered_multiset _pending_loopback_ids; + std::vector _hw_filters_container; + + struct { + uint32_t tx_requests; + uint32_t tx_full; + uint32_t tx_confirmed; + uint32_t tx_write_fail; + uint32_t tx_success; + uint32_t tx_timedout; + uint32_t rx_received; + uint32_t rx_errors; + uint32_t num_downs; + uint32_t num_rx_poll_req; + uint32_t num_tx_poll_req; + uint32_t num_poll_waits; + uint32_t num_poll_tx_events; + uint32_t num_poll_rx_events; + } stats; +}; + +} + +#endif //#if HAL_NUM_CAN_IFACES diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 8894b2964d..529cf093f3 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -44,6 +44,7 @@ #include "UARTDriver.h" #include "Util.h" #include "Util_RPI.h" +#include "CANSocketIface.h" using namespace Linux; @@ -218,6 +219,10 @@ static Empty::OpticalFlow opticalFlow; static Empty::DSP dspDriver; static Empty::Flash flashDriver; +#if HAL_NUM_CAN_IFACES +static CANIface* canDrivers[HAL_NUM_CAN_IFACES]; +#endif + HAL_Linux::HAL_Linux() : AP_HAL::HAL( &uartADriver, @@ -241,7 +246,12 @@ HAL_Linux::HAL_Linux() : &opticalFlow, &flashDriver, &dspDriver, - nullptr) +#if HAL_NUM_CAN_IFACES + (AP_HAL::CANIface**)canDrivers +#else + nullptr +#endif + ) {} void _usage(void) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.h b/libraries/AP_HAL_Linux/HAL_Linux_Class.h index ec45662c9d..87089427aa 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.h +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.h @@ -14,3 +14,10 @@ public: protected: bool _should_exit = false; }; + +#if HAL_NUM_CAN_IFACES +namespace Linux { + class CANIface; +} +typedef Linux::CANIface HAL_CANIface; +#endif