mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 22:58:10 +08:00
uavcan drivers: update code style
Pure refactoring, just running 'make format'.
This commit is contained in:
Beat Küng
@@ -11,7 +11,6 @@ exec find boards msg src platforms \
|
||||
-path msg/templates/urtps -prune -o \
|
||||
-path platforms/nuttx/NuttX -prune -o \
|
||||
-path src/drivers/uavcan/libuavcan -prune -o \
|
||||
-path src/drivers/uavcan/uavcan_drivers -prune -o \
|
||||
-path src/lib/DriverFramework -prune -o \
|
||||
-path src/lib/ecl -prune -o \
|
||||
-path src/lib/matrix -prune -o \
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -49,26 +49,25 @@ void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams
|
||||
{
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F),
|
||||
rate_i(0.02F),
|
||||
rate_error_corner_freq(0.01F),
|
||||
max_rate_correction_ppm(300.0F),
|
||||
lock_thres_rate_ppm(2.0F),
|
||||
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
|
||||
min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{
|
||||
}
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F),
|
||||
rate_i(0.02F),
|
||||
rate_error_corner_freq(0.01F),
|
||||
max_rate_correction_ppm(300.0F),
|
||||
lock_thres_rate_ppm(2.0F),
|
||||
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
|
||||
min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -96,7 +95,7 @@ bool isUtcLocked();
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams& params);
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
@@ -104,28 +103,32 @@ void setUtcSyncParams(const UtcSyncParams& params);
|
||||
* Adapter for uavcan::ISystemClock.
|
||||
*/
|
||||
class SystemClock : public uavcan::ISystemClock
|
||||
, uavcan::Noncopyable
|
||||
, uavcan::Noncopyable
|
||||
{
|
||||
SystemClock() {
|
||||
}
|
||||
SystemClock()
|
||||
{
|
||||
}
|
||||
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment) {
|
||||
clock::adjustUtc(adjustment);
|
||||
}
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
clock::adjustUtc(adjustment);
|
||||
}
|
||||
|
||||
public:
|
||||
virtual uavcan::MonotonicTime getMonotonic() const {
|
||||
return clock::getMonotonic();
|
||||
}
|
||||
virtual uavcan::UtcTime getUtc() const {
|
||||
return clock::getUtc();
|
||||
}
|
||||
virtual uavcan::MonotonicTime getMonotonic() const
|
||||
{
|
||||
return clock::getMonotonic();
|
||||
}
|
||||
virtual uavcan::UtcTime getUtc() const
|
||||
{
|
||||
return clock::getUtc();
|
||||
}
|
||||
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock& instance();
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock &instance();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
+478
-498
File diff suppressed because it is too large
Load Diff
+39
-39
@@ -28,68 +28,68 @@ class CanDriver;
|
||||
*/
|
||||
class BusEvent : uavcan::Noncopyable
|
||||
{
|
||||
using SignalCallbackHandler = void(*)();
|
||||
using SignalCallbackHandler = void(*)();
|
||||
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
public:
|
||||
|
||||
BusEvent(CanDriver& can_driver);
|
||||
~BusEvent();
|
||||
BusEvent(CanDriver &can_driver);
|
||||
~BusEvent();
|
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
|
||||
void signalFromInterrupt();
|
||||
void signalFromInterrupt();
|
||||
};
|
||||
|
||||
class Mutex
|
||||
{
|
||||
pthread_mutex_t mutex_;
|
||||
pthread_mutex_t mutex_;
|
||||
|
||||
public:
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class MutexLocker
|
||||
{
|
||||
Mutex& mutex_;
|
||||
Mutex &mutex_;
|
||||
|
||||
public:
|
||||
MutexLocker(Mutex& mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
MutexLocker(Mutex &mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -48,19 +48,18 @@ namespace uavcan_kinetis
|
||||
{
|
||||
#if UAVCAN_KINETIS_NUTTX
|
||||
|
||||
struct CriticalSectionLocker
|
||||
{
|
||||
const irqstate_t flags_;
|
||||
struct CriticalSectionLocker {
|
||||
const irqstate_t flags_;
|
||||
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{
|
||||
}
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{
|
||||
}
|
||||
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -66,167 +66,166 @@ uavcan::uint64_t time_utc = 0;
|
||||
|
||||
void init()
|
||||
{
|
||||
CriticalSectionLocker lock;
|
||||
if (initialized)
|
||||
{
|
||||
return;
|
||||
}
|
||||
initialized = true;
|
||||
CriticalSectionLocker lock;
|
||||
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
if (initialized) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Power-on Clock
|
||||
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
|
||||
initialized = true;
|
||||
|
||||
// Enable module
|
||||
putreg32(0, KINETIS_PIT_MCR);
|
||||
// Attach IRQ
|
||||
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL);
|
||||
|
||||
// Start the timer
|
||||
// Power-on Clock
|
||||
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
|
||||
|
||||
putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
|
||||
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start
|
||||
// Enable module
|
||||
putreg32(0, KINETIS_PIT_MCR);
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
// Start the timer
|
||||
|
||||
putreg32(CountsPerPeriod - 1, TMR_REG(KINETIS_PIT_LDVAL_OFFSET));
|
||||
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start
|
||||
|
||||
// Prioritize and Enable IRQ
|
||||
|
||||
#if 0
|
||||
// This has to be off or uses the default priority
|
||||
// Without the ability to point the vector
|
||||
// Directly to this ISR this will reenter the
|
||||
// exception_common and cause the interrupt
|
||||
// Stack pointer to be reset
|
||||
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
// This has to be off or uses the default priority
|
||||
// Without the ability to point the vector
|
||||
// Directly to this ISR this will reenter the
|
||||
// exception_common and cause the interrupt
|
||||
// Stack pointer to be reset
|
||||
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
|
||||
#endif
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
up_enable_irq(TIMX_IRQn);
|
||||
}
|
||||
|
||||
void setUtc(uavcan::UtcTime time)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
time_utc = time.toUSec();
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
|
||||
static uavcan::uint64_t sampleUtcFromCriticalSection()
|
||||
{
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
|
||||
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
volatile uavcan::uint64_t time = time_utc;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF)
|
||||
{
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
return time + (cnt / CountsPerUs);
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
|
||||
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
|
||||
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
|
||||
}
|
||||
|
||||
return time + (cnt / CountsPerUs);
|
||||
}
|
||||
|
||||
uavcan::uint64_t getUtcUSecFromCanInterrupt()
|
||||
{
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
return utc_set ? sampleUtcFromCriticalSection() : 0;
|
||||
}
|
||||
|
||||
uavcan::MonotonicTime getMonotonic()
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
uavcan::uint64_t usec = 0;
|
||||
// Scope Critical section
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
volatile uavcan::uint64_t time = time_mono;
|
||||
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF)
|
||||
{
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
usec = time + (cnt / CountsPerUs);
|
||||
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
|
||||
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
|
||||
time += USecPerOverflow;
|
||||
}
|
||||
|
||||
} // End Scope Critical section
|
||||
usec = time + (cnt / CountsPerUs);
|
||||
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
} // End Scope Critical section
|
||||
|
||||
return uavcan::MonotonicTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
|
||||
uavcan::UtcTime getUtc()
|
||||
{
|
||||
if (utc_set)
|
||||
{
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
return uavcan::UtcTime();
|
||||
if (utc_set) {
|
||||
uavcan::uint64_t usec = 0;
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
usec = sampleUtcFromCriticalSection();
|
||||
}
|
||||
return uavcan::UtcTime::fromUSec(usec);
|
||||
}
|
||||
|
||||
return uavcan::UtcTime();
|
||||
}
|
||||
|
||||
static float lowpass(float xold, float xnew, float corner, float dt)
|
||||
{
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
const float tau = 1.F / corner;
|
||||
return (dt * xnew + tau * xold) / (dt + tau);
|
||||
}
|
||||
|
||||
static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
const uavcan::MonotonicTime ts = getMonotonic();
|
||||
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
|
||||
prev_utc_adj_at = ts;
|
||||
const float adj_usec = float(adjustment.toUSec());
|
||||
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
/*
|
||||
* Target relative rate in PPM
|
||||
* Positive to go faster
|
||||
*/
|
||||
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
|
||||
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
/*
|
||||
* Current relative rate in PPM
|
||||
* Positive if the local clock is faster
|
||||
*/
|
||||
const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM
|
||||
utc_prev_adj = adj_usec;
|
||||
utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt);
|
||||
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
|
||||
|
||||
if (dt > 10)
|
||||
{
|
||||
utc_rel_rate_error_integral = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
if (dt > 10) {
|
||||
utc_rel_rate_error_integral = 0;
|
||||
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
} else {
|
||||
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
|
||||
utc_rel_rate_error_integral =
|
||||
uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm);
|
||||
}
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
/*
|
||||
* Rate controller
|
||||
*/
|
||||
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral;
|
||||
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm);
|
||||
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm);
|
||||
|
||||
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
|
||||
|
||||
// syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n",
|
||||
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
|
||||
@@ -235,96 +234,91 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
|
||||
|
||||
void adjustUtc(uavcan::UtcDuration adjustment)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
MutexLocker mlocker(mutex);
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set)
|
||||
{
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
|
||||
const uavcan::int64_t adj_usec = adjustment.toUSec();
|
||||
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
|
||||
{
|
||||
time_utc = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
{
|
||||
CriticalSectionLocker locker;
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
updateRatePID(adjustment);
|
||||
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
|
||||
time_utc = 1;
|
||||
|
||||
if (!utc_locked)
|
||||
{
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
} else {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
|
||||
}
|
||||
}
|
||||
|
||||
utc_set = true;
|
||||
utc_locked = false;
|
||||
utc_jump_cnt++;
|
||||
utc_prev_adj = 0;
|
||||
utc_rel_rate_ppm = 0;
|
||||
|
||||
} else {
|
||||
updateRatePID(adjustment);
|
||||
|
||||
if (!utc_locked) {
|
||||
utc_locked =
|
||||
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
|
||||
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
float getUtcRateCorrectionPPM()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
MutexLocker mlocker(mutex);
|
||||
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
|
||||
return 1e6F * rate_correction_mult;
|
||||
}
|
||||
|
||||
uavcan::uint32_t getUtcJumpCount()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_jump_cnt;
|
||||
}
|
||||
|
||||
bool isUtcLocked()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_locked;
|
||||
}
|
||||
|
||||
UtcSyncParams getUtcSyncParams()
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
MutexLocker mlocker(mutex);
|
||||
return utc_sync_params;
|
||||
}
|
||||
|
||||
void setUtcSyncParams(const UtcSyncParams& params)
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms)
|
||||
{
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
MutexLocker mlocker(mutex);
|
||||
// Add some sanity check
|
||||
utc_sync_params = params;
|
||||
}
|
||||
|
||||
} // namespace clock
|
||||
|
||||
SystemClock& SystemClock::instance()
|
||||
SystemClock &SystemClock::instance()
|
||||
{
|
||||
static union SystemClockStorage
|
||||
{
|
||||
uavcan::uint8_t buffer[sizeof(SystemClock)];
|
||||
long long _aligner_1;
|
||||
long double _aligner_2;
|
||||
} storage;
|
||||
static union SystemClockStorage {
|
||||
uavcan::uint8_t buffer[sizeof(SystemClock)];
|
||||
long long _aligner_1;
|
||||
long double _aligner_2;
|
||||
} storage;
|
||||
|
||||
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
|
||||
SystemClock *const ptr = reinterpret_cast<SystemClock *>(storage.buffer);
|
||||
|
||||
if (!clock::initialized)
|
||||
{
|
||||
MutexLocker mlocker(clock::mutex);
|
||||
clock::init();
|
||||
new (ptr)SystemClock();
|
||||
}
|
||||
return *ptr;
|
||||
if (!clock::initialized) {
|
||||
MutexLocker mlocker(clock::mutex);
|
||||
clock::init();
|
||||
new (ptr)SystemClock();
|
||||
}
|
||||
|
||||
return *ptr;
|
||||
}
|
||||
|
||||
} // namespace uavcan_kinetis
|
||||
@@ -337,38 +331,35 @@ SystemClock& SystemClock::instance()
|
||||
extern "C"
|
||||
UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
|
||||
{
|
||||
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
|
||||
putreg32(PIT_TFLG_TIF, TMR_REG(KINETIS_PIT_TFLG_OFFSET));
|
||||
|
||||
using namespace uavcan_kinetis::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
using namespace uavcan_kinetis::clock;
|
||||
UAVCAN_ASSERT(initialized);
|
||||
|
||||
time_mono += USecPerOverflow;
|
||||
time_mono += USecPerOverflow;
|
||||
|
||||
if (utc_set)
|
||||
{
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000)
|
||||
{
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
if (utc_set) {
|
||||
time_utc += USecPerOverflow;
|
||||
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0)
|
||||
{
|
||||
utc_correction_nsec_per_overflow--;
|
||||
}
|
||||
else if (utc_correction_nsec_per_overflow < 0)
|
||||
{
|
||||
utc_correction_nsec_per_overflow++;
|
||||
}
|
||||
else
|
||||
{
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
|
||||
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
|
||||
utc_accumulated_correction_nsec %= 1000;
|
||||
}
|
||||
|
||||
// Correction decay - 1 nsec per 65536 usec
|
||||
if (utc_correction_nsec_per_overflow > 0) {
|
||||
utc_correction_nsec_per_overflow--;
|
||||
|
||||
} else if (utc_correction_nsec_per_overflow < 0) {
|
||||
utc_correction_nsec_per_overflow++;
|
||||
|
||||
} else {
|
||||
; // Zero
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -11,50 +11,53 @@
|
||||
namespace uavcan_kinetis
|
||||
{
|
||||
|
||||
BusEvent::BusEvent(CanDriver& can_driver)
|
||||
BusEvent::BusEvent(CanDriver &can_driver)
|
||||
{
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
BusEvent::~BusEvent()
|
||||
{
|
||||
sem_destroy(&sem_);
|
||||
sem_destroy(&sem_);
|
||||
}
|
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
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;
|
||||
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;
|
||||
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_();
|
||||
}
|
||||
if (sem_.semcount <= 0) {
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
|
||||
if (signal_cb_) {
|
||||
signal_cb_();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -24,262 +24,257 @@ namespace uavcan_stm32
|
||||
namespace bxcan
|
||||
{
|
||||
|
||||
struct TxMailboxType
|
||||
{
|
||||
volatile uint32_t TIR;
|
||||
volatile uint32_t TDTR;
|
||||
volatile uint32_t TDLR;
|
||||
volatile uint32_t TDHR;
|
||||
struct TxMailboxType {
|
||||
volatile uint32_t TIR;
|
||||
volatile uint32_t TDTR;
|
||||
volatile uint32_t TDLR;
|
||||
volatile uint32_t TDHR;
|
||||
};
|
||||
|
||||
struct RxMailboxType
|
||||
{
|
||||
volatile uint32_t RIR;
|
||||
volatile uint32_t RDTR;
|
||||
volatile uint32_t RDLR;
|
||||
volatile uint32_t RDHR;
|
||||
struct RxMailboxType {
|
||||
volatile uint32_t RIR;
|
||||
volatile uint32_t RDTR;
|
||||
volatile uint32_t RDLR;
|
||||
volatile uint32_t RDHR;
|
||||
};
|
||||
|
||||
struct FilterRegisterType
|
||||
{
|
||||
volatile uint32_t FR1;
|
||||
volatile uint32_t FR2;
|
||||
struct FilterRegisterType {
|
||||
volatile uint32_t FR1;
|
||||
volatile uint32_t FR2;
|
||||
};
|
||||
|
||||
struct CanType
|
||||
{
|
||||
volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
|
||||
volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */
|
||||
volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
|
||||
volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */
|
||||
volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */
|
||||
volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */
|
||||
volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */
|
||||
volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */
|
||||
uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */
|
||||
TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */
|
||||
RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */
|
||||
uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */
|
||||
volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */
|
||||
volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */
|
||||
uint32_t RESERVED2; /*!< Reserved, 0x208 */
|
||||
volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */
|
||||
uint32_t RESERVED3; /*!< Reserved, 0x210 */
|
||||
volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */
|
||||
uint32_t RESERVED4; /*!< Reserved, 0x218 */
|
||||
volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */
|
||||
uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */
|
||||
FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
|
||||
struct CanType {
|
||||
volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
|
||||
volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */
|
||||
volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
|
||||
volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */
|
||||
volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */
|
||||
volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */
|
||||
volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */
|
||||
volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */
|
||||
uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */
|
||||
TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */
|
||||
RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */
|
||||
uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */
|
||||
volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */
|
||||
volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */
|
||||
uint32_t RESERVED2; /*!< Reserved, 0x208 */
|
||||
volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */
|
||||
uint32_t RESERVED3; /*!< Reserved, 0x210 */
|
||||
volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */
|
||||
uint32_t RESERVED4; /*!< Reserved, 0x218 */
|
||||
volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */
|
||||
uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */
|
||||
FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
|
||||
};
|
||||
|
||||
/**
|
||||
* CANx register sets
|
||||
*/
|
||||
CanType* const Can[UAVCAN_STM32_NUM_IFACES] =
|
||||
{
|
||||
reinterpret_cast<CanType*>(0x40006400)
|
||||
CanType *const Can[UAVCAN_STM32_NUM_IFACES] = {
|
||||
reinterpret_cast<CanType *>(0x40006400)
|
||||
#if UAVCAN_STM32_NUM_IFACES > 1
|
||||
,
|
||||
reinterpret_cast<CanType*>(0x40006800)
|
||||
,
|
||||
reinterpret_cast<CanType *>(0x40006800)
|
||||
#endif
|
||||
};
|
||||
|
||||
/* CAN master control register */
|
||||
|
||||
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
|
||||
constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
|
||||
constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */
|
||||
constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */
|
||||
constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
|
||||
constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */
|
||||
constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */
|
||||
constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */
|
||||
constexpr unsigned long MCR_RESET = (1U << 15);/* Bit 15: bxCAN software master reset */
|
||||
constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */
|
||||
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */
|
||||
constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
|
||||
constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */
|
||||
constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */
|
||||
constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
|
||||
constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */
|
||||
constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */
|
||||
constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */
|
||||
constexpr unsigned long MCR_RESET = (1U << 15); /* Bit 15: bxCAN software master reset */
|
||||
constexpr unsigned long MCR_DBF = (1U << 16); /* Bit 16: Debug freeze */
|
||||
|
||||
/* CAN master status register */
|
||||
|
||||
constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */
|
||||
constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
|
||||
constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */
|
||||
constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
|
||||
constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */
|
||||
constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */
|
||||
constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */
|
||||
constexpr unsigned long MSR_SAMP = (1U << 10);/* Bit 10: Last Sample Point */
|
||||
constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */
|
||||
constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */
|
||||
constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
|
||||
constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */
|
||||
constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
|
||||
constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */
|
||||
constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */
|
||||
constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */
|
||||
constexpr unsigned long MSR_SAMP = (1U << 10); /* Bit 10: Last Sample Point */
|
||||
constexpr unsigned long MSR_RX = (1U << 11); /* Bit 11: CAN Rx Signal */
|
||||
|
||||
/* CAN transmit status register */
|
||||
|
||||
constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */
|
||||
constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */
|
||||
constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */
|
||||
constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */
|
||||
constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ALST1 = (1U << 10);/* Bit 10 : Arbitration Lost for Mailbox 1 */
|
||||
constexpr unsigned long TSR_TERR1 = (1U << 11);/* Bit 11 : Transmission Error of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ABRQ1 = (1U << 15);/* Bit 15 : Abort Request for Mailbox 1 */
|
||||
constexpr unsigned long TSR_RQCP2 = (1U << 16);/* Bit 16 : Request Completed Mailbox 2 */
|
||||
constexpr unsigned long TSR_TXOK2 = (1U << 17);/* Bit 17 : Transmission OK of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ALST2 = (1U << 18);/* Bit 18: Arbitration Lost for Mailbox 2 */
|
||||
constexpr unsigned long TSR_TERR2 = (1U << 19);/* Bit 19: Transmission Error of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ABRQ2 = (1U << 23);/* Bit 23: Abort Request for Mailbox 2 */
|
||||
constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
|
||||
constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT);
|
||||
constexpr unsigned long TSR_TME0 = (1U << 26);/* Bit 26: Transmit Mailbox 0 Empty */
|
||||
constexpr unsigned long TSR_TME1 = (1U << 27);/* Bit 27: Transmit Mailbox 1 Empty */
|
||||
constexpr unsigned long TSR_TME2 = (1U << 28);/* Bit 28: Transmit Mailbox 2 Empty */
|
||||
constexpr unsigned long TSR_LOW0 = (1U << 29);/* Bit 29: Lowest Priority Flag for Mailbox 0 */
|
||||
constexpr unsigned long TSR_LOW1 = (1U << 30);/* Bit 30: Lowest Priority Flag for Mailbox 1 */
|
||||
constexpr unsigned long TSR_LOW2 = (1U << 31);/* Bit 31: Lowest Priority Flag for Mailbox 2 */
|
||||
constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */
|
||||
constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */
|
||||
constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */
|
||||
constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */
|
||||
constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */
|
||||
constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ALST1 = (1U << 10); /* Bit 10 : Arbitration Lost for Mailbox 1 */
|
||||
constexpr unsigned long TSR_TERR1 = (1U << 11); /* Bit 11 : Transmission Error of Mailbox 1 */
|
||||
constexpr unsigned long TSR_ABRQ1 = (1U << 15); /* Bit 15 : Abort Request for Mailbox 1 */
|
||||
constexpr unsigned long TSR_RQCP2 = (1U << 16); /* Bit 16 : Request Completed Mailbox 2 */
|
||||
constexpr unsigned long TSR_TXOK2 = (1U << 17); /* Bit 17 : Transmission OK of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ALST2 = (1U << 18); /* Bit 18: Arbitration Lost for Mailbox 2 */
|
||||
constexpr unsigned long TSR_TERR2 = (1U << 19); /* Bit 19: Transmission Error of Mailbox 2 */
|
||||
constexpr unsigned long TSR_ABRQ2 = (1U << 23); /* Bit 23: Abort Request for Mailbox 2 */
|
||||
constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
|
||||
constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT);
|
||||
constexpr unsigned long TSR_TME0 = (1U << 26); /* Bit 26: Transmit Mailbox 0 Empty */
|
||||
constexpr unsigned long TSR_TME1 = (1U << 27); /* Bit 27: Transmit Mailbox 1 Empty */
|
||||
constexpr unsigned long TSR_TME2 = (1U << 28); /* Bit 28: Transmit Mailbox 2 Empty */
|
||||
constexpr unsigned long TSR_LOW0 = (1U << 29); /* Bit 29: Lowest Priority Flag for Mailbox 0 */
|
||||
constexpr unsigned long TSR_LOW1 = (1U << 30); /* Bit 30: Lowest Priority Flag for Mailbox 1 */
|
||||
constexpr unsigned long TSR_LOW2 = (1U << 31); /* Bit 31: Lowest Priority Flag for Mailbox 2 */
|
||||
|
||||
/* CAN receive FIFO 0/1 registers */
|
||||
|
||||
constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */
|
||||
constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT);
|
||||
constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */
|
||||
constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */
|
||||
constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
|
||||
constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */
|
||||
constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT);
|
||||
constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */
|
||||
constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */
|
||||
constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
|
||||
|
||||
/* CAN interrupt enable register */
|
||||
|
||||
constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */
|
||||
constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */
|
||||
constexpr unsigned long IER_BOFIE = (1U << 10);/* Bit 10: Bus-Off Interrupt Enable */
|
||||
constexpr unsigned long IER_LECIE = (1U << 11);/* Bit 11: Last Error Code Interrupt Enable */
|
||||
constexpr unsigned long IER_ERRIE = (1U << 15);/* Bit 15: Error Interrupt Enable */
|
||||
constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup Interrupt Enable */
|
||||
constexpr unsigned long IER_SLKIE = (1U << 17);/* Bit 17: Sleep Interrupt Enable */
|
||||
constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */
|
||||
constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */
|
||||
constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */
|
||||
constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */
|
||||
constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */
|
||||
constexpr unsigned long IER_BOFIE = (1U << 10); /* Bit 10: Bus-Off Interrupt Enable */
|
||||
constexpr unsigned long IER_LECIE = (1U << 11); /* Bit 11: Last Error Code Interrupt Enable */
|
||||
constexpr unsigned long IER_ERRIE = (1U << 15); /* Bit 15: Error Interrupt Enable */
|
||||
constexpr unsigned long IER_WKUIE = (1U << 16); /* Bit 16: Wakeup Interrupt Enable */
|
||||
constexpr unsigned long IER_SLKIE = (1U << 17); /* Bit 17: Sleep Interrupt Enable */
|
||||
|
||||
/* CAN error status register */
|
||||
|
||||
constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */
|
||||
constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
|
||||
constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */
|
||||
constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */
|
||||
constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT);
|
||||
constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT);/* 000: No Error */
|
||||
constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff Error */
|
||||
constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT);/* 010: Form Error */
|
||||
constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment Error */
|
||||
constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT);/* 100: Bit recessive Error */
|
||||
constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT);/* 101: Bit dominant Error */
|
||||
constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT);/* 110: CRC Error */
|
||||
constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT);/* 111: Set by software */
|
||||
constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */
|
||||
constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
|
||||
constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
|
||||
constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT);
|
||||
constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */
|
||||
constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
|
||||
constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */
|
||||
constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */
|
||||
constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT);
|
||||
constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT); /* 000: No Error */
|
||||
constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT); /* 001: Stuff Error */
|
||||
constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT); /* 010: Form Error */
|
||||
constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT); /* 011: Acknowledgment Error */
|
||||
constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT); /* 100: Bit recessive Error */
|
||||
constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT); /* 101: Bit dominant Error */
|
||||
constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT); /* 110: CRC Error */
|
||||
constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT); /* 111: Set by software */
|
||||
constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */
|
||||
constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
|
||||
constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
|
||||
constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT);
|
||||
|
||||
/* CAN bit timing register */
|
||||
|
||||
constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */
|
||||
constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
|
||||
constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */
|
||||
constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
|
||||
constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */
|
||||
constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT);
|
||||
constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
|
||||
constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT);
|
||||
constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/
|
||||
constexpr unsigned long BTR_SILM = (1U << 31);/* Bit 31: Silent Mode (Debug);*/
|
||||
constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */
|
||||
constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
|
||||
constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */
|
||||
constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
|
||||
constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */
|
||||
constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT);
|
||||
constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
|
||||
constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT);
|
||||
constexpr unsigned long BTR_LBKM = (1U << 30); /* Bit 30: Loop Back Mode (Debug);*/
|
||||
constexpr unsigned long BTR_SILM = (1U << 31); /* Bit 31: Silent Mode (Debug);*/
|
||||
|
||||
constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
|
||||
constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
|
||||
constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
|
||||
|
||||
/* TX mailbox identifier register */
|
||||
|
||||
constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */
|
||||
constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT);
|
||||
constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
|
||||
constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */
|
||||
constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT);
|
||||
constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
|
||||
|
||||
/* Mailbox data length control and time stamp register */
|
||||
|
||||
constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT);
|
||||
constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */
|
||||
constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
|
||||
constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT);
|
||||
constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */
|
||||
constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
|
||||
|
||||
/* Mailbox data low register */
|
||||
|
||||
constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
|
||||
|
||||
/* Mailbox data high register */
|
||||
|
||||
constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
|
||||
|
||||
/* Rx FIFO mailbox identifier register */
|
||||
|
||||
constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
|
||||
constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
|
||||
constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
|
||||
constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */
|
||||
constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
|
||||
constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
|
||||
constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
|
||||
constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
|
||||
|
||||
/* Receive FIFO mailbox data length control and time stamp register */
|
||||
|
||||
constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT);
|
||||
constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
|
||||
constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT);
|
||||
constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
|
||||
constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */
|
||||
constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT);
|
||||
constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
|
||||
constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT);
|
||||
constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
|
||||
constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
|
||||
|
||||
/* Receive FIFO mailbox data low register */
|
||||
|
||||
constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */
|
||||
constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */
|
||||
constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */
|
||||
constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
|
||||
constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */
|
||||
constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
|
||||
|
||||
/* Receive FIFO mailbox data high register */
|
||||
|
||||
constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */
|
||||
constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */
|
||||
constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */
|
||||
constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
|
||||
constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */
|
||||
constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
|
||||
|
||||
/* CAN filter master register */
|
||||
|
||||
constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */
|
||||
constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -48,25 +48,24 @@ void adjustUtc(uavcan::UtcDuration adjustment);
|
||||
/**
|
||||
* UTC clock synchronization parameters
|
||||
*/
|
||||
struct UtcSyncParams
|
||||
{
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
struct UtcSyncParams {
|
||||
float offset_p; ///< PPM per one usec error
|
||||
float rate_i; ///< PPM per one PPM error for second
|
||||
float rate_error_corner_freq;
|
||||
float max_rate_correction_ppm;
|
||||
float lock_thres_rate_ppm;
|
||||
uavcan::UtcDuration lock_thres_offset;
|
||||
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
|
||||
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
UtcSyncParams()
|
||||
: offset_p(0.01F)
|
||||
, rate_i(0.02F)
|
||||
, rate_error_corner_freq(0.01F)
|
||||
, max_rate_correction_ppm(300.0F)
|
||||
, lock_thres_rate_ppm(2.0F)
|
||||
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
|
||||
, min_jump(uavcan::UtcDuration::fromMSec(10))
|
||||
{ }
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -94,7 +93,7 @@ bool isUtcLocked();
|
||||
* Both functions are thread safe.
|
||||
*/
|
||||
UtcSyncParams getUtcSyncParams();
|
||||
void setUtcSyncParams(const UtcSyncParams& params);
|
||||
void setUtcSyncParams(const UtcSyncParams ¶ms);
|
||||
|
||||
}
|
||||
|
||||
@@ -103,19 +102,19 @@ void setUtcSyncParams(const UtcSyncParams& params);
|
||||
*/
|
||||
class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
|
||||
{
|
||||
SystemClock() { }
|
||||
SystemClock() { }
|
||||
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
|
||||
virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
|
||||
|
||||
public:
|
||||
virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
|
||||
virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
|
||||
virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
|
||||
virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
|
||||
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock& instance();
|
||||
/**
|
||||
* Calls clock::init() as needed.
|
||||
* This function is thread safe.
|
||||
*/
|
||||
static SystemClock &instance();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -32,69 +32,69 @@ class CanDriver;
|
||||
*/
|
||||
class BusEvent : uavcan::Noncopyable
|
||||
{
|
||||
using SignalCallbackHandler = void(*)();
|
||||
using SignalCallbackHandler = void(*)();
|
||||
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
SignalCallbackHandler signal_cb_{nullptr};
|
||||
sem_t sem_;
|
||||
|
||||
public:
|
||||
BusEvent(CanDriver& can_driver);
|
||||
~BusEvent();
|
||||
BusEvent(CanDriver &can_driver);
|
||||
~BusEvent();
|
||||
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
|
||||
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
bool wait(uavcan::MonotonicDuration duration);
|
||||
|
||||
void signalFromInterrupt();
|
||||
void signalFromInterrupt();
|
||||
};
|
||||
|
||||
class Mutex
|
||||
{
|
||||
pthread_mutex_t mutex_;
|
||||
pthread_mutex_t mutex_;
|
||||
|
||||
public:
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
Mutex()
|
||||
{
|
||||
init();
|
||||
}
|
||||
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
int init()
|
||||
{
|
||||
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
|
||||
}
|
||||
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
int deinit()
|
||||
{
|
||||
return pthread_mutex_destroy(&mutex_);
|
||||
}
|
||||
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
void lock()
|
||||
{
|
||||
(void)pthread_mutex_lock(&mutex_);
|
||||
}
|
||||
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
void unlock()
|
||||
{
|
||||
(void)pthread_mutex_unlock(&mutex_);
|
||||
}
|
||||
};
|
||||
#endif
|
||||
|
||||
|
||||
class MutexLocker
|
||||
{
|
||||
Mutex& mutex_;
|
||||
Mutex &mutex_;
|
||||
|
||||
public:
|
||||
MutexLocker(Mutex& mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
MutexLocker(Mutex &mutex)
|
||||
: mutex_(mutex)
|
||||
{
|
||||
mutex_.lock();
|
||||
}
|
||||
~MutexLocker()
|
||||
{
|
||||
mutex_.unlock();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -50,18 +50,17 @@ namespace uavcan_stm32
|
||||
{
|
||||
#if UAVCAN_STM32_NUTTX
|
||||
|
||||
struct CriticalSectionLocker
|
||||
{
|
||||
const irqstate_t flags_;
|
||||
struct CriticalSectionLocker {
|
||||
const irqstate_t flags_;
|
||||
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{ }
|
||||
CriticalSectionLocker()
|
||||
: flags_(enter_critical_section())
|
||||
{ }
|
||||
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
~CriticalSectionLocker()
|
||||
{
|
||||
leave_critical_section(flags_);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -13,50 +13,53 @@ namespace uavcan_stm32
|
||||
|
||||
#if UAVCAN_STM32_NUTTX
|
||||
|
||||
BusEvent::BusEvent(CanDriver& can_driver)
|
||||
BusEvent::BusEvent(CanDriver &can_driver)
|
||||
{
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
sem_init(&sem_, 0, 0);
|
||||
sem_setprotocol(&sem_, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
BusEvent::~BusEvent()
|
||||
{
|
||||
sem_destroy(&sem_);
|
||||
sem_destroy(&sem_);
|
||||
}
|
||||
|
||||
bool BusEvent::wait(uavcan::MonotonicDuration duration)
|
||||
{
|
||||
if (duration.isPositive()) {
|
||||
timespec abstime;
|
||||
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;
|
||||
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;
|
||||
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_();
|
||||
}
|
||||
if (sem_.semcount <= 0) {
|
||||
(void)sem_post(&sem_);
|
||||
}
|
||||
|
||||
if (signal_cb_) {
|
||||
signal_cb_();
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
Reference in New Issue
Block a user