Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_PX4: support for several CAN interfaces and drivers, filtering support

Browse Source
This commit is contained in:
Eugene Shamaev 2017-05-06 12:12:35 +03:00 committed by Francisco Ferreira
parent ec8aa2e23f
commit 36655310b3
5 changed files with 264 additions and 165 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

323
libraries/AP_HAL_PX4/CAN.cpp
View File

@ -9,6 +9,7 @@
#include <AP_HAL/system.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#if HAL_WITH_UAVCAN
@ -58,6 +59,7 @@ uavcan::MonotonicTime clock::getMonotonic()
BusEvent::BusEvent(PX4CANManager& can_driver) :
_signal(0)
{
sem_init(&_wait_semaphore, 0, 0);
}
BusEvent::~BusEvent()
@ -110,12 +112,14 @@ static void handleTxInterrupt(uint8_t iface_index)
{
if (iface_index < CAN_STM32_NUM_IFACES) {
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
if (utc_usec > 0) {
utc_usec--;
}
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr)->getIface(iface_index);
if (iface != nullptr) {
iface->handleTxInterrupt(utc_usec);
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
if (iface != nullptr) {
iface->handleTxInterrupt(utc_usec);
}
}
}
}
}
@ -124,18 +128,19 @@ static void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index)
{
if (iface_index < CAN_STM32_NUM_IFACES) {
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
if (utc_usec > 0) {
utc_usec--;
}
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr)->getIface(iface_index);
if (iface != nullptr) {
iface->handleRxInterrupt(fifo_index, utc_usec);
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
if (iface != nullptr) {
iface->handleRxInterrupt(fifo_index, utc_usec);
}
}
}
}
}
const uint32_t PX4CAN::TSR_ABRQx[PX4CAN::NumTxMailboxes] = { bxcan::TSR_ABRQ0, bxcan::TSR_ABRQ1, bxcan::TSR_ABRQ2 };
PX4CAN* PX4CANManager::ifaces[CAN_STM32_NUM_IFACES] = {};
int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings)
{
@ -165,7 +170,7 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings)
const uint8_t max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
if (max_quanta_per_bit > (MaxBS1 + MaxBS2)) {
if (AP_BoardConfig::get_can_debug() >= 1) {
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
printf("PX4CAN::computeTimings max_quanta_per_bit problem\n\r");
}
}
@ -234,7 +239,7 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings)
uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
{
if (bs1_bs2_sum <= arg_bs1) {
if (AP_BoardConfig::get_can_debug() >= 1) {
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
AP_HAL::panic("PX4CAN::computeTimings bs1_bs2_sum <= arg_bs1");
}
}
@ -264,13 +269,13 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings)
* Final validation
*/
if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) {
if (AP_BoardConfig::get_can_debug() >= 1) {
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
printf("PX4CAN::computeTimings target_bitrate error\n\r");
}
return -ErrLogic;
}
if (AP_BoardConfig::get_can_debug() >= 2) {
if (AP_BoardConfig_CAN::get_can_debug() >= 2) {
printf("PX4CAN::computeTimings Timings: quanta/bit: %d, sample point location: %.1f%%\n\r",
int(1 + solution.bs1 + solution.bs2), double(solution.sample_point_permill / 10.0));
}
@ -380,11 +385,69 @@ int16_t PX4CAN::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_
int16_t PX4CAN::configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs)
{
// TODO: Hardware filter support
CriticalSectionLocker lock;
(void) filter_configs;
(void) num_configs;
return -ErrNotImplemented;
if (num_configs <= NumFilters) {
CriticalSectionLocker lock;
can_->FMR |= bxcan::FMR_FINIT;
// Slave (CAN2) gets half of the filters
can_->FMR = (can_->FMR & ~0x00003F00) | static_cast<uint32_t>(NumFilters) << 8;
can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters
can_->FM1R = 0; // Identifier Mask mode
can_->FS1R = 0x7ffffff; // Single 32-bit for all
const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters;
if (num_configs == 0) {
can_->FilterRegister[filter_start_index].FR1 = 0;
can_->FilterRegister[filter_start_index].FR2 = 0;
can_->FA1R = 1 << filter_start_index;
} else {
for (uint8_t i = 0; i < NumFilters; i++) {
if (i < num_configs) {
uint32_t id = 0;
uint32_t mask = 0;
const uavcan::CanFilterConfig* const cfg = filter_configs + i;
if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) {
id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3;
mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3;
id |= bxcan::RIR_IDE;
} else {
id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21;
mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21;
}
if (cfg->id & uavcan::CanFrame::FlagRTR) {
id |= bxcan::RIR_RTR;
}
if (cfg->mask & uavcan::CanFrame::FlagEFF) {
mask |= bxcan::RIR_IDE;
}
if (cfg->mask & uavcan::CanFrame::FlagRTR) {
mask |= bxcan::RIR_RTR;
}
can_->FilterRegister[filter_start_index + i].FR1 = id;
can_->FilterRegister[filter_start_index + i].FR2 = mask;
can_->FA1R |= (1 << (filter_start_index + i));
} else {
can_->FA1R &= ~(1 << (filter_start_index + i));
}
}
}
can_->FMR &= ~bxcan::FMR_FINIT;
return 0;
}
return -ErrFilterNumConfigs;
}
bool PX4CAN::waitMsrINakBitStateChange(bool target_state)
@ -414,7 +477,7 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode)
}
if (!waitMsrINakBitStateChange(true)) {
if (AP_BoardConfig::get_can_debug() >= 1) {
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
printf("PX4CAN::init MSR INAK not set\n\r");
}
can_->MCR = bxcan::MCR_RESET;
@ -440,7 +503,7 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode)
can_->MCR = bxcan::MCR_RESET;
return timings_res;
}
if (AP_BoardConfig::get_can_debug() >= 2) {
if (AP_BoardConfig_CAN::get_can_debug() >= 2) {
printf("PX4CAN::init Timings: presc=%u sjw=%u bs1=%u bs2=%u\n\r", unsigned(timings.prescaler),
unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));
}
@ -460,7 +523,7 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode)
can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode
if (!waitMsrINakBitStateChange(false)) {
if (AP_BoardConfig::get_can_debug() >= 1) {
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
printf("PX4CAN::init MSR INAK not cleared\n\r");
}
can_->MCR = bxcan::MCR_RESET;
@ -479,12 +542,19 @@ int PX4CAN::init(const uint32_t bitrate, const OperatingMode mode)
can_->FFA1R = 0; // All assigned to FIFO0 by default
can_->FM1R = 0; // Indentifier Mask mode
#if CAN_STM32_NUM_IFACES > 1
can_->FS1R = 0x7ffffff; // Single 32-bit for all
can_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything
can_->FilterRegister[0].FR2 = 0;
can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything
can_->FilterRegister[NumFilters].FR2 = 0;
can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface
#else
can_->FS1R = 0x1fff;
can_->FilterRegister[0].FR1 = 0;
can_->FilterRegister[0].FR2 = 0;
can_->FA1R = 1;
#endif
can_->FMR &= ~bxcan::FMR_FINIT;
}
@ -530,7 +600,10 @@ void PX4CAN::handleTxInterrupt(const uint64_t utc_usec)
can_->TSR = bxcan::TSR_RQCP2;
handleTxMailboxInterrupt(2, txok, utc_usec);
}
update_event_.signalFromInterrupt();
if(update_event_ != nullptr) {
update_event_->signalFromInterrupt();
}
pollErrorFlagsFromISR();
}
@ -591,7 +664,9 @@ void PX4CAN::handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec)
rx_queue_.push(frm);
had_activity_ = true;
update_event_.signalFromInterrupt();
if(update_event_ != nullptr) {
update_event_->signalFromInterrupt();
}
pollErrorFlagsFromISR();
}
@ -743,46 +818,50 @@ int32_t PX4CAN::tx_pending()
/*
* CanDriver
*/
PX4CANManager::PX4CANManager() :
update_event_(*this), if0_(bxcan::Can[0], nullptr, 0, CAN_STM32_RX_QUEUE_SIZE), if1_(
bxcan::Can[1], nullptr, 1, CAN_STM32_RX_QUEUE_SIZE), initialized_(false), p_uavcan(nullptr)
{
uavcan::StaticAssert<(CAN_STM32_RX_QUEUE_SIZE <= PX4CAN::MaxRxQueueCapacity)>::check();
for(uint8_t i = 0; i < CAN_STM32_NUM_IFACES; i++) {
_ifaces_out_to_in[i] = UINT8_MAX;
}
}
uavcan::CanSelectMasks PX4CANManager::makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const
{
uavcan::CanSelectMasks msk;
if (can_number_ >= 1) {
// Iface 0
msk.read = if0_.isRxBufferEmpty() ? 0 : 1;
for (uint8_t i = 0; i < _ifaces_num; i++) {
if (ifaces[i] != nullptr) {
if (!ifaces[i]->isRxBufferEmpty()) {
msk.read |= 1 << i;
}
if (pending_tx[0] != nullptr) {
msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0;
}
}
#if CAN_STM32_NUM_IFACES > 1
if (can_number_ >= 2) {
if (!if1_.isRxBufferEmpty()) {
msk.read |= 1 << 1;
}
// Iface 1
if (pending_tx[1] != nullptr) {
if (if1_.canAcceptNewTxFrame(*pending_tx[1])) {
msk.write |= 1 << 1;
if (pending_tx[i] != nullptr) {
if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) {
msk.write |= 1 << i;
}
}
}
}
#endif
return msk;
}
bool PX4CANManager::hasReadableInterfaces() const
{
#if CAN_STM32_NUM_IFACES > 1
if (can_number_ >= 2) {
return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty();
}
#endif
bool ret = false;
return !if0_.isRxBufferEmpty();
for (uint8_t i = 0; i < _ifaces_num; i++) {
if (ifaces[i] != nullptr) {
ret |= !ifaces[i]->isRxBufferEmpty();
}
}
return ret;
}
int16_t PX4CANManager::select(uavcan::CanSelectMasks& inout_masks,
@ -791,24 +870,16 @@ int16_t PX4CANManager::select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanSelectMasks in_masks = inout_masks;
const uavcan::MonotonicTime time = clock::getMonotonic();
if (can_number_ >= 1) {
if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots
{
CriticalSectionLocker cs_locker;
if0_.pollErrorFlagsFromISR();
for (uint8_t i = 0; i < _ifaces_num; i++) {
if (ifaces[i] != nullptr) {
ifaces[i]->discardTimedOutTxMailboxes(time);
{
CriticalSectionLocker cs_locker;
ifaces[i]->pollErrorFlagsFromISR();
}
}
}
#if CAN_STM32_NUM_IFACES > 1
if (can_number_ >= 2) {
if1_.discardTimedOutTxMailboxes(time);
{
CriticalSectionLocker cs_locker;
if1_.pollErrorFlagsFromISR();
}
}
#endif
inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events
if ((inout_masks.read & in_masks.read) != 0 || (inout_masks.write & in_masks.write) != 0) {
return 1;
@ -821,22 +892,19 @@ int16_t PX4CANManager::select(uavcan::CanSelectMasks& inout_masks,
void PX4CANManager::initOnce(uint8_t can_number)
{
/*
* CAN1, CAN2
*/
{
CriticalSectionLocker lock;
if (can_number >= 1) {
if (can_number == 0) {
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN);
}
#if CAN_STM32_NUM_IFACES > 1
if (can_number >= 2) {
if (can_number == 1) {
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN);
}
#endif
}
if (can_number >= 1) {
if (can_number == 0) {
#if defined(GPIO_CAN1_RX) && defined(GPIO_CAN1_TX)
stm32_configgpio(GPIO_CAN1_RX);
stm32_configgpio(GPIO_CAN1_TX);
@ -845,7 +913,7 @@ void PX4CANManager::initOnce(uint8_t can_number)
#endif
}
#if CAN_STM32_NUM_IFACES > 1
if (can_number >= 2) {
if (can_number == 1) {
#if defined(GPIO_CAN2_RX) && defined(GPIO_CAN2_TX)
stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
stm32_configgpio(GPIO_CAN2_TX);
@ -858,7 +926,7 @@ void PX4CANManager::initOnce(uint8_t can_number)
/*
* IRQ
*/
if (can_number >= 1) {
if (can_number == 0) {
#if defined(STM32_IRQ_CAN1TX) && defined(STM32_IRQ_CAN1RX0) && defined(STM32_IRQ_CAN1RX1)
CAN_IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq);
CAN_IRQ_ATTACH(STM32_IRQ_CAN1RX0, can1_irq);
@ -869,7 +937,7 @@ void PX4CANManager::initOnce(uint8_t can_number)
}
#if CAN_STM32_NUM_IFACES > 1
if (can_number >= 2) {
if (can_number == 1) {
#if defined(STM32_IRQ_CAN2TX) && defined(STM32_IRQ_CAN2RX0) && defined(STM32_IRQ_CAN2RX1)
CAN_IRQ_ATTACH(STM32_IRQ_CAN2TX, can2_irq);
CAN_IRQ_ATTACH(STM32_IRQ_CAN2RX0, can2_irq);
@ -884,61 +952,59 @@ void PX4CANManager::initOnce(uint8_t can_number)
int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode, uint8_t can_number)
{
int res = -ErrNotImplemented;
static bool initialized_once[CAN_STM32_NUM_IFACES];
if (can_number <= CAN_STM32_NUM_IFACES) {
if (can_number < CAN_STM32_NUM_IFACES) {
res = 0;
can_number_ = can_number;
if (AP_BoardConfig::get_can_debug() >= 2) {
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
printf("PX4CANManager::init Bitrate %lu mode %d bus %d\n\r", static_cast<unsigned long>(bitrate),
static_cast<int>(mode), static_cast<int>(can_number));
}
static bool initialized_once = false;
if (!initialized_once) {
initialized_once = true;
if (AP_BoardConfig::get_can_debug() >= 2) {
// If this outside physical interface was never inited - do this and add it to in/out conversion tables
if (!initialized_once[can_number]) {
initialized_once[can_number] = true;
_ifaces_num++;
_ifaces_out_to_in[can_number] = _ifaces_num - 1;
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
printf("PX4CANManager::init First initialization bus %d\n\r", static_cast<int>(can_number));
}
initOnce(can_number);
}
/*
* CAN1
*/
if (can_number >= 1) {
if (AP_BoardConfig::get_can_debug() >= 2) {
if (can_number == 0) {
if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) {
printf("PX4CANManager::init Initing iface 0...\n\r");
}
ifaces[0] = &if0_; // This link must be initialized first,
res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet;
if (res < 0) { // a typical race condition.
printf("PX4CANManager::init Iface 0 init failed %i\n\r", res);
ifaces[0] = nullptr;
return res;
}
ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first,
}
#if CAN_STM32_NUM_IFACES > 1
/*
* CAN2
*/
if (can_number >= 2) {
if (AP_BoardConfig::get_can_debug() >= 2) {
if (can_number == 1) {
if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) {
printf("PX4CANManager::init Initing iface 1...\n\r");
}
ifaces[1] = &if1_; // Same thing here.
res = if1_.init(bitrate, mode);
if (res < 0) {
printf("PX4CANManager::init Iface 1 init failed %i\n\r", res);
ifaces[1] = nullptr;
return res;
}
ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here.
}
#endif
if (AP_BoardConfig::get_can_debug() >= 2) {
ifaces[_ifaces_out_to_in[can_number]]->set_update_event(&update_event_);
res = ifaces[_ifaces_out_to_in[can_number]]->init(bitrate, mode);
if (res < 0) {
ifaces[_ifaces_out_to_in[can_number]] = nullptr;
return res;
}
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
printf("PX4CANManager::init CAN drv init OK, res = %d\n\r", res);
}
}
@ -948,39 +1014,47 @@ int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode
PX4CAN* PX4CANManager::getIface(uint8_t iface_index)
{
if (iface_index < CAN_STM32_NUM_IFACES) {
if (iface_index < _ifaces_num) {
return ifaces[iface_index];
}
return nullptr;
}
PX4CAN* PX4CANManager::getIface_out_to_in(uint8_t iface_index)
{
// Find which internal interface corresponds to required outside physical interface
if (iface_index < CAN_STM32_NUM_IFACES) {
if (_ifaces_out_to_in[iface_index] != UINT8_MAX) {
return ifaces[_ifaces_out_to_in[iface_index]];
}
}
return nullptr;
}
bool PX4CANManager::hadActivity()
{
bool ret = if0_.hadActivity();
#if CAN_STM32_NUM_IFACES > 1
ret |= if1_.hadActivity();
#endif
bool ret = false;
// Go through all interfaces that are present in this manager
for (uint8_t i = 0; i < _ifaces_num; i++)
{
if (ifaces[i] != nullptr) {
ret |= ifaces[i]->hadActivity();
}
}
return ret;
}
bool PX4CANManager::begin(uint32_t bitrate, uint8_t can_number)
{
// Try to init outside physical interface 'can_number'
if (init(bitrate, PX4CAN::OperatingMode::NormalMode, can_number) >= 0) {
initialized_ = true;
if (p_uavcan != nullptr) {
uint16_t UAVCAN_init_tries;
// TODO: Limit number of times we try to init UAVCAN and also provide
// the reasonable actions when it fails.
for (UAVCAN_init_tries = 0; UAVCAN_init_tries < 100; UAVCAN_init_tries++) {
if (p_uavcan->try_init() == true) {
return true;
}
hal.scheduler->delay(1);
}
}
return true;
}
return false;
}
@ -989,6 +1063,11 @@ bool PX4CANManager::is_initialized()
return initialized_;
}
void PX4CANManager::initialized(bool val)
{
initialized_ = val;
}
AP_UAVCAN *PX4CANManager::get_UAVCAN(void)
{
return p_uavcan;

37
libraries/AP_HAL_PX4/CAN.h
View File

@ -37,6 +37,7 @@ static const int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g.
static const int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1
static const int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0
static const int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished
static const int16_t ErrFilterNumConfigs = 1008; ///< Auto bit rate detection could not be finished
/**
* RX queue item.
@ -117,7 +118,7 @@ class PX4CAN: public AP_HAL::CAN {
NumTxMailboxes = 3
};
enum {
NumFilters = 28
NumFilters = 14
};
static const uint32_t TSR_ABRQx[NumTxMailboxes];
@ -126,7 +127,7 @@ class PX4CAN: public AP_HAL::CAN {
bxcan::CanType* const can_;
uint64_t error_cnt_;
uint32_t served_aborts_cnt_;
BusEvent& update_event_;
BusEvent* update_event_;
TxItem pending_tx_[NumTxMailboxes];
uint8_t peak_tx_mailbox_index_;
const uint8_t self_index_;
@ -164,7 +165,7 @@ public:
NormalMode, SilentMode
};
PX4CAN(bxcan::CanType* can, BusEvent& update_event, uint8_t self_index, uint8_t rx_queue_capacity) :
PX4CAN(bxcan::CanType* can, BusEvent* update_event, uint8_t self_index, uint8_t rx_queue_capacity) :
rx_queue_(rx_queue_capacity), can_(can), error_cnt_(0), served_aborts_cnt_(0), update_event_(
update_event), peak_tx_mailbox_index_(0), self_index_(self_index), had_activity_(false), bitrate_(
0), initialized_(false)
@ -181,6 +182,11 @@ public:
*/
int init(const uint32_t bitrate, const OperatingMode mode);
void set_update_event(BusEvent* update_event)
{
update_event_ = update_event;
}
void handleTxInterrupt(uint64_t utc_usec);
void handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec);
@ -253,25 +259,22 @@ class PX4CANManager: public AP_HAL::CANManager {
BusEvent update_event_;
PX4CAN if0_;
PX4CAN if1_;
uint8_t can_number_;
virtual int16_t select(uavcan::CanSelectMasks& inout_masks,
const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline) override;
static void initOnce(uint8_t can_number);
void initOnce(uint8_t can_number);
bool initialized_;
static PX4CAN* ifaces[CAN_STM32_NUM_IFACES];
PX4CAN* ifaces[CAN_STM32_NUM_IFACES];
uint8_t _ifaces_num;
uint8_t _ifaces_out_to_in[CAN_STM32_NUM_IFACES];
AP_UAVCAN *p_uavcan;
public:
PX4CANManager() :
update_event_(*this), if0_(bxcan::Can[0], update_event_, 0, CAN_STM32_RX_QUEUE_SIZE), if1_(
bxcan::Can[1], update_event_, 1, CAN_STM32_RX_QUEUE_SIZE), can_number_(0), initialized_(
false), p_uavcan(nullptr)
{
uavcan::StaticAssert<(CAN_STM32_RX_QUEUE_SIZE <= PX4CAN::MaxRxQueueCapacity)>::check();
}
PX4CANManager();
/**
* This function returns select masks indicating which interfaces are available for read/write.
@ -290,13 +293,11 @@ public:
int init(const uint32_t bitrate, const PX4CAN::OperatingMode mode, uint8_t can_number);
virtual PX4CAN* getIface(uint8_t iface_index) override;
PX4CAN* getIface_out_to_in(uint8_t iface_index);
virtual uint8_t getNumIfaces() const override
{
if (can_number_ >= 2) {
return CAN_STM32_NUM_IFACES;
}
return 1;
return _ifaces_num;
}
/**
@ -308,8 +309,8 @@ public:
bool begin(uint32_t bitrate, uint8_t can_number) override;
bool is_initialized() override;
void initialized(bool val) override;
AP_UAVCAN *p_uavcan;
AP_UAVCAN *get_UAVCAN(void) override;
void set_UAVCAN(AP_UAVCAN *uavcan) override;
};

38
libraries/AP_HAL_PX4/RCOutput.cpp
View File

@ -16,6 +16,7 @@
#include <AP_BoardConfig/AP_BoardConfig.h>
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#endif
@ -489,34 +490,35 @@ void PX4RCOutput::_send_outputs(void)
}
}
if(AP_BoardConfig::get_can_enable() >= 1)
{
#if HAL_WITH_UAVCAN
if(hal.can_mgr != nullptr)
{
AP_UAVCAN *ap_uc = hal.can_mgr->get_UAVCAN();
if(ap_uc != nullptr)
if (AP_BoardConfig_CAN::get_can_num_ifaces() >= 1)
{
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr)
{
if(ap_uc->rc_out_sem_take())
AP_UAVCAN *ap_uc = hal.can_mgr[i]->get_UAVCAN();
if (ap_uc != nullptr)
{
for(uint8_t i = 0; i < _max_channel; i++)
if (ap_uc->rc_out_sem_take())
{
ap_uc->rco_write(_period[i], i);
}
for (uint8_t j = 0; j < _max_channel; j++)
{
ap_uc->rco_write(_period[j], j);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
ap_uc->rco_arm_actuators(true);
} else {
ap_uc->rco_arm_actuators(false);
}
if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
ap_uc->rco_arm_actuators(true);
} else {
ap_uc->rco_arm_actuators(false);
}
ap_uc->rc_out_sem_give();
ap_uc->rc_out_sem_give();
}
}
}
}
#endif // HAL_WITH_UAVCAN
}
#endif // HAL_WITH_UAVCAN
perf_end(_perf_rcout);
_last_output = now;

26
libraries/AP_HAL_PX4/Scheduler.cpp
View File

@ -104,9 +104,17 @@ void PX4Scheduler::create_uavcan_thread()
(void) pthread_attr_setschedparam(&thread_attr, &param);
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
pthread_create(&_uavcan_thread_ctx, &thread_attr, &PX4Scheduler::_uavcan_thread, this);
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
if (hal.can_mgr[i] != nullptr) {
if (hal.can_mgr[i]->get_UAVCAN() != nullptr) {
_uavcan_thread_arg *arg = new _uavcan_thread_arg;
arg->sched = this;
arg->uavcan_number = i;
printf("UAVCAN thread started\n\r");
pthread_create(&_uavcan_thread_ctx, &thread_attr, &PX4Scheduler::_uavcan_thread, arg);
}
}
}
#endif
}
@ -408,18 +416,21 @@ void *PX4Scheduler::_storage_thread(void *arg)
#if HAL_WITH_UAVCAN
void *PX4Scheduler::_uavcan_thread(void *arg)
{
PX4Scheduler *sched = (PX4Scheduler *) arg;
PX4Scheduler *sched = ((_uavcan_thread_arg *) arg)->sched;
uint8_t uavcan_number = ((_uavcan_thread_arg *) arg)->uavcan_number;
pthread_setname_np(pthread_self(), "apm_uavcan");
char name[15];
snprintf(name, sizeof(name), "apm_uavcan:%u", uavcan_number);
pthread_setname_np(pthread_self(), name);
while (!sched->_hal_initialized) {
poll(nullptr, 0, 1);
}
while (!_px4_thread_should_exit) {
if (((PX4CANManager *)hal.can_mgr)->is_initialized()) {
if (((PX4CANManager *)hal.can_mgr)->get_UAVCAN() != nullptr) {
(((PX4CANManager *)hal.can_mgr)->get_UAVCAN())->do_cyclic();
if (((PX4CANManager *)hal.can_mgr[uavcan_number])->is_initialized()) {
if (((PX4CANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN() != nullptr) {
(((PX4CANManager *)hal.can_mgr[uavcan_number])->get_UAVCAN())->do_cyclic();
} else {
sched->delay_microseconds_semaphore(10000);
}
@ -427,6 +438,7 @@ void *PX4Scheduler::_uavcan_thread(void *arg)
sched->delay_microseconds_semaphore(10000);
}
}
return nullptr;
}
#endif

5
libraries/AP_HAL_PX4/Scheduler.h
View File

@ -90,6 +90,11 @@ private:
pthread_t _uart_thread_ctx;
pthread_t _uavcan_thread_ctx;
struct _uavcan_thread_arg {
PX4Scheduler *sched;
uint8_t uavcan_number;
};
static void *_timer_thread(void *arg);
static void *_io_thread(void *arg);
static void *_storage_thread(void *arg);