zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_HAL_VRBRAIN: reducing indenting by linearizing the logic

mission-4.1.18
Eugene Shamaev 7 years ago committed by Tom Pittenger
parent
2a6387fd3b
commit
b190b6793b
2 changed files with 226 additions and 210 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 417
      libraries/AP_HAL_VRBRAIN/CAN.cpp
  2. 19
      libraries/AP_HAL_VRBRAIN/Scheduler.cpp

417
libraries/AP_HAL_VRBRAIN/CAN.cpp
Unescape View File

@ -110,32 +110,36 @@ void BusEvent::signalFromInterrupt() @@ -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& @@ -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;
if (num_configs > NumFilters) {
return -ErrFilterNumConfigs;
}
can_->FMR |= bxcan::FMR_FINIT;
CriticalSectionLocker lock;
// Slave (CAN2) gets half of the filters
can_->FMR = (can_->FMR & ~0x00003F00) | static_cast<uint32_t>(NumFilters) << 8;
can_->FMR |= bxcan::FMR_FINIT;
can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters
can_->FM1R = 0; // Identifier Mask mode
can_->FS1R = 0x7ffffff; // Single 32-bit for all
// Slave (CAN2) gets half of the filters
can_->FMR = (can_->FMR & ~0x00003F00) | static_cast<uint32_t>(NumFilters) << 8;
const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters;
can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters
can_->FM1R = 0; // Identifier Mask mode
can_->FS1R = 0x7ffffff; // Single 32-bit for all
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));
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 {
can_->FA1R &= ~(1 << (filter_start_index + i));
id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21;
mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21;
}
}
}
can_->FMR &= ~bxcan::FMR_FINIT;
if (cfg->id & uavcan::CanFrame::FlagRTR) {
id |= bxcan::RIR_RTR;
}
return 0;
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));
}
}
}
return -ErrFilterNumConfigs;
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) @@ -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;
if (mailbox_index >= NumTxMailboxes) {
return;
}
TxItem& txi = pending_tx_[mailbox_index];
had_activity_ = had_activity_ || txok;
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);
}
TxItem& txi = pending_tx_[mailbox_index];
txi.pending = false;
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) @@ -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;
}
if (fifo_index >= 2) {
return;
}
/*
* Register overflow as a hardware error
*/
if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
error_cnt_++;
}
volatile uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) {
return;
}
/*
* Read the frame contents
*/
uavcan::CanFrame frame;
const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index];
/*
* Register overflow as a hardware error
*/
if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
error_cnt_++;
}
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;
}
/*
* Read the frame contents
*/
uavcan::CanFrame frame;
const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index];
if ((rf.RIR & bxcan::RIR_RTR) != 0) {
frame.id |= uavcan::CanFrame::FlagRTR;
}
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;
}
frame.dlc = rf.RDTR & 15;
if ((rf.RIR & bxcan::RIR_RTR) != 0) {
frame.id |= uavcan::CanFrame::FlagRTR;
}
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));
frame.dlc = rf.RDTR & 15;
*rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read
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));
/*
* 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);
*rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read
had_activity_ = true;
if(update_event_ != nullptr) {
update_event_->signalFromInterrupt();
}
/*
* 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);
pollErrorFlagsFromISR();
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() @@ -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() @@ -811,7 +820,6 @@ int32_t VRBRAINCAN::tx_pending()
}
}
}
return ret;
}
@ -835,16 +843,19 @@ uavcan::CanSelectMasks VRBRAINCANManager::makeSelectMasks(const uavcan::CanFrame @@ -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, @@ -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) @@ -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;
}
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));
}
int res = 0;
// 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));
}
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
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));
}
/*
* 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,
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,
}
#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;

19
libraries/AP_HAL_VRBRAIN/Scheduler.cpp
Unescape View File

@ -105,15 +105,18 @@ void VRBRAINScheduler::create_uavcan_thread() @@ -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
}

Write Preview
Loading…
Cancel
Save