Merge branch 'develop' into bh1750

src/graphics/draw/UIRenderer.cpp

@@ -563,6 +563,7 @@ void UIRenderer::drawDeviceFocused(OLEDDisplay *display, OLEDDisplayUiState *sta
     display->setTextAlignment(TEXT_ALIGN_LEFT);
     display->setFont(FONT_SMALL);
     int line = 1;
+    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
 
     // === Header ===
 #if defined(M5STACK_UNITC6L)
@@ -740,7 +741,6 @@ void UIRenderer::drawDeviceFocused(OLEDDisplay *display, OLEDDisplayUiState *sta
     int yOffset = (isHighResolution) ? 0 : 5;
     std::string longNameStr;
 
-    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
     if (ourNode && ourNode->has_user && strlen(ourNode->user.long_name) > 0) {
         longNameStr = sanitizeString(ourNode->user.long_name);
     }
@@ -1000,24 +1000,7 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
     const char *displayLine = ""; // Initialize to empty string by default
     meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
 
-    bool usePhoneGPS = (ourNode && nodeDB->hasValidPosition(ourNode) &&
-                        config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED);
-
-    if (usePhoneGPS) {
-        // Phone-provided GPS is active
-        displayLine = "Phone GPS";
-        int yOffset = (isHighResolution) ? 3 : 1;
-        if (isHighResolution) {
-            NodeListRenderer::drawScaledXBitmap16x16(x, getTextPositions(display)[line] + yOffset - 5, imgSatellite_width,
-                                                     imgSatellite_height, imgSatellite, display);
-        } else {
-            display->drawXbm(x + 1, getTextPositions(display)[line] + yOffset, imgSatellite_width, imgSatellite_height,
-                             imgSatellite);
-        }
-        int xOffset = (isHighResolution) ? 6 : 0;
-        display->drawString(x + 11 + xOffset, getTextPositions(display)[line++], displayLine);
-    } else if (config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED) {
-        // GPS disabled / not present
+    if (config.position.gps_mode != meshtastic_Config_PositionConfig_GpsMode_ENABLED) {
         if (config.position.fixed_position) {
             displayLine = "Fixed GPS";
         } else {
@@ -1108,9 +1091,7 @@ void UIRenderer::drawCompassAndLocationScreen(OLEDDisplay *display, OLEDDisplayU
 
         // === Final Row: Altitude ===
         char altitudeLine[32] = {0};
-        int32_t alt = (strcmp(displayLine, "Phone GPS") == 0 && ourNode && nodeDB->hasValidPosition(ourNode))
-                          ? ourNode->position.altitude
-                          : geoCoord.getAltitude();
+        int32_t alt = geoCoord.getAltitude();
         if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
             snprintf(altitudeLine, sizeof(altitudeLine), "Alt: %.0fft", alt * METERS_TO_FEET);
         } else {

src/graphics/niche/InkHUD/Applets/Bases/Map/MapApplet.cpp

@@ -13,45 +13,147 @@ void InkHUD::MapApplet::onRender()
         return;
     }
 
+    // Helper: draw rounded rectangle centered at x,y
+    auto fillRoundedRect = [&](int16_t cx, int16_t cy, int16_t w, int16_t h, int16_t r, uint16_t color) {
+        int16_t x = cx - (w / 2);
+        int16_t y = cy - (h / 2);
+
+        // center rects
+        fillRect(x + r, y, w - 2 * r, h, color);
+        fillRect(x, y + r, r, h - 2 * r, color);
+        fillRect(x + w - r, y + r, r, h - 2 * r, color);
+
+        // corners
+        fillCircle(x + r, y + r, r, color);
+        fillCircle(x + w - r - 1, y + r, r, color);
+        fillCircle(x + r, y + h - r - 1, r, color);
+        fillCircle(x + w - r - 1, y + h - r - 1, r, color);
+    };
+
     // Find center of map
-    // - latitude and longitude
-    // - will be placed at X(0.5), Y(0.5)
     getMapCenter(&latCenter, &lngCenter);
-
-    // Calculate North+East distance of each node to map center
-    // - which nodes to use controlled by virtual shouldDrawNode method
     calculateAllMarkers();
-
-    // Set the region shown on the map
-    // - default: fit all nodes, plus padding
-    // - maybe overriden by derived applet
-    // - getMapSize *sets* passed parameters (C-style)
     getMapSize(&widthMeters, &heightMeters);
-
-    // Set the metersToPx conversion value
     calculateMapScale();
 
-    // Special marker for own node
-    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
-    if (ourNode && nodeDB->hasValidPosition(ourNode))
-        drawLabeledMarker(ourNode);
-
-    // Draw all markers
+    // Draw all markers first
     for (Marker m : markers) {
         int16_t x = X(0.5) + (m.eastMeters * metersToPx);
         int16_t y = Y(0.5) - (m.northMeters * metersToPx);
 
-        // Cross Size
-        constexpr uint16_t csMin = 5;
-        constexpr uint16_t csMax = 12;
+        // Add white halo outline first
+        constexpr int outlinePad = 1;
+        int boxSize = 11;
+        int radius = 2; // rounded corner radius
 
-        // Too many hops away
-        if (m.hasHopsAway && m.hopsAway > config.lora.hop_limit) // Too many mops
-            printAt(x, y, "!", CENTER, MIDDLE);
-        else if (!m.hasHopsAway) // Unknown hops
-            drawCross(x, y, csMin);
-        else // The fewer hops, the larger the cross
-            drawCross(x, y, map(m.hopsAway, 0, config.lora.hop_limit, csMax, csMin));
+        // White halo background
+        fillRoundedRect(x, y, boxSize + (outlinePad * 2), boxSize + (outlinePad * 2), radius + 1, WHITE);
+
+        // Draw inner box
+        fillRoundedRect(x, y, boxSize, boxSize, radius, BLACK);
+
+        // Text inside
+        setFont(fontSmall);
+        setTextColor(WHITE);
+
+        // Draw actual marker on top
+        if (m.hasHopsAway && m.hopsAway > config.lora.hop_limit) {
+            printAt(x + 1, y + 1, "X", CENTER, MIDDLE);
+        } else if (!m.hasHopsAway) {
+            printAt(x + 1, y + 1, "?", CENTER, MIDDLE);
+        } else {
+            char hopStr[4];
+            snprintf(hopStr, sizeof(hopStr), "%d", m.hopsAway);
+            printAt(x, y + 1, hopStr, CENTER, MIDDLE);
+        }
+
+        // Restore default font and color
+        setFont(fontSmall);
+        setTextColor(BLACK);
+    }
+
+    // Dual map scale bars
+    int16_t horizPx = width() * 0.25f;
+    int16_t vertPx = height() * 0.25f;
+    float horizMeters = horizPx / metersToPx;
+    float vertMeters = vertPx / metersToPx;
+
+    auto formatDistance = [&](float meters, char *out, size_t len) {
+        if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
+            float feet = meters * 3.28084f;
+            if (feet < 528)
+                snprintf(out, len, "%.0f ft", feet);
+            else {
+                float miles = feet / 5280.0f;
+                snprintf(out, len, miles < 10 ? "%.1f mi" : "%.0f mi", miles);
+            }
+        } else {
+            if (meters >= 1000)
+                snprintf(out, len, "%.1f km", meters / 1000.0f);
+            else
+                snprintf(out, len, "%.0f m", meters);
+        }
+    };
+
+    // Horizontal scale bar
+    int16_t horizBarY = height() - 2;
+    int16_t horizBarX = 1;
+    drawLine(horizBarX, horizBarY, horizBarX + horizPx, horizBarY, BLACK);
+    drawLine(horizBarX, horizBarY - 3, horizBarX, horizBarY + 3, BLACK);
+    drawLine(horizBarX + horizPx, horizBarY - 3, horizBarX + horizPx, horizBarY + 3, BLACK);
+
+    char horizLabel[32];
+    formatDistance(horizMeters, horizLabel, sizeof(horizLabel));
+    int16_t horizLabelW = getTextWidth(horizLabel);
+    int16_t horizLabelH = getFont().lineHeight();
+    int16_t horizLabelX = horizBarX + horizPx + 4;
+    int16_t horizLabelY = horizBarY - horizLabelH + 1;
+    fillRect(horizLabelX - 2, horizLabelY - 1, horizLabelW + 4, horizLabelH + 2, WHITE);
+    printAt(horizLabelX, horizBarY, horizLabel, LEFT, BOTTOM);
+
+    // Vertical scale bar
+    int16_t vertBarX = 1;
+    int16_t vertBarBottom = horizBarY;
+    int16_t vertBarTop = vertBarBottom - vertPx;
+    drawLine(vertBarX, vertBarBottom, vertBarX, vertBarTop, BLACK);
+    drawLine(vertBarX - 3, vertBarBottom, vertBarX + 3, vertBarBottom, BLACK);
+    drawLine(vertBarX - 3, vertBarTop, vertBarX + 3, vertBarTop, BLACK);
+
+    char vertTopLabel[32];
+    formatDistance(vertMeters, vertTopLabel, sizeof(vertTopLabel));
+    int16_t topLabelY = vertBarTop - getFont().lineHeight() - 2;
+    int16_t topLabelW = getTextWidth(vertTopLabel);
+    int16_t topLabelH = getFont().lineHeight();
+    fillRect(vertBarX - 2, topLabelY - 1, topLabelW + 6, topLabelH + 2, WHITE);
+    printAt(vertBarX + (topLabelW / 2) + 1, topLabelY + (topLabelH / 2), vertTopLabel, CENTER, MIDDLE);
+
+    char vertBottomLabel[32];
+    formatDistance(vertMeters, vertBottomLabel, sizeof(vertBottomLabel));
+    int16_t bottomLabelY = vertBarBottom + 4;
+    int16_t bottomLabelW = getTextWidth(vertBottomLabel);
+    int16_t bottomLabelH = getFont().lineHeight();
+    fillRect(vertBarX - 2, bottomLabelY - 1, bottomLabelW + 6, bottomLabelH + 2, WHITE);
+    printAt(vertBarX + (bottomLabelW / 2) + 1, bottomLabelY + (bottomLabelH / 2), vertBottomLabel, CENTER, MIDDLE);
+
+    // Draw our node LAST with full white fill + outline
+    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
+    if (ourNode && nodeDB->hasValidPosition(ourNode)) {
+        Marker self = calculateMarker(ourNode->position.latitude_i * 1e-7, ourNode->position.longitude_i * 1e-7, false, 0);
+
+        int16_t centerX = X(0.5) + (self.eastMeters * metersToPx);
+        int16_t centerY = Y(0.5) - (self.northMeters * metersToPx);
+
+        // White fill background + halo
+        fillCircle(centerX, centerY, 8, WHITE); // big white base
+        drawCircle(centerX, centerY, 8, WHITE); // crisp edge
+
+        // Black bullseye on top
+        drawCircle(centerX, centerY, 6, BLACK);
+        fillCircle(centerX, centerY, 2, BLACK);
+
+        // Crosshairs
+        drawLine(centerX - 8, centerY, centerX + 8, centerY, BLACK);
+        drawLine(centerX, centerY - 8, centerX, centerY + 8, BLACK);
     }
 }
 
@@ -63,6 +165,12 @@ void InkHUD::MapApplet::onRender()
 
 void InkHUD::MapApplet::getMapCenter(float *lat, float *lng)
 {
+    // If we have a valid position for our own node, use that as the anchor
+    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());
+    if (ourNode && nodeDB->hasValidPosition(ourNode)) {
+        *lat = ourNode->position.latitude_i * 1e-7;
+        *lng = ourNode->position.longitude_i * 1e-7;
+    } else {
         // Find mean lat long coords
         // ============================
         // - assigning X, Y and Z values to position on Earth's surface in 3D space, relative to center of planet
@@ -70,7 +178,8 @@ void InkHUD::MapApplet::getMapCenter(float *lat, float *lng)
         // - uses tan to find angles for lat / long degrees
         //   - longitude: triangle formed by x and y (on plane of the equator)
         //   - latitude: triangle formed by z (north south),
-    //     and the line along plane of equator which stretches from earth's axis to where point xyz intersects planet's surface
+        //     and the line along plane of equator which stretches from earth's axis to where point xyz intersects planet's
+        //     surface
 
         // Working totals, averaged after nodeDB processed
         uint32_t positionCount = 0;
@@ -155,18 +264,24 @@ void InkHUD::MapApplet::getMapCenter(float *lat, float *lng)
 
         float hypotenuse = sqrt((xAvg * xAvg) + (yAvg * yAvg)); // Distance from globe's north-south axis to surface intersect
         *lat = atan2(zAvg, hypotenuse) * RAD_TO_DEG;
+    }
+
+    // Use either our node position, or the mean fallback as the center
+    latCenter = *lat;
+    lngCenter = *lng;
 
     // ----------------------------------------------
-    // This has given us the "mean position"
-    // This will be a position *somewhere* near the center of our nodes.
-    // What we actually want is to place our center so that our outermost nodes end up on the border of our map.
-    // The only real use of our "mean position" is to give us a reference frame:
-    // which direction is east, and which is west.
+    // This has given us either:
+    // - our actual position (preferred), or
+    // - a mean position (fallback if we had no fix)
+    //
+    // What we actually want is to place our center so that our outermost nodes
+    // end up on the border of our map. The only real use of our "center" is to give
+    // us a reference frame: which direction is east, and which is west.
     //------------------------------------------------
 
-    // Find furthest nodes from "mean lat long"
+    // Find furthest nodes from our center
     // ========================================
-
     float northernmost = latCenter;
     float southernmost = latCenter;
     float easternmost = lngCenter;
@@ -184,14 +299,14 @@ void InkHUD::MapApplet::getMapCenter(float *lat, float *lng)
             continue;
 
         // Check for a new top or bottom latitude
-        float lat = node->position.latitude_i * 1e-7;
-        northernmost = max(northernmost, lat);
-        southernmost = min(southernmost, lat);
+        float latNode = node->position.latitude_i * 1e-7;
+        northernmost = max(northernmost, latNode);
+        southernmost = min(southernmost, latNode);
 
         // Longitude is trickier
-        float lng = node->position.longitude_i * 1e-7;
-        float degEastward = fmod(((lng - lngCenter) + 360), 360);      // Degrees traveled east from lngCenter to reach node
-        float degWestward = abs(fmod(((lng - lngCenter) - 360), 360)); // Degrees traveled west from lngCenter to reach node
+        float lngNode = node->position.longitude_i * 1e-7;
+        float degEastward = fmod(((lngNode - lngCenter) + 360), 360);      // Degrees traveled east from lngCenter to reach node
+        float degWestward = abs(fmod(((lngNode - lngCenter) - 360), 360)); // Degrees traveled west from lngCenter to reach node
         if (degEastward < degWestward)
             easternmost = max(easternmost, lngCenter + degEastward);
         else
@@ -250,7 +365,6 @@ InkHUD::MapApplet::Marker InkHUD::MapApplet::calculateMarker(float lat, float ln
     m.hopsAway = hopsAway;
     return m;
 }
-
 // Draw a marker on the map for a node, with a shortname label, and backing box
 void InkHUD::MapApplet::drawLabeledMarker(meshtastic_NodeInfoLite *node)
 {
@@ -324,6 +438,18 @@ void InkHUD::MapApplet::drawLabeledMarker(meshtastic_NodeInfoLite *node)
         textX = labelX + paddingW;
     }
 
+    // Prevent overlap with scale bars and their labels
+    // Define a "safe zone" in the bottom-left where the scale bars and text are drawn
+    constexpr int16_t safeZoneHeight = 28; // adjust based on your label font height
+    constexpr int16_t safeZoneWidth = 60;  // adjust based on horizontal label width zone
+    bool overlapsScale = (labelY + labelH > height() - safeZoneHeight) && (labelX < safeZoneWidth);
+
+    // If it overlaps, shift label upward slightly above the safe zone
+    if (overlapsScale) {
+        labelY = height() - safeZoneHeight - labelH - 2;
+        textY = labelY + (labelH / 2);
+    }
+
     // Backing box
     fillRect(labelX, labelY, labelW, labelH, WHITE);
     drawRect(labelX, labelY, labelW, labelH, BLACK);

src/main.cpp

@@ -436,6 +436,12 @@ void setup()
 
     LOG_INFO("\n\n//\\ E S H T /\\ S T / C\n");
 
+#if defined(DEBUG_MUTE) && defined(DEBUG_PORT)
+    DEBUG_PORT.printf("\r\n\r\n//\\ E S H T /\\ S T / C\r\n");
+    DEBUG_PORT.printf("Version %s for %s from %s\r\n", optstr(APP_VERSION), optstr(APP_ENV), optstr(APP_REPO));
+    DEBUG_PORT.printf("Debug mute is enabled, there will be no serial output.\r\n");
+#endif
+
     initDeepSleep();
 
 #if defined(MODEM_POWER_EN)

src/mesh/PhoneAPI.cpp

@@ -57,6 +57,9 @@ void PhoneAPI::handleStartConfig()
 #endif
     }
 
+    // Allow subclasses to prepare for high-throughput config traffic
+    onConfigStart();
+
     // even if we were already connected - restart our state machine
     if (config_nonce == SPECIAL_NONCE_ONLY_NODES) {
         // If client only wants node info, jump directly to sending nodes
@@ -71,7 +74,7 @@ void PhoneAPI::handleStartConfig()
     spiLock->unlock();
     LOG_DEBUG("Got %d files in manifest", filesManifest.size());
 
-    LOG_INFO("Start API client config");
+    LOG_INFO("Start API client config millis=%u", millis());
     // Protect against concurrent BLE callbacks: they run in NimBLE's FreeRTOS task and also touch nodeInfoQueue.
     {
         concurrency::LockGuard guard(&nodeInfoMutex);
@@ -453,7 +456,10 @@ size_t PhoneAPI::getFromRadio(uint8_t *buf)
         break;
 
     case STATE_SEND_OTHER_NODEINFOS: {
-        LOG_DEBUG("Send known nodes");
+        if (readIndex == 2) { //  readIndex==2 will be true for the first non-us node
+            LOG_INFO("Start sending nodeinfos millis=%u", millis());
+        }
+
         meshtastic_NodeInfo infoToSend = {};
         {
             concurrency::LockGuard guard(&nodeInfoMutex);
@@ -470,13 +476,22 @@ size_t PhoneAPI::getFromRadio(uint8_t *buf)
         if (infoToSend.num != 0) {
             // Just in case we stored a different user.id in the past, but should never happen going forward
             sprintf(infoToSend.user.id, "!%08x", infoToSend.num);
-            LOG_DEBUG("nodeinfo: num=0x%x, lastseen=%u, id=%s, name=%s", infoToSend.num, infoToSend.last_heard,
-                      infoToSend.user.id, infoToSend.user.long_name);
+
+            // Logging this really slows down sending nodes on initial connection because the serial console is so slow, so only
+            // uncomment if you really need to:
+            // LOG_INFO("nodeinfo: num=0x%x, lastseen=%u, id=%s, name=%s", nodeInfoForPhone.num, nodeInfoForPhone.last_heard,
+            // nodeInfoForPhone.user.id, nodeInfoForPhone.user.long_name);
+
+            // Occasional progress logging. (readIndex==2 will be true for the first non-us node)
+            if (readIndex == 2 || readIndex % 20 == 0) {
+                LOG_DEBUG("nodeinfo: %d/%d", readIndex, nodeDB->getNumMeshNodes());
+            }
+
             fromRadioScratch.which_payload_variant = meshtastic_FromRadio_node_info_tag;
             fromRadioScratch.node_info = infoToSend;
             prefetchNodeInfos();
         } else {
-            LOG_DEBUG("Done sending nodeinfo");
+            LOG_DEBUG("Done sending %d of %d nodeinfos millis=%u", readIndex, nodeDB->getNumMeshNodes(), millis());
             concurrency::LockGuard guard(&nodeInfoMutex);
             nodeInfoQueue.clear();
             state = STATE_SEND_FILEMANIFEST;
@@ -558,11 +573,15 @@ size_t PhoneAPI::getFromRadio(uint8_t *buf)
 
 void PhoneAPI::sendConfigComplete()
 {
-    LOG_INFO("Config Send Complete");
+    LOG_INFO("Config Send Complete millis=%u", millis());
     fromRadioScratch.which_payload_variant = meshtastic_FromRadio_config_complete_id_tag;
     fromRadioScratch.config_complete_id = config_nonce;
     config_nonce = 0;
     state = STATE_SEND_PACKETS;
+
+    // Allow subclasses to know we've entered steady-state so they can lower power consumption
+    onConfigComplete();
+
     pauseBluetoothLogging = false;
 }
 

src/mesh/PhoneAPI.h

@@ -136,6 +136,7 @@ class PhoneAPI
     bool available();
 
     bool isConnected() { return state != STATE_SEND_NOTHING; }
+    bool isSendingPackets() { return state == STATE_SEND_PACKETS; }
 
   protected:
     /// Our fromradio packet while it is being assembled
@@ -158,6 +159,11 @@ class PhoneAPI
      */
     virtual void onNowHasData(uint32_t fromRadioNum) {}
 
+    /// Subclasses can use these lifecycle hooks for transport-specific behavior around config/steady-state
+    /// (i.e. BLE connection params)
+    virtual void onConfigStart() {}
+    virtual void onConfigComplete() {}
+
     /// begin a new connection
     void handleStartConfig();
 

src/mesh/Router.cpp

@@ -35,6 +35,15 @@
     (MAX_RX_TOPHONE + MAX_RX_FROMRADIO + 2 * MAX_TX_QUEUE +                                                                      \
      2) // max number of packets which can be in flight (either queued from reception or queued for sending)
 
+static MemoryDynamic<meshtastic_MeshPacket> dynamicPool;
+Allocator<meshtastic_MeshPacket> &packetPool = dynamicPool;
+#elif defined(ARCH_STM32WL)
+// On STM32 there isn't enough heap left over for the rest of the firmware if we allocate this statically.
+// For now, make it dynamic again.
+#define MAX_PACKETS                                                                                                              \
+    (MAX_RX_TOPHONE + MAX_RX_FROMRADIO + 2 * MAX_TX_QUEUE +                                                                      \
+     2) // max number of packets which can be in flight (either queued from reception or queued for sending)
+
 static MemoryDynamic<meshtastic_MeshPacket> dynamicPool;
 Allocator<meshtastic_MeshPacket> &packetPool = dynamicPool;
 #else

src/mesh/generated/meshtastic/mesh.pb.h

@@ -282,6 +282,8 @@ typedef enum _meshtastic_HardwareModel {
     meshtastic_HardwareModel_HELTEC_WIRELESS_TRACKER_V2 = 113,
     /* LilyGo T-Watch Ultra */
     meshtastic_HardwareModel_T_WATCH_ULTRA = 114,
+    /* Elecrow ThinkNode M3 */
+    meshtastic_HardwareModel_THINKNODE_M3 = 115,
     /* ------------------------------------------------------------------------------------------------------------------------------------------
  Reserved ID For developing private Ports. These will show up in live traffic sparsely, so we can use a high number. Keep it within 8 bits.
  ------------------------------------------------------------------------------------------------------------------------------------------ */

src/nimble/NimbleBluetooth.cpp

@@ -3,12 +3,15 @@
 #include "BluetoothCommon.h"
 #include "NimbleBluetooth.h"
 #include "PowerFSM.h"
+#include "StaticPointerQueue.h"
 
+#include "concurrency/OSThread.h"
 #include "main.h"
 #include "mesh/PhoneAPI.h"
 #include "mesh/mesh-pb-constants.h"
 #include "sleep.h"
 #include <NimBLEDevice.h>
+#include <atomic>
 #include <mutex>
 
 #ifdef NIMBLE_TWO
@@ -32,45 +35,276 @@ constexpr uint16_t kPreferredBleTxTimeUs = (kPreferredBleTxOctets + 14) * 8;
 } // namespace
 #endif
 
+// Debugging options: careful, they slow things down quite a bit!
+// #define DEBUG_NIMBLE_ON_READ_TIMING  // uncomment to time onRead duration
+// #define DEBUG_NIMBLE_ON_WRITE_TIMING // uncomment to time onWrite duration
+// #define DEBUG_NIMBLE_NOTIFY          // uncomment to enable notify logging
+
+#define NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE 3
+#define NIMBLE_BLUETOOTH_FROM_PHONE_QUEUE_SIZE 3
+
 NimBLECharacteristic *fromNumCharacteristic;
 NimBLECharacteristic *BatteryCharacteristic;
 NimBLECharacteristic *logRadioCharacteristic;
 NimBLEServer *bleServer;
 
 static bool passkeyShowing;
+static std::atomic<uint16_t> nimbleBluetoothConnHandle{BLE_HS_CONN_HANDLE_NONE}; // BLE_HS_CONN_HANDLE_NONE means "no connection"
 
 class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
 {
+    /*
+      CAUTION: There's a lot going on here and lots of room to break things.
+
+      This NimbleBluetooth.cpp file does some tricky synchronization between the NimBLE FreeRTOS task (which runs the onRead and
+      onWrite callbacks) and the main task (which runs runOnce and the rest of PhoneAPI).
+
+      The main idea is to add a little bit of synchronization here to make it so that the rest of the codebase doesn't have to
+      know about concurrency and mutexes, and can just run happily ever after as a cooperative multitasking OSThread system, where
+      locking isn't something that anyone has to worry about too much! :)
+
+      We achieve this by having some queues and mutexes in this file only, and ensuring that all calls to getFromRadio and
+      handleToRadio are only made from the main FreeRTOS task. This way, the rest of the codebase doesn't have to worry about
+      being run concurrently, which would make everything else much much much more complicated.
+
+      PHONE -> RADIO:
+        - [NimBLE FreeRTOS task:] onWrite callback holds fromPhoneMutex and pushes received packets into fromPhoneQueue.
+        - [Main task:] runOnceHandleFromPhoneQueue in main task holds fromPhoneMutex, pulls packets from fromPhoneQueue, and calls
+      handleToRadio **in main task**.
+
+      RADIO -> PHONE:
+        - [NimBLE FreeRTOS task:] onRead callback sets onReadCallbackIsWaitingForData flag and polls in a busy loop. (unless
+      there's already a packet waiting in toPhoneQueue)
+        - [Main task:] runOnceHandleToPhoneQueue sees onReadCallbackIsWaitingForData flag, calls getFromRadio **in main task** to
+      get packets from radio, holds toPhoneMutex, pushes the packet into toPhoneQueue, and clears the
+      onReadCallbackIsWaitingForData flag.
+        - [NimBLE FreeRTOS task:] onRead callback sees that the onReadCallbackIsWaitingForData flag cleared, holds toPhoneMutex,
+      pops the packet from toPhoneQueue, and returns it to NimBLE.
+
+      MUTEXES:
+        - fromPhoneMutex protects fromPhoneQueue and fromPhoneQueueSize
+        - toPhoneMutex protects toPhoneQueue, toPhoneQueueByteSizes, and toPhoneQueueSize
+
+      ATOMICS:
+        - fromPhoneQueueSize is only increased by onWrite, and only decreased by runOnceHandleFromPhoneQueue (or onDisconnect).
+        - toPhoneQueueSize is only increased by runOnceHandleToPhoneQueue, and only decreased by onRead (or onDisconnect).
+        - onReadCallbackIsWaitingForData is a flag. It's only set by onRead, and only cleared by runOnceHandleToPhoneQueue (or
+      onDisconnect).
+
+      PRELOADING: see comments in runOnceToPhoneCanPreloadNextPacket about when it's safe to preload packets from getFromRadio.
+
+      BLE CONNECTION PARAMS:
+        - During config, we request a high-throughput, low-latency BLE connection for speed.
+        - After config, we switch to a lower-power BLE connection for steady-state use to extend battery life.
+
+      MEMORY MANAGEMENT:
+        - We keep packets on the stack and do not allocate heap.
+        - We use std::array for fromPhoneQueue and toPhoneQueue to avoid mallocs and frees across FreeRTOS tasks.
+        - Yes, we have to do some copy operations on pop because of this, but it's worth it to avoid cross-task memory management.
+
+      NOTIFY IS BROKEN:
+        - Adding NIMBLE_PROPERTY::NOTIFY to FromRadioCharacteristic appears to break things. It is NOT backwards compatible.
+
+      ZERO-SIZE READS:
+        - Returning a zero-size read from onRead breaks some clients during the config phase. So we have to block onRead until we
+      have data.
+        - During the STATE_SEND_PACKETS phase, it's totally OK to return zero-size reads, as clients are expected to do reads
+      until they get a 0-byte response.
+
+      CROSS-TASK WAKEUP:
+        - If you call: bluetoothPhoneAPI->setIntervalFromNow(0); to schedule immediate processing of new data,
+        - Then you should also call: concurrency::mainDelay.interrupt(); to wake up the main loop if it's sleeping.
+        - Otherwise, you're going to wait ~100ms or so until the main loop wakes up from some other cause.
+    */
+
   public:
-    BluetoothPhoneAPI() : concurrency::OSThread("NimbleBluetooth") { nimble_queue.resize(3); }
-    std::vector<NimBLEAttValue> nimble_queue;
-    std::mutex nimble_mutex;
-    uint8_t queue_size = 0;
-    uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0};
-    size_t numBytes = 0;
-    bool hasChecked = false;
-    bool phoneWants = false;
+    BluetoothPhoneAPI() : concurrency::OSThread("NimbleBluetooth") {}
+
+    /* Packets from phone (BLE onWrite callback) */
+    std::mutex fromPhoneMutex;
+    std::atomic<size_t> fromPhoneQueueSize{0};
+    // We use array here (and pay the cost of memcpy) to avoid dynamic memory allocations and frees across FreeRTOS tasks.
+    std::array<NimBLEAttValue, NIMBLE_BLUETOOTH_FROM_PHONE_QUEUE_SIZE> fromPhoneQueue{};
+
+    /* Packets to phone (BLE onRead callback) */
+    std::mutex toPhoneMutex;
+    std::atomic<size_t> toPhoneQueueSize{0};
+    // We use array here (and pay the cost of memcpy) to avoid dynamic memory allocations and frees across FreeRTOS tasks.
+    std::array<std::array<uint8_t, meshtastic_FromRadio_size>, NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE> toPhoneQueue{};
+    std::array<size_t, NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE> toPhoneQueueByteSizes{};
+    // The onReadCallbackIsWaitingForData flag provides synchronization between the NimBLE task's onRead callback and our main
+    // task's runOnce. It's only set by onRead, and only cleared by runOnce.
+    std::atomic<bool> onReadCallbackIsWaitingForData{false};
+
+    /* Statistics/logging helpers */
+    std::atomic<int32_t> readCount{0};
+    std::atomic<int32_t> notifyCount{0};
+    std::atomic<int32_t> writeCount{0};
 
   protected:
     virtual int32_t runOnce() override
     {
-        std::lock_guard<std::mutex> guard(nimble_mutex);
-        if (queue_size > 0) {
-            for (uint8_t i = 0; i < queue_size; i++) {
-                handleToRadio(nimble_queue.at(i).data(), nimble_queue.at(i).length());
-            }
-            LOG_DEBUG("Queue_size %u", queue_size);
-            queue_size = 0;
-        }
-        if (!hasChecked && phoneWants) {
-            // Pull fresh data while we're outside of the NimBLE callback context.
-            numBytes = getFromRadio(fromRadioBytes);
-            hasChecked = true;
+        while (runOnceHasWorkToDo()) {
+            /*
+              PROCESS fromPhoneQueue BEFORE toPhoneQueue:
+
+              In normal STATE_SEND_PACKETS operation, it's unlikely that we'll have both writes and reads to process at the same
+              time, because either onWrite or onRead will trigger this runOnce. And in STATE_SEND_PACKETS, it's generally ok to
+              service either the reads or writes first.
+
+              However, during the initial setup wantConfig packet, the clients send a write and immediately send a read, and they
+              expect the read will respond to the write. (This also happens when a client goes from STATE_SEND_PACKETS back to
+              another wantConfig, like the iOS client does when requesting the nodedb after requesting the main config only.)
+
+              So it's safest to always service writes (fromPhoneQueue) before reads (toPhoneQueue), so that any "synchronous"
+              write-then-read sequences from the client work as expected, even if this means we block onRead for a while: this is
+              what the client wants!
+            */
+
+            // PHONE -> RADIO:
+            runOnceHandleFromPhoneQueue(); // pull data from onWrite to handleToRadio
+
+            // RADIO -> PHONE:
+            runOnceHandleToPhoneQueue(); // push data from getFromRadio to onRead
         }
 
         // the run is triggered via NimbleBluetoothToRadioCallback and NimbleBluetoothFromRadioCallback
         return INT32_MAX;
     }
+
+    virtual void onConfigStart() override
+    {
+        LOG_INFO("BLE onConfigStart");
+
+        // Prefer high throughput during config/setup, at the cost of high power consumption (for a few seconds)
+        if (bleServer && isConnected()) {
+            uint16_t conn_handle = nimbleBluetoothConnHandle.load();
+            if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+                requestHighThroughputConnection(conn_handle);
+            }
+        }
+    }
+
+    virtual void onConfigComplete() override
+    {
+        LOG_INFO("BLE onConfigComplete");
+
+        // Switch to lower power consumption BLE connection params for steady-state use after config/setup is complete
+        if (bleServer && isConnected()) {
+            uint16_t conn_handle = nimbleBluetoothConnHandle.load();
+            if (conn_handle != BLE_HS_CONN_HANDLE_NONE) {
+                requestLowerPowerConnection(conn_handle);
+            }
+        }
+    }
+
+    bool runOnceHasWorkToDo() { return runOnceHasWorkToPhone() || runOnceHasWorkFromPhone(); }
+
+    bool runOnceHasWorkToPhone() { return onReadCallbackIsWaitingForData || runOnceToPhoneCanPreloadNextPacket(); }
+
+    bool runOnceToPhoneCanPreloadNextPacket()
+    {
+        /*
+         * PRELOADING getFromRadio RESPONSES:
+         *
+         * It's not safe to preload packets if we're in STATE_SEND_PACKETS, because there may be a while between the time we call
+         * getFromRadio and when the client actually reads it. If the connection drops in that time, we might lose that packet
+         * forever. In STATE_SEND_PACKETS, if we wait for onRead before we call getFromRadio, we minimize the time window where
+         * the client might disconnect before completing the read.
+         *
+         * However, if we're in the setup states (sending config, nodeinfo, etc), it's safe and beneficial to preload packets into
+         * toPhoneQueue because the client will just reconnect after a disconnect, losing nothing.
+         */
+
+        if (!isConnected()) {
+            return false;
+        } else if (isSendingPackets()) {
+            // If we're in STATE_SEND_PACKETS, we must wait for onRead before calling getFromRadio.
+            return false;
+        } else {
+            // In other states, we can preload as long as there's space in the toPhoneQueue.
+            return toPhoneQueueSize < NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE;
+        }
+    }
+
+    void runOnceHandleToPhoneQueue()
+    {
+        // Stack buffer for getFromRadio packet
+        uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0};
+        size_t numBytes = 0;
+
+        if (onReadCallbackIsWaitingForData || runOnceToPhoneCanPreloadNextPacket()) {
+            numBytes = getFromRadio(fromRadioBytes);
+
+            if (numBytes == 0) {
+                /*
+                  Client expected a read, but we have nothing to send.
+
+                  In STATE_SEND_PACKETS, it is 100% OK to return a 0-byte response, as we expect clients to do read beyond
+                  notifies regularly, to make sure they have nothing else to read.
+
+                  In other states, this is fine **so long as we've already processed pending onWrites first**, because the client
+                  may requesting wantConfig and immediately doing a read.
+                */
+            } else {
+                // Push to toPhoneQueue, protected by toPhoneMutex. Hold the mutex as briefly as possible.
+                if (toPhoneQueueSize < NIMBLE_BLUETOOTH_TO_PHONE_QUEUE_SIZE) {
+                    // Note: the comparison above is safe without a mutex because we are the only method that *increases*
+                    // toPhoneQueueSize. (It's okay if toPhoneQueueSize *decreases* in the NimBLE task meanwhile.)
+
+                    { // scope for toPhoneMutex mutex
+                        std::lock_guard<std::mutex> guard(toPhoneMutex);
+                        size_t storeAtIndex = toPhoneQueueSize.load();
+                        memcpy(toPhoneQueue[storeAtIndex].data(), fromRadioBytes, numBytes);
+                        toPhoneQueueByteSizes[storeAtIndex] = numBytes;
+                        toPhoneQueueSize++;
+                    }
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                    LOG_DEBUG("BLE getFromRadio returned numBytes=%u, pushed toPhoneQueueSize=%u", numBytes,
+                              toPhoneQueueSize.load());
+#endif
+                } else {
+                    // Shouldn't happen because the onRead callback shouldn't be waiting if the queue is full!
+                    LOG_ERROR("Shouldn't happen! Drop FromRadio packet, toPhoneQueue full (%u bytes)", numBytes);
+                }
+            }
+
+            // Clear the onReadCallbackIsWaitingForData flag so onRead knows it can proceed.
+            onReadCallbackIsWaitingForData = false; // only clear this flag AFTER the push
+        }
+    }
+
+    bool runOnceHasWorkFromPhone() { return fromPhoneQueueSize > 0; }
+
+    void runOnceHandleFromPhoneQueue()
+    {
+        // Handle packets we received from onWrite from the phone.
+        if (fromPhoneQueueSize > 0) {
+            // Note: the comparison above is safe without a mutex because we are the only method that *decreases*
+            // fromPhoneQueueSize. (It's okay if fromPhoneQueueSize *increases* in the NimBLE task meanwhile.)
+
+            LOG_DEBUG("NimbleBluetooth: handling ToRadio packet, fromPhoneQueueSize=%u", fromPhoneQueueSize.load());
+
+            // Pop the front of fromPhoneQueue, holding the mutex only briefly while we pop.
+            NimBLEAttValue val;
+            { // scope for fromPhoneMutex mutex
+                std::lock_guard<std::mutex> guard(fromPhoneMutex);
+                val = fromPhoneQueue[0];
+
+                // Shift the rest of the queue down
+                for (uint8_t i = 1; i < fromPhoneQueueSize; i++) {
+                    fromPhoneQueue[i - 1] = fromPhoneQueue[i];
+                }
+
+                // Safe decrement due to onDisconnect
+                if (fromPhoneQueueSize > 0)
+                    fromPhoneQueueSize--;
+            }
+
+            handleToRadio(val.data(), val.length());
+        }
+    }
+
     /**
      * Subclasses can use this as a hook to provide custom notifications for their transport (i.e. bluetooth notifies)
      */
@@ -78,14 +312,22 @@ class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
     {
         PhoneAPI::onNowHasData(fromRadioNum);
 
+        int currentNotifyCount = notifyCount.fetch_add(1);
+
         uint8_t cc = bleServer->getConnectedCount();
-        LOG_DEBUG("BLE notify fromNum: %d connections: %d", fromRadioNum, cc);
+
+#ifdef DEBUG_NIMBLE_NOTIFY
+        // This logging slows things down when there are lots of packets going to the phone, like initial connection:
+        LOG_DEBUG("BLE notify(%d) fromNum: %d connections: %d", currentNotifyCount, fromRadioNum, cc);
+#endif
 
         uint8_t val[4];
         put_le32(val, fromRadioNum);
 
         fromNumCharacteristic->setValue(val, sizeof(val));
 #ifdef NIMBLE_TWO
+        // NOTE: I don't have any NIMBLE_TWO devices, but this line makes me suspicious, and I suspect it needs to just be
+        // notify().
         fromNumCharacteristic->notify(val, sizeof(val), BLE_HS_CONN_HANDLE_NONE);
 #else
         fromNumCharacteristic->notify();
@@ -94,6 +336,54 @@ class BluetoothPhoneAPI : public PhoneAPI, public concurrency::OSThread
 
     /// Check the current underlying physical link to see if the client is currently connected
     virtual bool checkIsConnected() { return bleServer && bleServer->getConnectedCount() > 0; }
+
+    void requestHighThroughputConnection(uint16_t conn_handle)
+    {
+        /* Request a lower-latency, higher-throughput BLE connection.
+
+        This comes at the cost of higher power consumption, so we may want to only use this for initial setup, and then switch to
+        a slower mode.
+
+        See https://developer.apple.com/library/archive/qa/qa1931/_index.html for formulas to calculate values, iOS/macOS
+        constraints, and recommendations. (Android doesn't have specific constraints, but seems to be compatible with the Apple
+        recommendations.)
+
+        Selected settings:
+            minInterval (units of 1.25ms): 7.5ms = 6 (lower than the Apple recommended minimum, but allows faster when the client
+        supports it.)
+            maxInterval (units of 1.25ms): 15ms = 12
+            latency: 0 (don't allow peripheral to skip any connection events)
+            timeout (units of 10ms): 6 seconds = 600 (supervision timeout)
+
+        These are intentionally aggressive to prioritize speed over power consumption, but are only used for a few seconds at
+        setup. Not worth adjusting much.
+        */
+        LOG_INFO("BLE requestHighThroughputConnection");
+        bleServer->updateConnParams(conn_handle, 6, 12, 0, 600);
+    }
+
+    void requestLowerPowerConnection(uint16_t conn_handle)
+    {
+        /* Request a lower power consumption (but higher latency, lower throughput) BLE connection.
+
+        This is suitable for steady-state operation after initial setup is complete.
+
+        See https://developer.apple.com/library/archive/qa/qa1931/_index.html for formulas to calculate values, iOS/macOS
+        constraints, and recommendations. (Android doesn't have specific constraints, but seems to be compatible with the Apple
+        recommendations.)
+
+        Selected settings:
+            minInterval (units of 1.25ms): 30ms = 24
+            maxInterval (units of 1.25ms): 50ms = 40
+            latency: 2 (allow peripheral to skip up to 2 consecutive connection events to save power)
+            timeout (units of 10ms): 6 seconds = 600 (supervision timeout)
+
+        There's an opportunity for tuning here if anyone wants to do some power measurements, but these should allow 10-20 packets
+        per second.
+        */
+        LOG_INFO("BLE requestLowerPowerConnection");
+        bleServer->updateConnParams(conn_handle, 24, 40, 2, 600);
+    }
 };
 
 static BluetoothPhoneAPI *bluetoothPhoneAPI;
@@ -113,18 +403,45 @@ class NimbleBluetoothToRadioCallback : public NimBLECharacteristicCallbacks
 
 #endif
     {
+        // CAUTION: This callback runs in the NimBLE task!!! Don't do anything except communicate with the main task's runOnce.
+        // Assumption: onWrite is serialized by NimBLE, so we don't need to lock here against multiple concurrent onWrite calls.
+
+        int currentWriteCount = bluetoothPhoneAPI->writeCount.fetch_add(1);
+
+#ifdef DEBUG_NIMBLE_ON_WRITE_TIMING
+        int startMillis = millis();
+        LOG_DEBUG("BLE onWrite(%d): start millis=%d", currentWriteCount, startMillis);
+#endif
+
         auto val = pCharacteristic->getValue();
 
         if (memcmp(lastToRadio, val.data(), val.length()) != 0) {
-            if (bluetoothPhoneAPI->queue_size < 3) {
+            if (bluetoothPhoneAPI->fromPhoneQueueSize < NIMBLE_BLUETOOTH_FROM_PHONE_QUEUE_SIZE) {
+                // Note: the comparison above is safe without a mutex because we are the only method that *increases*
+                // fromPhoneQueueSize. (It's okay if fromPhoneQueueSize *decreases* in the main task meanwhile.)
                 memcpy(lastToRadio, val.data(), val.length());
-                std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex);
-                bluetoothPhoneAPI->nimble_queue.at(bluetoothPhoneAPI->queue_size) = val;
-                bluetoothPhoneAPI->queue_size++;
+
+                { // scope for fromPhoneMutex mutex
+                    // Append to fromPhoneQueue, protected by fromPhoneMutex. Hold the mutex as briefly as possible.
+                    std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->fromPhoneMutex);
+                    bluetoothPhoneAPI->fromPhoneQueue.at(bluetoothPhoneAPI->fromPhoneQueueSize) = val;
+                    bluetoothPhoneAPI->fromPhoneQueueSize++;
+                }
+
+                // After releasing the mutex, schedule immediate processing of the new packet.
                 bluetoothPhoneAPI->setIntervalFromNow(0);
+                concurrency::mainDelay.interrupt(); // wake up main loop if sleeping
+
+#ifdef DEBUG_NIMBLE_ON_WRITE_TIMING
+                int finishMillis = millis();
+                LOG_DEBUG("BLE onWrite(%d): append to fromPhoneQueue took %u ms. numBytes=%d", currentWriteCount,
+                          finishMillis - startMillis, val.length());
+#endif
+            } else {
+                LOG_WARN("BLE onWrite(%d): Drop ToRadio packet, fromPhoneQueue full (%u bytes)", currentWriteCount, val.length());
             }
         } else {
-            LOG_DEBUG("Drop duplicate ToRadio packet (%u bytes)", val.length());
+            LOG_DEBUG("BLE onWrite(%d): Drop duplicate ToRadio packet (%u bytes)", currentWriteCount, val.length());
         }
     }
 };
@@ -137,32 +454,107 @@ class NimbleBluetoothFromRadioCallback : public NimBLECharacteristicCallbacks
     virtual void onRead(NimBLECharacteristic *pCharacteristic)
 #endif
     {
-        bluetoothPhoneAPI->phoneWants = true;
-        bluetoothPhoneAPI->setIntervalFromNow(0);
-        std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex); // BLE callbacks run in NimBLE task
+        // CAUTION: This callback runs in the NimBLE task!!! Don't do anything except communicate with the main task's runOnce.
 
-        if (!bluetoothPhoneAPI->hasChecked) {
-            // Fetch payload on demand; prefetch keeps this fast for the first read.
-            bluetoothPhoneAPI->numBytes = bluetoothPhoneAPI->getFromRadio(bluetoothPhoneAPI->fromRadioBytes);
-            bluetoothPhoneAPI->hasChecked = true;
-        }
+        int currentReadCount = bluetoothPhoneAPI->readCount.fetch_add(1);
+        int tries = 0;
+        int startMillis = millis();
 
-        pCharacteristic->setValue(bluetoothPhoneAPI->fromRadioBytes, bluetoothPhoneAPI->numBytes);
-
-        if (bluetoothPhoneAPI->numBytes != 0) {
-#ifdef NIMBLE_TWO
-            // Notify immediately so subscribed clients see the packet without an extra read.
-            pCharacteristic->notify(bluetoothPhoneAPI->fromRadioBytes, bluetoothPhoneAPI->numBytes, BLE_HS_CONN_HANDLE_NONE);
-#else
-            pCharacteristic->notify();
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+        LOG_DEBUG("BLE onRead(%d): start millis=%d", currentReadCount, startMillis);
 #endif
+
+        // Is there a packet ready to go, or do we have to ask the main task to get one for us?
+        if (bluetoothPhoneAPI->toPhoneQueueSize > 0) {
+            // Note: the comparison above is safe without a mutex because we are the only method that *decreases*
+            // toPhoneQueueSize. (It's okay if toPhoneQueueSize *increases* in the main task meanwhile.)
+
+            // There's already a packet queued. Great! We don't need to wait for onReadCallbackIsWaitingForData.
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+            LOG_DEBUG("BLE onRead(%d): packet already waiting, no need to set onReadCallbackIsWaitingForData", currentReadCount);
+#endif
+        } else {
+            // Tell the main task that we'd like a packet.
+            bluetoothPhoneAPI->onReadCallbackIsWaitingForData = true;
+
+            // Wait for the main task to produce a packet for us, up to about 20 seconds.
+            // It normally takes just a few milliseconds, but at initial startup, etc, the main task can get blocked for longer
+            // doing various setup tasks.
+            while (bluetoothPhoneAPI->onReadCallbackIsWaitingForData && tries < 4000) {
+                // Schedule the main task runOnce to run ASAP.
+                bluetoothPhoneAPI->setIntervalFromNow(0);
+                concurrency::mainDelay.interrupt(); // wake up main loop if sleeping
+
+                if (!bluetoothPhoneAPI->onReadCallbackIsWaitingForData) {
+                    // we may be able to break even before a delay, if the call to interrupt woke up the main loop and it ran
+                    // already
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+                    LOG_DEBUG("BLE onRead(%d): broke before delay after %u ms, %d tries", currentReadCount,
+                              millis() - startMillis, tries);
+#endif
+                    break;
                 }
 
-        if (bluetoothPhoneAPI->numBytes != 0) // if we did send something, queue it up right away to reload
+                // This delay happens in the NimBLE FreeRTOS task, which really can't do anything until we get a value back.
+                // No harm in polling pretty frequently.
+                delay(tries < 20 ? 1 : 5);
+                tries++;
+
+                if (tries == 4000) {
+                    LOG_WARN(
+                        "BLE onRead(%d): timeout waiting for data after %u ms, %d tries, giving up and returning 0-size response",
+                        currentReadCount, millis() - startMillis, tries);
+                }
+            }
+        }
+
+        // Pop from toPhoneQueue, protected by toPhoneMutex. Hold the mutex as briefly as possible.
+        uint8_t fromRadioBytes[meshtastic_FromRadio_size] = {0}; // Stack buffer for getFromRadio packet
+        size_t numBytes = 0;
+        { // scope for toPhoneMutex mutex
+            std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->toPhoneMutex);
+            size_t toPhoneQueueSize = bluetoothPhoneAPI->toPhoneQueueSize.load();
+            if (toPhoneQueueSize > 0) {
+                // Copy from the front of the toPhoneQueue
+                memcpy(fromRadioBytes, bluetoothPhoneAPI->toPhoneQueue[0].data(), bluetoothPhoneAPI->toPhoneQueueByteSizes[0]);
+                numBytes = bluetoothPhoneAPI->toPhoneQueueByteSizes[0];
+
+                // Shift the rest of the queue down
+                for (uint8_t i = 1; i < toPhoneQueueSize; i++) {
+                    memcpy(bluetoothPhoneAPI->toPhoneQueue[i - 1].data(), bluetoothPhoneAPI->toPhoneQueue[i].data(),
+                           bluetoothPhoneAPI->toPhoneQueueByteSizes[i]);
+                    // The above line is similar to:
+                    //   bluetoothPhoneAPI->toPhoneQueue[i - 1] = bluetoothPhoneAPI->toPhoneQueue[i]
+                    // but is usually faster because it doesn't have to copy all the trailing bytes beyond
+                    // toPhoneQueueByteSizes[i].
+                    //
+                    // We deliberately use an array here (and pay the CPU cost of some memcpy) to avoid synchronizing dynamic
+                    // memory allocations and frees across FreeRTOS tasks.
+
+                    bluetoothPhoneAPI->toPhoneQueueByteSizes[i - 1] = bluetoothPhoneAPI->toPhoneQueueByteSizes[i];
+                }
+
+                // Safe decrement due to onDisconnect
+                if (bluetoothPhoneAPI->toPhoneQueueSize > 0)
+                    bluetoothPhoneAPI->toPhoneQueueSize--;
+            } else {
+                // nothing in the toPhoneQueue; that's fine, and we'll just have numBytes=0.
+            }
+        }
+
+#ifdef DEBUG_NIMBLE_ON_READ_TIMING
+        int finishMillis = millis();
+        LOG_DEBUG("BLE onRead(%d): onReadCallbackIsWaitingForData took %u ms, %d tries. numBytes=%d", currentReadCount,
+                  finishMillis - startMillis, tries, numBytes);
+#endif
+
+        pCharacteristic->setValue(fromRadioBytes, numBytes);
+
+        // If we sent something, wake up the main loop if it's sleeping in case there are more packets ready to enqueue.
+        if (numBytes != 0) {
             bluetoothPhoneAPI->setIntervalFromNow(0);
-        bluetoothPhoneAPI->numBytes = 0;
-        bluetoothPhoneAPI->hasChecked = false;
-        bluetoothPhoneAPI->phoneWants = false;
+            concurrency::mainDelay.interrupt(); // wake up main loop if sleeping
+        }
     }
 };
 
@@ -244,6 +636,13 @@ class NimbleBluetoothServerCallback : public NimBLEServerCallbacks
             if (screen)
                 screen->endAlert();
         }
+
+        // Store the connection handle for future use
+#ifdef NIMBLE_TWO
+        nimbleBluetoothConnHandle = connInfo.getConnHandle();
+#else
+        nimbleBluetoothConnHandle = desc->conn_handle;
+#endif
     }
 
 #ifdef NIMBLE_TWO
@@ -290,16 +689,29 @@ class NimbleBluetoothServerCallback : public NimBLEServerCallbacks
         bluetoothStatus->updateStatus(&newStatus);
 
         if (bluetoothPhoneAPI) {
-            std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->nimble_mutex);
             bluetoothPhoneAPI->close();
-            bluetoothPhoneAPI->numBytes = 0;
-            bluetoothPhoneAPI->queue_size = 0;
-            bluetoothPhoneAPI->hasChecked = false;
-            bluetoothPhoneAPI->phoneWants = false;
+
+            { // scope for fromPhoneMutex mutex
+                std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->fromPhoneMutex);
+                bluetoothPhoneAPI->fromPhoneQueueSize = 0;
+            }
+
+            bluetoothPhoneAPI->onReadCallbackIsWaitingForData = false;
+            { // scope for toPhoneMutex mutex
+                std::lock_guard<std::mutex> guard(bluetoothPhoneAPI->toPhoneMutex);
+                bluetoothPhoneAPI->toPhoneQueueSize = 0;
+            }
+
+            bluetoothPhoneAPI->readCount = 0;
+            bluetoothPhoneAPI->notifyCount = 0;
+            bluetoothPhoneAPI->writeCount = 0;
         }
 
         // Clear the last ToRadio packet buffer to avoid rejecting first packet from new connection
         memset(lastToRadio, 0, sizeof(lastToRadio));
+
+        nimbleBluetoothConnHandle = BLE_HS_CONN_HANDLE_NONE; // BLE_HS_CONN_HANDLE_NONE means "no connection"
+
 #ifdef NIMBLE_TWO
         // Restart Advertising
         ble->startAdvertising();
@@ -436,17 +848,15 @@ void NimbleBluetooth::setupService()
     if (config.bluetooth.mode == meshtastic_Config_BluetoothConfig_PairingMode_NO_PIN) {
         ToRadioCharacteristic = bleService->createCharacteristic(TORADIO_UUID, NIMBLE_PROPERTY::WRITE);
         // Allow notifications so phones can stream FromRadio without polling.
-        FromRadioCharacteristic =
-            bleService->createCharacteristic(FROMRADIO_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::NOTIFY);
+        FromRadioCharacteristic = bleService->createCharacteristic(FROMRADIO_UUID, NIMBLE_PROPERTY::READ);
         fromNumCharacteristic = bleService->createCharacteristic(FROMNUM_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ);
         logRadioCharacteristic =
             bleService->createCharacteristic(LOGRADIO_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ, 512U);
     } else {
         ToRadioCharacteristic = bleService->createCharacteristic(
             TORADIO_UUID, NIMBLE_PROPERTY::WRITE | NIMBLE_PROPERTY::WRITE_AUTHEN | NIMBLE_PROPERTY::WRITE_ENC);
-        FromRadioCharacteristic =
-            bleService->createCharacteristic(FROMRADIO_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_AUTHEN |
-                                                                 NIMBLE_PROPERTY::READ_ENC | NIMBLE_PROPERTY::NOTIFY);
+        FromRadioCharacteristic = bleService->createCharacteristic(
+            FROMRADIO_UUID, NIMBLE_PROPERTY::READ | NIMBLE_PROPERTY::READ_AUTHEN | NIMBLE_PROPERTY::READ_ENC);
         fromNumCharacteristic =
             bleService->createCharacteristic(FROMNUM_UUID, NIMBLE_PROPERTY::NOTIFY | NIMBLE_PROPERTY::READ |
                                                                NIMBLE_PROPERTY::READ_AUTHEN | NIMBLE_PROPERTY::READ_ENC);

variants/esp32/tlora_v2_1_16/platformio.ini

@@ -5,3 +5,12 @@ board_check = true
 build_flags = 
   ${esp32_base.build_flags} -D TLORA_V2_1_16 -I variants/esp32/tlora_v2_1_16
 upload_speed = 115200
+
+[env:sugarcube]
+extends = env:tlora-v2-1-1_6
+board_level = extra
+build_flags =
+  ${env:tlora-v2-1-1_6.build_flags}
+  -DBUTTON_PIN=0
+  -DPIN_BUZZER=25
+  -DLED_PIN=-1

variants/esp32/tlora_v2_1_16/variant.h

@@ -8,7 +8,11 @@
 #define I2C_SDA 21 // I2C pins for this board
 #define I2C_SCL 22
 
+#if defined(LED_PIN) && LED_PIN == -1
+#undef LED_PIN
+#else
 #define LED_PIN 25 // If defined we will blink this LED
+#endif
 
 #define USE_RF95
 #define LORA_DIO0 26 // a No connect on the SX1262 module