diff --git a/examples/companion_radio/MyMesh.cpp b/examples/companion_radio/MyMesh.cpp index 755c56d14c0..74319fe6d54 100644 --- a/examples/companion_radio/MyMesh.cpp +++ b/examples/companion_radio/MyMesh.cpp @@ -259,6 +259,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; } @@ -1416,9 +1434,11 @@ 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 (_listener) _listener->onQueueSizeChanged(offline_queue_len); + if ((out_len = peekOfflineQueue(out_frame)) > 0) { + if (_serial->writeFrame(out_frame, out_len) >= out_len) { + popOfflineQueue(); + if (_listener) _listener->onQueueSizeChanged(offline_queue_len); + } } 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 944790df922..180e81b4aa2 100644 --- a/examples/companion_radio/MyMesh.h +++ b/examples/companion_radio/MyMesh.h @@ -223,6 +223,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); } diff --git a/src/helpers/MultiSerialInterface.h b/src/helpers/MultiSerialInterface.h index f7742b24c38..a3d9a5fbc81 100644 --- a/src/helpers/MultiSerialInterface.h +++ b/src/helpers/MultiSerialInterface.h @@ -163,18 +163,22 @@ class MultiSerialInterface : public BaseSerialInterface { return 0; } - // write frame to all enabled interfaces + // write frame to all enabled interfaces that have a client connected + // (an idle interface with no client must not count as a failed write) + bool anyWritten = false; bool allSuccessful = true; for(auto iface : _interfaces){ - if(iface.instance && iface.instance->isEnabled()){ - if(iface.instance->writeFrame(src, len) != len){ + if(iface.instance && iface.instance->isEnabled() && iface.instance->isConnected()){ + if(iface.instance->writeFrame(src, len) == len){ + anyWritten = true; + } else { allSuccessful = false; } } } - // report success if all writes completed successfully - return allSuccessful ? len : 0; + // report success if all connected interfaces accepted the frame + return (anyWritten && allSuccessful) ? len : 0; } size_t checkRecvFrame(uint8_t dest[]) override {