diff --git a/examples/companion_radio/MyMesh.cpp b/examples/companion_radio/MyMesh.cpp index 1f71a9bc6..0cfb0f50a 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) >= out_len) { + 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); }