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)
|
||||
{
|
||||
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) {
|
||||
PX4CAN* iface = ((PX4CANManager*) 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;
|
||||
}
|
||||
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)
|
||||
{
|
||||
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) {
|
||||
PX4CAN* iface = ((PX4CANManager*) 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;
|
||||
}
|
||||
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(
|
||||
uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
|
||||
{
|
||||
if (bs1_bs2_sum <= arg_bs1) {
|
||||
if (AP_BoardConfig_CAN::get_can_debug() >= 1) {
|
||||
AP_HAL::panic("PX4CAN::computeTimings bs1_bs2_sum <= arg_bs1");
|
||||
}
|
||||
if (bs1_bs2_sum <= arg_bs1 && (AP_BoardConfig_CAN::get_can_debug() >= 1)) {
|
||||
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)
|
||||
{
|
||||
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 PX4CAN::waitMsrINakBitStateChange(bool target_state)
|
||||
|
|
@ -601,7 +603,7 @@ void PX4CAN::handleTxInterrupt(const uint64_t utc_usec)
|
|||
handleTxMailboxInterrupt(2, txok, utc_usec);
|
||||
}
|
||||
|
||||
if(update_event_ != nullptr) {
|
||||
if (update_event_ != nullptr) {
|
||||
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)
|
||||
{
|
||||
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 PX4CAN::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,16 +803,17 @@ int32_t PX4CAN::available()
|
|||
|
||||
int32_t PX4CAN::tx_pending()
|
||||
{
|
||||
int32_t ret = -1;
|
||||
if (!initialized_) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (initialized_) {
|
||||
ret = 0;
|
||||
CriticalSectionLocker lock;
|
||||
int32_t ret = 0;
|
||||
|
||||
for (int mbx = 0; mbx < NumTxMailboxes; mbx++) {
|
||||
if (pending_tx_[mbx].pending) {
|
||||
ret++;
|
||||
}
|
||||
CriticalSectionLocker lock;
|
||||
|
||||
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;
|
||||
|
||||
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 (pending_tx[i] != nullptr) {
|
||||
if (ifaces[i]->canAcceptNewTxFrame(*pending_tx[i])) {
|
||||
msk.write |= 1 << i;
|
||||
}
|
||||
if (!ifaces[i]->isRxBufferEmpty()) {
|
||||
msk.read |= 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();
|
||||
|
||||
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 +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 res = -ErrNotImplemented;
|
||||
if (can_number >= CAN_STM32_NUM_IFACES) {
|
||||
return -ErrNotImplemented;
|
||||
}
|
||||
static bool initialized_once[CAN_STM32_NUM_IFACES];
|
||||
|
||||
if (can_number < CAN_STM32_NUM_IFACES) {
|
||||
res = 0;
|
||||
int 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) {
|
||||
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));
|
||||
printf("PX4CANManager::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("PX4CANManager::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("PX4CANManager::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("PX4CANManager::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("PX4CANManager::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("PX4CANManager::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("PX4CANManager::init CAN drv init OK, res = %d\n\r", res);
|
||||
}
|
||||
if (AP_BoardConfig_CAN::get_can_debug(can_number) >= 2) {
|
||||
printf("PX4CANManager::init CAN drv init OK, res = %d\n\r", res);
|
||||
}
|
||||
|
||||
return res;
|
||||
|
|
|
|||
|
|
@ -518,7 +518,7 @@ void PX4RCOutput::_send_outputs(void)
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
perf_end(_perf_rcout);
|
||||
_last_output = now;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -105,15 +105,17 @@ void PX4Scheduler::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, &PX4Scheduler::_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, &PX4Scheduler::_uavcan_thread, arg);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue