Peter van der Perk
3 years ago
committed by
Daniel Agar
17 changed files with 910 additions and 8 deletions
@ -0,0 +1,4 @@
@@ -0,0 +1,4 @@
|
||||
CONFIG_DRIVERS_CYPHAL=y |
||||
CONFIG_CYPHAL_CLIENT=y |
||||
CONFIG_CYPHAL_APP_DESCRIPTOR=y |
||||
CONFIG_CYPHAL_UORB_SENSOR_GPS_PUBLISHER=y |
||||
@ -0,0 +1,49 @@
@@ -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 |
||||
@ -0,0 +1,16 @@
@@ -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 :) |
||||
@ -0,0 +1,202 @@
@@ -0,0 +1,202 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <cassert> |
||||
#include <time.h> |
||||
#include <cstdint> |
||||
|
||||
#include <unistd.h> |
||||
#include <sys/time.h> |
||||
#include <sys/types.h> |
||||
|
||||
#include <uavcan/driver/system_clock.hpp> |
||||
|
||||
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; |
||||
} |
||||
|
||||
|
||||
}; |
||||
|
||||
|
||||
} |
||||
@ -0,0 +1,160 @@
@@ -0,0 +1,160 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan_nuttx/thread.hpp> |
||||
#include <uavcan_nuttx/clock.hpp> |
||||
#include <uavcan/driver/can.hpp> |
||||
|
||||
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 <unsigned RxQueueCapacity = 128> |
||||
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 <typename DelayCallable> |
||||
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); |
||||
} |
||||
}; |
||||
|
||||
} |
||||
@ -0,0 +1,94 @@
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Copyright (C) 2014, 2018 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* Kinetis Port Author David Sidrane <david_s5@nscdg.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
|
||||
#include <nuttx/config.h> |
||||
#include <nuttx/fs/fs.h> |
||||
#include <poll.h> |
||||
#include <errno.h> |
||||
#include <cstdio> |
||||
#include <ctime> |
||||
#include <cstring> |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
|
||||
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(); |
||||
} |
||||
}; |
||||
|
||||
} |
||||
@ -0,0 +1,11 @@
@@ -0,0 +1,11 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
|
||||
#include <uavcan_nuttx/thread.hpp> |
||||
#include <uavcan_nuttx/clock.hpp> |
||||
#include <uavcan_nuttx/socketcan.hpp> |
||||
@ -0,0 +1,276 @@
@@ -0,0 +1,276 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
*/ |
||||
|
||||
#include <uavcan_nuttx/socketcan.hpp> |
||||
#include <uavcan_nuttx/clock.hpp> |
||||
#include <uavcan/util/templates.hpp> |
||||
|
||||
#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; |
||||
} |
||||
|
||||
} |
||||
@ -0,0 +1,61 @@
@@ -0,0 +1,61 @@
|
||||
/*
|
||||
* Copyright (C) 2014, 2018 Pavel Kirienko <pavel.kirienko@gmail.com> |
||||
* Kinetis Port Author David Sidrane <david_s5@nscdg.com> |
||||
*/ |
||||
|
||||
#include <uavcan_nuttx/thread.hpp> |
||||
#include <uavcan_nuttx/socketcan.hpp> |
||||
|
||||
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_(); |
||||
} |
||||
} |
||||
|
||||
} |
||||
Loading…
Reference in new issue