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

14
libraries/AP_HAL_ChibiOS/CANFDIface.cpp
View File

@ -326,6 +326,7 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
return -1; return -1;
} }
{
CriticalSectionLocker lock; CriticalSectionLocker lock;
/* /*
@ -356,14 +357,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
buffer[1] = frame.dlc << 16 | index << 24; buffer[1] = frame.dlc << 16 | index << 24;
// Write Frame to the message RAM // Write Frame to the message RAM
buffer[2] = (uint32_t(frame.data[3]) << 24) | buffer[2] = frame.data_32[0];
(uint32_t(frame.data[2]) << 16) | buffer[3] = frame.data_32[1];
(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 //Set Add Request
can_->TXBAR = (1 << index); can_->TXBAR = (1 << index);
@ -378,11 +373,13 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
pending_tx_[index].aborted = false; pending_tx_[index].aborted = false;
pending_tx_[index].setup = true; pending_tx_[index].setup = true;
pending_tx_[index].pushed = false; 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; CriticalSectionLocker lock;
CanRxItem rx_item; CanRxItem rx_item;
@ -392,6 +389,7 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
out_frame = rx_item.frame; out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us; out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags; 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);
} }

14
libraries/AP_HAL_ChibiOS/CanIface.cpp
View File

@ -308,6 +308,7 @@ 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;
/* /*
@ -341,14 +342,8 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
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.
@ -362,11 +357,13 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, uint64_t tx_deadline,
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; CriticalSectionLocker lock;
CanRxItem rx_item; CanRxItem rx_item;
@ -376,6 +373,7 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
out_frame = rx_item.frame; out_frame = rx_item.frame;
out_timestamp_us = rx_item.timestamp_us; out_timestamp_us = rx_item.timestamp_us;
out_flags = rx_item.flags; 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);
} }