HAL_SITL: avoid use of std::queue

use ObjectArray instead

libraries/AP_HAL_SITL/CANSocketIface.cpp

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

libraries/AP_HAL_SITL/CANSocketIface.h

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