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
Split 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, @@ -326,72 +326,70 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
return -1;
}
CriticalSectionLocker lock;
{
CriticalSectionLocker lock;
/*
* Seeking for an empty slot
*/
uint8_t index;
/*
* Seeking for an empty slot
*/
uint8_t index;
if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) {
stats.tx_rejected++;
return 0; //we don't have free space
}
index = ((can_->TXFQS & FDCAN_TXFQS_TFQPI) >> FDCAN_TXFQS_TFQPI_Pos);
if ((can_->TXFQS & FDCAN_TXFQS_TFQF) != 0) {
stats.tx_rejected++;
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));
// 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;
//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;
}
//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);
}
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
{
CriticalSectionLocker lock;
CanRxItem rx_item;
if (!rx_queue_.pop(rx_item) || !initialised_) {
return 0;
{
CriticalSectionLocker lock;
CanRxItem rx_item;
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);
}

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, @@ -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.
*/
CriticalSectionLocker lock;
{
CriticalSectionLocker lock;
/*
* Seeking for an empty slot
*/
uint8_t txmailbox = 0xFF;
if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) {
txmailbox = 0;
} else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) {
txmailbox = 1;
} else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
txmailbox = 2;
} else {
PERF_STATS(stats.tx_rejected);
return 0; // No transmission for you.
}
/*
* Seeking for an empty slot
*/
uint8_t txmailbox = 0xFF;
if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) {
txmailbox = 0;
} else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) {
txmailbox = 1;
} else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
txmailbox = 2;
} else {
PERF_STATS(stats.tx_rejected);
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);
}
/*
* 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;
}
if (frame.isRemoteTransmissionRequest()) {
mb.TIR |= bxcan::TIR_RTR;
}
mb.TDTR = frame.dlc;
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.TDHR = frame.data_32[1];
mb.TDLR = frame.data_32[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
*/
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;
/*
* 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);
}
int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_us, CanIOFlags& out_flags)
{
CriticalSectionLocker lock;
CanRxItem rx_item;
if (!rx_queue_.pop(rx_item)) {
return 0;
{
CriticalSectionLocker lock;
CanRxItem rx_item;
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);
}

Write Preview
Loading…
Cancel
Save