fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into lpc11c24

Browse Source
This commit is contained in:
Pavel Kirienko
2015-10-11 18:36:34 +03:00
parent d75e76555d 8effe93d6e
commit 3f8a61a4c1
37 changed files with 854 additions and 312 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp
+1 -1
View File
@@ -44,7 +44,7 @@ uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector<std::string>& ifac
*/
uavcan::DynamicNodeIDClient client(*node);
ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion(), preferred_node_id));
ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion().unique_id, preferred_node_id));
std::cout << "Waiting for dynamic node ID allocation..." << std::endl;
libuavcan_drivers/linux/apps/test_file_server.cpp
+1 -1
View File
@@ -48,7 +48,7 @@ uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::N
void runForever(const uavcan_linux::NodePtr& node)
{
uavcan_posix::BasicFileSeverBackend backend(*node);
uavcan_posix::BasicFileServerBackend backend(*node);
uavcan::FileServer server(*node, backend);
libuavcan_drivers/linux/apps/test_multithreading.cpp
-4
View File
@@ -354,10 +354,6 @@ class VirtualCanDriver : public uavcan::ICanDriver,
ifaces_[frame.iface_index]->addRxFrame(frame, flags);
event_.signal();
}
else
{
assert(false);
}
}
/**
libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp
+4 -4
View File
@@ -138,11 +138,11 @@ public:
* It takes the following inputs:
* - Unique machine ID
* - Node name string (e.g. "org.uavcan.linux_app.dynamic_node_id_server")
* - Instance ID byte, e.g. node ID
* - Instance ID byte, e.g. node ID (optional)
*/
std::array<std::uint8_t, 16> makeApplicationID(const MachineIDReader::MachineID& machine_id,
const std::string& node_name,
const std::uint8_t instance_id)
inline std::array<std::uint8_t, 16> makeApplicationID(const MachineIDReader::MachineID& machine_id,
const std::string& node_name,
const std::uint8_t instance_id = 0)
{
union HalfID
{
libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp
+12 -3
View File
@@ -15,6 +15,7 @@
#include <cstdlib>
#include <cstring>
#include <cerrno>
#include <ctime>
#include <unistd.h>
#include <fcntl.h>
@@ -31,7 +32,7 @@ namespace uavcan_posix
/**
* This interface implements a POSIX compliant IFileServerBackend interface
*/
class BasicFileSeverBackend : public uavcan::IFileServerBackend
class BasicFileServerBackend : public uavcan::IFileServerBackend
{
enum { FilePermissions = 438 }; ///< 0o666
@@ -433,12 +434,12 @@ protected:
}
public:
BasicFileSeverBackend(uavcan::INode& node) :
BasicFileServerBackend(uavcan::INode& node) :
fdcache_(NULL),
node_(node)
{ }
~BasicFileSeverBackend()
~BasicFileServerBackend()
{
if (fdcache_ != &fallback_)
{
@@ -447,6 +448,14 @@ public:
}
}
};
#if __GNUC__
/// Typo fix in a backwards-compatible way (only for GCC projects). Will be removed someday.
typedef BasicFileServerBackend
BasicFileSeverBackend // Missing 'r'
__attribute__((deprecated));
#endif
}
#endif // Include guard
libuavcan_drivers/stm32/driver/CMakeLists.txt
+17
View File
@@ -0,0 +1,17 @@
include_directories(
./include
)
add_library(uavcan_stm32_driver STATIC
./src/uc_stm32_can.cpp
./src/uc_stm32_clock.cpp
./src/uc_stm32_thread.cpp
)
add_dependencies(uavcan_stm32_driver uavcan)
install(DIRECTORY include/uavcan_stm32 DESTINATION include)
install(TARGETS uavcan_stm32_driver DESTINATION lib)
# vim: set et ft=cmake fenc=utf-8 ff=unix sts=4 sw=4 ts=4 :)
libuavcan_drivers/stm32/driver/include/uavcan_stm32/build_config.hpp
+4
View File
@@ -15,6 +15,10 @@
# define UAVCAN_STM32_NUTTX 0
#endif
#ifndef UAVCAN_STM32_BAREMETAL
# define UAVCAN_STM32_BAREMETAL 0
#endif
/**
* Number of interfaces must be enabled explicitly
*/
libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
+2
View File
@@ -223,6 +223,8 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline);
static void initOnce();
public:
template <unsigned RxQueueCapacity>
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
+25 -7
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.
@@ -42,13 +50,23 @@ void adjustUtc(uavcan::UtcDuration adjustment);
*/
struct UtcSyncParams
{
float offset_p = 0.01F; ///< PPM per one usec error
float rate_i = 0.02F; ///< PPM per one PPM error for second
float rate_error_corner_freq = 0.01F;
float max_rate_correction_ppm = 300.0F;
float lock_thres_rate_ppm = 2.0F;
uavcan::UtcDuration lock_thres_offset = uavcan::UtcDuration::fromMSec(4);
uavcan::UtcDuration min_jump = uavcan::UtcDuration::fromMSec(10); ///< Min error to jump rather than change rate
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))
{ }
};
/**
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
+39 -10
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
@@ -50,7 +54,20 @@
* Priority mask for timer and CAN interrupts.
*/
# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK
# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY)
# if (CH_KERNEL_MAJOR == 2)
# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY)
# else // ChibiOS 3
# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_MAX_KERNEL_PRIORITY
# endif
# 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
@@ -65,7 +82,6 @@
namespace uavcan_stm32
{
#if UAVCAN_STM32_CHIBIOS
struct CriticalSectionLocker
@@ -90,13 +106,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
+128 -83
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 && CH_KERNEL_MAJOR == 2) || UAVCAN_STM32_BAREMETAL
# if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F4XX))
// IRQ numbers
# define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn
@@ -31,6 +33,17 @@
# endif
#endif
#if (UAVCAN_STM32_CHIBIOS && CH_KERNEL_MAJOR == 3)
#define CAN1_TX_IRQHandler STM32_CAN1_TX_HANDLER
#define CAN1_RX0_IRQHandler STM32_CAN1_RX0_HANDLER
#define CAN1_RX1_IRQHandler STM32_CAN1_RX1_HANDLER
#define CAN1_SCE_IRQHandler STM32_CAN1_SCE_HANDLER
#define CAN2_TX_IRQHandler STM32_CAN2_TX_HANDLER
#define CAN2_RX0_IRQHandler STM32_CAN2_RX0_HANDLER
#define CAN2_RX1_IRQHandler STM32_CAN2_RX1_HANDLER
#define CAN2_SCE_IRQHandler STM32_CAN2_SCE_HANDLER
#endif
#if UAVCAN_STM32_NUTTX
# if !defined(STM32_IRQ_CAN1TX) && !defined(STM32_IRQ_CAN1RX0)
# define STM32_IRQ_CAN1TX STM32_IRQ_USBHPCANTX
@@ -45,6 +58,17 @@ static int can2_irq(const int irq, void*);
}
#endif
/* STM32F3's only CAN inteface does not have a number. */
#if defined(STM32F3XX)
#define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN
#define RCC_APB1RSTR_CAN1RST RCC_APB1RSTR_CANRST
#define CAN1_TX_IRQn CAN_TX_IRQn
#define CAN1_RX0_IRQn CAN_RX0_IRQn
#define CAN1_RX1_IRQn CAN_RX1_IRQn
#define CAN1_SCE_IRQn CAN_SCE_IRQn
#endif
namespace uavcan_stm32
{
namespace
@@ -188,7 +212,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;
@@ -462,10 +488,26 @@ bool CanIface::waitMsrINakBitStateChange(bool target_state)
int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
{
int res = 0;
/*
* We need to silence the controller in the first order, otherwise it may interfere with the following operations.
*/
{
CriticalSectionLocker lock;
can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode
can_->MCR |= bxcan::MCR_INRQ; // Request init
can_->IER = 0; // Disable interrupts while initialization is in progress
}
if (!waitMsrINakBitStateChange(true))
{
UAVCAN_STM32_LOG("MSR INAK not set");
return -1;
}
/*
* Object state
* Object state - interrupts are disabled, so it's safe to modify it now
*/
rx_queue_.reset();
error_cnt_ = 0;
@@ -479,27 +521,17 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
* CAN timings for this bitrate
*/
Timings timings;
res = computeTimings(bitrate, timings);
if (res < 0)
const int timings_res = computeTimings(bitrate, timings);
if (timings_res < 0)
{
goto leave;
return timings_res;
}
UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u",
unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));
/*
* Hardware initialization
* Hardware initialization (the hardware has already confirmed initialization mode, see above)
*/
can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode
can_->MCR |= bxcan::MCR_INRQ; // Request init
if (!waitMsrINakBitStateChange(true))
{
UAVCAN_STM32_LOG("MSR INAK not set");
res = -1;
goto leave;
}
can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648
can_->BTR = ((timings.sjw & 3U) << 24) |
@@ -519,8 +551,7 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
if (!waitMsrINakBitStateChange(false))
{
UAVCAN_STM32_LOG("MSR INAK not cleared");
res = -1;
goto leave;
return -1;
}
/*
@@ -553,8 +584,7 @@ int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
can_->FMR &= ~bxcan::FMR_FINIT;
}
leave:
return res;
return 0;
}
void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec)
@@ -838,63 +868,48 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
return 1; // Return value doesn't matter as long as it is non-negative
}
int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
#if UAVCAN_STM32_BAREMETAL
static void nvicEnableVector(int irq, uint8_t prio)
{
int res = 0;
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);
}
UAVCAN_STM32_LOG("Bitrate %lu", static_cast<unsigned long>(bitrate));
#endif
void CanDriver::initOnce()
{
/*
* CAN1
* CAN1, CAN2
*/
{
CriticalSectionLocker lock;
#if UAVCAN_STM32_NUTTX
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN);
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN);
modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN1RST);
modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN1RST, 0);
# if UAVCAN_STM32_NUM_IFACES > 1
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN);
modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST);
modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0);
# endif
#else
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
#endif
}
UAVCAN_STM32_LOG("Initing iface 0...");
res = if0_.init(bitrate, mode);
if (res < 0)
{
UAVCAN_STM32_LOG("Iface 0 init failed %i", res);
goto fail;
}
ifaces[0] = &if0_;
/*
* CAN2
*/
#if UAVCAN_STM32_NUM_IFACES > 1
{
CriticalSectionLocker lock;
# if UAVCAN_STM32_NUTTX
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN);
modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST);
modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0);
# else
# if UAVCAN_STM32_NUM_IFACES > 1
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
# endif
}
UAVCAN_STM32_LOG("Initing iface 1...");
res = if1_.init(bitrate, mode);
if (res < 0)
{
UAVCAN_STM32_LOG("Iface 1 init failed %i", res);
goto fail;
}
ifaces[1] = &if1_;
#endif
}
/*
* IRQ
@@ -902,12 +917,9 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod
#if UAVCAN_STM32_NUTTX
# define IRQ_ATTACH(irq, handler) \
{ \
res = irq_attach(irq, handler); \
if (res < 0) \
{ \
UAVCAN_STM32_LOG("IRQ attach failed %i", irq); \
goto fail; \
} \
const int res = irq_attach(irq, handler); \
(void)res; \
assert(res >= 0); \
up_enable_irq(irq); \
}
IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq);
@@ -921,7 +933,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);
@@ -936,6 +948,47 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod
# endif
}
#endif
}
int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
{
int res = 0;
UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
static bool initialized_once = false;
if (!initialized_once)
{
initialized_once = true;
UAVCAN_STM32_LOG("First initialization");
initOnce();
}
/*
* CAN1
*/
UAVCAN_STM32_LOG("Initing iface 0...");
res = if0_.init(bitrate, mode);
if (res < 0)
{
UAVCAN_STM32_LOG("Iface 0 init failed %i", res);
goto fail;
}
ifaces[0] = &if0_;
/*
* CAN2
*/
#if UAVCAN_STM32_NUM_IFACES > 1
UAVCAN_STM32_LOG("Initing iface 1...");
res = if1_.init(bitrate, mode);
if (res < 0)
{
UAVCAN_STM32_LOG("Iface 1 init failed %i", res);
goto fail;
}
ifaces[1] = &if1_;
#endif
UAVCAN_STM32_LOG("CAN drv init OK");
UAVCAN_ASSERT(res >= 0);
@@ -944,21 +997,6 @@ int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMod
fail:
UAVCAN_STM32_LOG("CAN drv init failed %i", res);
UAVCAN_ASSERT(res < 0);
CriticalSectionLocker lock;
#if UAVCAN_STM32_NUTTX
// TODO: Unattach and disable all IRQs
modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_CAN1EN, 0);
# if UAVCAN_STM32_NUM_IFACES > 1
modifyreg32(STM32_RCC_APB1ENR, RCC_APB1ENR_CAN2EN, 0);
# endif
#else
RCC->APB1ENR &= ~RCC_APB1ENR_CAN1EN;
# if UAVCAN_STM32_NUM_IFACES > 1
RCC->APB1ENR &= ~RCC_APB1ENR_CAN2EN;
# endif
#endif
return res;
}
@@ -1044,7 +1082,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 +1090,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 +1098,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 +1106,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 +1116,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 +1124,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 +1132,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 +1140,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
+156 -24
View File
@@ -14,19 +14,43 @@
/*
* 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 && CH_KERNEL_MAJOR == 2) || 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_CHIBIOS && CH_KERNEL_MAJOR == 3)
# define TIMX UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER)
# define TIMX_IRQn UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER)
# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER)
# define TIMX_INPUT_CLOCK STM32_TIMCLK1
# else
# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler)
# 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
# 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 +81,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 +105,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 +115,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 +127,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 +193,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 +222,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 +325,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 +413,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 +430,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);
libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
+22
View File
@@ -23,13 +23,25 @@ bool BusEvent::wait(uavcan::MonotonicDuration duration)
if (msec <= 0)
{
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout(TIME_IMMEDIATE);
# else // ChibiOS 3
ret = sem_.wait(TIME_IMMEDIATE);
# endif
}
else
{
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
# else // ChibiOS 3
ret = sem_.wait((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
# endif
}
# if (CH_KERNEL_MAJOR == 2)
return ret == RDY_OK;
# else // ChibiOS 3
return ret == MSG_OK;
# endif
}
void BusEvent::signal()
@@ -39,9 +51,15 @@ void BusEvent::signal()
void BusEvent::signalFromInterrupt()
{
# if (CH_KERNEL_MAJOR == 2)
chSysLockFromIsr();
sem_.signalI();
chSysUnlockFromIsr();
# else // ChibiOS 3
chSysLockFromISR();
sem_.signalI();
chSysUnlockFromISR();
# endif
}
/*
@@ -54,7 +72,11 @@ void Mutex::lock()
void Mutex::unlock()
{
# if (CH_KERNEL_MAJOR == 2)
chibios_rt::BaseThread::unlockMutex();
# else // ChibiOS 3
mtx_.unlock();
# endif
}
#elif UAVCAN_STM32_NUTTX
libuavcan_drivers/stm32/test_stm32f107/Makefile
+10 -4
View File
@@ -10,8 +10,9 @@ PROJECT = uavcan_test_stm32f107
MAIN ?= main.cpp
CPPSRC = src/$(MAIN) \
src/dummy.cpp
CPPSRC = src/$(MAIN) \
src/dummy.cpp \
src/board/board.cpp
#
# UAVCAN library
@@ -37,10 +38,15 @@ $(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
UINCDIR += dsdlc_generated
#
# Platform
# Git commit hash
#
CSRC += src/sys/board.c
GIT_HASH := $(shell git rev-parse --short HEAD)
UDEFS += -DGIT_HASH=0x$(GIT_HASH)
#
# Platform
#
UINCDIR += src/sys
libuavcan_drivers/stm32/test_stm32f107/src/board/board.cpp
+99
View File
@@ -0,0 +1,99 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include "board.hpp"
#include <cstring>
#include <unistd.h>
#include <ch.h>
#include <ch.hpp>
#include <hal.h>
#include <zubax_chibios/sys/sys.h>
/**
* GPIO config for ChibiOS PAL driver
*/
const PALConfig pal_default_config =
{
{ VAL_GPIOAODR, VAL_GPIOACRL, VAL_GPIOACRH },
{ VAL_GPIOBODR, VAL_GPIOBCRL, VAL_GPIOBCRH },
{ VAL_GPIOCODR, VAL_GPIOCCRL, VAL_GPIOCCRH },
{ VAL_GPIODODR, VAL_GPIODCRL, VAL_GPIODCRH },
{ VAL_GPIOEODR, VAL_GPIOECRL, VAL_GPIOECRH }
};
namespace board
{
void init()
{
halInit();
chibios_rt::System::init();
sdStart(&STDOUT_SD, NULL);
}
__attribute__((noreturn))
void die(int error)
{
lowsyslog("Fatal error %i\n", error);
while (1)
{
setLed(false);
::sleep(1);
setLed(true);
::sleep(1);
}
}
void setLed(bool state)
{
palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state);
}
void restart()
{
NVIC_SystemReset();
}
void readUniqueID(std::uint8_t bytes[UniqueIDSize])
{
std::memcpy(bytes, reinterpret_cast<const void*>(0x1FFFF7E8), UniqueIDSize);
}
}
/*
* Early init from ChibiOS
*/
extern "C"
{
void __early_init(void)
{
stm32_clock_init();
}
void boardInit(void)
{
AFIO->MAPR |=
AFIO_MAPR_CAN_REMAP_REMAP3 |
AFIO_MAPR_CAN2_REMAP |
AFIO_MAPR_USART2_REMAP;
/*
* Enabling the CAN controllers, then configuring GPIO functions for CAN_TX.
* Order matters, otherwise the CAN_TX pins will twitch, disturbing the CAN bus.
* This is why we can't perform this initialization using ChibiOS GPIO configuration.
*/
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
palSetPadMode(GPIOD, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
#if UAVCAN_STM32_NUM_IFACES > 1
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
palSetPadMode(GPIOB, 6, PAL_MODE_STM32_ALTERNATE_PUSHPULL);
#endif
}
}
libuavcan_drivers/stm32/test_stm32f107/src/board/board.hpp
+25
View File
@@ -0,0 +1,25 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <cstdint>
namespace board
{
void init();
__attribute__((noreturn))
void die(int error);
void setLed(bool state);
void restart();
constexpr unsigned UniqueIDSize = 12;
void readUniqueID(std::uint8_t bytes[UniqueIDSize]);
}
libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
+90 -75
View File
@@ -6,6 +6,8 @@
#include <zubax_chibios/sys/sys.h>
#include <uavcan_stm32/uavcan_stm32.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include "board/board.hpp"
namespace app
{
@@ -14,29 +16,17 @@ namespace
uavcan_stm32::CanInitHelper<128> can;
typedef uavcan::Node<16384> Node;
constexpr unsigned NodePoolSize = 16384;
uavcan::LazyConstructor<Node> node_;
Node& getNode()
uavcan::Node<NodePoolSize>& getNode()
{
if (!node_.isConstructed())
{
node_.construct<uavcan::ICanDriver&, uavcan::ISystemClock&>(can.driver, uavcan_stm32::SystemClock::instance());
}
return *node_;
}
void ledSet(bool state)
{
palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state);
static uavcan::Node<NodePoolSize> node(can.driver, uavcan_stm32::SystemClock::instance());
return node;
}
void init()
{
halInit();
chibios_rt::System::init();
sdStart(&STDOUT_SD, NULL);
board::init();
int res = 0;
do
@@ -54,80 +44,103 @@ void init()
while (res < 0);
}
#if __GNUC__
__attribute__((noreturn))
#endif
void die(int status)
{
lowsyslog("Initialization failure %i\n", status);
while (1)
{
ledSet(false);
sleep(1);
ledSet(true);
sleep(1);
}
}
class : public chibios_rt::BaseStaticThread<8192>
{
void configureNodeInfo()
{
getNode().setName("org.uavcan.stm32_test_stm32f107");
/*
* Software version
* TODO: Fill other fields too
*/
uavcan::protocol::SoftwareVersion swver;
swver.vcs_commit = GIT_HASH;
swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
getNode().setSoftwareVersion(swver);
lowsyslog("Git commit hash: 0x%08x\n", GIT_HASH);
/*
* Hardware version
* TODO: Fill other fields too
*/
uavcan::protocol::HardwareVersion hwver;
std::uint8_t uid[board::UniqueIDSize] = {};
board::readUniqueID(uid);
std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
getNode().setHardwareVersion(hwver);
lowsyslog("UDID:");
for (auto b : hwver.unique_id)
{
lowsyslog(" %02x", unsigned(b));
}
lowsyslog("\n");
}
void performDynamicNodeIDAllocation()
{
uavcan::DynamicNodeIDClient client(getNode());
const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
if (client_start_res < 0)
{
board::die(client_start_res);
}
lowsyslog("Waiting for dynamic node ID allocation...\n");
while (!client.isAllocationComplete())
{
const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
if (spin_res < 0)
{
lowsyslog("Spin failure: %i\n", spin_res);
}
}
lowsyslog("Dynamic node ID %d allocated by %d\n",
int(client.getAllocatedNodeID().get()),
int(client.getAllocatorNodeID().get()));
getNode().setNodeID(client.getAllocatedNodeID());
}
public:
msg_t main()
{
/*
* Setting up the node parameters
*/
Node& node = app::getNode();
node.setNodeID(64);
node.setName("org.uavcan.stm32_test_stm32f107");
// TODO: fill software version info (version number, VCS commit hash, ...)
// TODO: fill hardware version info (version number, unique ID)
configureNodeInfo();
/*
* Initializing the UAVCAN node - this may take a while
* Initializing the UAVCAN node
*/
while (true)
const int node_init_res = getNode().start();
if (node_init_res < 0)
{
// Calling start() multiple times is OK - only the first successfull call will be effective
int res = node.start();
#if !UAVCAN_TINY
uavcan::NetworkCompatibilityCheckResult ncc_result;
if (res >= 0)
{
lowsyslog("Checking network compatibility...\n");
res = node.checkNetworkCompatibility(ncc_result);
}
#endif
if (res < 0)
{
lowsyslog("Node initialization failure: %i, will try agin soon\n", res);
}
#if !UAVCAN_TINY
else if (!ncc_result.isOk())
{
lowsyslog("Network conflict with %u, will try again soon\n", ncc_result.conflicting_node.get());
}
#endif
else
{
break;
}
::sleep(3);
board::die(node_init_res);
}
/*
* Waiting for a dynamic node ID allocation
*/
performDynamicNodeIDAllocation();
/*
* Time synchronizer
*/
static uavcan::GlobalTimeSyncSlave time_sync_slave(node);
static uavcan::GlobalTimeSyncSlave time_sync_slave(getNode());
{
const int res = time_sync_slave.start();
if (res < 0)
{
die(res);
board::die(res);
}
}
@@ -135,10 +148,10 @@ public:
* Main loop
*/
lowsyslog("UAVCAN node started\n");
node.setModeOperational();
getNode().setModeOperational();
while (true)
{
const int spin_res = node.spin(uavcan::MonotonicDuration::fromMSec(5000));
const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(5000));
if (spin_res < 0)
{
lowsyslog("Spin failure: %i\n", spin_res);
@@ -146,8 +159,10 @@ public:
lowsyslog("Time sync master: %u\n", unsigned(time_sync_slave.getMasterNodeID().get()));
lowsyslog("Memory usage: used=%u free=%u\n",
node.getAllocator().getNumUsedBlocks(), node.getAllocator().getNumFreeBlocks());
lowsyslog("Memory usage: free=%u used=%u worst=%u\n",
getNode().getAllocator().getNumFreeBlocks(),
getNode().getAllocator().getNumUsedBlocks(),
getNode().getAllocator().getPeakNumUsedBlocks());
lowsyslog("CAN errors: %lu %lu\n",
static_cast<unsigned long>(can.driver.getIface(0)->getErrorCount()),
@@ -179,7 +194,7 @@ int main()
{
for (int i = 0; i < 200; i++)
{
app::ledSet(app::can.driver.hadActivity());
board::setLed(app::can.driver.hadActivity());
::usleep(25000);
}
libuavcan_drivers/stm32/test_stm32f107/src/sys/board.c
-27
View File
@@ -1,27 +0,0 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <ch.h>
#include <hal.h>
const PALConfig pal_default_config = {
{ VAL_GPIOAODR, VAL_GPIOACRL, VAL_GPIOACRH },
{ VAL_GPIOBODR, VAL_GPIOBCRL, VAL_GPIOBCRH },
{ VAL_GPIOCODR, VAL_GPIOCCRL, VAL_GPIOCCRH },
{ VAL_GPIODODR, VAL_GPIODCRL, VAL_GPIODCRH },
{ VAL_GPIOEODR, VAL_GPIOECRL, VAL_GPIOECRH }
};
void __early_init(void)
{
stm32_clock_init();
}
void boardInit(void)
{
AFIO->MAPR |=
AFIO_MAPR_CAN_REMAP_REMAP3 |
AFIO_MAPR_CAN2_REMAP |
AFIO_MAPR_USART2_REMAP;
}
libuavcan_drivers/stm32/test_stm32f107/src/sys/board.h
+6 -4
View File
@@ -20,6 +20,8 @@
#define GPIO_PORT_LED GPIOB
#define GPIO_PIN_LED 9
// GPIOD 10 is configured as OUTPUT, it is used as board reboot monitor.
/*
* I/O ports initial setup, this configuration is established soon after reset
* in the initialization code.
@@ -48,7 +50,7 @@
#define VAL_GPIOACRH 0x88888888 // 15..8
#define VAL_GPIOAODR 0x00000000
#define VAL_GPIOBCRL 0x8B488888
#define VAL_GPIOBCRL 0x84488888 // CAN2 TX initialized as INPUT, it must be configured later!
#define VAL_GPIOBCRH 0x88888828
#define VAL_GPIOBODR 0x00000000
@@ -56,9 +58,9 @@
#define VAL_GPIOCCRH 0x88888888
#define VAL_GPIOCODR 0x00000000
#define VAL_GPIODCRL 0x88b888B4
#define VAL_GPIODCRH 0x88888888
#define VAL_GPIODODR 0x00000000
#define VAL_GPIODCRL 0x88b88844 // CAN1 TX initialized as INPUT, it must be configured later!
#define VAL_GPIODCRH 0x88888288
#define VAL_GPIODODR ((1 << 10))
#define VAL_GPIOECRL 0x88888888
#define VAL_GPIOECRH 0x88888888