Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

uavcan drivers: update code style

Browse Source 
Pure refactoring, just running 'make format'.
This commit is contained in:
Beat Küng
2019-10-31 11:11:05 +01:00
parent 6049a95073
commit 6622f04feb
17 changed files with 3307 additions and 3429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
-1
View File
@@ -11,7 +11,6 @@ exec find boards msg src platforms \
-path msg/templates/urtps -prune -o \ -path msg/templates/urtps -prune -o \
-path platforms/nuttx/NuttX -prune -o \ -path platforms/nuttx/NuttX -prune -o \
-path src/drivers/uavcan/libuavcan -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/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \ -path src/lib/ecl -prune -o \
-path src/lib/matrix -prune -o \ -path src/lib/matrix -prune -o \
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp
+292 -296
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp
+40 -37
View File
@@ -49,26 +49,25 @@ void adjustUtc(uavcan::UtcDuration adjustment);
/** /**
* UTC clock synchronization parameters * UTC clock synchronization parameters
*/ */
struct UtcSyncParams struct UtcSyncParams {
{ float offset_p; ///< PPM per one usec error
float offset_p; ///< PPM per one usec error float rate_i; ///< PPM per one PPM error for second
float rate_i; ///< PPM per one PPM error for second float rate_error_corner_freq;
float rate_error_corner_freq; float max_rate_correction_ppm;
float max_rate_correction_ppm; float lock_thres_rate_ppm;
float lock_thres_rate_ppm; uavcan::UtcDuration lock_thres_offset;
uavcan::UtcDuration lock_thres_offset; uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
UtcSyncParams() UtcSyncParams()
: offset_p(0.01F), : offset_p(0.01F),
rate_i(0.02F), rate_i(0.02F),
rate_error_corner_freq(0.01F), rate_error_corner_freq(0.01F),
max_rate_correction_ppm(300.0F), max_rate_correction_ppm(300.0F),
lock_thres_rate_ppm(2.0F), lock_thres_rate_ppm(2.0F),
lock_thres_offset(uavcan::UtcDuration::fromMSec(4)), lock_thres_offset(uavcan::UtcDuration::fromMSec(4)),
min_jump(uavcan::UtcDuration::fromMSec(10)) min_jump(uavcan::UtcDuration::fromMSec(10))
{ {
} }
}; };
/** /**
@@ -96,7 +95,7 @@ bool isUtcLocked();
* Both functions are thread safe. * Both functions are thread safe.
*/ */
UtcSyncParams getUtcSyncParams(); UtcSyncParams getUtcSyncParams();
void setUtcSyncParams(const UtcSyncParams& params); void setUtcSyncParams(const UtcSyncParams &params);
} }
@@ -104,28 +103,32 @@ void setUtcSyncParams(const UtcSyncParams& params);
* Adapter for uavcan::ISystemClock. * Adapter for uavcan::ISystemClock.
*/ */
class SystemClock : public uavcan::ISystemClock class SystemClock : public uavcan::ISystemClock
, uavcan::Noncopyable , uavcan::Noncopyable
{ {
SystemClock() { SystemClock()
} {
}
virtual void adjustUtc(uavcan::UtcDuration adjustment) { virtual void adjustUtc(uavcan::UtcDuration adjustment)
clock::adjustUtc(adjustment); {
} clock::adjustUtc(adjustment);
}
public: public:
virtual uavcan::MonotonicTime getMonotonic() const { virtual uavcan::MonotonicTime getMonotonic() const
return clock::getMonotonic(); {
} return clock::getMonotonic();
virtual uavcan::UtcTime getUtc() const { }
return clock::getUtc(); virtual uavcan::UtcTime getUtc() const
} {
return clock::getUtc();
}
/** /**
* Calls clock::init() as needed. * Calls clock::init() as needed.
* This function is thread safe. * This function is thread safe.
*/ */
static SystemClock& instance(); static SystemClock &instance();
}; };
} }
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp
+478 -498
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp
+39 -39
View File
@@ -28,68 +28,68 @@ class CanDriver;
*/ */
class BusEvent : uavcan::Noncopyable class BusEvent : uavcan::Noncopyable
{ {
using SignalCallbackHandler = void(*)(); using SignalCallbackHandler = void(*)();
SignalCallbackHandler signal_cb_{nullptr}; SignalCallbackHandler signal_cb_{nullptr};
sem_t sem_; sem_t sem_;
public: public:
BusEvent(CanDriver& can_driver); BusEvent(CanDriver &can_driver);
~BusEvent(); ~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 class Mutex
{ {
pthread_mutex_t mutex_; pthread_mutex_t mutex_;
public: public:
Mutex() Mutex()
{ {
init(); init();
} }
int init() int init()
{ {
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
} }
int deinit() int deinit()
{ {
return pthread_mutex_destroy(&mutex_); return pthread_mutex_destroy(&mutex_);
} }
void lock() void lock()
{ {
(void)pthread_mutex_lock(&mutex_); (void)pthread_mutex_lock(&mutex_);
} }
void unlock() void unlock()
{ {
(void)pthread_mutex_unlock(&mutex_); (void)pthread_mutex_unlock(&mutex_);
} }
}; };
class MutexLocker class MutexLocker
{ {
Mutex& mutex_; Mutex &mutex_;
public: public:
MutexLocker(Mutex& mutex) MutexLocker(Mutex &mutex)
: mutex_(mutex) : mutex_(mutex)
{ {
mutex_.lock(); mutex_.lock();
} }
~MutexLocker() ~MutexLocker()
{ {
mutex_.unlock(); mutex_.unlock();
} }
}; };
} }
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp
+10 -11
View File
@@ -48,19 +48,18 @@ namespace uavcan_kinetis
{ {
#if UAVCAN_KINETIS_NUTTX #if UAVCAN_KINETIS_NUTTX
struct CriticalSectionLocker struct CriticalSectionLocker {
{ const irqstate_t flags_;
const irqstate_t flags_;
CriticalSectionLocker() CriticalSectionLocker()
: flags_(enter_critical_section()) : flags_(enter_critical_section())
{ {
} }
~CriticalSectionLocker() ~CriticalSectionLocker()
{ {
leave_critical_section(flags_); leave_critical_section(flags_);
} }
}; };
#endif #endif
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp
+188 -197
View File
@@ -66,167 +66,166 @@ uavcan::uint64_t time_utc = 0;
void init() void init()
{ {
CriticalSectionLocker lock; CriticalSectionLocker lock;
if (initialized)
{
return;
}
initialized = true;
// Attach IRQ if (initialized) {
irq_attach(TIMX_IRQn, &TIMX_IRQHandler, NULL); return;
}
// Power-on Clock initialized = true;
modifyreg32(KINETIS_SIM_SCGC6, 0, SIM_SCGC6_PIT);
// Enable module // Attach IRQ
putreg32(0, KINETIS_PIT_MCR); 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)); // Enable module
putreg32(PIT_TCTRL_TEN | PIT_TCTRL_TIE, TMR_REG(KINETIS_PIT_TCTRL_OFFSET)); // Start 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 #if 0
// This has to be off or uses the default priority // This has to be off or uses the default priority
// Without the ability to point the vector // Without the ability to point the vector
// Directly to this ISR this will reenter the // Directly to this ISR this will reenter the
// exception_common and cause the interrupt // exception_common and cause the interrupt
// Stack pointer to be reset // Stack pointer to be reset
up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY); up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
#endif #endif
up_enable_irq(TIMX_IRQn); up_enable_irq(TIMX_IRQn);
} }
void setUtc(uavcan::UtcTime time) void setUtc(uavcan::UtcTime time)
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
{ {
CriticalSectionLocker locker; CriticalSectionLocker locker;
time_utc = time.toUSec(); time_utc = time.toUSec();
} }
utc_set = true; utc_set = true;
utc_locked = false; utc_locked = false;
utc_jump_cnt++; utc_jump_cnt++;
utc_prev_adj = 0; utc_prev_adj = 0;
utc_rel_rate_ppm = 0; utc_rel_rate_ppm = 0;
} }
static uavcan::uint64_t sampleUtcFromCriticalSection() static uavcan::uint64_t sampleUtcFromCriticalSection()
{ {
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE); UAVCAN_ASSERT(getreg32(TMR_REG(KINETIS_PIT_TCTRL_OFFSET)) & PIT_TCTRL_TIE);
volatile uavcan::uint64_t time = time_utc; volatile uavcan::uint64_t time = time_utc;
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
{ cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; time = uavcan::uint64_t(uavcan::int64_t(time) + add);
time = uavcan::uint64_t(uavcan::int64_t(time) + add); }
}
return time + (cnt / CountsPerUs); return time + (cnt / CountsPerUs);
} }
uavcan::uint64_t getUtcUSecFromCanInterrupt() uavcan::uint64_t getUtcUSecFromCanInterrupt()
{ {
return utc_set ? sampleUtcFromCriticalSection() : 0; return utc_set ? sampleUtcFromCriticalSection() : 0;
} }
uavcan::MonotonicTime getMonotonic() uavcan::MonotonicTime getMonotonic()
{ {
uavcan::uint64_t usec = 0; uavcan::uint64_t usec = 0;
// Scope Critical section // Scope Critical section
{ {
CriticalSectionLocker locker; CriticalSectionLocker locker;
volatile uavcan::uint64_t time = time_mono; volatile uavcan::uint64_t time = time_mono;
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
{ cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); time += USecPerOverflow;
time += USecPerOverflow; }
}
usec = time + (cnt / CountsPerUs);
} // 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() uavcan::UtcTime getUtc()
{ {
if (utc_set) if (utc_set) {
{ uavcan::uint64_t usec = 0;
uavcan::uint64_t usec = 0; {
{ CriticalSectionLocker locker;
CriticalSectionLocker locker; usec = sampleUtcFromCriticalSection();
usec = sampleUtcFromCriticalSection(); }
} return uavcan::UtcTime::fromUSec(usec);
return uavcan::UtcTime::fromUSec(usec); }
}
return uavcan::UtcTime(); return uavcan::UtcTime();
} }
static float lowpass(float xold, float xnew, float corner, float dt) static float lowpass(float xold, float xnew, float corner, float dt)
{ {
const float tau = 1.F / corner; const float tau = 1.F / corner;
return (dt * xnew + tau * xold) / (dt + tau); return (dt * xnew + tau * xold) / (dt + tau);
} }
static void updateRatePID(uavcan::UtcDuration adjustment) static void updateRatePID(uavcan::UtcDuration adjustment)
{ {
const uavcan::MonotonicTime ts = getMonotonic(); const uavcan::MonotonicTime ts = getMonotonic();
const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F; const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F;
prev_utc_adj_at = ts; prev_utc_adj_at = ts;
const float adj_usec = float(adjustment.toUSec()); const float adj_usec = float(adjustment.toUSec());
/* /*
* Target relative rate in PPM * Target relative rate in PPM
* Positive to go faster * Positive to go faster
*/ */
const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p; const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p;
/* /*
* Current relative rate in PPM * Current relative rate in PPM
* Positive if the local clock is faster * 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 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_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); 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) if (dt > 10) {
{ utc_rel_rate_error_integral = 0;
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);
}
/* } else {
* Rate controller utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
*/ utc_rel_rate_error_integral =
float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral; uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); utc_rel_rate_error_integral =
total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); 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", // 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, // 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) void adjustUtc(uavcan::UtcDuration adjustment)
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
{ const uavcan::int64_t adj_usec = adjustment.toUSec();
const uavcan::int64_t adj_usec = adjustment.toUSec();
{ {
CriticalSectionLocker locker; 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);
}
}
utc_set = true; if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
utc_locked = false; time_utc = 1;
utc_jump_cnt++;
utc_prev_adj = 0;
utc_rel_rate_ppm = 0;
}
else
{
updateRatePID(adjustment);
if (!utc_locked) } else {
{ time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
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()));
} 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() float getUtcRateCorrectionPPM()
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000);
return 1e6F * rate_correction_mult; return 1e6F * rate_correction_mult;
} }
uavcan::uint32_t getUtcJumpCount() uavcan::uint32_t getUtcJumpCount()
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
return utc_jump_cnt; return utc_jump_cnt;
} }
bool isUtcLocked() bool isUtcLocked()
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
return utc_locked; return utc_locked;
} }
UtcSyncParams getUtcSyncParams() UtcSyncParams getUtcSyncParams()
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
return utc_sync_params; return utc_sync_params;
} }
void setUtcSyncParams(const UtcSyncParams& params) void setUtcSyncParams(const UtcSyncParams &params)
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
// Add some sanity check // Add some sanity check
utc_sync_params = params; utc_sync_params = params;
} }
} // namespace clock } // namespace clock
SystemClock& SystemClock::instance() SystemClock &SystemClock::instance()
{ {
static union SystemClockStorage static union SystemClockStorage {
{ uavcan::uint8_t buffer[sizeof(SystemClock)];
uavcan::uint8_t buffer[sizeof(SystemClock)]; long long _aligner_1;
long long _aligner_1; long double _aligner_2;
long double _aligner_2; } storage;
} storage;
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer); SystemClock *const ptr = reinterpret_cast<SystemClock *>(storage.buffer);
if (!clock::initialized) if (!clock::initialized) {
{ MutexLocker mlocker(clock::mutex);
MutexLocker mlocker(clock::mutex); clock::init();
clock::init(); new (ptr)SystemClock();
new (ptr)SystemClock(); }
}
return *ptr; return *ptr;
} }
} // namespace uavcan_kinetis } // namespace uavcan_kinetis
@@ -337,38 +331,35 @@ SystemClock& SystemClock::instance()
extern "C" extern "C"
UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler) 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; using namespace uavcan_kinetis::clock;
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
time_mono += USecPerOverflow; time_mono += USecPerOverflow;
if (utc_set) if (utc_set) {
{ time_utc += USecPerOverflow;
time_utc += USecPerOverflow; utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
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;
}
// Correction decay - 1 nsec per 65536 usec if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
if (utc_correction_nsec_per_overflow > 0) time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000);
{ utc_accumulated_correction_nsec %= 1000;
utc_correction_nsec_per_overflow--; }
}
else if (utc_correction_nsec_per_overflow < 0) // Correction decay - 1 nsec per 65536 usec
{ if (utc_correction_nsec_per_overflow > 0) {
utc_correction_nsec_per_overflow++; utc_correction_nsec_per_overflow--;
}
else } else if (utc_correction_nsec_per_overflow < 0) {
{ utc_correction_nsec_per_overflow++;
; // Zero
} } else {
} ; // Zero
return 0; }
}
return 0;
} }
#endif #endif
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp
+731 -757
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp
+32 -29
View File
@@ -11,50 +11,53 @@
namespace uavcan_kinetis namespace uavcan_kinetis
{ {
BusEvent::BusEvent(CanDriver& can_driver) BusEvent::BusEvent(CanDriver &can_driver)
{ {
sem_init(&sem_, 0, 0); sem_init(&sem_, 0, 0);
sem_setprotocol(&sem_, SEM_PRIO_NONE); sem_setprotocol(&sem_, SEM_PRIO_NONE);
} }
BusEvent::~BusEvent() BusEvent::~BusEvent()
{ {
sem_destroy(&sem_); sem_destroy(&sem_);
} }
bool BusEvent::wait(uavcan::MonotonicDuration duration) bool BusEvent::wait(uavcan::MonotonicDuration duration)
{ {
if (duration.isPositive()) { if (duration.isPositive()) {
timespec abstime; timespec abstime;
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
const unsigned billion = 1000 * 1000 * 1000; const unsigned billion = 1000 * 1000 * 1000;
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
abstime.tv_sec += nsecs / billion; abstime.tv_sec += nsecs / billion;
nsecs -= (nsecs / billion) * billion; nsecs -= (nsecs / billion) * billion;
abstime.tv_nsec = nsecs; abstime.tv_nsec = nsecs;
int ret; int ret;
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
if (ret == -1) { // timed out or error while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
return false;
} if (ret == -1) { // timed out or error
return true; return false;
} }
}
return false; return true;
}
}
return false;
} }
void BusEvent::signalFromInterrupt() void BusEvent::signalFromInterrupt()
{ {
if (sem_.semcount <= 0) if (sem_.semcount <= 0) {
{ (void)sem_post(&sem_);
(void)sem_post(&sem_); }
}
if (signal_cb_) if (signal_cb_) {
{ signal_cb_();
signal_cb_(); }
}
} }
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp
+188 -193
View File
@@ -24,262 +24,257 @@ namespace uavcan_stm32
namespace bxcan namespace bxcan
{ {
struct TxMailboxType struct TxMailboxType {
{ volatile uint32_t TIR;
volatile uint32_t TIR; volatile uint32_t TDTR;
volatile uint32_t TDTR; volatile uint32_t TDLR;
volatile uint32_t TDLR; volatile uint32_t TDHR;
volatile uint32_t TDHR;
}; };
struct RxMailboxType struct RxMailboxType {
{ volatile uint32_t RIR;
volatile uint32_t RIR; volatile uint32_t RDTR;
volatile uint32_t RDTR; volatile uint32_t RDLR;
volatile uint32_t RDLR; volatile uint32_t RDHR;
volatile uint32_t RDHR;
}; };
struct FilterRegisterType struct FilterRegisterType {
{ volatile uint32_t FR1;
volatile uint32_t FR1; volatile uint32_t FR2;
volatile uint32_t FR2;
}; };
struct CanType struct CanType {
{ volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */
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 MSR; /*!< CAN master status register, Address offset: 0x04 */ volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
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 RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */ volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */
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 IER; /*!< CAN interrupt enable register, Address offset: 0x14 */ volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */
volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */ volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */
volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */ uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */
uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */ TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */
TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */ RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */
RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */ uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */
uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */ volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */
volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */ volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */
volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */ uint32_t RESERVED2; /*!< Reserved, 0x208 */
uint32_t RESERVED2; /*!< Reserved, 0x208 */ volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */
volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */ uint32_t RESERVED3; /*!< Reserved, 0x210 */
uint32_t RESERVED3; /*!< Reserved, 0x210 */ volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */
volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */ uint32_t RESERVED4; /*!< Reserved, 0x218 */
uint32_t RESERVED4; /*!< Reserved, 0x218 */ volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */
volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */ uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */
uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */ FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */
}; };
/** /**
* CANx register sets * CANx register sets
*/ */
CanType* const Can[UAVCAN_STM32_NUM_IFACES] = CanType *const Can[UAVCAN_STM32_NUM_IFACES] = {
{ reinterpret_cast<CanType *>(0x40006400)
reinterpret_cast<CanType*>(0x40006400)
#if UAVCAN_STM32_NUM_IFACES > 1 #if UAVCAN_STM32_NUM_IFACES > 1
, ,
reinterpret_cast<CanType*>(0x40006800) reinterpret_cast<CanType *>(0x40006800)
#endif #endif
}; };
/* CAN master control register */ /* CAN master control register */
constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */ 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_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */
constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */ 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_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_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */
constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */ 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_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_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_RESET = (1U << 15); /* Bit 15: bxCAN software master reset */
constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */ constexpr unsigned long MCR_DBF = (1U << 16); /* Bit 16: Debug freeze */
/* CAN master status register */ /* CAN master status register */
constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */ 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_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */
constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */ 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_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */
constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge 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_TXM = (1U << 8); /* Bit 8: Transmit Mode */
constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive 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_SAMP = (1U << 10); /* Bit 10: Last Sample Point */
constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */ constexpr unsigned long MSR_RX = (1U << 11); /* Bit 11: CAN Rx Signal */
/* CAN transmit status register */ /* CAN transmit status register */
constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */ 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_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_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_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_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_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_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_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_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_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_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_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_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_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_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_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT); 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_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_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_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_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_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_LOW2 = (1U << 31); /* Bit 31: Lowest Priority Flag for Mailbox 2 */
/* CAN receive FIFO 0/1 registers */ /* CAN receive FIFO 0/1 registers */
constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */ 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_FMP_MASK = (3U << RFR_FMP_SHIFT);
constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */ 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_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_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */
/* CAN interrupt enable register */ /* CAN interrupt enable register */
constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty 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_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_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_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_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_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_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_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_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_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_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_ERRIE = (1U << 15); /* Bit 15: Error Interrupt Enable */
constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup 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_SLKIE = (1U << 17); /* Bit 17: Sleep Interrupt Enable */
/* CAN error status register */ /* CAN error status register */
constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */ 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_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */
constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off 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_SHIFT = (4U); /* Bits 6-4: Last Error Code */
constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT); 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_NOERROR = (0U << ESR_LEC_SHIFT); /* 000: No Error */
constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff 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_FORMERROR = (2U << ESR_LEC_SHIFT); /* 010: Form Error */
constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment 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_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_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_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_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_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_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */ 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_REC_MASK = (0xFFU << ESR_REC_SHIFT);
/* CAN bit timing register */ /* CAN bit timing register */
constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */ 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_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT);
constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */ 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_TS1_MASK = (0x0FU << BTR_TS1_SHIFT);
constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */ 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_TS2_MASK = (7U << BTR_TS2_SHIFT);
constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */ 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_SJW_MASK = (3U << BTR_SJW_SHIFT);
constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/ 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_SILM = (1U << 31); /* Bit 31: Silent Mode (Debug);*/
constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR 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_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/ constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/
/* TX mailbox identifier register */ /* TX mailbox identifier register */
constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */ 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_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */
constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ 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_SHIFT = (3U); /* Bit 3-31: Extended Identifier */
constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT); 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_SHIFT = (21U); /* Bits 21-31: Standard Identifier */
constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT); constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT);
/* Mailbox data length control and time stamp register */ /* 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_SHIFT = (0U); /* Bits 3:0: Data Length Code */
constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT); 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_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_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT); constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT);
/* Mailbox data low register */ /* Mailbox data low register */
constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ 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_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT);
constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ 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_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT);
constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ 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_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT);
constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ 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_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT);
/* Mailbox data high register */ /* Mailbox data high register */
constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ 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_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT);
constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ 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_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT);
constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ 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_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT);
constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ 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_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT);
/* Rx FIFO mailbox identifier register */ /* Rx FIFO mailbox identifier register */
constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ 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_IDE = (1U << 2); /* Bit 2: Identifier Extension */
constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ 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_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT);
constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ 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_STID_MASK = (0x07FFU << RIR_STID_SHIFT);
/* Receive FIFO mailbox data length control and time stamp register */ /* 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_SHIFT = (0U); /* Bits 3:0: Data Length Code */
constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT); 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_SHIFT = (8U); /* Bits 15-8: Filter Match Index */
constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT); 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_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */
constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT); constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT);
/* Receive FIFO mailbox data low register */ /* Receive FIFO mailbox data low register */
constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ 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_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT);
constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ 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_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT);
constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ 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_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT);
constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ 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_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT);
/* Receive FIFO mailbox data high register */ /* Receive FIFO mailbox data high register */
constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ 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_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT);
constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ 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_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT);
constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ 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_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT);
constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ 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_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT);
/* CAN filter master register */ /* 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 */
} }
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
+251 -262
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
+27 -28
View File
@@ -48,25 +48,24 @@ void adjustUtc(uavcan::UtcDuration adjustment);
/** /**
* UTC clock synchronization parameters * UTC clock synchronization parameters
*/ */
struct UtcSyncParams struct UtcSyncParams {
{ float offset_p; ///< PPM per one usec error
float offset_p; ///< PPM per one usec error float rate_i; ///< PPM per one PPM error for second
float rate_i; ///< PPM per one PPM error for second float rate_error_corner_freq;
float rate_error_corner_freq; float max_rate_correction_ppm;
float max_rate_correction_ppm; float lock_thres_rate_ppm;
float lock_thres_rate_ppm; uavcan::UtcDuration lock_thres_offset;
uavcan::UtcDuration lock_thres_offset; uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate
UtcSyncParams() UtcSyncParams()
: offset_p(0.01F) : offset_p(0.01F)
, rate_i(0.02F) , rate_i(0.02F)
, rate_error_corner_freq(0.01F) , rate_error_corner_freq(0.01F)
, max_rate_correction_ppm(300.0F) , max_rate_correction_ppm(300.0F)
, lock_thres_rate_ppm(2.0F) , lock_thres_rate_ppm(2.0F)
, lock_thres_offset(uavcan::UtcDuration::fromMSec(4)) , lock_thres_offset(uavcan::UtcDuration::fromMSec(4))
, min_jump(uavcan::UtcDuration::fromMSec(10)) , min_jump(uavcan::UtcDuration::fromMSec(10))
{ } { }
}; };
/** /**
@@ -94,7 +93,7 @@ bool isUtcLocked();
* Both functions are thread safe. * Both functions are thread safe.
*/ */
UtcSyncParams getUtcSyncParams(); UtcSyncParams getUtcSyncParams();
void setUtcSyncParams(const UtcSyncParams& params); void setUtcSyncParams(const UtcSyncParams &params);
} }
@@ -103,19 +102,19 @@ void setUtcSyncParams(const UtcSyncParams& params);
*/ */
class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable 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: public:
virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); } virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); } virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
/** /**
* Calls clock::init() as needed. * Calls clock::init() as needed.
* This function is thread safe. * This function is thread safe.
*/ */
static SystemClock& instance(); static SystemClock &instance();
}; };
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
+39 -39
View File
@@ -32,69 +32,69 @@ class CanDriver;
*/ */
class BusEvent : uavcan::Noncopyable class BusEvent : uavcan::Noncopyable
{ {
using SignalCallbackHandler = void(*)(); using SignalCallbackHandler = void(*)();
SignalCallbackHandler signal_cb_{nullptr}; SignalCallbackHandler signal_cb_{nullptr};
sem_t sem_; sem_t sem_;
public: public:
BusEvent(CanDriver& can_driver); BusEvent(CanDriver &can_driver);
~BusEvent(); ~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 class Mutex
{ {
pthread_mutex_t mutex_; pthread_mutex_t mutex_;
public: public:
Mutex() Mutex()
{ {
init(); init();
} }
int init() int init()
{ {
return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR); return pthread_mutex_init(&mutex_, UAVCAN_NULLPTR);
} }
int deinit() int deinit()
{ {
return pthread_mutex_destroy(&mutex_); return pthread_mutex_destroy(&mutex_);
} }
void lock() void lock()
{ {
(void)pthread_mutex_lock(&mutex_); (void)pthread_mutex_lock(&mutex_);
} }
void unlock() void unlock()
{ {
(void)pthread_mutex_unlock(&mutex_); (void)pthread_mutex_unlock(&mutex_);
} }
}; };
#endif #endif
class MutexLocker class MutexLocker
{ {
Mutex& mutex_; Mutex &mutex_;
public: public:
MutexLocker(Mutex& mutex) MutexLocker(Mutex &mutex)
: mutex_(mutex) : mutex_(mutex)
{ {
mutex_.lock(); mutex_.lock();
} }
~MutexLocker() ~MutexLocker()
{ {
mutex_.unlock(); mutex_.unlock();
} }
}; };
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp
+9 -10
View File
@@ -50,18 +50,17 @@ namespace uavcan_stm32
{ {
#if UAVCAN_STM32_NUTTX #if UAVCAN_STM32_NUTTX
struct CriticalSectionLocker struct CriticalSectionLocker {
{ const irqstate_t flags_;
const irqstate_t flags_;
CriticalSectionLocker() CriticalSectionLocker()
: flags_(enter_critical_section()) : flags_(enter_critical_section())
{ } { }
~CriticalSectionLocker() ~CriticalSectionLocker()
{ {
leave_critical_section(flags_); leave_critical_section(flags_);
} }
}; };
#endif #endif
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+760 -802
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+191 -201
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
+32 -29
View File
@@ -13,50 +13,53 @@ namespace uavcan_stm32
#if UAVCAN_STM32_NUTTX #if UAVCAN_STM32_NUTTX
BusEvent::BusEvent(CanDriver& can_driver) BusEvent::BusEvent(CanDriver &can_driver)
{ {
sem_init(&sem_, 0, 0); sem_init(&sem_, 0, 0);
sem_setprotocol(&sem_, SEM_PRIO_NONE); sem_setprotocol(&sem_, SEM_PRIO_NONE);
} }
BusEvent::~BusEvent() BusEvent::~BusEvent()
{ {
sem_destroy(&sem_); sem_destroy(&sem_);
} }
bool BusEvent::wait(uavcan::MonotonicDuration duration) bool BusEvent::wait(uavcan::MonotonicDuration duration)
{ {
if (duration.isPositive()) { if (duration.isPositive()) {
timespec abstime; timespec abstime;
if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) { if (clock_gettime(CLOCK_REALTIME, &abstime) == 0) {
const unsigned billion = 1000 * 1000 * 1000; const unsigned billion = 1000 * 1000 * 1000;
uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000; uint64_t nsecs = abstime.tv_nsec + (uint64_t)duration.toUSec() * 1000;
abstime.tv_sec += nsecs / billion; abstime.tv_sec += nsecs / billion;
nsecs -= (nsecs / billion) * billion; nsecs -= (nsecs / billion) * billion;
abstime.tv_nsec = nsecs; abstime.tv_nsec = nsecs;
int ret; int ret;
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
if (ret == -1) { // timed out or error while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
return false;
} if (ret == -1) { // timed out or error
return true; return false;
} }
}
return false; return true;
}
}
return false;
} }
void BusEvent::signalFromInterrupt() void BusEvent::signalFromInterrupt()
{ {
if (sem_.semcount <= 0) if (sem_.semcount <= 0) {
{ (void)sem_post(&sem_);
(void)sem_post(&sem_); }
}
if (signal_cb_) if (signal_cb_) {
{ signal_cb_();
signal_cb_(); }
}
} }
#endif #endif