aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_ChibiOS: reduced time in critical sections for CAN processing

Browse Source
This commit is contained in:
Andrew Tridgell 2022-02-14 09:04:14 +11:00
parent 88e7efcf9c
commit fd8d28e23c
2 changed files with 111 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -326,72 +326,70 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
return -1; return -1;
} }
CriticalSectionLocker lock; {
CriticalSectionLocker lock;
/* /*
* Seeking for an empty slot * Seeking for an empty slot
*/ */
uint8_t index; uint8_t index;
if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) { if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) {
stats.tx_rejected++; stats.tx_rejected++;
return 0; //we don't have free space return 0; //we don't have free space
}
index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
// Copy Frame to RAM
// Calculate Tx element address
uint32_t* buffer = (uint32_t *)(MessageRam_.TxFIFOQSA + (index * FDCAN_FRAME_BUFFER_SIZE * 4));
//Setup Frame ID
if (frame.isExtended()) {
buffer[0] = (IDE | frame.id);
} else {
buffer[0] = (frame.id << 18);
}
if (frame.isRemoteTransmissionRequest()) {
buffer[0] |= RTR;
}
//Write Data Length Code, and Message Marker
buffer[1] = frame.dlc << 16 | index << 24;
// Write Frame to the message RAM
buffer[2] = frame.data_32[0];
buffer[3] = frame.data_32[1];
//Set Add Request
can_->TXBAR = (1 << index);
//Registering the pending transmission so we can track its deadline and loopback it as needed
pending_tx_[index].deadline = tx_deadline;
pending_tx_[index].frame = frame;
pending_tx_[index].loopback = (flags & AP_HAL::CANIface::Loopback) != 0;
pending_tx_[index].abort_on_error = (flags & AP_HAL::CANIface::AbortOnError) != 0;
pending_tx_[index].index = index;
// setup frame initial state
pending_tx_[index].aborted = false;
pending_tx_[index].setup = true;
pending_tx_[index].pushed = false;
} }
index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
// Copy Frame to RAM
// Calculate Tx element address
uint32_t* buffer = (uint32_t *)(MessageRam_.TxFIFOQSA + (index * FDCAN_FRAME_BUFFER_SIZE * 4));
//Setup Frame ID
if (frame.isExtended()) {
buffer[0] = (IDE | frame.id);
} else {
buffer[0] = (frame.id << 18);
}
if (frame.isRemoteTransmissionRequest()) {
buffer[0] |= RTR;
}
//Write Data Length Code, and Message Marker
buffer[1] = frame.dlc << 16 | index << 24;
// Write Frame to the message RAM
buffer[2] = (uint32_t(frame.data[3]) << 24) |
(uint32_t(frame.data[2]) << 16) |
(uint32_t(frame.data[1]) << 8) |
(uint32_t(frame.data[0]) << 0);
buffer[3] = (uint32_t(frame.data[7]) << 24) |
(uint32_t(frame.data[6]) << 16) |
(uint32_t(frame.data[5]) << 8) |
(uint32_t(frame.data[4]) << 0);
//Set Add Request
can_->TXBAR = (1 << index);
//Registering the pending transmission so we can track its deadline and loopback it as needed
pending_tx_[index].deadline = tx_deadline;
pending_tx_[index].frame = frame;
pending_tx_[index].loopback = (flags & AP_HAL::CANIface::Loopback) != 0;
pending_tx_[index].abort_on_error = (flags & AP_HAL::CANIface::AbortOnError) != 0;
pending_tx_[index].index = index;
// setup frame initial state
pending_tx_[index].aborted = false;
pending_tx_[index].setup = true;
pending_tx_[index].pushed = false;
return AP_HAL::CANIface::send(frame, tx_deadline, flags); return AP_HAL::CANIface::send(frame, tx_deadline, flags);
} }
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags) int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
{ {
CriticalSectionLocker lock; {
CanRxItem rx_item; CriticalSectionLocker lock;
if (!rx_queue_.pop(rx_item) || !initialised_) { CanRxItem rx_item;
return 0; if (!rx_queue_.pop(rx_item) || !initialised_) {
return 0;
}
out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags;
} }
out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags;
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags); return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
} }

116
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -308,74 +308,72 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
* - It takes CPU time. Not just CPU time, but critical section time, which is expensive. * - It takes CPU time. Not just CPU time, but critical section time, which is expensive.
*/ */
CriticalSectionLocker lock; {
CriticalSectionLocker lock;
/* /*
* Seeking for an empty slot * Seeking for an empty slot
*/ */
uint8_t txmailbox = 0xFF; uint8_t txmailbox = 0xFF;
if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) { if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) {
txmailbox = 0; txmailbox = 0;
} else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) { } else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) {
txmailbox = 1; txmailbox = 1;
} else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) { } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
txmailbox = 2; txmailbox = 2;
} else { } else {
PERF_STATS(stats.tx_rejected); PERF_STATS(stats.tx_rejected);
return 0; // No transmission for you. return 0; // No transmission for you.
}
/*
* Setting up the mailbox
*/
bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox];
if (frame.isExtended()) {
mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
} else {
mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskStdID) << 21);
}
if (frame.isRemoteTransmissionRequest()) {
mb.TIR |= bxcan::TIR_RTR;
}
mb.TDTR = frame.dlc;
mb.TDHR = frame.data_32[1];
mb.TDLR = frame.data_32[0];
mb.TIR |= bxcan::TIR_TXRQ; // Go.
/*
* Registering the pending transmission so we can track its deadline and loopback it as needed
*/
CanTxItem& txi = pending_tx_[txmailbox];
txi.deadline = tx_deadline;
txi.frame = frame;
txi.loopback = (flags & Loopback) != 0;
txi.abort_on_error = (flags & AbortOnError) != 0;
// setup frame initial state
txi.pushed = false;
} }
/*
* Setting up the mailbox
*/
bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox];
if (frame.isExtended()) {
mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
} else {
mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskStdID) << 21);
}
if (frame.isRemoteTransmissionRequest()) {
mb.TIR |= bxcan::TIR_RTR;
}
mb.TDTR = frame.dlc;
mb.TDHR = (uint32_t(frame.data[7]) << 24) |
(uint32_t(frame.data[6]) << 16) |
(uint32_t(frame.data[5]) << 8) |
(uint32_t(frame.data[4]) << 0);
mb.TDLR = (uint32_t(frame.data[3]) << 24) |
(uint32_t(frame.data[2]) << 16) |
(uint32_t(frame.data[1]) << 8) |
(uint32_t(frame.data[0]) << 0);
mb.TIR |= bxcan::TIR_TXRQ; // Go.
/*
* Registering the pending transmission so we can track its deadline and loopback it as needed
*/
CanTxItem& txi = pending_tx_[txmailbox];
txi.deadline = tx_deadline;
txi.frame = frame;
txi.loopback = (flags & Loopback) != 0;
txi.abort_on_error = (flags & AbortOnError) != 0;
// setup frame initial state
txi.pushed = false;
return AP_HAL::CANIface::send(frame, tx_deadline, flags); return AP_HAL::CANIface::send(frame, tx_deadline, flags);
} }
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags) int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
{ {
CriticalSectionLocker lock; {
CanRxItem rx_item; CriticalSectionLocker lock;
if (!rx_queue_.pop(rx_item)) { CanRxItem rx_item;
return 0; if (!rx_queue_.pop(rx_item)) {
return 0;
}
out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags;
} }
out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags;
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags); return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
} }