Bugfix for low-priority packet replacement when TX queue is full (mes…

…htastic#5827)

* Correct function comment

* Enqueue the intended packet, not the pointer to what we just dropped!

* Add some log output when we drop packets due to a full queue

* Make it clear when a non-late packet is dropped

* Remove from queue before release, not after

* Erase dropped packet from queue

* Declared type

* Log TX queue length after every send

* Fix operand order

* Add worst-case cap on TX delay vs current time

src/mesh/MeshPacketQueue.cpp

@@ -69,7 +69,11 @@ bool MeshPacketQueue::enqueue(meshtastic_MeshPacket *p)
 {
     // no space - try to replace a lower priority packet in the queue
     if (queue.size() >= maxLen) {
-        return replaceLowerPriorityPacket(p);
+        bool replaced = replaceLowerPriorityPacket(p);
+        if (!replaced) {
+            LOG_WARN("TX queue is full, and there is no lower-priority packet available to evict in favour of 0x%08x", p->id);
+        }
+        return replaced;
     }
 
     // Find the correct position using upper_bound to maintain a stable order
@@ -113,7 +117,10 @@ meshtastic_MeshPacket *MeshPacketQueue::remove(NodeNum from, PacketId id, bool t
     return NULL;
 }
 
-/** Attempt to find and remove a packet from this queue.  Returns the packet which was removed from the queue */
+/**
+ * Attempt to find a lower-priority packet in the queue and replace it with the provided one.
+ * @return True if the replacement succeeded, false otherwise
+ */
 bool MeshPacketQueue::replaceLowerPriorityPacket(meshtastic_MeshPacket *p)
 {
 
@@ -122,11 +129,12 @@ bool MeshPacketQueue::replaceLowerPriorityPacket(meshtastic_MeshPacket *p)
     }
 
     // Check if the packet at the back has a lower priority than the new packet
-    auto &backPacket = queue.back();
+    auto *backPacket = queue.back();
     if (!backPacket->tx_after && backPacket->priority < p->priority) {
+        LOG_WARN("Dropping packet 0x%08x to make room in the TX queue for higher-priority packet 0x%08x", backPacket->id, p->id);
         // Remove the back packet
-        packetPool.release(backPacket);
         queue.pop_back();
+        packetPool.release(backPacket);
         // Insert the new packet in the correct order
         enqueue(p);
         return true;
@@ -139,8 +147,12 @@ bool MeshPacketQueue::replaceLowerPriorityPacket(meshtastic_MeshPacket *p)
         for (; refPacket->tx_after && it != queue.begin(); refPacket = *--it)
             ;
         if (!refPacket->tx_after && refPacket->priority < p->priority) {
+            LOG_WARN("Dropping non-late packet 0x%08x to make room in the TX queue for higher-priority packet 0x%08x",
+                     refPacket->id, p->id);
+            queue.erase(it);
             packetPool.release(refPacket);
-            enqueue(refPacket);
+            // Insert the new packet in the correct order
+            enqueue(p);
             return true;
         }
     }

src/mesh/RadioLibInterface.cpp

@@ -271,6 +271,7 @@ void RadioLibInterface::onNotify(uint32_t notification)
                             uint32_t xmitMsec = getPacketTime(txp);
                             airTime->logAirtime(TX_LOG, xmitMsec);
                         }
+                        LOG_DEBUG("%d packets remain in the TX queue", txQueue.getMaxLen() - txQueue.getFree());
                     }
                 }
             }
@@ -297,8 +298,8 @@ void RadioLibInterface::setTransmitDelay()
     if (p->tx_after) {
         unsigned long add_delay = p->rx_rssi ? getTxDelayMsecWeighted(p->rx_snr) : getTxDelayMsec();
         unsigned long now = millis();
-        p->tx_after = max(p->tx_after + add_delay, now + add_delay);
-        notifyLater(now - p->tx_after, TRANSMIT_DELAY_COMPLETED, false);
+        p->tx_after = min(max(p->tx_after + add_delay, now + add_delay), now + 2 * getTxDelayMsecWeightedWorst(p->rx_snr));
+        notifyLater(p->tx_after - now, TRANSMIT_DELAY_COMPLETED, false);
     } else if (p->rx_snr == 0 && p->rx_rssi == 0) {
         /* We assume if rx_snr = 0 and rx_rssi = 0, the packet was generated locally.
          *   This assumption is valid because of the offset generated by the radio to account for the noise