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

HAL_ChibiOS: reduced time in critical sections for CAN processing

gps-1.3.1
Andrew Tridgell 3 years ago
parent
88e7efcf9c
commit
fd8d28e23c
2 changed files with 104 additions and 108 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 106
      libraries/AP_HAL_ChibiOS/CANFDIface.cpp
  2. 106
      libraries/AP_HAL_ChibiOS/CanIface.cpp

106
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
Unescape 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); index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
// Copy Frame to RAM // Copy Frame to RAM
// Calculate Tx element address // Calculate Tx element address
uint32_t* buffer = (uint32_t *)(MessageRam_.TxFIFOQSA + (index * FDCAN_FRAME_BUFFER_SIZE * 4)); uint32_t* buffer = (uint32_t *)(MessageRam_.TxFIFOQSA + (index * FDCAN_FRAME_BUFFER_SIZE * 4));
//Setup Frame ID //Setup Frame ID
if (frame.isExtended()) { if (frame.isExtended()) {
buffer[0] = (IDE | frame.id); buffer[0] = (IDE | frame.id);
} else { } else {
buffer[0] = (frame.id << 18); buffer[0] = (frame.id << 18);
} }
if (frame.isRemoteTransmissionRequest()) { if (frame.isRemoteTransmissionRequest()) {
buffer[0] |= RTR; 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;
} }
//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);
} }

106
libraries/AP_HAL_ChibiOS/CanIface.cpp
Unescape 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 * Setting up the mailbox
*/ */
bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox]; bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox];
if (frame.isExtended()) { if (frame.isExtended()) {
mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskExtID) << 3) | bxcan::TIR_IDE; mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
} else { } else {
mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskStdID) << 21); mb.TIR = ((frame.id & AP_HAL::CANFrame::MaskStdID) << 21);
} }
if (frame.isRemoteTransmissionRequest()) { if (frame.isRemoteTransmissionRequest()) {
mb.TIR |= bxcan::TIR_RTR; mb.TIR |= bxcan::TIR_RTR;
} }
mb.TDTR = frame.dlc; mb.TDTR = frame.dlc;
mb.TDHR = (uint32_t(frame.data[7]) << 24) | mb.TDHR = frame.data_32[1];
(uint32_t(frame.data[6]) << 16) | mb.TDLR = frame.data_32[0];
(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. mb.TIR |= bxcan::TIR_TXRQ; // Go.
/* /*
* Registering the pending transmission so we can track its deadline and loopback it as needed * Registering the pending transmission so we can track its deadline and loopback it as needed
*/ */
CanTxItem& txi = pending_tx_[txmailbox]; CanTxItem& txi = pending_tx_[txmailbox];
txi.deadline = tx_deadline; txi.deadline = tx_deadline;
txi.frame = frame; txi.frame = frame;
txi.loopback = (flags & Loopback) != 0; txi.loopback = (flags & Loopback) != 0;
txi.abort_on_error = (flags & AbortOnError) != 0; txi.abort_on_error = (flags & AbortOnError) != 0;
// setup frame initial state // setup frame initial state
txi.pushed = false; 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);
} }

Write Preview
Loading…
Cancel
Save