dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

UAVCANNODE NuttX SocketCAN driver 

Change init mode for CAN driver so it get executed in a single context
This commit is contained in:
Peter van der Perk 2022-07-19 14:46:54 +02:00 committed by Daniel Agar
parent 47aaa38d5f
commit 089fbdccc9
12 changed files with 576 additions and 346 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/nxp/ucans32k146/default.px4board
View File

@ -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

7
boards/nxp/ucans32k146/init/rc.board_defaults
View File

@ -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

2
src/drivers/cyphal/parameters.c
View File

@ -41,7 +41,7 @@
* @reboot_required true
* @group Cyphal
*/
PARAM_DEFINE_INT32(CYPHAL_ENABLE, 0);
PARAM_DEFINE_INT32(CYPHAL_ENABLE, 1);
/**
* Cyphal Node ID.

33
src/drivers/uavcan/uavcan_drivers/socketcan/README.md
View File

@ -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.

36
src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp
View File

@ -1,6 +1,36 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
/****************************************************************************
*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* 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

134
src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp
View File

@ -1,6 +1,36 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
/****************************************************************************
*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* 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 <uavcan_nuttx/clock.hpp>
#include <uavcan/driver/can.hpp>
#include <sys/time.h>
#include <sys/socket.h>
#include <nuttx/can.h>
#include <netpacket/can.h>
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);
}

38
src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp
View File

@ -1,7 +1,37 @@
/*
* Copyright (C) 2014, 2018 Pavel Kirienko <pavel.kirienko@gmail.com>
* Kinetis Port Author David Sidrane <david_s5@nscdg.com>
*/
/****************************************************************************
*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* Kinetis Port Author David Sidrane <david_s5@nscdg.com>
* 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

36
src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp
View File

@ -1,6 +1,36 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
/****************************************************************************
*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* 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

474
src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp
View File

@ -1,136 +1,64 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* NuttX SocketCAN port Copyright (C) 2022 NXP Semiconductors
*
*
*
*/
/****************************************************************************
*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* 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 <uavcan_nuttx/socketcan.hpp>
#include <uavcan_nuttx/clock.hpp>
#include <uavcan/util/templates.hpp>
#define UAVCAN_SOCKETCAN_RX_QUEUE_LEN 64
#include <net/if.h>
#include <sys/ioctl.h>
#include <string.h>
struct CriticalSectionLocker {
const irqstate_t flags_;
#include <nuttx/can.h>
#include <netpacket/can.h>
CriticalSectionLocker()
: flags_(enter_critical_section())
{
}
#define MODULE_NAME "UAVCAN_SOCKETCAN"
~CriticalSectionLocker()
{
leave_critical_section(flags_);
}
};
#include <px4_platform_common/log.h>
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;
}
}

38
src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp
View File

@ -1,7 +1,37 @@
/*
* Copyright (C) 2014, 2018 Pavel Kirienko <pavel.kirienko@gmail.com>
* Kinetis Port Author David Sidrane <david_s5@nscdg.com>
*/
/****************************************************************************
*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
* Kinetis Port Author David Sidrane <david_s5@nscdg.com>
* 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 <uavcan_nuttx/thread.hpp>
#include <uavcan_nuttx/socketcan.hpp>

117
src/drivers/uavcannode/UavcanNode.cpp
View File

@ -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);

6
src/drivers/uavcannode/UavcanNode.hpp
View File

@ -120,7 +120,8 @@ public:
typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> 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;