diff --git a/boards/nxp/ucans32k146/cyphal.px4board b/boards/nxp/ucans32k146/cyphal.px4board new file mode 100644 index 0000000000..7cfb3c78b4 --- /dev/null +++ b/boards/nxp/ucans32k146/cyphal.px4board @@ -0,0 +1,4 @@ +CONFIG_DRIVERS_CYPHAL=y +CONFIG_CYPHAL_CLIENT=y +CONFIG_CYPHAL_APP_DESCRIPTOR=y +CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y diff --git a/boards/nxp/ucans32k146/default.px4board b/boards/nxp/ucans32k146/default.px4board index 50b749dafb..e38e438573 100644 --- a/boards/nxp/ucans32k146/default.px4board +++ b/boards/nxp/ucans32k146/default.px4board @@ -7,11 +7,10 @@ CONFIG_DRIVERS_BOOTLOADERS=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_LIGHTS_RGBLED_PWM=y CONFIG_DRIVERS_PWM_OUT=y -CONFIG_DRIVERS_CYPHAL=y -CONFIG_CYPHAL_CLIENT=y -CONFIG_CYPHAL_APP_DESCRIPTOR=y -CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y +CONFIG_BOARD_UAVCAN_INTERFACES=1 +CONFIG_DRIVERS_UAVCANNODE=y CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_I2CDETECT=y CONFIG_SYSTEMCMDS_LED_CONTROL=y CONFIG_SYSTEMCMDS_MFT=y @@ -23,5 +22,6 @@ CONFIG_SYSTEMCMDS_REBOOT=y CONFIG_SYSTEMCMDS_SYSTEM_TIME=y CONFIG_SYSTEMCMDS_TOP=y CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_UORB=y CONFIG_SYSTEMCMDS_VER=y CONFIG_SYSTEMCMDS_WORK_QUEUE=y diff --git a/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig b/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig index 54fd7511e4..72c84a0de2 100644 --- a/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig +++ b/boards/nxp/ucans32k146/nuttx-config/nsh/defconfig @@ -112,6 +112,6 @@ CONFIG_SYMTAB_ORDEREDBYNAME=y CONFIG_SYSTEM_I2CTOOL=y CONFIG_SYSTEM_NSH=y CONFIG_SYSTEM_SPITOOL=y -CONFIG_USER_ENTRYPOINT="nsh_main" CONFIG_USERMAIN_STACKSIZE=2176 +CONFIG_USER_ENTRYPOINT="nsh_main" CONFIG_WATCHDOG=y diff --git a/boards/nxp/ucans32k146/servo.px4board b/boards/nxp/ucans32k146/servo.px4board deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt b/platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt index 02bc4d920d..97b84ec743 100644 --- a/platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt +++ b/platforms/nuttx/src/px4/nxp/s32k14x/CMakeLists.txt @@ -40,3 +40,4 @@ add_subdirectory(../s32k1xx/hrt hrt) add_subdirectory(../s32k1xx/io_pins io_pins) add_subdirectory(../s32k1xx/tone_alarm tone_alarm) add_subdirectory(../s32k1xx/version version) +add_subdirectory(../s32k1xx/watchdog watchdog) diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp index 978a74fca0..6883d77f55 100644 --- a/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp +++ b/platforms/nuttx/src/px4/nxp/s32k1xx/board_reset/board_reset.cpp @@ -39,6 +39,7 @@ */ #include +#include #include #include #include @@ -53,6 +54,26 @@ static int board_reset_enter_bootloader() return OK; } +static const uint32_t modes[] = { + /* to tb */ + /* BOARD_RESET_MODE_CLEAR 5 y */ 0, + /* BOARD_RESET_MODE_BOOT_TO_BL 0 n */ 0xb007b007, + /* BOARD_RESET_MODE_BOOT_TO_VALID_APP 0 y */ 0xb0070002, + /* BOARD_RESET_MODE_CAN_BL 10 n */ 0xb0080000, + /* BOARD_RESET_MODE_RTC_BOOT_FWOK 0 n */ 0xb0093a26 +}; + +int board_configure_reset(reset_mode_e mode, uint32_t arg) +{ + int rv = -1; + + if (mode < arraySize(modes)) { + //FIXME implemented this + } + + return rv; +} + /**************************************************************************** * Name: board_reset * diff --git a/src/drivers/uavcan/uavcan_driver.hpp b/src/drivers/uavcan/uavcan_driver.hpp index 6a681427a6..3eae57a63d 100644 --- a/src/drivers/uavcan/uavcan_driver.hpp +++ b/src/drivers/uavcan/uavcan_driver.hpp @@ -36,7 +36,9 @@ */ #pragma once -#if defined(UAVCAN_KINETIS_NUTTX) +#if defined(UAVCAN_SOCKETCAN_NUTTX) +# include +#elif defined(UAVCAN_KINETIS_NUTTX) # include #elif defined(UAVCAN_STM32_NUTTX) # include diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/README.md b/src/drivers/uavcan/uavcan_drivers/socketcan/README.md new file mode 100644 index 0000000000..2edad1a833 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/README.md @@ -0,0 +1,49 @@ +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 +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. + +NuttX applications that use libuavcan need to link the following libraries: + +* libuavcan diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt new file mode 100644 index 0000000000..e3c5344d79 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/CMakeLists.txt @@ -0,0 +1,16 @@ +include_directories( + ./include + ) + +add_compile_options(-Wno-unused-variable) +add_library(uavcan_socketcan_driver STATIC + src/socketcan.cpp + src/thread.cpp + ) + +add_dependencies(uavcan_socketcan_driver uavcan) + +install(DIRECTORY include/uavcan_nuttx DESTINATION include) +install(TARGETS uavcan_socketcan_driver DESTINATION lib) + +# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :) 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 new file mode 100644 index 0000000000..29c9bb6604 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/clock.hpp @@ -0,0 +1,202 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include + +namespace uavcan_socketcan +{ +/** + * Different adjustment modes can be used for time synchronization + */ +enum class ClockAdjustmentMode { + SystemWide, ///< Adjust the clock globally for the whole system; requires root privileges + PerDriverPrivate ///< Adjust the clock only for the current driver instance +}; + +/** + * Linux system clock driver. + * Requires librt. + */ +class SystemClock : public uavcan::ISystemClock +{ + uavcan::UtcDuration private_adj_; + uavcan::UtcDuration gradual_adj_limit_; + const ClockAdjustmentMode adj_mode_; + std::uint64_t step_adj_cnt_; + std::uint64_t gradual_adj_cnt_; + + static constexpr std::int64_t Int1e6 = 1000000; + static constexpr std::uint64_t UInt1e6 = 1000000; + + bool performStepAdjustment(const uavcan::UtcDuration adjustment) + { + step_adj_cnt_++; + const std::int64_t usec = adjustment.toUSec(); + timeval tv; + + if (gettimeofday(&tv, NULL) != 0) { + return false; + } + + tv.tv_sec += usec / Int1e6; + tv.tv_usec += usec % Int1e6; + return settimeofday(&tv, nullptr) == 0; + } + +#ifdef CONFIG_CLOCK_TIMEKEEPING + bool performGradualAdjustment(const uavcan::UtcDuration adjustment) + { + gradual_adj_cnt_++; + const std::int64_t usec = adjustment.toUSec(); + timeval tv; + tv.tv_sec = usec / Int1e6; + tv.tv_usec = usec % Int1e6; + return adjtime(&tv, nullptr) == 0; + } +#endif + +public: + /** + * By default, the clock adjustment mode will be selected automatically - global if root, private otherwise. + */ + explicit SystemClock(ClockAdjustmentMode adj_mode = detectPreferredClockAdjustmentMode()) + : gradual_adj_limit_(uavcan::UtcDuration::fromMSec(4000)) + , adj_mode_(adj_mode) + , step_adj_cnt_(0) + , gradual_adj_cnt_(0) + { } + + /** + * Returns monotonic timestamp from librt. + * @throws uavcan_nuttx::Exception. + */ + uavcan::MonotonicTime getMonotonic() const override + { + timespec ts; + + if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0) { + //throw Exception("Failed to get monotonic time"); + } + + return uavcan::MonotonicTime::fromUSec(std::uint64_t(ts.tv_sec) * UInt1e6 + ts.tv_nsec / 1000); + } + + /** + * Returns wall time from gettimeofday(). + */ + uavcan::UtcTime getUtc() const override + { + timeval tv; + + if (gettimeofday(&tv, NULL) != 0) { + //throw Exception("Failed to get UTC time"); + } + + uavcan::UtcTime utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * UInt1e6 + tv.tv_usec); + + if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate) { + utc += private_adj_; + } + + return utc; + } + + /** + * Adjusts the wall clock. + * Behavior depends on the selected clock adjustment mode - @ref ClockAdjustmentMode. + * Clock adjustment mode can be set only once via constructor. + * + * If the system wide adjustment mode is selected, two ways for performing adjustment exist: + * - Gradual adjustment using adjtime(), if the phase error is less than gradual adjustment limit. + * - Step adjustment using settimeofday(), if the phase error is above gradual adjustment limit. + * The gradual adjustment limit can be configured at any time via the setter method. + * + */ + void adjustUtc(const uavcan::UtcDuration adjustment) override + { + if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate) { + private_adj_ += adjustment; + + } else { + assert(private_adj_.isZero()); + assert(!gradual_adj_limit_.isNegative()); + +#ifdef CONFIG_CLOCK_TIMEKEEPING + + if (adjustment.getAbs() < gradual_adj_limit_) { + performGradualAdjustment(adjustment); + + } else +#endif + { + performStepAdjustment(adjustment); + } + } + } + + /** + * Sets the maximum phase error to use adjtime(). + * If the phase error exceeds this value, settimeofday() will be used instead. + */ + void setGradualAdjustmentLimit(uavcan::UtcDuration limit) + { + if (limit.isNegative()) { + limit = uavcan::UtcDuration(); + } + + gradual_adj_limit_ = limit; + } + + uavcan::UtcDuration getGradualAdjustmentLimit() const { return gradual_adj_limit_; } + + ClockAdjustmentMode getAdjustmentMode() const { return adj_mode_; } + + /** + * This is only applicable if the selected clock adjustment mode is private. + * In system wide mode this method will always return zero duration. + */ + uavcan::UtcDuration getPrivateAdjustment() const { return private_adj_; } + + /** + * Statistics that allows to evaluate clock sync preformance. + */ + std::uint64_t getStepAdjustmentCount() const { return step_adj_cnt_; } + std::uint64_t getGradualAdjustmentCount() const { return gradual_adj_cnt_; } + std::uint64_t getAdjustmentCount() const + { + return getStepAdjustmentCount() + getGradualAdjustmentCount(); + } + + /** + * This static method decides what is the optimal clock sync adjustment mode for the current configuration. + * It selects system wide mode if the application is running as root; otherwise it prefers + * the private adjustment mode because the system wide mode requires root privileges. + */ + static ClockAdjustmentMode detectPreferredClockAdjustmentMode() + { + const bool godmode = 0; // geteuid() == 0; + return godmode ? ClockAdjustmentMode::SystemWide : ClockAdjustmentMode::PerDriverPrivate; + } + + static SystemClock &instance() + { + static SystemClock self; + return self; + } + + +}; + + +} 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 new file mode 100644 index 0000000000..90a7e423d5 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/socketcan.hpp @@ -0,0 +1,160 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +namespace uavcan_socketcan +{ + +/** + * 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; + SystemClock clock; + +public: + CanDriver() : update_event_(*this) + {} + + /** + * Returns the singleton reference. + * No other copies can be created. + */ + static CanDriver &instance() { return self; } + + /** + * Attempts to detect bit rate of the CAN bus. + * This function may block for up to X seconds, where X is the number of bit rates to try. + * This function is NOT guaranteed to reset the CAN controller upon return. + * @return On success: detected bit rate, in bits per second. + * On failure: zero. + */ + static uavcan::uint32_t detectBitRate(void (*idle_callback)() = nullptr); + + /** + * Returns negative value if the requested baudrate can't be used. + * Returns zero if OK. + */ + 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. + */ + uavcan::uint32_t getRxQueueOverflowCount() const; + + /** + * Whether the controller is currently in bus off state. + * Note that the driver recovers the CAN controller from the bus off state automatically! + * Therefore, this method serves only monitoring purposes and is not necessary to use. + */ + 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; + + BusEvent &updateEvent() { return update_event_; } +}; + + +template +class CanInitHelper +{ + //CanRxItem queue_storage_[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]; + +public: + enum { BitRateAutoDetect = 0 }; + + CanDriver driver; + + CanInitHelper(uint32_t unused = 0x7) : + driver() + { + } + + /** + * This overload simply configures the provided bitrate. + * Auto bit rate detection will not be performed. + * Bitrate value must be positive. + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + int init(uavcan::uint32_t bitrate) + { + return driver.init(bitrate); + } + + /** + * This function can either initialize the driver at a fixed bit rate, or it can perform + * automatic bit rate detection. For theory please refer to the CiA application note #801. + * + * @param delay_callable A callable entity that suspends execution for strictly more than one second. + * The callable entity will be invoked without arguments. + * @ref getRecommendedListeningDelay(). + * + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * If auto detection was used, the function will update the argument + * with established bit rate. In case of an error the value will be undefined. + * + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + template + int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = 1000000) + { + if (inout_bitrate > 0) { + return driver.init(inout_bitrate); + + } + } + + /** + * Use this value for listening delay during automatic bit rate detection. + */ + static uavcan::MonotonicDuration getRecommendedListeningDelay() + { + return uavcan::MonotonicDuration::fromMSec(1050); + } +}; + +} 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 new file mode 100644 index 0000000000..64a6355bd6 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/thread.hpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#pragma once + + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_socketcan +{ + +class CanDriver; + + +/** + * All bus events are reported as POLLIN. + */ +class BusEvent : uavcan::Noncopyable +{ + using SignalCallbackHandler = void(*)(); + + SignalCallbackHandler signal_cb_{nullptr}; + sem_t sem_; +public: + + BusEvent(CanDriver &can_driver); + ~BusEvent(); + + void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } + + bool wait(uavcan::MonotonicDuration duration); + + void signalFromInterrupt(); +}; + +class Mutex +{ + pthread_mutex_t mutex_; + +public: + Mutex() + { + init(); + } + + int init() + { + return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); + } + + int deinit() + { + return pthread_mutex_destroy(&mutex_); + } + + void lock() + { + (void)pthread_mutex_lock(&mutex_); + } + + void unlock() + { + (void)pthread_mutex_unlock(&mutex_); + } +}; + + +class MutexLocker +{ + Mutex &mutex_; + +public: + MutexLocker(Mutex &mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } +}; + +} 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 new file mode 100644 index 0000000000..e6aebeab1f --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/include/uavcan_nuttx/uavcan_nuttx.hpp @@ -0,0 +1,11 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +#include +#include +#include diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp new file mode 100644 index 0000000000..9c2260d583 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/socketcan.cpp @@ -0,0 +1,276 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +#define UAVCAN_SOCKETCAN_RX_QUEUE_LEN 64 + +struct CriticalSectionLocker { + const irqstate_t flags_; + + CriticalSectionLocker() + : flags_(enter_critical_section()) + { + } + + ~CriticalSectionLocker() + { + leave_critical_section(flags_); + } +}; + +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; + std::uint32_t canbtr; + + bool isValid() const { return canbtr != 0; } +}; + +} // namespace + +CanDriver CanDriver::self; + +uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) +{ + //FIXME + return 1; +} + +int CanDriver::init(uavcan::uint32_t bitrate) +{ + { + //FIXME + } + + /* + * Applying default filter configuration (accept all) + */ + 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(); +} + +bool CanDriver::isInBusOffState() const +{ + //FIXME + 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) +{ + + + const bool noblock = ((inout_masks.read == 1) && hasReadyRx()) || + ((inout_masks.write == 1) && hasEmptyTx()); + + if (!noblock && (clock.getMonotonic() > blocking_deadline)) { + + } + + inout_masks.read = hasReadyRx() ? 1 : 0; + + inout_masks.write = (hasEmptyTx()) ? 1 : 0; // Disable write while in bus-off + + 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; +} + +uavcan::uint8_t CanDriver::getNumIfaces() const +{ + return 1; +} + +} diff --git a/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp new file mode 100644 index 0000000000..165873b698 --- /dev/null +++ b/src/drivers/uavcan/uavcan_drivers/socketcan/driver/src/thread.cpp @@ -0,0 +1,61 @@ +/* + * Copyright (C) 2014, 2018 Pavel Kirienko + * Kinetis Port Author David Sidrane + */ + +#include +#include + +namespace uavcan_socketcan +{ + +BusEvent::BusEvent(CanDriver &can_driver) +{ + sem_init(&sem_, 0, 0); + sem_setprotocol(&sem_, SEM_PRIO_NONE); +} + +BusEvent::~BusEvent() +{ + sem_destroy(&sem_); +} + +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + if (duration.isPositive()) { + timespec abstime; + + if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { + const unsigned billion = 1000 * 1000 * 1000; + uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; + abstime.tv_sec += nsecs / billion; + nsecs -= (nsecs / billion) * billion; + abstime.tv_nsec = nsecs; + + int ret; + + while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); + + if (ret == -1) { // timed out or error + return false; + } + + return true; + } + } + + return false; +} + +void BusEvent::signalFromInterrupt() +{ + if (sem_.semcount <= 0) { + (void)sem_post(&sem_); + } + + if (signal_cb_) { + signal_cb_(); + } +} + +} diff --git a/src/drivers/uavcannode/CMakeLists.txt b/src/drivers/uavcannode/CMakeLists.txt index a9fbda10af..fde0f23356 100644 --- a/src/drivers/uavcannode/CMakeLists.txt +++ b/src/drivers/uavcannode/CMakeLists.txt @@ -40,7 +40,10 @@ set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03") set(UAVCAN_PLATFORM "generic") if(CONFIG_ARCH_CHIP) - if(${CONFIG_ARCH_CHIP} MATCHES "kinetis") + if(${CONFIG_NET_CAN} MATCHES "y") + set(UAVCAN_DRIVER "socketcan") + set(UAVCAN_TIMER 1) + elseif(${CONFIG_ARCH_CHIP} MATCHES "kinetis") set(UAVCAN_DRIVER "kinetis") set(UAVCAN_TIMER 1) elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32h7") diff --git a/src/drivers/uavcannode/uavcan_driver.hpp b/src/drivers/uavcannode/uavcan_driver.hpp index bd951b5e61..3ab9746773 100644 --- a/src/drivers/uavcannode/uavcan_driver.hpp +++ b/src/drivers/uavcannode/uavcan_driver.hpp @@ -36,7 +36,9 @@ */ #pragma once -#if defined(UAVCAN_KINETIS_NUTTX) +#if defined(UAVCAN_SOCKETCAN_NUTTX) +# include +#elif defined(UAVCAN_KINETIS_NUTTX) # include #elif defined(UAVCAN_STM32_NUTTX) # include