mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
AP_HAL_VRBRAIN: reducing indenting by linearizing the logic
This commit is contained in:
Eugene Shamaev
Tom Pittenger
parent
2a6387fd3b
commit
b190b6793b
@ -110,32 +110,36 @@ void BusEvent::signalFromInterrupt()
|
||||
|
||||
static void handleTxInterrupt(uint8_t iface_index)
|
||||
{
|
||||
if (iface_index < CAN_STM32_NUM_IFACES) {
|
||||
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
||||
if (iface_index >= CAN_STM32_NUM_IFACES) {
|
||||
return;
|
||||
}
|
||||
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
||||
|
||||
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
||||
if (hal.can_mgr[i] != nullptr) {
|
||||
VRBRAINCAN* iface = ((VRBRAINCANManager*) hal.can_mgr[i])->getIface_out_to_in(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) {
|
||||
continue;
|
||||
}
|
||||
VRBRAINCAN* iface = ((VRBRAINCANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
|
||||
if (iface != nullptr) {
|
||||
iface->handleTxInterrupt(utc_usec);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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 (iface_index >= CAN_STM32_NUM_IFACES) {
|
||||
return;
|
||||
}
|
||||
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
||||
|
||||
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
||||
if (hal.can_mgr[i] != nullptr) {
|
||||
VRBRAINCAN* iface = ((VRBRAINCANManager*) hal.can_mgr[i])->getIface_out_to_in(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) {
|
||||
continue;
|
||||
}
|
||||
VRBRAINCAN* iface = ((VRBRAINCANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
|
||||
if (iface != nullptr) {
|
||||
iface->handleRxInterrupt(fifo_index, utc_usec);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -385,69 +389,69 @@ int16_t VRBRAINCAN::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime&
|
||||
|
||||
int16_t VRBRAINCAN::configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs)
|
||||
{
|
||||
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;
|
||||
if (num_configs > NumFilters) {
|
||||
return -ErrFilterNumConfigs;
|
||||
}
|
||||
|
||||
return -ErrFilterNumConfigs;
|
||||
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;
|
||||
}
|
||||
|
||||
bool VRBRAINCAN::waitMsrINakBitStateChange(bool target_state)
|
||||
@ -564,22 +568,23 @@ int VRBRAINCAN::init(const uint32_t bitrate, const OperatingMode mode)
|
||||
|
||||
void VRBRAINCAN::handleTxMailboxInterrupt(uint8_t mailbox_index, bool txok, const uint64_t utc_usec)
|
||||
{
|
||||
if (mailbox_index < NumTxMailboxes) {
|
||||
|
||||
had_activity_ = had_activity_ || txok;
|
||||
|
||||
TxItem& txi = pending_tx_[mailbox_index];
|
||||
|
||||
if (txi.loopback && txok && txi.pending) {
|
||||
CanRxItem frm;
|
||||
frm.frame = txi.frame;
|
||||
frm.flags = uavcan::CanIOFlagLoopback;
|
||||
frm.utc_usec = utc_usec;
|
||||
rx_queue_.push(frm);
|
||||
}
|
||||
|
||||
txi.pending = false;
|
||||
if (mailbox_index >= NumTxMailboxes) {
|
||||
return;
|
||||
}
|
||||
|
||||
had_activity_ = had_activity_ || txok;
|
||||
|
||||
TxItem& txi = pending_tx_[mailbox_index];
|
||||
|
||||
if (txi.loopback && txok && txi.pending) {
|
||||
CanRxItem frm;
|
||||
frm.frame = txi.frame;
|
||||
frm.flags = uavcan::CanIOFlagLoopback;
|
||||
frm.utc_usec = utc_usec;
|
||||
rx_queue_.push(frm);
|
||||
}
|
||||
|
||||
txi.pending = false;
|
||||
}
|
||||
|
||||
void VRBRAINCAN::handleTxInterrupt(const uint64_t utc_usec)
|
||||
@ -610,83 +615,85 @@ void VRBRAINCAN::handleTxInterrupt(const uint64_t utc_usec)
|
||||
|
||||
void VRBRAINCAN::handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec)
|
||||
{
|
||||
if (fifo_index < 2) {
|
||||
|
||||
volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
|
||||
if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Register overflow as a hardware error
|
||||
*/
|
||||
if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
|
||||
error_cnt_++;
|
||||
}
|
||||
|
||||
/*
|
||||
* Read the frame contents
|
||||
*/
|
||||
uavcan::CanFrame frame;
|
||||
const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index];
|
||||
|
||||
if ((rf.RIR & bxcan::RIR_IDE) == 0) {
|
||||
frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21);
|
||||
} else {
|
||||
frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3);
|
||||
frame.id |= uavcan::CanFrame::FlagEFF;
|
||||
}
|
||||
|
||||
if ((rf.RIR & bxcan::RIR_RTR) != 0) {
|
||||
frame.id |= uavcan::CanFrame::FlagRTR;
|
||||
}
|
||||
|
||||
frame.dlc = rf.RDTR & 15;
|
||||
|
||||
frame.data[0] = uint8_t(0xFF & (rf.RDLR >> 0));
|
||||
frame.data[1] = uint8_t(0xFF & (rf.RDLR >> 8));
|
||||
frame.data[2] = uint8_t(0xFF & (rf.RDLR >> 16));
|
||||
frame.data[3] = uint8_t(0xFF & (rf.RDLR >> 24));
|
||||
frame.data[4] = uint8_t(0xFF & (rf.RDHR >> 0));
|
||||
frame.data[5] = uint8_t(0xFF & (rf.RDHR >> 8));
|
||||
frame.data[6] = uint8_t(0xFF & (rf.RDHR >> 16));
|
||||
frame.data[7] = uint8_t(0xFF & (rf.RDHR >> 24));
|
||||
|
||||
*rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read
|
||||
|
||||
/*
|
||||
* Store with timeout into the FIFO buffer and signal update event
|
||||
*/
|
||||
CanRxItem frm;
|
||||
frm.frame = frame;
|
||||
frm.flags = 0;
|
||||
frm.utc_usec = utc_usec;
|
||||
rx_queue_.push(frm);
|
||||
|
||||
had_activity_ = true;
|
||||
if(update_event_ != nullptr) {
|
||||
update_event_->signalFromInterrupt();
|
||||
}
|
||||
|
||||
pollErrorFlagsFromISR();
|
||||
if (fifo_index >= 2) {
|
||||
return;
|
||||
}
|
||||
|
||||
volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
|
||||
if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* Register overflow as a hardware error
|
||||
*/
|
||||
if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
|
||||
error_cnt_++;
|
||||
}
|
||||
|
||||
/*
|
||||
* Read the frame contents
|
||||
*/
|
||||
uavcan::CanFrame frame;
|
||||
const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index];
|
||||
|
||||
if ((rf.RIR & bxcan::RIR_IDE) == 0) {
|
||||
frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21);
|
||||
} else {
|
||||
frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3);
|
||||
frame.id |= uavcan::CanFrame::FlagEFF;
|
||||
}
|
||||
|
||||
if ((rf.RIR & bxcan::RIR_RTR) != 0) {
|
||||
frame.id |= uavcan::CanFrame::FlagRTR;
|
||||
}
|
||||
|
||||
frame.dlc = rf.RDTR & 15;
|
||||
|
||||
frame.data[0] = uint8_t(0xFF & (rf.RDLR >> 0));
|
||||
frame.data[1] = uint8_t(0xFF & (rf.RDLR >> 8));
|
||||
frame.data[2] = uint8_t(0xFF & (rf.RDLR >> 16));
|
||||
frame.data[3] = uint8_t(0xFF & (rf.RDLR >> 24));
|
||||
frame.data[4] = uint8_t(0xFF & (rf.RDHR >> 0));
|
||||
frame.data[5] = uint8_t(0xFF & (rf.RDHR >> 8));
|
||||
frame.data[6] = uint8_t(0xFF & (rf.RDHR >> 16));
|
||||
frame.data[7] = uint8_t(0xFF & (rf.RDHR >> 24));
|
||||
|
||||
*rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read
|
||||
|
||||
/*
|
||||
* Store with timeout into the FIFO buffer and signal update event
|
||||
*/
|
||||
CanRxItem frm;
|
||||
frm.frame = frame;
|
||||
frm.flags = 0;
|
||||
frm.utc_usec = utc_usec;
|
||||
rx_queue_.push(frm);
|
||||
|
||||
had_activity_ = true;
|
||||
if(update_event_ != nullptr) {
|
||||
update_event_->signalFromInterrupt();
|
||||
}
|
||||
|
||||
pollErrorFlagsFromISR();
|
||||
}
|
||||
|
||||
void VRBRAINCAN::pollErrorFlagsFromISR()
|
||||
{
|
||||
const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
|
||||
if (lec != 0) {
|
||||
can_->ESR = 0;
|
||||
error_cnt_++;
|
||||
if (lec == 0) {
|
||||
return;
|
||||
}
|
||||
can_->ESR = 0;
|
||||
error_cnt_++;
|
||||
|
||||
// Serving abort requests
|
||||
for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please.
|
||||
TxItem& txi = pending_tx_[i];
|
||||
if (txi.pending && txi.abort_on_error) {
|
||||
can_->TSR = TSR_ABRQx[i];
|
||||
txi.pending = false;
|
||||
served_aborts_cnt_++;
|
||||
}
|
||||
// Serving abort requests
|
||||
for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please.
|
||||
TxItem& txi = pending_tx_[i];
|
||||
if (txi.pending && txi.abort_on_error) {
|
||||
can_->TSR = TSR_ABRQx[i];
|
||||
txi.pending = false;
|
||||
served_aborts_cnt_++;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -799,10 +806,12 @@ int32_t VRBRAINCAN::available()
|
||||
|
||||
int32_t VRBRAINCAN::tx_pending()
|
||||
{
|
||||
int32_t ret = -1;
|
||||
if (!initialized_) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (initialized_) {
|
||||
ret = 0;
|
||||
int32_t ret = 0;
|
||||
{
|
||||
CriticalSectionLocker lock;
|
||||
|
||||
for (int mbx = 0; mbx < NumTxMailboxes; mbx++) {
|
||||
@ -811,7 +820,6 @@ int32_t VRBRAINCAN::tx_pending()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
@ -835,16 +843,19 @@ uavcan::CanSelectMasks VRBRAINCANManager::makeSelectMasks(const uavcan::CanFrame
|
||||
uavcan::CanSelectMasks msk;
|
||||
|
||||
for (uint8_t i = 0; i < _ifaces_num; i++) {
|
||||
if (ifaces[i] != nullptr) {
|
||||
if (!ifaces[i]->isRxBufferEmpty()) {
|
||||
msk.read |= 1 << i;
|
||||
}
|
||||
if (ifaces[i] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
if (!ifaces[i]->isRxBufferEmpty()) {
|
||||
msk.read |= 1 << i;
|
||||
}
|
||||
|
||||
if (pending_tx[i] != nullptr) {
|
||||
if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) {
|
||||
msk.write |= 1 << i;
|
||||
}
|
||||
}
|
||||
if (pending_tx[i] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) {
|
||||
msk.write |= 1 << i;
|
||||
}
|
||||
}
|
||||
|
||||
@ -871,12 +882,13 @@ int16_t VRBRAINCANManager::select(uavcan::CanSelectMasks& inout_masks,
|
||||
const uavcan::MonotonicTime time = clock::getMonotonic();
|
||||
|
||||
for (uint8_t i = 0; i < _ifaces_num; i++) {
|
||||
if (ifaces[i] != nullptr) {
|
||||
ifaces[i]->discardTimedOutTxMailboxes(time);
|
||||
{
|
||||
CriticalSectionLocker cs_locker;
|
||||
ifaces[i]->pollErrorFlagsFromISR();
|
||||
}
|
||||
if (ifaces[i] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
ifaces[i]->discardTimedOutTxMailboxes(time);
|
||||
{
|
||||
CriticalSectionLocker cs_locker;
|
||||
ifaces[i]->pollErrorFlagsFromISR();
|
||||
}
|
||||
}
|
||||
|
||||
@ -951,62 +963,63 @@ void VRBRAINCANManager::initOnce(uint8_t can_number)
|
||||
|
||||
int VRBRAINCANManager::init(const uint32_t bitrate, const VRBRAINCAN::OperatingMode mode, uint8_t can_number)
|
||||
{
|
||||
int res = -ErrNotImplemented;
|
||||
static bool initialized_once[CAN_STM32_NUM_IFACES];
|
||||
|
||||
if (can_number < CAN_STM32_NUM_IFACES) {
|
||||
res = 0;
|
||||
if (can_number >= CAN_STM32_NUM_IFACES) {
|
||||
return -ErrNotImplemented;
|
||||
}
|
||||
|
||||
int res = 0;
|
||||
|
||||
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
|
||||
printf("VRBRAINCANManager::init Bitrate %lu mode %d bus %d\n\r", static_cast<unsigned long>(bitrate),
|
||||
static_cast<int>(mode), static_cast<int>(can_number));
|
||||
}
|
||||
|
||||
// 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("VRBRAINCANManager::init Bitrate %lu mode %d bus %d\n\r", static_cast<unsigned long>(bitrate),
|
||||
static_cast<int>(mode), static_cast<int>(can_number));
|
||||
printf("VRBRAINCANManager::init First initialization bus %d\n\r", static_cast<int>(can_number));
|
||||
}
|
||||
|
||||
// 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;
|
||||
initOnce(can_number);
|
||||
}
|
||||
|
||||
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
|
||||
printf("VRBRAINCANManager::init First initialization bus %d\n\r", static_cast<int>(can_number));
|
||||
}
|
||||
|
||||
initOnce(can_number);
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN1
|
||||
*/
|
||||
if (can_number == 0) {
|
||||
if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) {
|
||||
printf("VRBRAINCANManager::init Initing iface 0...\n\r");
|
||||
}
|
||||
ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first,
|
||||
/*
|
||||
* CAN1
|
||||
*/
|
||||
if (can_number == 0) {
|
||||
if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) {
|
||||
printf("VRBRAINCANManager::init Initing iface 0...\n\r");
|
||||
}
|
||||
ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first,
|
||||
}
|
||||
|
||||
#if CAN_STM32_NUM_IFACES > 1
|
||||
/*
|
||||
* CAN2
|
||||
*/
|
||||
if (can_number == 1) {
|
||||
if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) {
|
||||
printf("VRBRAINCANManager::init Initing iface 1...\n\r");
|
||||
}
|
||||
ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here.
|
||||
/*
|
||||
* CAN2
|
||||
*/
|
||||
if (can_number == 1) {
|
||||
if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) {
|
||||
printf("VRBRAINCANManager::init Initing iface 1...\n\r");
|
||||
}
|
||||
ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here.
|
||||
}
|
||||
#endif
|
||||
|
||||
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;
|
||||
}
|
||||
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("VRBRAINCANManager::init CAN drv init OK, res = %d\n\r", res);
|
||||
}
|
||||
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
|
||||
printf("VRBRAINCANManager::init CAN drv init OK, res = %d\n\r", res);
|
||||
}
|
||||
|
||||
return res;
|
||||
|
||||
@ -105,15 +105,18 @@ void VRBRAINScheduler::create_uavcan_thread()
|
||||
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
|
||||
|
||||
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;
|
||||
|
||||
pthread_create(&_uavcan_thread_ctx, &thread_attr, &VRBRAINScheduler::_uavcan_thread, arg);
|
||||
}
|
||||
if (hal.can_mgr[i] == nullptr) {
|
||||
continue;
|
||||
}
|
||||
if (hal.can_mgr[i]->get_UAVCAN() == nullptr) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_uavcan_thread_arg *arg = new _uavcan_thread_arg;
|
||||
arg->sched = this;
|
||||
arg->uavcan_number = i;
|
||||
|
||||
pthread_create(&_uavcan_thread_ctx, &thread_attr, &VRBRAINScheduler::_uavcan_thread, arg);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user