fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 17:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #71 from UAVCAN/lpc11c24

Browse Source 
Missing features for LPC11C24
This commit is contained in:
Pavel Kirienko
2015-10-13 17:23:26 +03:00
parent 8effe93d6e e060962261
commit 995a5510e5
12 changed files with 1128 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/can.hpp
+38 -14
View File
@@ -29,11 +29,20 @@ public:
*/
static CanDriver& instance() { return self; }
/**
* Attempts to detect bit rate of the CAN bus.
* This function may block for up to X seconds, where X is the number of bit rates to try.
* This function is NOT guaranteed to reset the CAN controller upon return.
* @return On success: detected bit rate, in bits per second.
* On failure: zero.
*/
static uavcan::uint32_t detectBitRate(void (*idle_callback)() = nullptr);
/**
* Returns negative value if the requested baudrate can't be used.
* Returns zero if OK.
*/
int init(uavcan::uint32_t baudrate);
int init(uavcan::uint32_t bitrate);
bool hasReadyRx() const;
bool hasEmptyTx() const;
@@ -44,26 +53,41 @@ public:
*/
bool hadActivity();
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags);
/**
* Returns the number of times the RX queue was overrun.
*/
uavcan::uint32_t getRxQueueOverflowCount() const;
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags);
/**
* Whether the controller is currently in bus off state.
* Note that the driver recovers the CAN controller from the bus off state automatically!
* Therefore, this method serves only monitoring purposes and is not necessary to use.
*/
bool isInBusOffState() const;
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline);
uavcan::int16_t send(const uavcan::CanFrame& frame,
uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags) override;
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs);
uavcan::int16_t receive(uavcan::CanFrame& out_frame,
uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc,
uavcan::CanIOFlags& out_flags) override;
virtual uavcan::uint64_t getErrorCount() const;
uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline) override;
virtual uavcan::uint16_t getNumFilters() const;
uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs) override;
virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index);
uavcan::uint64_t getErrorCount() const override;
virtual uavcan::uint8_t getNumIfaces() const;
uavcan::uint16_t getNumFilters() const override;
uavcan::ICanIface* getIface(uavcan::uint8_t iface_index) override;
uavcan::uint8_t getNumIfaces() const override;
};
}
libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp
+3 -3
View File
@@ -51,9 +51,9 @@ class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable
SystemClock() { }
virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); }
virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); }
virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); }
uavcan::MonotonicTime getMonotonic() const override { return clock::getMonotonic(); }
uavcan::UtcTime getUtc() const override { return clock::getUtc(); }
void adjustUtc(uavcan::UtcDuration adjustment) override { clock::adjustUtc(adjustment); }
public:
/**
libuavcan_drivers/lpc11c24/driver/src/c_can.hpp
+191
View File
@@ -0,0 +1,191 @@
/*
* Bosch C_CAN controller API.
*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <cstdint>
#include <cstddef>
namespace uavcan_lpc11c24
{
namespace c_can
{
struct MsgIfaceType
{
std::uint32_t CMDREQ;
union
{
std::uint32_t W;
std::uint32_t R;
} CMDMSK;
std::uint32_t MSK1;
std::uint32_t MSK2;
std::uint32_t ARB1;
std::uint32_t ARB2;
std::uint32_t MCTRL;
std::uint32_t DA1;
std::uint32_t DA2;
std::uint32_t DB1;
std::uint32_t DB2;
const std::uint32_t _skip[13];
};
static_assert(offsetof(MsgIfaceType, CMDMSK) == 0x04, "C_CAN offset");
static_assert(offsetof(MsgIfaceType, MSK1) == 0x08, "C_CAN offset");
static_assert(offsetof(MsgIfaceType, ARB1) == 0x10, "C_CAN offset");
static_assert(offsetof(MsgIfaceType, MCTRL) == 0x18, "C_CAN offset");
static_assert(offsetof(MsgIfaceType, DA1) == 0x1c, "C_CAN offset");
static_assert(offsetof(MsgIfaceType, DB2) == 0x28, "C_CAN offset");
static_assert(sizeof(MsgIfaceType) == 96, "C_CAN size");
struct Type
{
std::uint32_t CNTL;
std::uint32_t STAT;
const std::uint32_t EC;
std::uint32_t BT;
const std::uint32_t INT;
std::uint32_t TEST;
std::uint32_t BRPE;
const std::uint32_t _skip_a[1];
MsgIfaceType IF[2]; // [0] @ 0x020, [1] @ 0x080
const std::uint32_t _skip_b[8];
const std::uint32_t TXREQ[2]; // 0x100
const std::uint32_t _skip_c[6];
const std::uint32_t ND[2]; // 0x120
const std::uint32_t _skip_d[6];
const std::uint32_t IR[2]; // 0x140
const std::uint32_t _skip_e[6];
const std::uint32_t MSGV[2]; // 0x160
const std::uint32_t _skip_f[6];
std::uint32_t CLKDIV; // 0x180
};
static_assert(offsetof(Type, CNTL) == 0x000, "C_CAN offset");
static_assert(offsetof(Type, STAT) == 0x004, "C_CAN offset");
static_assert(offsetof(Type, TEST) == 0x014, "C_CAN offset");
static_assert(offsetof(Type, BRPE) == 0x018, "C_CAN offset");
static_assert(offsetof(Type, IF[0]) == 0x020, "C_CAN offset");
static_assert(offsetof(Type, IF[1]) == 0x080, "C_CAN offset");
static_assert(offsetof(Type, TXREQ) == 0x100, "C_CAN offset");
static_assert(offsetof(Type, ND) == 0x120, "C_CAN offset");
static_assert(offsetof(Type, IR) == 0x140, "C_CAN offset");
static_assert(offsetof(Type, MSGV) == 0x160, "C_CAN offset");
static_assert(offsetof(Type, CLKDIV) == 0x180, "C_CAN offset");
static_assert(offsetof(Type, IF[0].DB2) == 0x048, "C_CAN offset");
static_assert(offsetof(Type, IF[1].DB2) == 0x0A8, "C_CAN offset");
volatile Type& CAN = *reinterpret_cast<volatile Type*>(0x40050000);
/*
* CNTL
*/
static constexpr std::uint32_t CNTL_TEST = 1 << 7;
static constexpr std::uint32_t CNTL_CCE = 1 << 6;
static constexpr std::uint32_t CNTL_DAR = 1 << 5;
static constexpr std::uint32_t CNTL_EIE = 1 << 3;
static constexpr std::uint32_t CNTL_SIE = 1 << 2;
static constexpr std::uint32_t CNTL_IE = 1 << 1;
static constexpr std::uint32_t CNTL_INIT = 1 << 0;
static constexpr std::uint32_t CNTL_IRQ_MASK = CNTL_EIE | CNTL_IE | CNTL_SIE;
/*
* TEST
*/
static constexpr std::uint32_t TEST_RX = 1 << 7;
static constexpr std::uint32_t TEST_LBACK = 1 << 4;
static constexpr std::uint32_t TEST_SILENT = 1 << 3;
static constexpr std::uint32_t TEST_BASIC = 1 << 2;
static constexpr std::uint32_t TEST_TX_SHIFT = 5;
enum class TestTx : std::uint32_t
{
Controller = 0,
SamplePoint = 1,
LowDominant = 2,
HighRecessive = 3
};
/*
* STAT
*/
static constexpr std::uint32_t STAT_BOFF = 1 << 7;
static constexpr std::uint32_t STAT_EWARN = 1 << 6;
static constexpr std::uint32_t STAT_EPASS = 1 << 5;
static constexpr std::uint32_t STAT_RXOK = 1 << 4;
static constexpr std::uint32_t STAT_TXOK = 1 << 3;
static constexpr std::uint32_t STAT_LEC_MASK = 7;
static constexpr std::uint32_t STAT_LEC_SHIFT = 0;
enum class StatLec : std::uint32_t
{
NoError = 0,
StuffError = 1,
FormError = 2,
AckError = 3,
Bit1Error = 4,
Bit0Error = 5,
CRCError = 6,
Unused = 7
};
/*
* IF.CMDREQ
*/
static constexpr std::uint32_t IF_CMDREQ_BUSY = 1 << 15;
/*
* IF.CMDMSK
*/
static constexpr std::uint32_t IF_CMDMSK_W_DATA_A = 1 << 0;
static constexpr std::uint32_t IF_CMDMSK_W_DATA_B = 1 << 1;
static constexpr std::uint32_t IF_CMDMSK_W_TXRQST = 1 << 2;
static constexpr std::uint32_t IF_CMDMSK_W_CTRL = 1 << 4;
static constexpr std::uint32_t IF_CMDMSK_W_ARB = 1 << 5;
static constexpr std::uint32_t IF_CMDMSK_W_MASK = 1 << 6;
static constexpr std::uint32_t IF_CMDMSK_W_WR_RD = 1 << 7;
/*
* IF.MCTRL
*/
static constexpr std::uint32_t IF_MCTRL_NEWDAT = 1 << 15;
static constexpr std::uint32_t IF_MCTRL_MSGLST = 1 << 14;
static constexpr std::uint32_t IF_MCTRL_INTPND = 1 << 13;
static constexpr std::uint32_t IF_MCTRL_UMASK = 1 << 12;
static constexpr std::uint32_t IF_MCTRL_TXIE = 1 << 11;
static constexpr std::uint32_t IF_MCTRL_RXIE = 1 << 10;
static constexpr std::uint32_t IF_MCTRL_RMTEN = 1 << 9;
static constexpr std::uint32_t IF_MCTRL_TXRQST = 1 << 8;
static constexpr std::uint32_t IF_MCTRL_EOB = 1 << 7;
static constexpr std::uint32_t IF_MCTRL_DLC_MASK = 15;
}
}
libuavcan_drivers/lpc11c24/driver/src/can.cpp
+335 -111
View File
@@ -6,6 +6,7 @@
#include <uavcan_lpc11c24/clock.hpp>
#include <uavcan/util/templates.hpp>
#include <chip.h>
#include "c_can.hpp"
#include "internal.hpp"
/**
@@ -19,9 +20,9 @@
# error UAVCAN_LPC11C24_RX_QUEUE_LEN is too large
#endif
extern "C" void canRxCallback(uint8_t msg_obj_num);
extern "C" void canTxCallback(uint8_t msg_obj_num);
extern "C" void canErrorCallback(uint32_t error_info);
extern "C" void canRxCallback(std::uint8_t msg_obj_num);
extern "C" void canTxCallback(std::uint8_t msg_obj_num);
extern "C" void canErrorCallback(std::uint32_t error_info);
namespace uavcan_lpc11c24
{
@@ -34,25 +35,36 @@ namespace
* TX priority is defined by the message object number, not by the CAN ID (chapter 16.7.3.5 of the user manual),
* hence we can't use more than one object because that would cause priority inversion on long transfers.
*/
const unsigned NumMsgObjects = 32;
constexpr unsigned NumberOfMessageObjects = 32;
constexpr unsigned NumberOfTxMessageObjects = 1;
constexpr unsigned NumberOfRxMessageObjects = NumberOfMessageObjects - NumberOfTxMessageObjects;
constexpr unsigned TxMessageObjectNumber = 1;
/**
* Total number of CAN errors.
* Does not overflow.
*/
uint32_t error_cnt;
volatile std::uint32_t error_cnt = 0;
/**
* True if there's no pending TX frame, i.e. write is possible.
* False if there's no pending TX frame, i.e. write is possible.
*/
bool tx_free = true;
volatile bool tx_pending = false;
/**
* Currently pending frame must be aborted on first error.
*/
volatile bool tx_abort_on_error = false;
/**
* Gets updated every time the CAN IRQ handler is being called.
*/
uint64_t last_irq_utc_timestamp = 0;
volatile std::uint64_t last_irq_utc_timestamp = 0;
bool had_activity;
/**
* Set by the driver on every successful TX or RX; reset by the application.
*/
volatile bool had_activity = false;
/**
* After a received message gets extracted from C_CAN, it will be stored in the RX queue until libuavcan
@@ -62,26 +74,18 @@ class RxQueue
{
struct Item
{
uint64_t utc_usec;
std::uint64_t utc_usec = 0;
uavcan::CanFrame frame;
Item() : utc_usec(0) { }
};
Item buf_[UAVCAN_LPC11C24_RX_QUEUE_LEN];
uint32_t overflow_cnt_;
uint8_t in_;
uint8_t out_;
uint8_t len_;
std::uint32_t overflow_cnt_ = 0;
std::uint8_t in_ = 0;
std::uint8_t out_ = 0;
std::uint8_t len_ = 0;
public:
RxQueue()
: overflow_cnt_(0)
, in_(0)
, out_(0)
, len_(0)
{ }
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec)
void push(const uavcan::CanFrame& frame, const volatile std::uint64_t& utc_usec)
{
buf_[in_].frame = frame;
buf_[in_].utc_usec = utc_usec;
@@ -106,7 +110,7 @@ public:
}
}
void pop(uavcan::CanFrame& out_frame, uint64_t& out_utc_usec)
void pop(uavcan::CanFrame& out_frame, std::uint64_t& out_utc_usec)
{
if (len_ > 0)
{
@@ -123,90 +127,200 @@ public:
unsigned getLength() const { return len_; }
uint32_t getOverflowCount() const { return overflow_cnt_; }
std::uint32_t getOverflowCount() const { return overflow_cnt_; }
};
RxQueue rx_queue;
int computeBaudrate(uint32_t baud_rate, uint32_t can_api_timing_cfg[2])
struct BitTimingSettings
{
const uint32_t pclk = Chip_Clock_GetMainClockRate();
const uint32_t clk_per_bit = pclk / baud_rate;
for (uint32_t div = 0; div <= 15; div++)
std::uint32_t canclkdiv;
std::uint32_t canbtr;
bool isValid() const { return canbtr != 0; }
};
/**
* http://www.bittiming.can-wiki.info
*/
BitTimingSettings computeBitTimings(std::uint32_t bitrate)
{
if (Chip_Clock_GetSystemClockRate() == 48000000) // 48 MHz is optimal for CAN timings
{
for (uint32_t quanta = 1; quanta <= 32; quanta++)
switch (bitrate)
{
for (uint32_t segs = 3; segs <= 17; segs++)
{
if (clk_per_bit == (segs * quanta * (div + 1)))
{
segs -= 3;
const uint32_t seg1 = segs / 2;
const uint32_t seg2 = segs - seg1;
const uint32_t can_sjw = (seg1 > 3) ? 3 : seg1;
can_api_timing_cfg[0] = div;
can_api_timing_cfg[1] = ((quanta - 1) & 0x3F) |
(can_sjw & 0x03) << 6 |
(seg1 & 0x0F) << 8 |
(seg2 & 0x07) << 12;
return 0;
}
}
case 1000000: return BitTimingSettings{ 0, 0x0505 }; // 8 quanta, 87.5%
case 500000: return BitTimingSettings{ 0, 0x1c05 }; // 16 quanta, 87.5%
case 250000: return BitTimingSettings{ 0, 0x1c0b }; // 16 quanta, 87.5%
case 125000: return BitTimingSettings{ 0, 0x1c17 }; // 16 quanta, 87.5%
default: return BitTimingSettings{ 0, 0 };
}
}
return -1;
else
{
return BitTimingSettings{ 0, 0 };
}
}
} // namespace
CanDriver CanDriver::self;
int CanDriver::init(uavcan::uint32_t baudrate)
uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)())
{
CriticalSectionLocker locker;
static constexpr uavcan::uint32_t BitRatesToTry[] =
{
1000000,
500000,
250000,
125000
};
error_cnt = 0;
tx_free = true;
last_irq_utc_timestamp = 0;
had_activity = false;
const auto ListeningDuration = uavcan::MonotonicDuration::fromMSec(1050);
NVIC_DisableIRQ(CAN_IRQn);
Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
for (auto bitrate : BitRatesToTry)
{
// Computing bit timings
const auto bit_timings = computeBitTimings(bitrate);
if (!bit_timings.isValid())
{
return 0;
}
// Configuring the CAN controller
{
CriticalSectionLocker locker;
LPC_SYSCTL->PRESETCTRL |= (1U << RESET_CAN0);
// Entering initialization mode
c_can::CAN.CNTL = c_can::CNTL_INIT | c_can::CNTL_CCE;
while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0)
{
; // Nothing to do
}
// Configuring bit rate
c_can::CAN.CLKDIV = bit_timings.canclkdiv;
c_can::CAN.BT = bit_timings.canbtr;
c_can::CAN.BRPE = 0;
// Configuring silent mode
c_can::CAN.CNTL |= c_can::CNTL_TEST;
c_can::CAN.TEST = c_can::TEST_SILENT;
// Configuring status monitor
c_can::CAN.STAT = (unsigned(c_can::StatLec::Unused) << c_can::STAT_LEC_SHIFT);
// Leaving initialization mode
c_can::CAN.CNTL &= ~(c_can::CNTL_INIT | c_can::CNTL_CCE);
while ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0)
{
; // Nothing to do
}
}
// Listening
const auto deadline = clock::getMonotonic() + ListeningDuration;
bool match_detected = false;
while (clock::getMonotonic() < deadline)
{
if (idle_callback != nullptr)
{
idle_callback();
}
const auto LastErrorCode = (c_can::CAN.STAT >> c_can::STAT_LEC_SHIFT) & c_can::STAT_LEC_MASK;
if (LastErrorCode == unsigned(c_can::StatLec::NoError))
{
match_detected = true;
break;
}
}
// De-configuring the CAN controller back to reset state
{
CriticalSectionLocker locker;
c_can::CAN.CNTL = c_can::CNTL_INIT;
while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0)
{
; // Nothing to do
}
LPC_SYSCTL->PRESETCTRL &= ~(1U << RESET_CAN0);
}
// Termination condition
if (match_detected)
{
return bitrate;
}
}
return 0; // No match
}
int CanDriver::init(uavcan::uint32_t bitrate)
{
{
auto bit_timings = computeBitTimings(bitrate);
if (!bit_timings.isValid())
{
return -1;
}
CriticalSectionLocker locker;
error_cnt = 0;
tx_abort_on_error = false;
tx_pending = false;
last_irq_utc_timestamp = 0;
had_activity = false;
/*
* C_CAN init
*/
Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
LPC_CCAN_API->init_can(reinterpret_cast<std::uint32_t*>(&bit_timings), true);
static CCAN_CALLBACKS_T ccan_callbacks =
{
canRxCallback,
canTxCallback,
canErrorCallback,
nullptr,
nullptr,
nullptr,
nullptr,
nullptr
};
LPC_CCAN_API->config_calb(&ccan_callbacks);
/*
* Interrupts
*/
c_can::CAN.CNTL |= c_can::CNTL_SIE; // This is necessary for transmission aborts on error
NVIC_EnableIRQ(CAN_IRQn);
}
/*
* C_CAN init
* Applying default filter configuration (accept all)
*/
Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN);
static uint32_t can_api_init_table[2];
if (computeBaudrate(baudrate, can_api_init_table) != 0)
if (configureFilters(nullptr, 0) < 0)
{
return -1;
}
LPC_CCAN_API->init_can(can_api_init_table, true);
static CCAN_CALLBACKS_T ccan_callbacks =
{
canRxCallback,
canTxCallback,
canErrorCallback,
0,
0,
0,
0,
0
};
LPC_CCAN_API->config_calb(&ccan_callbacks);
NVIC_EnableIRQ(CAN_IRQn);
/*
* Default RX msgobj config:
* 31 - all STD
* 32 - all EXT
* RTR ignored
*/
CCAN_MSG_OBJ_T msg_obj = CCAN_MSG_OBJ_T();
msg_obj.msgobj = 31;
LPC_CCAN_API->config_rxmsgobj(&msg_obj);
msg_obj.mode_id = CAN_MSGOBJ_EXT;
msg_obj.msgobj = 32;
LPC_CCAN_API->config_rxmsgobj(&msg_obj);
return 0;
}
@@ -220,7 +334,7 @@ bool CanDriver::hasReadyRx() const
bool CanDriver::hasEmptyTx() const
{
CriticalSectionLocker locker;
return tx_free;
return !tx_pending;
}
bool CanDriver::hadActivity()
@@ -231,12 +345,23 @@ bool CanDriver::hadActivity()
return ret;
}
uavcan::uint32_t CanDriver::getRxQueueOverflowCount() const
{
CriticalSectionLocker locker;
return rx_queue.getOverflowCount();
}
bool CanDriver::isInBusOffState() const
{
return (c_can::CAN.STAT & c_can::STAT_BOFF) != 0;
}
uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags)
{
if (frame.isErrorFrame() ||
frame.dlc > 8 ||
flags != 0) // Only plain IO is allowed. Loopback, TX timestamping are not supported by this driver.
(flags & uavcan::CanIOFlagLoopback) != 0) // TX timestamping is not supported by this driver.
{
return -1;
}
@@ -264,10 +389,11 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic
CriticalSectionLocker locker;
if (tx_free)
if (!tx_pending)
{
tx_free = false; // Mark as pending - will be released in TX callback
msgobj.msgobj = 1;
tx_pending = true; // Mark as pending - will be released in TX callback
tx_abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
msgobj.msgobj = TxMessageObjectNumber;
LPC_CCAN_API->can_transmit(&msgobj);
return 1;
}
@@ -277,7 +403,7 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic
uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
{
out_ts_monotonic = uavcan_lpc11c24::clock::getMonotonic();
out_ts_monotonic = clock::getMonotonic();
out_flags = 0; // We don't support any IO flags
CriticalSectionLocker locker;
@@ -285,7 +411,7 @@ uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::Monotoni
{
return 0;
}
uint64_t ts_utc = 0;
std::uint64_t ts_utc = 0;
rx_queue.pop(out_frame, ts_utc);
out_ts_utc = uavcan::UtcTime::fromUSec(ts_utc);
return 1;
@@ -295,6 +421,16 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline)
{
const bool bus_off = isInBusOffState();
if (bus_off) // Recover automatically on bus-off
{
CriticalSectionLocker locker;
if ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0)
{
c_can::CAN.CNTL &= ~c_can::CNTL_INIT;
}
}
const bool noblock = ((inout_masks.read == 1) && hasReadyRx()) ||
((inout_masks.write == 1) && hasEmptyTx());
@@ -322,32 +458,98 @@ uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks,
}
inout_masks.read = hasReadyRx() ? 1 : 0;
inout_masks.write = hasEmptyTx() ? 1 : 0;
return 0; // Return value doesn't matter as long as it is non-negative
inout_masks.write = (hasEmptyTx() && !bus_off) ? 1 : 0; // Disable write while in bus-off
return 0; // Return value doesn't matter as long as it is non-negative
}
uavcan::int16_t CanDriver::configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs)
{
(void)filter_configs;
(void)num_configs;
return -1;
CriticalSectionLocker locker;
/*
* If C_CAN is active (INIT=0) and the CAN bus has intensive traffic, RX object configuration may fail.
* The solution is to disable the controller while configuration is in progress.
* The documentation, as always, doesn't bother to mention this detail. Shame on you, NXP.
*/
struct RAIIDisabler
{
RAIIDisabler()
{
c_can::CAN.CNTL |= c_can::CNTL_INIT;
}
~RAIIDisabler()
{
c_can::CAN.CNTL &= ~c_can::CNTL_INIT;
}
} can_disabler; // Must be instantiated AFTER the critical section locker
if (num_configs == 0)
{
auto msg_obj = CCAN_MSG_OBJ_T();
msg_obj.msgobj = NumberOfTxMessageObjects + 1;
LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all STD frames
msg_obj.mode_id = CAN_MSGOBJ_EXT;
msg_obj.msgobj = NumberOfTxMessageObjects + 2;
LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all EXT frames
}
else if (num_configs <= NumberOfRxMessageObjects)
{
// Making sure the configs use only EXT frames; otherwise we can't accept them
for (unsigned i = 0; i < num_configs; i++)
{
auto& f = filter_configs[i];
if ((f.id & f.mask & uavcan::CanFrame::FlagEFF) == 0)
{
return -1;
}
}
// Installing the configuration
for (unsigned i = 0; i < NumberOfRxMessageObjects; i++)
{
auto msg_obj = CCAN_MSG_OBJ_T();
msg_obj.msgobj = std::uint8_t(i + 1U + NumberOfTxMessageObjects); // Message objects are numbered from 1
if (i < num_configs)
{
msg_obj.mode_id = (filter_configs[i].id & uavcan::CanFrame::MaskExtID) | CAN_MSGOBJ_EXT; // Only EXT
msg_obj.mask = filter_configs[i].mask & uavcan::CanFrame::MaskExtID;
}
else
{
msg_obj.mode_id = CAN_MSGOBJ_RTR; // Using this configuration to disable the object
msg_obj.mask = uavcan::CanFrame::MaskStdID;
}
LPC_CCAN_API->config_rxmsgobj(&msg_obj);
}
}
else
{
return -1;
}
return 0;
}
uavcan::uint64_t CanDriver::getErrorCount() const
{
CriticalSectionLocker locker;
return uint64_t(error_cnt) + uint64_t(rx_queue.getOverflowCount());
return std::uint64_t(error_cnt) + std::uint64_t(rx_queue.getOverflowCount());
}
uavcan::uint16_t CanDriver::getNumFilters() const
{
return NumMsgObjects - 1; // First msgobj is reserved for TX frame
return NumberOfRxMessageObjects;
}
uavcan::ICanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
{
return (iface_index == 0) ? this : NULL;
return (iface_index == 0) ? this : nullptr;
}
uavcan::uint8_t CanDriver::getNumIfaces() const
@@ -363,9 +565,11 @@ uavcan::uint8_t CanDriver::getNumIfaces() const
extern "C"
{
void canRxCallback(uint8_t msg_obj_num)
void canRxCallback(std::uint8_t msg_obj_num)
{
CCAN_MSG_OBJ_T msg_obj = CCAN_MSG_OBJ_T();
using namespace uavcan_lpc11c24;
auto msg_obj = CCAN_MSG_OBJ_T();
msg_obj.msgobj = msg_obj_num;
LPC_CCAN_API->can_receive(&msg_obj);
@@ -392,23 +596,40 @@ void canRxCallback(uint8_t msg_obj_num)
frame.dlc = msg_obj.dlc;
uavcan::copy(msg_obj.data, msg_obj.data + msg_obj.dlc, frame.data);
uavcan_lpc11c24::rx_queue.push(frame, uavcan_lpc11c24::last_irq_utc_timestamp);
uavcan_lpc11c24::had_activity = true;
rx_queue.push(frame, last_irq_utc_timestamp);
had_activity = true;
}
void canTxCallback(uint8_t msg_obj_num)
void canTxCallback(std::uint8_t msg_obj_num)
{
using namespace uavcan_lpc11c24;
(void)msg_obj_num;
uavcan_lpc11c24::tx_free = true;
uavcan_lpc11c24::had_activity = true;
tx_pending = false;
had_activity = true;
}
void canErrorCallback(uint32_t error_info)
void canErrorCallback(std::uint32_t error_info)
{
(void)error_info;
if (uavcan_lpc11c24::error_cnt < 0xFFFFFFFF)
using namespace uavcan_lpc11c24;
// Updating the error counter
if ((error_info != 0) && (error_cnt < 0xFFFFFFFFUL))
{
uavcan_lpc11c24::error_cnt++;
error_cnt++;
}
// Serving abort requests
if (tx_pending && tx_abort_on_error)
{
tx_pending = false;
tx_abort_on_error = false;
// Using the first interface, because this approach seems to be compliant with the BASIC mode (just in case)
c_can::CAN.IF[0].CMDREQ = TxMessageObjectNumber;
c_can::CAN.IF[0].CMDMSK.W = c_can::IF_CMDMSK_W_WR_RD; // Clearing IF_CMDMSK_W_TXRQST
c_can::CAN.IF[0].MCTRL &= ~c_can::IF_MCTRL_TXRQST; // Clearing IF_MCTRL_TXRQST
}
}
@@ -416,7 +637,10 @@ void CAN_IRQHandler();
void CAN_IRQHandler()
{
uavcan_lpc11c24::last_irq_utc_timestamp = uavcan_lpc11c24::clock::getUtcUSecFromCanInterrupt();
using namespace uavcan_lpc11c24;
last_irq_utc_timestamp = clock::getUtcUSecFromCanInterrupt();
LPC_CCAN_API->isr();
}
libuavcan_drivers/lpc11c24/driver/src/clock.cpp
+27 -19
View File
@@ -17,18 +17,18 @@ namespace
bool initialized = false;
bool utc_set = false;
int32_t utc_correction_usec_per_overflow_x16 = 0;
int64_t prev_adjustment = 0;
std::int32_t utc_correction_usec_per_overflow_x16 = 0;
std::int64_t prev_adjustment = 0;
uint64_t time_mono = 0;
uint64_t time_utc = 0;
std::uint64_t time_mono = 0;
std::uint64_t time_utc = 0;
/**
* If this value is too large for the given core clock, reload value will be out of the 24-bit integer range.
* This will be detected at run time during timer initialization - refer to SysTick_Config().
*/
const uint32_t USecPerOverflow = 65536 * 2;
const int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
constexpr std::uint32_t USecPerOverflow = 65536 * 2;
constexpr std::int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
}
@@ -59,30 +59,34 @@ void init()
}
}
static uint64_t sampleFromCriticalSection(const volatile uint64_t* const value)
static std::uint64_t sampleFromCriticalSection(const volatile std::uint64_t* const value)
{
const uint32_t reload = SysTick->LOAD + 1; // SysTick counts downwards, hence the value subtracted from reload
const std::uint32_t reload = SysTick->LOAD + 1; // SysTick counts downwards, hence the value subtracted from reload
volatile uint64_t time = *value;
volatile uint32_t cycles = reload - SysTick->VAL;
volatile std::uint64_t time = *value;
volatile std::uint32_t cycles = reload - SysTick->VAL;
if ((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) == SCB_ICSR_PENDSTSET_Msk)
{
cycles = reload - SysTick->VAL;
time += USecPerOverflow;
}
const uint32_t cycles_per_usec = SystemCoreClock / 1000000;
const std::uint32_t cycles_per_usec = SystemCoreClock / 1000000;
return time + (cycles / cycles_per_usec);
}
uint64_t getUtcUSecFromCanInterrupt()
std::uint64_t getUtcUSecFromCanInterrupt()
{
return utc_set ? sampleFromCriticalSection(&time_utc) : 0;
}
uavcan::MonotonicTime getMonotonic()
{
uint64_t usec = 0;
if (!initialized)
{
fail();
}
std::uint64_t usec = 0;
{
CriticalSectionLocker locker;
usec = sampleFromCriticalSection(&time_mono);
@@ -92,7 +96,11 @@ uavcan::MonotonicTime getMonotonic()
uavcan::UtcTime getUtc()
{
uint64_t usec = 0;
if (!initialized)
{
fail();
}
std::uint64_t usec = 0;
if (utc_set)
{
CriticalSectionLocker locker;
@@ -108,7 +116,7 @@ uavcan::UtcDuration getPrevUtcAdjustment()
void adjustUtc(uavcan::UtcDuration adjustment)
{
const int64_t adj_delta = adjustment.toUSec() - prev_adjustment; // This is the P term
const std::int64_t adj_delta = adjustment.toUSec() - prev_adjustment; // This is the P term
prev_adjustment = adjustment.toUSec();
utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
@@ -121,16 +129,16 @@ void adjustUtc(uavcan::UtcDuration adjustment)
if (adjustment.getAbs().toMSec() > 9 || !utc_set)
{
const int64_t adj_usec = adjustment.toUSec();
const std::int64_t adj_usec = adjustment.toUSec();
{
CriticalSectionLocker locker;
if ((adj_usec < 0) && uint64_t(-adj_usec) > time_utc)
if ((adj_usec < 0) && std::uint64_t(-adj_usec) > time_utc)
{
time_utc = 1;
}
else
{
time_utc = uint64_t(int64_t(time_utc) + adj_usec);
time_utc = std::uint64_t(std::int64_t(time_utc) + adj_usec);
}
}
if (!utc_set)
@@ -170,7 +178,7 @@ void SysTick_Handler()
if (utc_set)
{
// Values below 16 are ignored
time_utc += uint64_t(int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
time_utc += std::uint64_t(std::int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
}
}
else
libuavcan_drivers/lpc11c24/driver/src/internal.hpp
+12 -1
View File
@@ -4,8 +4,19 @@
#pragma once
#include <cstdint>
#include <chip.h>
/*
* Compiler version check
*/
#ifdef __GNUC__
# if (__GNUC__ * 10 + __GNUC_MINOR__) < 49
# error "Use GCC 4.9 or newer"
# endif
#endif
namespace uavcan_lpc11c24
{
@@ -31,7 +42,7 @@ struct CriticalSectionLocker
namespace clock
{
uint64_t getUtcUSecFromCanInterrupt();
std::uint64_t getUtcUSecFromCanInterrupt();
}
libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/Makefile
-2
View File
@@ -49,10 +49,8 @@ DEPDIR = $(BUILDDIR)/dep
DEF += -DNDEBUG -DCHIP_LPC11CXX -DCORE_M0 -DTHUMB_NO_INTERWORKING -U__STRICT_ANSI__
# Removing -fconserve-stack reduces code size a little
FLAGS = -mthumb -mcpu=cortex-m0 -mno-thumb-interwork -flto -Os -g3 -Wall -Wextra -Werror -Wundef -ffunction-sections \
-fdata-sections -fno-common -fno-exceptions -fno-unwind-tables -fno-stack-protector -fomit-frame-pointer \
-ftracer -ftree-loop-distribute-patterns -frename-registers -freorder-blocks -fconserve-stack \
-Wfloat-equal -Wconversion -Wsign-conversion -Wmissing-declarations
C_CPP_FLAGS = $(FLAGS) -MD -MP -MF $(DEPDIR)/$(@F).d
libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/lpc_chip_11cxx_lib/src/uart_11xx.c
+289
View File
@@ -0,0 +1,289 @@
/*
* @brief LPC11xx UART chip driver
*
* @note
* Copyright(C) NXP Semiconductors, 2012
* All rights reserved.
*
* @par
* Software that is described herein is for illustrative purposes only
* which provides customers with programming information regarding the
* LPC products. This software is supplied "AS IS" without any warranties of
* any kind, and NXP Semiconductors and its licensor disclaim any and
* all warranties, express or implied, including all implied warranties of
* merchantability, fitness for a particular purpose and non-infringement of
* intellectual property rights. NXP Semiconductors assumes no responsibility
* or liability for the use of the software, conveys no license or rights under any
* patent, copyright, mask work right, or any other intellectual property rights in
* or to any products. NXP Semiconductors reserves the right to make changes
* in the software without notification. NXP Semiconductors also makes no
* representation or warranty that such application will be suitable for the
* specified use without further testing or modification.
*
* @par
* Permission to use, copy, modify, and distribute this software and its
* documentation is hereby granted, under NXP Semiconductors' and its
* licensor's relevant copyrights in the software, without fee, provided that it
* is used in conjunction with NXP Semiconductors microcontrollers. This
* copyright, permission, and disclaimer notice must appear in all copies of
* this code.
*/
#include "chip.h"
#if __GNUC__
# pragma GCC diagnostic ignored "-Wsign-conversion"
# pragma GCC diagnostic ignored "-Wconversion"
# pragma GCC diagnostic ignored "-Wmissing-declarations"
# pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
/*****************************************************************************
* Private types/enumerations/variables
****************************************************************************/
/*****************************************************************************
* Public types/enumerations/variables
****************************************************************************/
/*****************************************************************************
* Private functions
****************************************************************************/
/*****************************************************************************
* Public functions
****************************************************************************/
/* Initializes the pUART peripheral */
void Chip_UART_Init(LPC_USART_T *pUART)
{
Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_UART0);
Chip_Clock_SetUARTClockDiv(1);
/* Enable FIFOs by default, reset them */
Chip_UART_SetupFIFOS(pUART, (UART_FCR_FIFO_EN | UART_FCR_RX_RS | UART_FCR_TX_RS));
/* Default 8N1, with DLAB disabled */
Chip_UART_ConfigData(pUART, (UART_LCR_WLEN8 | UART_LCR_SBS_1BIT | UART_LCR_PARITY_DIS));
/* Disable fractional divider */
pUART->FDR = 0x10;
}
/* De-initializes the pUART peripheral */
void Chip_UART_DeInit(LPC_USART_T *pUART)
{
Chip_Clock_DisablePeriphClock(SYSCTL_CLOCK_UART0);
}
/* Transmit a byte array through the UART peripheral (non-blocking) */
int Chip_UART_Send(LPC_USART_T *pUART, const void *data, int numBytes)
{
int sent = 0;
uint8_t *p8 = (uint8_t *) data;
/* Send until the transmit FIFO is full or out of bytes */
while ((sent < numBytes) &&
((Chip_UART_ReadLineStatus(pUART) & UART_LSR_THRE) != 0)) {
Chip_UART_SendByte(pUART, *p8);
p8++;
sent++;
}
return sent;
}
/* Transmit a byte array through the UART peripheral (blocking) */
int Chip_UART_SendBlocking(LPC_USART_T *pUART, const void *data, int numBytes)
{
int pass, sent = 0;
uint8_t *p8 = (uint8_t *) data;
while (numBytes > 0) {
pass = Chip_UART_Send(pUART, p8, numBytes);
numBytes -= pass;
sent += pass;
p8 += pass;
}
return sent;
}
/* Read data through the UART peripheral (non-blocking) */
int Chip_UART_Read(LPC_USART_T *pUART, void *data, int numBytes)
{
int readBytes = 0;
uint8_t *p8 = (uint8_t *) data;
/* Send until the transmit FIFO is full or out of bytes */
while ((readBytes < numBytes) &&
((Chip_UART_ReadLineStatus(pUART) & UART_LSR_RDR) != 0)) {
*p8 = Chip_UART_ReadByte(pUART);
p8++;
readBytes++;
}
return readBytes;
}
/* Read data through the UART peripheral (blocking) */
int Chip_UART_ReadBlocking(LPC_USART_T *pUART, void *data, int numBytes)
{
int pass, readBytes = 0;
uint8_t *p8 = (uint8_t *) data;
while (readBytes < numBytes) {
pass = Chip_UART_Read(pUART, p8, numBytes);
numBytes -= pass;
readBytes += pass;
p8 += pass;
}
return readBytes;
}
/* Determines and sets best dividers to get a target bit rate */
uint32_t Chip_UART_SetBaud(LPC_USART_T *pUART, uint32_t baudrate)
{
uint32_t div, divh, divl, clkin;
/* Determine UART clock in rate without FDR */
clkin = Chip_Clock_GetMainClockRate();
div = clkin / (baudrate * 16);
/* High and low halves of the divider */
divh = div / 256;
divl = div - (divh * 256);
Chip_UART_EnableDivisorAccess(pUART);
Chip_UART_SetDivisorLatches(pUART, divl, divh);
Chip_UART_DisableDivisorAccess(pUART);
/* Fractional FDR alreadt setup for 1 in UART init */
return clkin / div;
}
/* UART receive-only interrupt handler for ring buffers */
void Chip_UART_RXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB)
{
/* New data will be ignored if data not popped in time */
while (Chip_UART_ReadLineStatus(pUART) & UART_LSR_RDR) {
uint8_t ch = Chip_UART_ReadByte(pUART);
RingBuffer_Insert(pRB, &ch);
}
}
/* UART transmit-only interrupt handler for ring buffers */
void Chip_UART_TXIntHandlerRB(LPC_USART_T *pUART, RINGBUFF_T *pRB)
{
uint8_t ch;
/* Fill FIFO until full or until TX ring buffer is empty */
while ((Chip_UART_ReadLineStatus(pUART) & UART_LSR_THRE) != 0 &&
RingBuffer_Pop(pRB, &ch)) {
Chip_UART_SendByte(pUART, ch);
}
}
/* Populate a transmit ring buffer and start UART transmit */
uint32_t Chip_UART_SendRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, const void *data, int bytes)
{
uint32_t ret;
uint8_t *p8 = (uint8_t *) data;
/* Don't let UART transmit ring buffer change in the UART IRQ handler */
Chip_UART_IntDisable(pUART, UART_IER_THREINT);
/* Move as much data as possible into transmit ring buffer */
ret = RingBuffer_InsertMult(pRB, p8, bytes);
Chip_UART_TXIntHandlerRB(pUART, pRB);
/* Add additional data to transmit ring buffer if possible */
ret += RingBuffer_InsertMult(pRB, (p8 + ret), (bytes - ret));
/* Enable UART transmit interrupt */
Chip_UART_IntEnable(pUART, UART_IER_THREINT);
return ret;
}
/* Copy data from a receive ring buffer */
int Chip_UART_ReadRB(LPC_USART_T *pUART, RINGBUFF_T *pRB, void *data, int bytes)
{
(void) pUART;
return RingBuffer_PopMult(pRB, (uint8_t *) data, bytes);
}
/* UART receive/transmit interrupt handler for ring buffers */
void Chip_UART_IRQRBHandler(LPC_USART_T *pUART, RINGBUFF_T *pRXRB, RINGBUFF_T *pTXRB)
{
/* Handle transmit interrupt if enabled */
if (pUART->IER & UART_IER_THREINT) {
Chip_UART_TXIntHandlerRB(pUART, pTXRB);
/* Disable transmit interrupt if the ring buffer is empty */
if (RingBuffer_IsEmpty(pTXRB)) {
Chip_UART_IntDisable(pUART, UART_IER_THREINT);
}
}
/* Handle receive interrupt */
Chip_UART_RXIntHandlerRB(pUART, pRXRB);
}
/* Determines and sets best dividers to get a target baud rate */
uint32_t Chip_UART_SetBaudFDR(LPC_USART_T *pUART, uint32_t baudrate)
{
uint32_t uClk = 0;
uint32_t dval = 0;
uint32_t mval = 0;
uint32_t dl = 0;
uint32_t rate16 = 16 * baudrate;
uint32_t actualRate = 0;
/* Get Clock rate */
uClk = Chip_Clock_GetMainClockRate();
/* The fractional is calculated as (PCLK % (16 * Baudrate)) / (16 * Baudrate)
* Let's make it to be the ratio DivVal / MulVal
*/
dval = uClk % rate16;
/* The PCLK / (16 * Baudrate) is fractional
* => dval = pclk % rate16
* mval = rate16;
* now mormalize the ratio
* dval / mval = 1 / new_mval
* new_mval = mval / dval
* new_dval = 1
*/
if (dval > 0) {
mval = rate16 / dval;
dval = 1;
/* In case mval still bigger then 4 bits
* no adjustment require
*/
if (mval > 12) {
dval = 0;
}
}
dval &= 0xf;
mval &= 0xf;
dl = uClk / (rate16 + rate16 *dval / mval);
/* Update UART registers */
Chip_UART_EnableDivisorAccess(pUART);
Chip_UART_SetDivisorLatches(pUART, UART_LOAD_DLL(dl), UART_LOAD_DLM(dl));
Chip_UART_DisableDivisorAccess(pUART);
/* Set best fractional divider */
pUART->FDR = (UART_FDR_MULVAL(mval) | UART_FDR_DIVADDVAL(dval));
/* Return actual baud rate */
actualRate = uClk / (16 * dl + 16 * dl * dval / mval);
return actualRate;
}
libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp
+170 -54
View File
@@ -3,21 +3,74 @@
*/
#include <cstdio>
#include <cstdlib>
#include <algorithm>
#include <board.hpp>
#include <chip.h>
#include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include <uavcan/protocol/logger.hpp>
/*
* GCC 4.9 cannot generate a working binary with higher optimization levels, although
* rest of the firmware can be compiled with -Os.
* GCC 4.8 and earlier don't work at all on this firmware.
*/
#if __GNUC__
# pragma GCC optimize 1
#endif
/**
* This function re-defines the standard ::rand(), which is used by the class uavcan::DynamicNodeIDClient.
* Redefinition is normally not needed, but GCC 4.9 tends to generate broken binaries if it is not redefined.
*/
int rand()
{
static int x = 1;
x = x * 48271 % 2147483647;
return x;
}
namespace
{
typedef uavcan::Node<2800> Node;
static constexpr unsigned NodeMemoryPoolSize = 2800;
Node& getNode()
/**
* This is a compact, reentrant and thread-safe replacement to standard llto().
* It returns the string by value, no extra storage is needed.
*/
typename uavcan::MakeString<22>::Type intToString(long long n)
{
static Node node(uavcan_lpc11c24::CanDriver::instance(), uavcan_lpc11c24::SystemClock::instance());
char buf[24] = {};
const short sign = (n < 0) ? -1 : 1;
if (sign < 0)
{
n = -n;
}
unsigned pos = 0;
do
{
buf[pos++] = char(n % 10 + '0');
}
while ((n /= 10) > 0);
if (sign < 0)
{
buf[pos++] = '-';
}
buf[pos] = '\0';
for (unsigned i = 0, j = pos - 1U; i < j; i++, j--)
{
std::swap(buf[i], buf[j]);
}
return static_cast<const char*>(buf);
}
uavcan::Node<NodeMemoryPoolSize>& getNode()
{
static uavcan::Node<NodeMemoryPoolSize> node(uavcan_lpc11c24::CanDriver::instance(),
uavcan_lpc11c24::SystemClock::instance());
return node;
}
@@ -33,29 +86,63 @@ uavcan::Logger& getLogger()
return logger;
}
#if __GNUC__
__attribute__((noreturn))
#endif
void die()
uavcan::NodeID performDynamicNodeIDAllocation()
{
while (true) { }
uavcan::DynamicNodeIDClient client(getNode());
const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
if (client_start_res < 0)
{
board::die();
}
while (!client.isAllocationComplete())
{
board::resetWatchdog();
(void)getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
}
return client.getAllocatedNodeID();
}
#if __GNUC__
__attribute__((noinline))
#endif
void init()
{
board::resetWatchdog();
board::syslog("Boot\r\n");
if (uavcan_lpc11c24::CanDriver::instance().init(1000000) < 0)
board::setErrorLed(false);
board::setStatusLed(true);
/*
* Configuring the clock - this must be done before the CAN controller is initialized
*/
uavcan_lpc11c24::clock::init();
/*
* Configuring the CAN controller
*/
std::uint32_t bit_rate = 0;
while (bit_rate == 0)
{
die();
board::syslog("CAN auto bitrate...\r\n");
bit_rate = uavcan_lpc11c24::CanDriver::detectBitRate(&board::resetWatchdog);
}
board::syslog("Bitrate: ");
board::syslog(intToString(bit_rate).c_str());
board::syslog("\r\n");
if (uavcan_lpc11c24::CanDriver::instance().init(bit_rate) < 0)
{
board::die();
}
board::syslog("CAN init ok\r\n");
board::resetWatchdog();
getNode().setNodeID(72);
/*
* Configuring the node
*/
getNode().setName("org.uavcan.lpc11c24_test");
uavcan::protocol::SoftwareVersion swver;
@@ -73,55 +160,69 @@ void init()
board::resetWatchdog();
while (getNode().start() < 0)
/*
* Starting the node and performing dynamic node ID allocation
*/
if (getNode().start() < 0)
{
board::die();
}
board::syslog("Node ID allocation...\r\n");
getNode().setNodeID(performDynamicNodeIDAllocation());
board::syslog("Node ID ");
board::syslog(intToString(getNode().getNodeID().get()).c_str());
board::syslog("\r\n");
board::resetWatchdog();
while (getTimeSyncSlave().start() < 0)
/*
* Example filter configuration.
* Can be removed safely.
*/
{
constexpr unsigned NumFilters = 3;
uavcan::CanFilterConfig filters[NumFilters];
// Acepting all service transfers addressed to us
filters[0].id = (unsigned(getNode().getNodeID().get()) << 8) | (1U << 7) | uavcan::CanFrame::FlagEFF;
filters[0].mask = 0x7F80 | uavcan::CanFrame::FlagEFF;
// Accepting time sync messages
filters[1].id = (4U << 8) | uavcan::CanFrame::FlagEFF;
filters[1].mask = 0xFFFF80 | uavcan::CanFrame::FlagEFF;
// Accepting zero CAN ID (just for the sake of testing)
filters[2].id = 0 | uavcan::CanFrame::FlagEFF;
filters[2].mask = uavcan::CanFrame::MaskExtID | uavcan::CanFrame::FlagEFF;
if (uavcan_lpc11c24::CanDriver::instance().configureFilters(filters, NumFilters) < 0)
{
board::syslog("Filter init failed\r\n");
board::die();
}
}
while (getLogger().init() < 0)
/*
* Initializing other libuavcan-related objects
*/
if (getTimeSyncSlave().start() < 0)
{
board::die();
}
if (getLogger().init() < 0)
{
board::die();
}
getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
board::resetWatchdog();
}
void reverse(char* s)
{
for (int i = 0, j = int(std::strlen(s)) - 1; i < j; i++, j--)
{
const char c = s[i];
s[i] = s[j];
s[j] = c;
}
}
void lltoa(long long n, char buf[24])
{
const short sign = (n < 0) ? -1 : 1;
if (sign < 0)
{
n = -n;
}
unsigned i = 0;
do
{
buf[i++] = char(n % 10 + '0');
}
while ((n /= 10) > 0);
if (sign < 0)
{
buf[i++] = '-';
}
buf[i] = '\0';
reverse(buf);
}
}
int main()
@@ -143,16 +244,31 @@ int main()
{
prev_log_at = ts;
// We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf()
char buf[24];
lltoa(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec(), buf);
buf[sizeof(buf) - 1] = '\0';
/*
* CAN bus off state monitoring
*/
if (uavcan_lpc11c24::CanDriver::instance().isInBusOffState())
{
board::syslog("CAN BUS OFF\r\n");
}
// ...hence we need to construct the message manually:
/*
* CAN error counter, for debugging purposes
*/
board::syslog("CAN errors: ");
board::syslog(intToString(static_cast<long long>(uavcan_lpc11c24::CanDriver::instance().getErrorCount())).c_str());
board::syslog(" ");
board::syslog(intToString(uavcan_lpc11c24::CanDriver::instance().getRxQueueOverflowCount()).c_str());
board::syslog("\r\n");
/*
* We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf(),
* so we need to construct the message manually:
*/
uavcan::protocol::debug::LogMessage logmsg;
logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO;
logmsg.source = "app";
logmsg.text = buf;
logmsg.text = intToString(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec()).c_str();
(void)getLogger().log(logmsg);
}
libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp
+45 -14
View File
@@ -9,24 +9,26 @@
#include <cstring>
#include <numeric>
#define PDRUNCFGUSEMASK 0x0000ED00
#define PDRUNCFGMASKTMP 0x000000FF
static constexpr unsigned long PDRUNCFGUSEMASK = 0x0000ED00U;
static constexpr unsigned long PDRUNCFGMASKTMP = 0x000000FFU;
const uint32_t OscRateIn = 12000000; ///< External crystal
const uint32_t ExtRateIn = 0;
const std::uint32_t OscRateIn = 12000000; ///< External crystal
const std::uint32_t ExtRateIn = 0;
uint32_t SystemCoreClock = 12000000; ///< Initialized to default clock value, will be changed on init
std::uint32_t SystemCoreClock = 12000000; ///< Initialized to default clock value, will be changed on init
namespace board
{
namespace
{
const unsigned ErrorLedPort = 1;
const unsigned ErrorLedPin = 10;
constexpr unsigned TargetSystemCoreClock = 48000000;
const unsigned StatusLedPort = 1;
const unsigned StatusLedPin = 11;
constexpr unsigned ErrorLedPort = 1;
constexpr unsigned ErrorLedPin = 10;
constexpr unsigned StatusLedPort = 1;
constexpr unsigned StatusLedPin = 11;
struct PinMuxGroup
{
@@ -34,10 +36,11 @@ struct PinMuxGroup
unsigned modefunc : 24;
};
const PinMuxGroup pinmux[] =
constexpr PinMuxGroup pinmux[] =
{
{ IOCON_PIO1_10, IOCON_FUNC0 | IOCON_MODE_INACT }, // Error LED
{ IOCON_PIO1_11, IOCON_FUNC0 | IOCON_MODE_INACT } // Status LED
{ IOCON_PIO1_10, IOCON_FUNC0 | IOCON_MODE_INACT }, // Error LED
{ IOCON_PIO1_11, IOCON_FUNC0 | IOCON_MODE_INACT }, // Status LED
{ IOCON_PIO1_7, IOCON_FUNC1 | IOCON_HYS_EN | IOCON_MODE_PULLUP }, // UART_TXD
};
@@ -92,7 +95,7 @@ void initClock()
SystemCoreClock = Chip_Clock_GetSystemClockRate();
while (SystemCoreClock != 48000000) { } // Loop forever if the clock failed to initialize properly
while (SystemCoreClock != TargetSystemCoreClock) { } // Loop forever if the clock failed to initialize properly
}
void initGpio()
@@ -109,6 +112,13 @@ void initGpio()
LPC_GPIO[StatusLedPort].DIR |= 1 << StatusLedPin;
}
void initUart()
{
Chip_UART_Init(LPC_USART);
Chip_UART_SetBaud(LPC_USART, 115200);
Chip_UART_TXEnable(LPC_USART);
}
void init()
{
Chip_SYSCTL_SetBODLevels(SYSCTL_BODRSTLVL_2_06V, SYSCTL_BODINTVAL_RESERVED1);
@@ -117,16 +127,32 @@ void init()
initWatchdog();
initClock();
initGpio();
initUart();
resetWatchdog();
}
} // namespace
void die()
{
static const volatile unsigned& DHCSR = *reinterpret_cast<unsigned*>(0xE000EDF0U);
syslog("FATAL\r\n");
while (true)
{
if ((DHCSR & 1U) != 0)
{
__asm volatile ("bkpt #0\n"); // Break into the debugger
}
}
}
#if __GNUC__
__attribute__((optimize(0))) // Optimization must be disabled lest it hardfaults in the IAP call
#endif
void readUniqueID(uint8_t out_uid[UniqueIDSize])
void readUniqueID(std::uint8_t out_uid[UniqueIDSize])
{
unsigned aligned_array[4] = {}; // out_uid may be unaligned, so we need to use temp array
unsigned iap_command = 58;
@@ -149,6 +175,11 @@ void resetWatchdog()
Chip_WWDT_Feed(LPC_WWDT);
}
void syslog(const char* msg)
{
Chip_UART_SendBlocking(LPC_USART, msg, static_cast<int>(std::strlen(msg)));
}
} // namespace board
extern "C"
libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp
+16 -3
View File
@@ -2,18 +2,31 @@
* Pavel Kirienko, 2014 <pavel.kirienko@gmail.com>
*/
#include <stdint.h>
#include <cstdint>
namespace board
{
static const unsigned UniqueIDSize = 16;
#if __GNUC__
__attribute__((noreturn))
#endif
void die();
void readUniqueID(uint8_t out_uid[UniqueIDSize]);
static constexpr unsigned UniqueIDSize = 16;
/**
* Reads the globally unique 128-bit hardware ID from the MCU.
*/
void readUniqueID(std::uint8_t out_uid[UniqueIDSize]);
void setStatusLed(bool state);
void setErrorLed(bool state);
void resetWatchdog();
/**
* Sends the string to UART.
*/
void syslog(const char* msg);
}
libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/libstubs.cpp
+2 -2
View File
@@ -13,13 +13,13 @@
void* __dso_handle;
void* operator new(size_t)
void* operator new(std::size_t)
{
std::abort();
return reinterpret_cast<void*>(0xFFFFFFFF);
}
void* operator new[](size_t)
void* operator new[](std::size_t)
{
std::abort();
return reinterpret_cast<void*>(0xFFFFFFFF);