fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 06:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #62 from UAVCAN/master_baremetal

Browse Source 
Master baremetal
This commit is contained in:
David Sidrane
2015-08-24 11:02:57 -10:00
parent be7f68763e 7fc43d0cbf
commit e6bdbaeb72
5 changed files with 295 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
+8
View File
@@ -24,6 +24,14 @@ void init();
*/
uavcan::MonotonicTime getMonotonic();
/**
* Sets the driver's notion of the system UTC. It should be called
* at startup and any time the system clock is updated from an
* external source that is not the UAVCAN Timesync master.
* This function is thread safe.
*/
void setUtc(uavcan::UtcTime time);
/**
* Returns UTC time if it has been set, otherwise returns zero time.
* This function is thread safe.
libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
+74 -4
View File
@@ -16,6 +16,7 @@
# include <cstdio>
# include <ctime>
# include <cstring>
#elif UAVCAN_STM32_BAREMETAL
#else
# error "Unknown OS"
#endif
@@ -92,11 +93,82 @@ public:
void signalFromInterrupt();
};
class Mutex
{
pthread_mutex_t mutex_;
public:
Mutex()
{
init();
}
int init()
{
return pthread_mutex_init(&mutex_, NULL);
}
int deinit()
{
return pthread_mutex_destroy(&mutex_);
}
void lock()
{
(void)pthread_mutex_lock(&mutex_);
}
void unlock()
{
(void)pthread_mutex_unlock(&mutex_);
}
};
#elif UAVCAN_STM32_BAREMETAL
class BusEvent
{
volatile bool ready;
public:
BusEvent(CanDriver& can_driver)
: ready(false)
{
(void)can_driver;
}
bool wait(uavcan::MonotonicDuration duration)
{
bool lready = ready;
return __atomic_exchange_n (&lready, false, __ATOMIC_SEQ_CST);
}
void signal()
{
__atomic_store_n (&ready, true, __ATOMIC_SEQ_CST);
}
void signalFromInterrupt()
{
__atomic_store_n (&ready, true, __ATOMIC_SEQ_CST);
}
};
class Mutex
{
public:
void lock()
{
};
void unlock()
{
};
};
#endif
#if UAVCAN_STM32_CHIBIOS
class MutexLocker
{
Mutex& mutex_;
@@ -113,6 +185,4 @@ public:
}
};
#endif
}
libuavcan_drivers/stm32/driver/src/internal.hpp
+34 -9
View File
@@ -10,7 +10,11 @@
# include <hal.h>
#elif UAVCAN_STM32_NUTTX
# include <nuttx/arch.h>
# include <arch/board/board.h>
# include <chip/stm32_tim.h>
# include <syslog.h>
#elif UAVCAN_STM32_BAREMETAL
# include <chip.h>
#else
# error "Unknown OS"
#endif
@@ -20,7 +24,7 @@
*/
#ifndef UAVCAN_STM32_LOG
// lowsyslog() crashes the system in this context
//# if UAVCAN_STM32_NUTTX && CONFIG_ARCH_LOWPUTC
// # if UAVCAN_STM32_NUTTX && CONFIG_ARCH_LOWPUTC
# if 0
# define UAVCAN_STM32_LOG(fmt, ...) lowsyslog("uavcan_stm32: " fmt "\n", ##__VA_ARGS__)
# else
@@ -32,17 +36,17 @@
* IRQ handler macros
*/
#if UAVCAN_STM32_CHIBIOS
# define UAVCAN_STM32_IRQ_HANDLER(id) CH_IRQ_HANDLER(id)
# define UAVCAN_STM32_IRQ_PROLOGUE() CH_IRQ_PROLOGUE()
# define UAVCAN_STM32_IRQ_EPILOGUE() CH_IRQ_EPILOGUE()
#elif UAVCAN_STM32_NUTTX
# define UAVCAN_STM32_IRQ_HANDLER(id) int id(int irq, FAR void* context)
# define UAVCAN_STM32_IRQ_PROLOGUE()
# define UAVCAN_STM32_IRQ_EPILOGUE() return 0;
#else
# define UAVCAN_STM32_IRQ_HANDLER(id) void id(void)
# define UAVCAN_STM32_IRQ_PROLOGUE()
# define UAVCAN_STM32_IRQ_EPILOGUE()
#endif
#if UAVCAN_STM32_CHIBIOS
@@ -54,6 +58,15 @@
# endif
#endif
#if UAVCAN_STM32_BAREMETAL
/**
* Priority mask for timer and CAN interrupts.
*/
# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
# define UAVCAN_STM32_IRQ_PRIORITY_MASK 0
# endif
#endif
/**
* Glue macros
*/
@@ -65,7 +78,6 @@
namespace uavcan_stm32
{
#if UAVCAN_STM32_CHIBIOS
struct CriticalSectionLocker
@@ -90,13 +102,26 @@ struct CriticalSectionLocker
}
};
#elif UAVCAN_STM32_BAREMETAL
struct CriticalSectionLocker
{
CriticalSectionLocker()
{
__disable_irq();
}
~CriticalSectionLocker()
{
__enable_irq();
}
};
#endif
namespace clock
{
uavcan::uint64_t getUtcUSecFromCanInterrupt();
}
}
libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+31 -4
View File
@@ -14,11 +14,13 @@
# include <nuttx/arch.h>
# include <nuttx/irq.h>
# include <arch/board/board.h>
#elif UAVCAN_STM32_BAREMETAL
#include <chip.h>
#else
# error "Unknown OS"
#endif
#if !UAVCAN_STM32_NUTTX
#if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
# if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F4XX))
// IRQ numbers
# define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn
@@ -188,7 +190,9 @@ int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out
/*
* Hardware configuration
*/
#if UAVCAN_STM32_CHIBIOS
#if UAVCAN_STM32_BAREMETAL
const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY;
#elif UAVCAN_STM32_CHIBIOS
const uavcan::uint32_t pclk = STM32_PCLK1;
#elif UAVCAN_STM32_NUTTX
const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY;
@@ -838,6 +842,22 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
return 1; // Return value doesn't matter as long as it is non-negative
}
#if UAVCAN_STM32_BAREMETAL
static void nvicEnableVector(int irq, uint8_t prio)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = irq;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
#endif
int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
{
int res = 0;
@@ -921,7 +941,7 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod
IRQ_ATTACH(STM32_IRQ_CAN2SCE, can2_irq);
# endif
# undef IRQ_ATTACH
#else
#elif UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
{
CriticalSectionLocker lock;
nvicEnableVector(CAN1_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
@@ -1044,7 +1064,7 @@ static int can2_irq(const int irq, void*)
# endif
#else // UAVCAN_STM32_NUTTX
UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1052,6 +1072,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1059,6 +1080,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler)
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1066,6 +1088,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler)
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1075,6 +1098,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler)
# if UAVCAN_STM32_NUM_IFACES > 1
UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1082,6 +1106,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler)
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1089,6 +1114,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler)
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
@@ -1096,6 +1122,7 @@ UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN2_SCE_IRQHandler);
UAVCAN_STM32_IRQ_HANDLER(CAN2_SCE_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+148 -24
View File
@@ -14,19 +14,35 @@
/*
* Timer instance
*/
#define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
#define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
#define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
# define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER)
# define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn)
# define TIMX_INPUT_CLOCK STM32_TIMCLK1
# endif
#if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
# define TIMX_RCC_ENR RCC->APB1ENR
# define TIMX_RCC_RSTR RCC->APB1RSTR
# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
# define TIMX_INPUT_CLOCK STM32_TIMCLK1
#else
# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
#endif
# if UAVCAN_STM32_NUTTX
# define TIMX UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _BASE)
# define TMR_REG(o) (TIMX + (o))
# define TIMX_INPUT_CLOCK STM32_TIM27_FREQUENCY
# define TIMX_IRQn UAVCAN_STM32_GLUE2(STM32_IRQ_TIM, UAVCAN_STM32_TIMER_NUMBER)
# endif
# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7
# define TIMX_RCC_ENR RCC->APB1ENR
# define TIMX_RCC_RSTR RCC->APB1RSTR
# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN)
# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST)
# else
# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet"
# endif
# if (TIMX_INPUT_CLOCK % 1000000) != 0
# error "No way, timer clock must be divisible to 1e6. FIXME!"
# endif
extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler);
namespace uavcan_stm32
{
@@ -57,6 +73,21 @@ uavcan::uint64_t time_utc = 0;
}
#if UAVCAN_STM32_BAREMETAL
static void nvicEnableVector(int irq, uint8_t prio)
{
NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = irq;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
}
#endif
void init()
{
CriticalSectionLocker lock;
@@ -66,6 +97,8 @@ void init()
}
initialized = true;
# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
// Power-on and reset
TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK;
TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK;
@@ -74,9 +107,9 @@ void init()
// Enable IRQ
nvicEnableVector(TIMX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
#if (TIMX_INPUT_CLOCK % 1000000) != 0
# error "No way, timer clock must be divisible to 1e6. FIXME!"
#endif
# if (TIMX_INPUT_CLOCK % 1000000) != 0
# error "No way, timer clock must be divisible to 1e6. FIXME!"
# endif
// Start the timer
TIMX->ARR = 0xFFFF;
@@ -86,10 +119,58 @@ void init()
TIMX->EGR = TIM_EGR_UG; // Reload immediately
TIMX->DIER = TIM_DIER_UIE;
TIMX->CR1 = TIM_CR1_CEN; // Start
# endif
# if UAVCAN_STM32_NUTTX
// Attach IRQ
irq_attach(TIMX_IRQn, &TIMX_IRQHandler);
// Power-on and reset
modifyreg32(STM32_RCC_APB1ENR, 0, TIMX_RCC_ENR_MASK);
modifyreg32(STM32_RCC_APB1RSTR, 0, TIMX_RCC_RSTR_MASK);
modifyreg32(STM32_RCC_APB1RSTR, TIMX_RCC_RSTR_MASK, 0);
// Start the timer
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET));
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
putreg16(BTIM_DIER_UIE, TMR_REG(STM32_BTIM_DIER_OFFSET));
putreg16(BTIM_CR1_CEN, TMR_REG(STM32_BTIM_CR1_OFFSET)); // Start
// Prioritize and Enable IRQ
// todo: Currently changing the NVIC_SYSH_HIGH_PRIORITY is HARD faulting
// need to investigate
// up_prioritize_irq(TIMX_IRQn, NVIC_SYSH_HIGH_PRIORITY);
up_enable_irq(TIMX_IRQn);
# endif
}
void setUtc(uavcan::UtcTime time)
{
MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized);
{
CriticalSectionLocker locker;
time_utc = time.toUSec();
}
utc_set = true;
utc_locked = false;
utc_jump_cnt++;
utc_prev_adj = 0;
utc_rel_rate_ppm = 0;
}
static uavcan::uint64_t sampleUtcFromCriticalSection()
{
# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
UAVCAN_ASSERT(initialized);
UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE);
@@ -104,6 +185,25 @@ static uavcan::uint64_t sampleUtcFromCriticalSection()
time = uavcan::uint64_t(uavcan::int64_t(time) + add);
}
return time + cnt;
# endif
# if UAVCAN_STM32_NUTTX
UAVCAN_ASSERT(initialized);
UAVCAN_ASSERT(getreg16(TMR_REG(STM32_BTIM_DIER_OFFSET)) & BTIM_DIER_UIE);
volatile uavcan::uint64_t time = time_utc;
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
{
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_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;
# endif
}
uavcan::uint64_t getUtcUSecFromCanInterrupt()
@@ -114,25 +214,41 @@ uavcan::uint64_t getUtcUSecFromCanInterrupt()
uavcan::MonotonicTime getMonotonic()
{
uavcan::uint64_t usec = 0;
// Scope Critical section
{
CriticalSectionLocker locker;
volatile uavcan::uint64_t time = time_mono;
# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
volatile uavcan::uint32_t cnt = TIMX->CNT;
if (TIMX->SR & TIM_SR_UIF)
{
cnt = TIMX->CNT;
# endif
# if UAVCAN_STM32_NUTTX
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF)
{
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
# endif
time += USecPerOverflow;
}
usec = time + cnt;
#ifndef NDEBUG
static uavcan::uint64_t prev_usec = 0; // Self-test
UAVCAN_ASSERT(prev_usec <= usec);
prev_usec = usec;
#endif
}
return uavcan::MonotonicTime::fromUSec(usec);
# ifndef NDEBUG
static uavcan::uint64_t prev_usec = 0; // Self-test
UAVCAN_ASSERT(prev_usec <= usec);
(void)prev_usec;
prev_usec = usec;
# endif
} // End Scope Critical section
return uavcan::MonotonicTime::fromUSec(usec);
}
uavcan::UtcTime getUtc()
@@ -201,7 +317,8 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F));
// lowsyslog("$ 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, total_rate_correction_ppm);
// adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm,
// total_rate_correction_ppm);
}
void adjustUtc(uavcan::UtcDuration adjustment)
@@ -288,12 +405,13 @@ SystemClock& SystemClock::instance()
long long _aligner_1;
long double _aligner_2;
} storage;
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer);
if (!clock::initialized)
{
clock::init();
new (ptr) SystemClock();
new (ptr)SystemClock();
}
return *ptr;
}
@@ -304,12 +422,18 @@ SystemClock& SystemClock::instance()
/**
* Timer interrupt handler
*/
extern "C"
UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
# if UAVCAN_STM32_CHIBIOS || UAVCAN_STM32_BAREMETAL
TIMX->SR = 0;
# endif
# if UAVCAN_STM32_NUTTX
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
# endif
using namespace uavcan_stm32::clock;
UAVCAN_ASSERT(initialized);