robdobsn · Oct 25, 2026 · Oct 25, 2026 · Oct 25, 2026
diff --git a/components/BLEManager/BLEConfig.h b/components/BLEManager/BLEConfig.h
@@ -70,6 +70,8 @@ class BLEConfig
     static const uint32_t BLE_OUTBOUND_MSGS_IN_FLIGHT_TIMEOUT_MS = 500;
     static const uint32_t DEFAULT_OUTBOUND_QUEUE_RESERVE_FOR_NON_PUBLISH = 10;
     static const uint32_t DEFAULT_PUBLISH_QUEUE_SIZE = 30;
+    // About a second of link time with notifications
+    static const uint32_t DEFAULT_PUBLISH_QUEUE_MAX_BYTES = 32768;
     static const bool DEFAULT_PUBLISH_USE_INDICATE = false;
     static const uint32_t DEFAULT_CONN_INTERVAL_BLE_UNITS = 12; // 15ms
     static const uint32_t DEFAULT_CONN_LATENCY = 0;
@@ -148,6 +150,7 @@ class BLEConfig
         outMsgsInFlightTimeoutMs = config.getLong("outMsgsInFlightMs", BLE_OUTBOUND_MSGS_IN_FLIGHT_TIMEOUT_MS);
         outQReserveForNonPublish = config.getLong("outQResvNonPub", DEFAULT_OUTBOUND_QUEUE_RESERVE_FOR_NON_PUBLISH);
         publishQueueSize = config.getLong("pubQSize", DEFAULT_PUBLISH_QUEUE_SIZE);
+        publishQueueMaxBytes = config.getLong("pubQMaxBytes", DEFAULT_PUBLISH_QUEUE_MAX_BYTES);
         publishUseIndication = config.getBool("pubUseInd", DEFAULT_PUBLISH_USE_INDICATE);
 
         // Task settings
@@ -290,6 +293,7 @@ class BLEConfig
     uint32_t outMsgsInFlightTimeoutMs = BLE_OUTBOUND_MSGS_IN_FLIGHT_TIMEOUT_MS;
     uint16_t outQReserveForNonPublish = DEFAULT_OUTBOUND_QUEUE_RESERVE_FOR_NON_PUBLISH;
     uint16_t publishQueueSize = DEFAULT_PUBLISH_QUEUE_SIZE;
+    uint32_t publishQueueMaxBytes = DEFAULT_PUBLISH_QUEUE_MAX_BYTES;
     bool publishUseIndication:1 = DEFAULT_PUBLISH_USE_INDICATE;
 
     // Task settings

diff --git a/components/BLEManager/BLEGattOutbound.cpp b/components/BLEManager/BLEGattOutbound.cpp
@@ -56,6 +56,7 @@ bool BLEGattOutbound::setup(const BLEConfig& bleConfig)
     // Setup queues
     _commandQueue.setMaxLen(bleConfig.outboundQueueSize);
     _publishQueue.setMaxLen(bleConfig.publishQueueSize);
+    _publishQueueMaxBytes = bleConfig.publishQueueMaxBytes;
 
     // Check if a thread should be started for sending
     if (bleConfig.useTaskForSending)
@@ -149,24 +150,37 @@ bool BLEGattOutbound::isReadyToSend(uint32_t channelID, CommsMsgTypeCode msgType
 {
     if (msgType == MSG_TYPE_PUBLISH)
     {
-        bool canSend = _publishQueue.count() < _publishQueue.maxLen();
+        bool canSend = (_publishQueue.count() < _publishQueue.maxLen()) &&
+                        (_publishQueueBytes.load() < _publishQueueMaxBytes);
         if (!canSend)
         {
+            // A publisher asks again (each loop) while the queue is full, so a drop is counted once per spell of
+            // the queue being full, and the warning is limited to one a second (with the number of refusals)
             // A drop within PUBLISH_DROP_CONNECT_GRACE_MS of connection is the harmless startup burst
             // (link still ramping up, backlog flushing). A later drop is operational - the publish rate
             // is exceeding the BLE drain rate - and is the case worth flagging.
             uint32_t connStartMs = _connStartMs.load();
             bool operationalDrop = (connStartMs != 0) &&
                         Raft::isTimeout(millis(), connStartMs, PUBLISH_DROP_CONNECT_GRACE_MS);
-            _bleStats.txPublishDropped(operationalDrop);
+            if (!_publishQueueFull)
+                _bleStats.txPublishDropped(operationalDrop);
+            _publishQueueFull = true;
+            _publishRefusedSinceWarn++;
 #ifdef WARN_ON_PUBLISH_QUEUE_FULL
-            if (operationalDrop)
+            if (operationalDrop && Raft::isTimeout(millis(), _publishFullLastWarnMs, PUBLISH_FULL_WARN_INTERVAL_MS))
             {
-                LOG_W(MODULE_PREFIX, "isReadyToSend PUBLISH DROPPED (operational) qCount %d maxLen %d",
-                            _publishQueue.count(), _publishQueue.maxLen());
+                LOG_W(MODULE_PREFIX, "isReadyToSend PUBLISH QUEUE FULL (operational) qCount %d maxLen %d qBytes %d maxBytes %d refused %d",
+                            _publishQueue.count(), _publishQueue.maxLen(),
+                            (int)_publishQueueBytes.load(), (int)_publishQueueMaxBytes, (int)_publishRefusedSinceWarn);
+                _publishFullLastWarnMs = millis();
+                _publishRefusedSinceWarn = 0;
             }
 #endif
         }
+        else
+        {
+            _publishQueueFull = false;
+        }
         return canSend;
     }
 
@@ -200,25 +214,36 @@ bool BLEGattOutbound::sendMsg(CommsChannelMsg& msg)
 #endif
 #endif
 
+    // The message's buffer is taken if it may be (e.g. a message just encoded for sending), and copied otherwise,
+    // then moved into the queue
+    uint32_t msgLen = msg.getBufLen();
+    ProtocolRawMsg bleOutMsg;
+    if (msg.isBufTakeable())
+        bleOutMsg.takeBuffer(msg.getCmdVector());
+    else
+        bleOutMsg.setFromBuffer(msg.getBuf(), msg.getBufLen());
+
     // Route to appropriate queue based on message type
-    ProtocolRawMsg bleOutMsg(msg.getBuf(), msg.getBufLen());
     bool putOk = false;
     if (msg.getMsgTypeCode() == MSG_TYPE_PUBLISH)
     {
-        putOk = _publishQueue.put(bleOutMsg, OUTBOUND_QUEUE_MAX_MS_TO_WAIT);
+        // Counted before it is queued, as the sending side may take it off the queue at once
+        _publishQueueBytes += msgLen;
+        putOk = _publishQueue.put(std::move(bleOutMsg), OUTBOUND_QUEUE_MAX_MS_TO_WAIT);
         if (!putOk)
         {
+            _publishQueueBytes -= msgLen;
             LOG_W(MODULE_PREFIX, "sendBLEMsg PUBLISH FAILEDTOSEND totalLen %d qCount %d", 
-                        msg.getBufLen(), _publishQueue.count());
+                        (int)msgLen, _publishQueue.count());
         }
     }
     else
     {
-        putOk = _commandQueue.put(bleOutMsg, OUTBOUND_QUEUE_MAX_MS_TO_WAIT);
+        putOk = _commandQueue.put(std::move(bleOutMsg), OUTBOUND_QUEUE_MAX_MS_TO_WAIT);
         if (!putOk)
         {
             LOG_W(MODULE_PREFIX, "sendBLEMsg CMD FAILEDTOSEND totalLen %d qCount %d", 
-                        msg.getBufLen(), _commandQueue.count());
+                        (int)msgLen, _commandQueue.count());
         }
     }
 
@@ -285,7 +310,7 @@ int32_t BLEGattOutbound::decrementMsgsInFlight()
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue, 
-                uint16_t& msgPos, bool useIndication, const char* queueName)
+                uint32_t& msgPos, bool useIndication, const char* queueName)
 {
     // If this queue uses indication, check if an indication is already in flight
     if (useIndication && isIndicationInFlight())
@@ -298,21 +323,22 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue
             return false;
     }
 
-    // Peek next message in queue
-    ProtocolRawMsg bleOutMsg;
-    if (!queue.peek(bleOutMsg, OUTBOUND_QUEUE_MAX_MS_TO_WAIT))
-        return false;
-
-    // Extract next section of message to send
-    uint32_t toSendLen = 0;
-    bool removeFromQueue = true;
-    if (bleOutMsg.getBufLen() > msgPos)
-        toSendLen = bleOutMsg.getBufLen() - msgPos;
+    // Copy the next section of the message at the front of the queue (copying the whole message for each
+    // section sent took many ms per section for a large message, and held the queue locked meanwhile)
     uint32_t maxLen = getMaxSendLen();
-    if (toSendLen > maxLen)
-        toSendLen = maxLen;
-    if (bleOutMsg.getBufLen() > msgPos + toSendLen)
-        removeFromQueue = false;
+    uint32_t msgLen = 0;
+    uint32_t toSendLen = 0;
+    bool msgFound = queue.peekWith([&](const ProtocolRawMsg& bleOutMsg) {
+            msgLen = bleOutMsg.getBufLen();
+            if (msgLen > msgPos)
+                toSendLen = msgLen - msgPos;
+            if (toSendLen > maxLen)
+                toSendLen = maxLen;
+            _sendChunk.assign(bleOutMsg.getBuf() + msgPos, bleOutMsg.getBuf() + msgPos + toSendLen);
+        }, OUTBOUND_QUEUE_MAX_MS_TO_WAIT);
+    if (!msgFound)
+        return false;
+    bool removeFromQueue = msgLen <= msgPos + toSendLen;
 
     BLEGattServerSendResult rslt = BLEGATT_SERVER_SEND_RESULT_TRY_AGAIN;
     if (toSendLen != 0)
@@ -329,7 +355,7 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue
             _lastNotifySendMs = millis();
 
         // Send to central
-        rslt = _gattServer.sendToCentral(bleOutMsg.getBuf() + msgPos, toSendLen, useIndication);
+        rslt = _gattServer.sendToCentral(_sendChunk.data(), toSendLen, useIndication);
         if (rslt == BLEGATT_SERVER_SEND_RESULT_OK)
         {
             _bleStats.txMsg(toSendLen, true);
@@ -365,12 +391,14 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue
         if (queue.pop(OUTBOUND_QUEUE_MAX_MS_TO_WAIT))
         {
             msgPos = 0;
+            if (&queue == &_publishQueue)
+                _publishQueueBytes -= msgLen;
         }
         else
         {
             // Failed to remove (queue mutex not obtained) so set the position to the end of the message to
             // ensure that no part of it is sent again - removal will be attempted again on the next call
-            msgPos = bleOutMsg.getBufLen() > UINT16_MAX ? UINT16_MAX : bleOutMsg.getBufLen();
+            msgPos = msgLen;
         }
     }
 
@@ -379,12 +407,12 @@ bool BLEGattOutbound::handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue
     {
         int32_t msgsInFlight = _outboundMsgsInFlight.load();
         LOG_I(MODULE_PREFIX, "handleSendFromQ %s sendLen %d totalLen %d msgPos %d sendOk %d inFlight %d leftInQ %d removeFromQ %d", 
-                queueName, toSendLen, bleOutMsg.getBufLen(), msgPos, rslt, msgsInFlight, queue.count(), removeFromQueue);
+                queueName, toSendLen, msgLen, msgPos, rslt, msgsInFlight, queue.count(), removeFromQueue);
     }
     else
     {
         LOG_I(MODULE_PREFIX, "handleSendFromQ %s sendLen %d totalLen %d msgPos %d sendOk %s leftInQ %d removeFromQ %d", 
-            queueName, toSendLen, bleOutMsg.getBufLen(), msgPos, 
+            queueName, toSendLen, msgLen, msgPos, 
             rslt == BLEGATT_SERVER_SEND_RESULT_OK ? "OK" : rslt == BLEGATT_SERVER_SEND_RESULT_FAIL ? "FAIL" : "TRYAGAIN", 
             queue.count(), removeFromQueue);
     }

diff --git a/components/BLEManager/BLEGattOutbound.h b/components/BLEManager/BLEGattOutbound.h
@@ -70,18 +70,34 @@ class BLEGattOutbound
     // Command queue: typically uses indication (ACK'd, reliable)
     ThreadSafeQueue<ProtocolRawMsg> _commandQueue;
     std::atomic<bool> _commandUseIndication{true};
-    uint16_t _commandMsgPos = 0;
+    uint32_t _commandMsgPos = 0;
 
     // Publish queue: typically uses notification (faster, no ACK wait)
     ThreadSafeQueue<ProtocolRawMsg> _publishQueue;
     std::atomic<bool> _publishUseIndication{false};
-    uint16_t _publishMsgPos = 0;
+    uint32_t _publishMsgPos = 0;
+
+    // Bytes in the publish queue, and the most there may be before more publishes are refused (one message may
+    // take it over the limit, so a message larger than the limit is still sent when the queue is empty). A limit
+    // on the count alone lets large messages (e.g. camera frames) pile up for many seconds of link time
+    std::atomic<uint32_t> _publishQueueBytes{0};
+    uint32_t _publishQueueMaxBytes = BLEConfig::DEFAULT_PUBLISH_QUEUE_MAX_BYTES;
+
+    // The part of a message being sent, copied out of the queue (so the queue isn't locked during the send)
+    std::vector<uint8_t> _sendChunk;
 
     // Time the current connection was established (0 if not connected) - used to distinguish
     // publish-queue drops during the post-connection burst from drops during normal operation
     std::atomic<uint32_t> _connStartMs{0};
     static const uint32_t PUBLISH_DROP_CONNECT_GRACE_MS = 5000;
 
+    // Publish queue full: whether it was full when last asked, publishes refused since the last warning, and the
+    // time of that warning (warnings are limited as a publisher asks again each loop while the queue is full)
+    bool _publishQueueFull = false;
+    uint32_t _publishRefusedSinceWarn = 0;
+    uint32_t _publishFullLastWarnMs = 0;
+    static const uint32_t PUBLISH_FULL_WARN_INTERVAL_MS = 1000;
+
     // Max time to wait for the mutex on the outbound queues
     static const uint32_t OUTBOUND_QUEUE_MAX_MS_TO_WAIT = 10;
 
@@ -117,7 +133,7 @@ class BLEGattOutbound
 
     // Outbound queue handling
     void serviceOutboundQueue();
-    bool handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue, uint16_t& msgPos, 
+    bool handleSendFromQueue(ThreadSafeQueue<ProtocolRawMsg>& queue, uint32_t& msgPos, 
                 bool useIndication, const char* queueName);
     void outboundMsgTask();
 

diff --git a/components/BLEManager/BLEGattServer.cpp b/components/BLEManager/BLEGattServer.cpp
@@ -16,6 +16,7 @@
 #include "RaftArduino.h"
 #include "NamedValueProvider.h"
 #include "BLEGattServer.h"
+#include "BLEManStats.h"
 #include "services/gap/ble_svc_gap.h"
 #include "services/gatt/ble_svc_gatt.h"
 
@@ -71,6 +72,7 @@ ble_uuid128_t BLEGattServer::DEFAULT_MESSAGE_RESPONSE_UUID =
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
 BLEGattServer::BLEGattServer(BLEGattServerAccessCBType callback, BLEManStats& bleStats) :
+    _bleStats(bleStats),
     _bleOutbound(*this, bleStats)
 {
     _accessCallback = callback;
@@ -364,6 +366,15 @@ BLEGattServerSendResult BLEGattServer::sendToCentral(const uint8_t* pBuf, uint32
 
     // Form buffer to send
     struct os_mbuf *om = ble_hs_mbuf_from_flat(pBuf, bufLen);
+
+    // No buffer (NimBLE's pool is used up, e.g. by notifications sent faster than the link takes them): send
+    // it later. It mustn't be passed on: NimBLE takes a null buffer to mean "send the characteristic's value",
+    // which is empty here, and reports success - so the packet would be lost from the message
+    if (!om)
+    {
+        _bleStats.txNoBuf();
+        return BLEGATT_SERVER_SEND_RESULT_TRY_AGAIN;
+    }
 #ifdef DEBUG_RESP_CHARACTERISTIC
     LOG_I(MODULE_PREFIX, "sendToCentral sending bufLen %d om %p", bufLen, om);
 #endif

diff --git a/components/BLEManager/BLEGattServer.h b/components/BLEManager/BLEGattServer.h
@@ -176,6 +176,9 @@ class BLEGattServer
     uint32_t _lastBLEErrorMsgMs = 0;
     uint32_t _lastBLEErrorMsgCode = 0;
 
+    // Stats
+    BLEManStats& _bleStats;
+
     // Outbound handler
     BLEGattOutbound _bleOutbound;
 

diff --git a/components/BLEManager/BLEManStats.h b/components/BLEManager/BLEManStats.h
@@ -35,6 +35,7 @@ class BLEManStats
         _rxTestFrameBytes = 0;
         _txPublishDropCount = 0;
         _txPublishDropOpCount = 0;
+        _txNoBufCount = 0;
         _rxRate.clear();
         _txRate.clear();
         _txErrRate.clear();
@@ -63,15 +64,22 @@ class BLEManStats
         _txErrRate.sample(_txErrCount);
     }
 
-    // A publish was dropped because the outbound publish queue was full. operational is true if the drop
-    // occurred during normal operation (not during the post-connection burst) - i.e. the case that matters
+    // Publishes were refused because the outbound publish queue was full (counted once each time it fills, as a
+    // publisher asks again while it is full). operational is true if this occurred during normal operation (not
+    // during the post-connection burst) - i.e. the case that matters
     void txPublishDropped(bool operational)
     {
         _txPublishDropCount++;
         if (operational)
             _txPublishDropOpCount++;
     }
 
+    // NimBLE had no buffer for a packet (it is sent again later)
+    void txNoBuf()
+    {
+        _txNoBufCount++;
+    }
+
     void rxTestFrame(uint32_t msgSize, bool seqOK, bool dataOK)
     {
         _rxTestFrameCount++;
@@ -114,6 +122,11 @@ class BLEManStats
                     (int)_txPublishDropCount.load(), (int)_txPublishDropOpCount.load());
             json += buf;
         }
+        if (_txNoBufCount.load() > 0)
+        {
+            snprintf(buf, sizeof(buf), R"(,"txNoBuf":%d)", (int)_txNoBufCount.load());
+            json += buf;
+        }
         if (_rxTestFrameCount.load() > 0)
         {
             snprintf(buf, sizeof(buf), R"("tM":%d,"tB":%d,"tBPS":%.1f,"tSeqErrs":%d,"tDatErrs":%d)",
@@ -156,6 +169,7 @@ class BLEManStats
     std::atomic<uint32_t> _txErrCount{0};
     std::atomic<uint32_t> _txPublishDropCount{0};
     std::atomic<uint32_t> _txPublishDropOpCount{0};
+    std::atomic<uint32_t> _txNoBufCount{0};
     #define MOVING_AVERAGE_WINDOW_SIZE 5
     MovingRate<MOVING_AVERAGE_WINDOW_SIZE> _rxRate;
     MovingRate<MOVING_AVERAGE_WINDOW_SIZE> _txRate;