diff --git a/src/mesh/MeshService.cpp b/src/mesh/MeshService.cpp index 038861937d..44094f1bba 100644 --- a/src/mesh/MeshService.cpp +++ b/src/mesh/MeshService.cpp @@ -267,14 +267,22 @@ void MeshService::sendNetworkPing(NodeNum dest, bool wantReplies) void MeshService::sendToPhone(meshtastic_MeshPacket *p) { + perhapsDecode(p); + if (toPhoneQueue.numFree() == 0) { - LOG_WARN("ToPhone queue is full, discarding oldest\n"); - meshtastic_MeshPacket *d = toPhoneQueue.dequeuePtr(0); - if (d) - releaseToPool(d); + if (p->decoded.portnum == meshtastic_PortNum_TEXT_MESSAGE_APP || + p->decoded.portnum == meshtastic_PortNum_RANGE_TEST_APP) { + LOG_WARN("ToPhone queue is full, discarding oldest\n"); + meshtastic_MeshPacket *d = toPhoneQueue.dequeuePtr(0); + if (d) + releaseToPool(d); + } else { + LOG_WARN("ToPhone queue is full, dropping packet.\n"); + releaseToPool(p); + return; + } } - perhapsDecode(p); assert(toPhoneQueue.enqueue(p, 0)); fromNum++; }