Drop oldest packet from radio when queue is full (#5212)

And still notify Router

Co-authored-by: Ben Meadors <benmmeadors@gmail.com>

src/mesh/Router.cpp

@@ -81,14 +81,17 @@ int32_t Router::runOnce()
  */
 void Router::enqueueReceivedMessage(meshtastic_MeshPacket *p)
 {
-    if (fromRadioQueue.enqueue(p, 0)) { // NOWAIT - fixme, if queue is full, delete older messages
+    // Try enqueue until successful
-
+    while (!fromRadioQueue.enqueue(p, 0)) {
-        // Nasty hack because our threading is primitive.  interfaces shouldn't need to know about routers FIXME
+        meshtastic_MeshPacket *old_p;
-        setReceivedMessage();
+        old_p = fromRadioQueue.dequeuePtr(0); // Dequeue and discard the oldest packet
-    } else {
+        if (old_p) {
-        printPacket("BUG! fromRadioQueue is full! Discarding!", p);
+            printPacket("fromRadioQ full, drop oldest!", old_p);
-        packetPool.release(p);
+            packetPool.release(old_p);
+        }
     }
+    // Nasty hack because our threading is primitive.  interfaces shouldn't need to know about routers FIXME
+    setReceivedMessage();
 }
 
 /// Generate a unique packet id