Merge pull request #829 from audunf/mesh-packet-queue-priority

Drop lower priority packets when tx queue is full.

src/mesh/MeshPacketQueue.cpp

@@ -4,14 +4,14 @@
 #include <algorithm>
 
 /// @return the priority of the specified packet
-inline uint32_t getPriority(MeshPacket *p)
+inline uint32_t getPriority(const MeshPacket *p)
 {
     auto pri = p->priority;
     return pri;
 }
 
 /// @return "true" if "p1" is ordered before "p2"
-bool CompareMeshPacket::operator()(MeshPacket *p1, MeshPacket *p2)
+bool CompareMeshPacketFunc(const MeshPacket *p1, const MeshPacket *p2)
 {
     assert(p1 && p2);
     auto p1p = getPriority(p1), p2p = getPriority(p2);
@@ -25,7 +25,12 @@ bool CompareMeshPacket::operator()(MeshPacket *p1, MeshPacket *p2)
 
 MeshPacketQueue::MeshPacketQueue(size_t _maxLen) : maxLen(_maxLen) {}
 
-/** Some clients might not properly set priority, therefore we fix it here.
+bool MeshPacketQueue::empty() {
+    return queue.empty();
+}
+
+/**
+ * Some clients might not properly set priority, therefore we fix it here.
  */
 void fixPriority(MeshPacket *p)
 {
@@ -42,51 +47,70 @@ void fixPriority(MeshPacket *p)
 /** enqueue a packet, return false if full */
 bool MeshPacketQueue::enqueue(MeshPacket *p)
 {
-
     fixPriority(p);
 
-    // fixme if there is something lower priority in the queue that can be deleted to make space, delete that instead
+    // no space - try to replace a lower priority packet in the queue
-    if (size() >= maxLen)
+    if (queue.size() >= maxLen) {
-        return false;
+        return replaceLowerPriorityPacket(p);
-    else {
-        push(p);
-        return true;
     }
+
+    queue.push_back(p);
+    std::push_heap(queue.begin(), queue.end(), &CompareMeshPacketFunc);
+    return true;
 }
 
 MeshPacket *MeshPacketQueue::dequeue()
 {
-    if (empty())
+    if (empty()) {
         return NULL;
-    else {
+    }
-        auto p = top();
+
-        pop(); // remove the first item
+    auto *p = queue.front();
+    std::pop_heap(queue.begin(), queue.end(), &CompareMeshPacketFunc);
+    queue.pop_back();
+
     return p;
 }
-}
 
-// this is kinda yucky, but I'm not sure if all arduino c++ compilers support closuers.  And we only have one
+/** Attempt to find and remove a packet from this queue.  Returns a pointer to the removed packet, or NULL if not found */
-// thread that can run at a time - so safe
-static NodeNum findFrom;
-static PacketId findId;
-
-static bool isMyPacket(MeshPacket *p)
-{
-    return p->id == findId && getFrom(p) == findFrom;
-}
-
-/** Attempt to find and remove a packet from this queue.  Returns true the packet which was removed from the queue */
 MeshPacket *MeshPacketQueue::remove(NodeNum from, PacketId id)
 {
-    findFrom = from;
+    for (auto it = queue.begin(); it != queue.end(); it++) {
-    findId = id;
+        auto p = (*it);
-    auto it = std::find_if(this->c.begin(), this->c.end(), isMyPacket);
+        if (getFrom(p) == from && p->id == id) {
-    if (it != this->c.end()) {
+            queue.erase(it);
-        auto p = *it;
+            std::make_heap(queue.begin(), queue.end(), &CompareMeshPacketFunc);
-        this->c.erase(it);
-        std::make_heap(this->c.begin(), this->c.end(), this->comp);
             return p;
-    } else {
+        }
+    }
+
     return NULL;
 }
+
+/** Attempt to find and remove a packet from this queue.  Returns the packet which was removed from the queue */
+bool MeshPacketQueue::replaceLowerPriorityPacket(MeshPacket *p) {
+    std::sort_heap(queue.begin(), queue.end(), &CompareMeshPacketFunc); // sort ascending based on priority (0 -> 127)
+
+    // find first packet which does not compare less (in priority) than parameter packet
+    auto low = std::lower_bound(queue.begin(), queue.end(), p, &CompareMeshPacketFunc);
+
+    if (low == queue.begin()) { // if already at start, there are no packets with lower priority
+        return false;
+    }
+
+    if (low == queue.end()) {
+        // all priorities in the vector are smaller than the incoming packet. Replace the lowest priority (first) element
+        low = queue.begin();
+    } else {
+        // 'low' iterator points to first packet which does not compare less than parameter
+        --low; // iterate to lower priority packet
+    }
+
+    if (getPriority(p) > getPriority(*low)) {
+        packetPool.release(*low); // deallocate and drop the packet we're replacing
+        *low = p; // replace low-pri packet at this position with incoming packet with higher priority
+    }
+
+    std::make_heap(queue.begin(), queue.end(), &CompareMeshPacketFunc);
+    return true;
 }

src/mesh/MeshPacketQueue.h

@@ -5,29 +5,29 @@
 #include <assert.h>
 #include <queue>
 
-// this is an strucure which implements the
-// operator overloading
-struct CompareMeshPacket {
-    bool operator()(MeshPacket *p1, MeshPacket *p2);
-};
 
 /**
- * A priority queue of packets.
+ * A priority queue of packets
- *
  */
-class MeshPacketQueue : public std::priority_queue<MeshPacket *, std::vector<MeshPacket *>, CompareMeshPacket>
+class MeshPacketQueue
 {
     size_t maxLen;
+    std::vector<MeshPacket *> queue;
+
+    /** Replace a lower priority package in the queue with 'mp' (provided there are lower pri packages). Return true if replaced. */
+    bool replaceLowerPriorityPacket(MeshPacket *mp);
+
   public:
     MeshPacketQueue(size_t _maxLen);
 
     /** enqueue a packet, return false if full */
     bool enqueue(MeshPacket *p);
 
-    // bool isEmpty();
+    /** return true if the queue is empty */
+    bool empty();
 
     MeshPacket *dequeue();
 
-    /** Attempt to find and remove a packet from this queue.  Returns true the packet which was removed from the queue */
+    /** Attempt to find and remove a packet from this queue.  Returns the packet which was removed from the queue */
     MeshPacket *remove(NodeNum from, PacketId id);
 };