From 782118d46ce4af05389d097df75d6ae95cf6f4a4 Mon Sep 17 00:00:00 2001
From: Wessel Nieboer <wessel@weebl.me>
Date: Mon, 2 Mar 2026 23:19:36 +0100
Subject: [PATCH 1/2] Peek & pop separately when pushing messages to BLE client

I've noticed very often that the first few messages can get lost after
reconnecting with BLE. This should fix that.
---
 examples/companion_radio/MyMesh.cpp | 26 +++++++++++++++++++++++---
 examples/companion_radio/MyMesh.h   |  2 ++
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/examples/companion_radio/MyMesh.cpp b/examples/companion_radio/MyMesh.cpp
index 1f71a9bc6..937474768 100644
--- a/examples/companion_radio/MyMesh.cpp
+++ b/examples/companion_radio/MyMesh.cpp
@@ -245,6 +245,24 @@ int MyMesh::getFromOfflineQueue(uint8_t frame[]) {
   return 0; // queue is empty
 }
 
+int MyMesh::peekOfflineQueue(uint8_t frame[]) {
+  if (offline_queue_len > 0) {
+    size_t len = offline_queue[0].len;
+    memcpy(frame, offline_queue[0].buf, len);
+    return len;
+  }
+  return 0;
+}
+
+void MyMesh::popOfflineQueue() {
+  if (offline_queue_len > 0) {
+    offline_queue_len--;
+    for (int i = 0; i < offline_queue_len; i++) {
+      offline_queue[i] = offline_queue[i + 1];
+    }
+  }
+}
+
 float MyMesh::getAirtimeBudgetFactor() const {
   return _prefs.airtime_factor;
 }
@@ -1228,11 +1246,13 @@ void MyMesh::handleCmdFrame(size_t len) {
     }
   } else if (cmd_frame[0] == CMD_SYNC_NEXT_MESSAGE) {
     int out_len;
-    if ((out_len = getFromOfflineQueue(out_frame)) > 0) {
-      _serial->writeFrame(out_frame, out_len);
+    if ((out_len = peekOfflineQueue(out_frame)) > 0) {
+      if (_serial->writeFrame(out_frame, out_len) > 0) {
+        popOfflineQueue();
 #ifdef DISPLAY_CLASS
-      if (_ui) _ui->msgRead(offline_queue_len);
+        if (_ui) _ui->msgRead(offline_queue_len);
 #endif
+      }
     } else {
       out_frame[0] = RESP_CODE_NO_MORE_MESSAGES;
       _serial->writeFrame(out_frame, 1);
diff --git a/examples/companion_radio/MyMesh.h b/examples/companion_radio/MyMesh.h
index fe2c19bf2..dcfd903ba 100644
--- a/examples/companion_radio/MyMesh.h
+++ b/examples/companion_radio/MyMesh.h
@@ -171,6 +171,8 @@ class MyMesh : public BaseChatMesh, public DataStoreHost {
   void updateContactFromFrame(ContactInfo &contact, uint32_t& last_mod, const uint8_t *frame, int len);
   void addToOfflineQueue(const uint8_t frame[], int len);
   int getFromOfflineQueue(uint8_t frame[]);
+  int peekOfflineQueue(uint8_t frame[]);
+  void popOfflineQueue();
   int getBlobByKey(const uint8_t key[], int key_len, uint8_t dest_buf[]) override { 
     return _store->getBlobByKey(key, key_len, dest_buf);
   }

From ef73ffe25d322da03e3f682528bc21025ce325fd Mon Sep 17 00:00:00 2001
From: Wessel Nieboer <wessel@weebl.me>
Date: Wed, 4 Mar 2026 21:07:53 +0100
Subject: [PATCH 2/2] >= out_len

It shouldn't ever happen that it's less so why not
---
 examples/companion_radio/MyMesh.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/companion_radio/MyMesh.cpp b/examples/companion_radio/MyMesh.cpp
index 937474768..0cfb0f50a 100644
--- a/examples/companion_radio/MyMesh.cpp
+++ b/examples/companion_radio/MyMesh.cpp
@@ -1247,7 +1247,7 @@ void MyMesh::handleCmdFrame(size_t len) {
   } else if (cmd_frame[0] == CMD_SYNC_NEXT_MESSAGE) {
     int out_len;
     if ((out_len = peekOfflineQueue(out_frame)) > 0) {
-      if (_serial->writeFrame(out_frame, out_len) > 0) {
+      if (_serial->writeFrame(out_frame, out_len) >= out_len) {
         popOfflineQueue();
 #ifdef DISPLAY_CLASS
         if (_ui) _ui->msgRead(offline_queue_len);