Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

HAL_SITL: avoid use of std::queue 

use ObjectArray instead
This commit is contained in:
Andrew Tridgell 2024-05-27 11:20:32 +10:00
parent eec7876028
commit c764e60aa0
2 changed files with 19 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_HAL_SITL/CANSocketIface.cpp
View File

@ -74,9 +74,12 @@ int16_t CANIface::send(const AP_HAL::CANFrame& frame, const uint64_t tx_deadline
tx_item.setup = true; tx_item.setup = true;
tx_item.index = _tx_frame_counter; tx_item.index = _tx_frame_counter;
tx_item.deadline = tx_deadline; tx_item.deadline = tx_deadline;
_tx_queue.emplace(tx_item); if (_tx_queue.push(tx_item)) {
_tx_frame_counter++; _tx_frame_counter++;
stats.tx_requests++; stats.tx_requests++;
} else {
stats.tx_overflow++;
}
_pollRead(); // Read poll is necessary because it can release the pending TX flag _pollRead(); // Read poll is necessary because it can release the pending TX flag
_pollWrite(); _pollWrite();
@ -87,32 +90,32 @@ int16_t CANIface::receive(AP_HAL::CANFrame& out_frame, uint64_t& out_timestamp_u
CANIface::CanIOFlags& out_flags) CANIface::CanIOFlags& out_flags)
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
if (_rx_queue.empty()) { if (_rx_queue.is_empty()) {
_pollRead(); // This allows to use the socket not calling poll() explicitly. _pollRead(); // This allows to use the socket not calling poll() explicitly.
if (_rx_queue.empty()) { if (_rx_queue.is_empty()) {
return 0; return 0;
} }
} }
{ {
const CanRxItem& rx = _rx_queue.front(); const CanRxItem &rx = *_rx_queue[0];
out_frame = rx.frame; out_frame = rx.frame;
out_timestamp_us = rx.timestamp_us; out_timestamp_us = rx.timestamp_us;
out_flags = rx.flags; out_flags = rx.flags;
} }
(void)_rx_queue.pop(); IGNORE_RETURN(_rx_queue.pop());
return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags); return AP_HAL::CANIface::receive(out_frame, out_timestamp_us, out_flags);
} }
bool CANIface::_hasReadyTx() bool CANIface::_hasReadyTx()
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
return !_tx_queue.empty(); return !_tx_queue.is_empty();
} }
bool CANIface::_hasReadyRx() bool CANIface::_hasReadyRx()
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
return !_rx_queue.empty(); return !_rx_queue.is_empty();
} }
void CANIface::_poll(bool read, bool write) void CANIface::_poll(bool read, bool write)
@ -137,7 +140,7 @@ void CANIface::_pollWrite()
} }
while (_hasReadyTx()) { while (_hasReadyTx()) {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
const CanTxItem tx = _tx_queue.top(); const CanTxItem tx = *_tx_queue[0];
const uint64_t curr_time = AP_HAL::micros64(); const uint64_t curr_time = AP_HAL::micros64();
if (tx.deadline >= curr_time) { if (tx.deadline >= curr_time) {
// hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time); // hal.console->printf("%x TDEAD: %lu CURRT: %lu DEL: %lu\n",tx.frame.id, tx.deadline, curr_time, tx.deadline-curr_time);
@ -153,7 +156,7 @@ void CANIface::_pollWrite()
} }
// Removing the frame from the queue // Removing the frame from the queue
(void)_tx_queue.pop(); IGNORE_RETURN(_tx_queue.pop());
} }
} }
@ -180,15 +183,14 @@ void CANIface::flush_tx()
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
do { do {
_poll(true, true); _poll(true, true);
} while(!_tx_queue.empty()); } while(!_tx_queue.is_empty());
} }
void CANIface::clear_rx() void CANIface::clear_rx()
{ {
WITH_SEMAPHORE(sem); WITH_SEMAPHORE(sem);
// Clean Rx Queue // Clean Rx Queue
std::queue<CanRxItem> empty; _rx_queue.clear();
std::swap( _rx_queue, empty );
} }
void CANIface::_confirmSentFrame() void CANIface::_confirmSentFrame()

7
libraries/AP_HAL_SITL/CANSocketIface.h
View File

@ -20,9 +20,8 @@
#if HAL_NUM_CAN_IFACES #if HAL_NUM_CAN_IFACES
#include <AP_HAL/CANIface.h> #include <AP_HAL/CANIface.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <string> #include <string>
#include <queue>
#include <memory> #include <memory>
#include <map> #include <map>
#include <unordered_set> #include <unordered_set>
@ -127,8 +126,8 @@ private:
AP_HAL::BinarySemaphore *sem_handle; AP_HAL::BinarySemaphore *sem_handle;
pollfd _pollfd; pollfd _pollfd;
std::priority_queue<CanTxItem> _tx_queue; ObjectArray<CanTxItem> _tx_queue{100};
std::queue<CanRxItem> _rx_queue; ObjectArray<CanRxItem> _rx_queue{100};
/* /*
bus statistics bus statistics