mirror of https://github.com/ArduPilot/ardupilot
AP_HAL_PX4: reducing indenting by linearizing the logic
This commit is contained in:
Eugene Shamaev
Tom Pittenger
parent
7dd8308a8c
commit
2a6387fd3b
|
|
@ -110,32 +110,36 @@ void BusEvent::signalFromInterrupt()
|
||||||
|
|
||||||
static void handleTxInterrupt(uint8_t iface_index)
|
static void handleTxInterrupt(uint8_t iface_index)
|
||||||
{
|
{
|
||||||
if (iface_index < CAN_STM32_NUM_IFACES) {
|
if (iface_index >= CAN_STM32_NUM_IFACES) {
|
||||||
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
return;
|
||||||
|
}
|
||||||
|
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
||||||
|
|
||||||
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
||||||
if (hal.can_mgr[i] != nullptr) {
|
if (hal.can_mgr[i] == nullptr) {
|
||||||
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
|
continue;
|
||||||
if (iface != nullptr) {
|
}
|
||||||
iface->handleTxInterrupt(utc_usec);
|
PX4CAN* iface = ((PX4CANManager*) 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)
|
static void handleRxInterrupt(uint8_t iface_index, uint8_t fifo_index)
|
||||||
{
|
{
|
||||||
if (iface_index < CAN_STM32_NUM_IFACES) {
|
if (iface_index >= CAN_STM32_NUM_IFACES) {
|
||||||
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
return;
|
||||||
|
}
|
||||||
|
uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
|
||||||
|
|
||||||
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
||||||
if (hal.can_mgr[i] != nullptr) {
|
if (hal.can_mgr[i] == nullptr) {
|
||||||
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
|
continue;
|
||||||
if (iface != nullptr) {
|
}
|
||||||
iface->handleRxInterrupt(fifo_index, utc_usec);
|
PX4CAN* iface = ((PX4CANManager*) hal.can_mgr[i])->getIface_out_to_in(iface_index);
|
||||||
}
|
if (iface != nullptr) {
|
||||||
}
|
iface->handleRxInterrupt(fifo_index, utc_usec);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -238,10 +242,8 @@ int PX4CAN::computeTimings(const uint32_t target_bitrate, Timings& out_timings)
|
||||||
bs1(arg_bs1), bs2(uint8_t(bs1_bs2_sum - bs1)), sample_point_permill(
|
bs1(arg_bs1), bs2(uint8_t(bs1_bs2_sum - bs1)), sample_point_permill(
|
||||||
uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
|
uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
|
||||||
{
|
{
|
||||||
if (bs1_bs2_sum <= arg_bs1) {
|
if (bs1_bs2_sum <= arg_bs1 && (AP_BoardConfig_CAN::get_can_debug() >= 1)) {
|
||||||
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
|
AP_HAL::panic("PX4CAN::computeTimings bs1_bs2_sum <= arg_bs1");
|
||||||
AP_HAL::panic("PX4CAN::computeTimings bs1_bs2_sum <= arg_bs1");
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -385,69 +387,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)
|
int16_t PX4CAN::configureFilters(const uavcan::CanFilterConfig* filter_configs, uint16_t num_configs)
|
||||||
{
|
{
|
||||||
if (num_configs <= NumFilters) {
|
if (num_configs > NumFilters) {
|
||||||
CriticalSectionLocker lock;
|
return -ErrFilterNumConfigs;
|
||||||
|
|
||||||
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;
|
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 PX4CAN::waitMsrINakBitStateChange(bool target_state)
|
bool PX4CAN::waitMsrINakBitStateChange(bool target_state)
|
||||||
|
|
@ -601,7 +603,7 @@ void PX4CAN::handleTxInterrupt(const uint64_t utc_usec)
|
||||||
handleTxMailboxInterrupt(2, txok, utc_usec);
|
handleTxMailboxInterrupt(2, txok, utc_usec);
|
||||||
}
|
}
|
||||||
|
|
||||||
if(update_event_ != nullptr) {
|
if (update_event_ != nullptr) {
|
||||||
update_event_->signalFromInterrupt();
|
update_event_->signalFromInterrupt();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -610,83 +612,85 @@ void PX4CAN::handleTxInterrupt(const uint64_t utc_usec)
|
||||||
|
|
||||||
void PX4CAN::handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec)
|
void PX4CAN::handleRxInterrupt(uint8_t fifo_index, uint64_t utc_usec)
|
||||||
{
|
{
|
||||||
if (fifo_index < 2) {
|
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();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
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 PX4CAN::pollErrorFlagsFromISR()
|
void PX4CAN::pollErrorFlagsFromISR()
|
||||||
{
|
{
|
||||||
const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
|
const uint8_t lec = uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
|
||||||
if (lec != 0) {
|
if (lec == 0) {
|
||||||
can_->ESR = 0;
|
return;
|
||||||
error_cnt_++;
|
}
|
||||||
|
can_->ESR = 0;
|
||||||
|
error_cnt_++;
|
||||||
|
|
||||||
// Serving abort requests
|
// Serving abort requests
|
||||||
for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please.
|
for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please.
|
||||||
TxItem& txi = pending_tx_[i];
|
TxItem& txi = pending_tx_[i];
|
||||||
if (txi.pending && txi.abort_on_error) {
|
if (txi.pending && txi.abort_on_error) {
|
||||||
can_->TSR = TSR_ABRQx[i];
|
can_->TSR = TSR_ABRQx[i];
|
||||||
txi.pending = false;
|
txi.pending = false;
|
||||||
served_aborts_cnt_++;
|
served_aborts_cnt_++;
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -799,16 +803,17 @@ int32_t PX4CAN::available()
|
||||||
|
|
||||||
int32_t PX4CAN::tx_pending()
|
int32_t PX4CAN::tx_pending()
|
||||||
{
|
{
|
||||||
int32_t ret = -1;
|
if (!initialized_) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
if (initialized_) {
|
int32_t ret = 0;
|
||||||
ret = 0;
|
|
||||||
CriticalSectionLocker lock;
|
|
||||||
|
|
||||||
for (int mbx = 0; mbx < NumTxMailboxes; mbx++) {
|
CriticalSectionLocker lock;
|
||||||
if (pending_tx_[mbx].pending) {
|
|
||||||
ret++;
|
for (int mbx = 0; mbx < NumTxMailboxes; mbx++) {
|
||||||
}
|
if (pending_tx_[mbx].pending) {
|
||||||
|
ret++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -835,15 +840,17 @@ uavcan::CanSelectMasks PX4CANManager::makeSelectMasks(const uavcan::CanFrame* (&
|
||||||
uavcan::CanSelectMasks msk;
|
uavcan::CanSelectMasks msk;
|
||||||
|
|
||||||
for (uint8_t i = 0; i < _ifaces_num; i++) {
|
for (uint8_t i = 0; i < _ifaces_num; i++) {
|
||||||
if (ifaces[i] != nullptr) {
|
if (ifaces[i] == nullptr) {
|
||||||
if (!ifaces[i]->isRxBufferEmpty()) {
|
continue;
|
||||||
msk.read |= 1 << i;
|
}
|
||||||
}
|
|
||||||
|
|
||||||
if (pending_tx[i] != nullptr) {
|
if (!ifaces[i]->isRxBufferEmpty()) {
|
||||||
if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) {
|
msk.read |= 1 << i;
|
||||||
msk.write |= 1 << i;
|
}
|
||||||
}
|
|
||||||
|
if (pending_tx[i] != nullptr) {
|
||||||
|
if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) {
|
||||||
|
msk.write |= 1 << i;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -871,12 +878,13 @@ int16_t PX4CANManager::select(uavcan::CanSelectMasks& inout_masks,
|
||||||
const uavcan::MonotonicTime time = clock::getMonotonic();
|
const uavcan::MonotonicTime time = clock::getMonotonic();
|
||||||
|
|
||||||
for (uint8_t i = 0; i < _ifaces_num; i++) {
|
for (uint8_t i = 0; i < _ifaces_num; i++) {
|
||||||
if (ifaces[i] != nullptr) {
|
if (ifaces[i] == nullptr) {
|
||||||
ifaces[i]->discardTimedOutTxMailboxes(time);
|
continue;
|
||||||
{
|
}
|
||||||
CriticalSectionLocker cs_locker;
|
ifaces[i]->discardTimedOutTxMailboxes(time);
|
||||||
ifaces[i]->pollErrorFlagsFromISR();
|
{
|
||||||
}
|
CriticalSectionLocker cs_locker;
|
||||||
|
ifaces[i]->pollErrorFlagsFromISR();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -951,62 +959,62 @@ void PX4CANManager::initOnce(uint8_t can_number)
|
||||||
|
|
||||||
int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode, uint8_t can_number)
|
int PX4CANManager::init(const uint32_t bitrate, const PX4CAN::OperatingMode mode, uint8_t can_number)
|
||||||
{
|
{
|
||||||
int res = -ErrNotImplemented;
|
if (can_number >= CAN_STM32_NUM_IFACES) {
|
||||||
|
return -ErrNotImplemented;
|
||||||
|
}
|
||||||
static bool initialized_once[CAN_STM32_NUM_IFACES];
|
static bool initialized_once[CAN_STM32_NUM_IFACES];
|
||||||
|
|
||||||
if (can_number < CAN_STM32_NUM_IFACES) {
|
int res = 0;
|
||||||
res = 0;
|
|
||||||
|
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));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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) {
|
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),
|
printf("PX4CANManager::init First initialization bus %d\n\r", static_cast<int>(can_number));
|
||||||
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
|
initOnce(can_number);
|
||||||
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));
|
* CAN1
|
||||||
}
|
*/
|
||||||
|
if (can_number == 0) {
|
||||||
initOnce(can_number);
|
if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) {
|
||||||
}
|
printf("PX4CANManager::init Initing iface 0...\n\r");
|
||||||
|
|
||||||
/*
|
|
||||||
* CAN1
|
|
||||||
*/
|
|
||||||
if (can_number == 0) {
|
|
||||||
if (AP_BoardConfig_CAN::get_can_debug(0) >= 2) {
|
|
||||||
printf("PX4CANManager::init Initing iface 0...\n\r");
|
|
||||||
}
|
|
||||||
ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first,
|
|
||||||
}
|
}
|
||||||
|
ifaces[_ifaces_out_to_in[can_number]] = &if0_; // This link must be initialized first,
|
||||||
|
}
|
||||||
|
|
||||||
#if CAN_STM32_NUM_IFACES > 1
|
#if CAN_STM32_NUM_IFACES > 1
|
||||||
/*
|
/*
|
||||||
* CAN2
|
* CAN2
|
||||||
*/
|
*/
|
||||||
if (can_number == 1) {
|
if (can_number == 1) {
|
||||||
if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) {
|
if (AP_BoardConfig_CAN::get_can_debug(1) >= 2) {
|
||||||
printf("PX4CANManager::init Initing iface 1...\n\r");
|
printf("PX4CANManager::init Initing iface 1...\n\r");
|
||||||
}
|
|
||||||
ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here.
|
|
||||||
}
|
}
|
||||||
|
ifaces[_ifaces_out_to_in[can_number]] = &if1_; // Same thing here.
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
ifaces[_ifaces_out_to_in[can_number]]->set_update_event(&update_event_);
|
ifaces[_ifaces_out_to_in[can_number]]->set_update_event(&update_event_);
|
||||||
res = ifaces[_ifaces_out_to_in[can_number]]->init(bitrate, mode);
|
res = ifaces[_ifaces_out_to_in[can_number]]->init(bitrate, mode);
|
||||||
if (res < 0) {
|
if (res < 0) {
|
||||||
ifaces[_ifaces_out_to_in[can_number]] = nullptr;
|
ifaces[_ifaces_out_to_in[can_number]] = nullptr;
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
|
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
|
||||||
printf("PX4CANManager::init CAN drv init OK, res = %d\n\r", res);
|
printf("PX4CANManager::init CAN drv init OK, res = %d\n\r", res);
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return res;
|
return res;
|
||||||
|
|
|
||||||
|
|
@ -105,15 +105,17 @@ void PX4Scheduler::create_uavcan_thread()
|
||||||
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
|
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
|
||||||
|
|
||||||
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
|
||||||
if (hal.can_mgr[i] != nullptr) {
|
if (hal.can_mgr[i] == nullptr) {
|
||||||
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, &PX4Scheduler::_uavcan_thread, arg);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
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, &PX4Scheduler::_uavcan_thread, arg);
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue