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

@@ -254,6 +254,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;
 }
@@ -1353,11 +1371,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) {
+    if ((out_len = peekOfflineQueue(out_frame)) > 0) {
-      _serial->writeFrame(out_frame, out_len);
+      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);

examples/companion_radio/MyMesh.h

@@ -188,6 +188,8 @@ private:
   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);
   }