fix: keep a repeater frame until the radio queue takes it

The frame was popped before queueTx(), so whenever the radio queue was busy
it was lost.  Peek it instead, only remove it once it is queued (or TX is
suspended), and don't try while the queue has no room.  The repeater buffer
moves to TxBuffer.h/.cpp so it can be unit tested.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

platformio.ini

@@ -104,7 +104,7 @@ lib_deps =
 platform = native
 test_framework = unity
 test_build_src = yes
-build_src_filter = -<*> +<Utils.cpp> +<SomfyFrame.cpp> +<JsonFormatter.cpp> +<ShadeMotion.cpp>
+build_src_filter = -<*> +<Utils.cpp> +<SomfyFrame.cpp> +<JsonFormatter.cpp> +<ShadeMotion.cpp> +<TxBuffer.cpp>
 build_flags =
 	-std=gnu++17
 	-Isrc

src/Somfy.cpp

@@ -3805,52 +3805,6 @@ static int16_t  bitMin = SYMBOL * TOLERANCE_MIN;
 static somfy_rx_t somfy_rx;
 static somfy_rx_queue_t rx_queue;
 static somfy_tx_queue_t tx_queue;
-bool somfy_tx_queue_t::pop(somfy_tx_t *tx) {
-  // Read the oldest index.
-  for(int8_t i = MAX_TX_BUFFER - 1; i >= 0; i--) {
-    if(this->index[i] < MAX_TX_BUFFER) {
-      uint8_t ndx = this->index[i];
-      memcpy(tx, &this->items[ndx], sizeof(somfy_tx_t));
-      this->items[ndx].clear();
-      if(this->length > 0) this->length--;
-      this->index[i] = 255;
-      return true;
-    }
-  }
-  return false;
-}
-void somfy_tx_queue_t::push(somfy_rx_t *rx) { this->push(rx->cpt_synchro_hw, rx->payload, rx->bit_length); }
-void somfy_tx_queue_t::push(uint8_t hwsync, uint8_t *payload, uint8_t bit_length) {
-  if(this->length >= MAX_TX_BUFFER) {
-    // We have overflowed the buffer simply empty the last item
-    // in this instance we will simply throw it away.
-    uint8_t ndx = this->index[MAX_TX_BUFFER - 1];
-    if(ndx < MAX_TX_BUFFER) this->items[ndx].clear();
-    this->index[MAX_TX_BUFFER - 1] = 255;
-    this->length--;
-  }
-  uint8_t first = 0;
-  // Place this record in the first empty slot.  There will
-  // be one since we cleared a space above should there
-  // be an overflow.
-  for(uint8_t i = 0; i < MAX_TX_BUFFER; i++) {
-    if(this->items[i].bit_length == 0) {
-      first = i;
-      this->items[i].bit_length = bit_length;
-      this->items[i].hwsync = hwsync;
-      memcpy(&this->items[i].payload, payload, sizeof(this->items[i].payload));
-      break;
-    }
-  }
-  // Move the index so that it is the at position 0.  The oldest item will fall off.
-  for(uint8_t i = MAX_TX_BUFFER - 1; i > 0; i--) {
-    this->index[i] = this->index[i - 1];
-  }
-  this->length++;
-  // When popping from the queue we always pull from the end
-  this->index[0] = first;
-  this->delay_time = millis() + TX_QUEUE_DELAY; // We do not want to process this frame until a full frame beat has passed.
-}
 void somfy_rx_queue_t::init() { 
   ESP_LOGD(TAG, "Initializing RX Queue");
   for (uint8_t i = 0; i < MAX_RX_BUFFER; i++)
@@ -4715,7 +4669,9 @@ void Transceiver::loop() {
   else if (this->receive(&rx)) {
     for(uint8_t i = 0; i < SOMFY_MAX_REPEATERS; i++) {
       if(somfy.repeaters[i] == frame.remoteAddress) {
-        tx_queue.push(&rx);
+        tx_queue.push(rx.cpt_synchro_hw, rx.payload, rx.bit_length);
+        // We do not want to process this frame until a full frame beat has passed.
+        tx_queue.delay_time = millis() + TX_QUEUE_DELAY;
         ESP_LOGD(TAG, "Queued repeater frame...");
         break;
       }
@@ -4725,11 +4681,14 @@ void Transceiver::loop() {
   else {
     somfy.processWaitingFrame();
     // Check to see if there is anything in the buffer
-    if(tx_queue.length > 0 && millis() > tx_queue.delay_time && somfy_rx.cpt_synchro_hw == 0) {
+    // Repeater frames are handed to the radio task like any other transmission.
-      // Repeater frames are handed to the radio task like any other transmission.
+    // One stays here until the radio has room for it rather than being lost
+    // while the radio is busy.  While TX is suspended nothing goes out anyway.
+    if(tx_queue.length > 0 && millis() > tx_queue.delay_time && somfy_rx.cpt_synchro_hw == 0 &&
+      (this->txSuspended() || this->txHasRoom())) {
       somfy_tx_job_t job;
       job.raw = true;
-      tx_queue.pop(&job.tx);
+      tx_queue.peek(&job.tx);
       {
         somfy_tx_t &tx = job.tx;
         char frame_buf[128];
@@ -4740,7 +4699,10 @@ void Transceiver::loop() {
         snprintf(frame_buf + fpos, sizeof(frame_buf) - fpos, "]");
         ESP_LOGD(TAG, "%s", frame_buf);
       }
-      this->queueTx(job);
+      if(this->queueTx(job) || this->txSuspended()) {
+        somfy_tx_t sent;
+        tx_queue.pop(&sent);
+      }
       tx_queue.delay_time = millis() + TX_QUEUE_DELAY;
     }
   }

src/Somfy.h

@@ -22,6 +22,7 @@
 
 
 #include "SomfyFrame.h"
+#include "TxBuffer.h"
 enum class group_types : byte {
   channel = 0x00
 };
@@ -54,7 +55,6 @@ enum class tilt_types : byte {
 
 #define MAX_TIMINGS 300
 #define MAX_RX_BUFFER 3
-#define MAX_TX_BUFFER 5
 
 typedef enum {
     waiting_synchro = 0,
@@ -95,33 +95,6 @@ struct somfy_rx_queue_t {
   void push(somfy_rx_t *rx);
   bool pop(somfy_rx_t *rx);
 };
-struct somfy_tx_t {
-  void clear() {
-    this->hwsync = 0;
-    this->bit_length = 0;
-    memset(this->payload, 0x00, sizeof(this->payload));
-  }
-  uint8_t hwsync = 0;
-  uint8_t bit_length = 0;
-  uint8_t payload[10] = {};
-};
-struct somfy_tx_queue_t {
-  somfy_tx_queue_t() { this->clear(); }
-  void clear() {
-    for (uint8_t i = 0; i < MAX_TX_BUFFER; i++) {
-      this->index[i] = 255;
-      this->items[i].clear();
-    }
-    this->length = 0;
-  }
-  unsigned long delay_time = 0;
-  uint8_t length = 0;
-  uint8_t index[MAX_TX_BUFFER] = {255};
-  somfy_tx_t items[MAX_TX_BUFFER];
-  bool pop(somfy_tx_t *tx);
-  void push(somfy_rx_t *rx); // Used for repeats
-  void push(uint8_t hwsync, byte *payload, uint8_t bit_length);
-};
 
 enum class somfy_flags_t : byte {
     SunFlag = 0x01,

src/TxBuffer.cpp

@@ -0,0 +1,55 @@
+#include "TxBuffer.h"
+
+int8_t somfy_tx_queue_t::oldest() const {
+  // The newest index is at position 0 so the oldest is the last one in use.
+  for(int8_t i = MAX_TX_BUFFER - 1; i >= 0; i--) {
+    if(this->index[i] < MAX_TX_BUFFER) return i;
+  }
+  return -1;
+}
+bool somfy_tx_queue_t::peek(somfy_tx_t *tx) const {
+  const int8_t i = this->oldest();
+  if(i < 0) return false;
+  memcpy(tx, &this->items[this->index[i]], sizeof(somfy_tx_t));
+  return true;
+}
+bool somfy_tx_queue_t::pop(somfy_tx_t *tx) {
+  const int8_t i = this->oldest();
+  if(i < 0) return false;
+  uint8_t ndx = this->index[i];
+  memcpy(tx, &this->items[ndx], sizeof(somfy_tx_t));
+  this->items[ndx].clear();
+  if(this->length > 0) this->length--;
+  this->index[i] = 255;
+  return true;
+}
+void somfy_tx_queue_t::push(uint8_t hwsync, uint8_t *payload, uint8_t bit_length) {
+  if(this->length >= MAX_TX_BUFFER) {
+    // We have overflowed the buffer simply empty the last item
+    // in this instance we will simply throw it away.
+    uint8_t ndx = this->index[MAX_TX_BUFFER - 1];
+    if(ndx < MAX_TX_BUFFER) this->items[ndx].clear();
+    this->index[MAX_TX_BUFFER - 1] = 255;
+    this->length--;
+  }
+  uint8_t first = 0;
+  // Place this record in the first empty slot.  There will
+  // be one since we cleared a space above should there
+  // be an overflow.
+  for(uint8_t i = 0; i < MAX_TX_BUFFER; i++) {
+    if(this->items[i].bit_length == 0) {
+      first = i;
+      this->items[i].bit_length = bit_length;
+      this->items[i].hwsync = hwsync;
+      memcpy(&this->items[i].payload, payload, sizeof(this->items[i].payload));
+      break;
+    }
+  }
+  // Move the index so that it is the at position 0.  The oldest item will fall off.
+  for(uint8_t i = MAX_TX_BUFFER - 1; i > 0; i--) {
+    this->index[i] = this->index[i - 1];
+  }
+  this->length++;
+  // When popping from the queue we always pull from the end
+  this->index[0] = first;
+}

src/TxBuffer.h

@@ -0,0 +1,43 @@
+#ifndef TX_BUFFER_H
+#define TX_BUFFER_H
+#include <Arduino.h>
+
+// Repeater frames waiting to be handed to the radio task.  Kept free of the
+// radio and timers so it can be unit tested on the PC (pio test -e native).
+
+#define MAX_TX_BUFFER 5
+
+struct somfy_tx_t {
+  void clear() {
+    this->hwsync = 0;
+    this->bit_length = 0;
+    memset(this->payload, 0x00, sizeof(this->payload));
+  }
+  uint8_t hwsync = 0;
+  uint8_t bit_length = 0;
+  uint8_t payload[10] = {};
+};
+struct somfy_tx_queue_t {
+  somfy_tx_queue_t() { this->clear(); }
+  void clear() {
+    for (uint8_t i = 0; i < MAX_TX_BUFFER; i++) {
+      this->index[i] = 255;
+      this->items[i].clear();
+    }
+    this->length = 0;
+  }
+  unsigned long delay_time = 0;  // Set by the caller, not before this millis().
+  uint8_t length = 0;
+  uint8_t index[MAX_TX_BUFFER] = {255};
+  somfy_tx_t items[MAX_TX_BUFFER];
+  // The oldest frame without removing it, so it can stay queued until the radio
+  // takes it.
+  bool peek(somfy_tx_t *tx) const;
+  bool pop(somfy_tx_t *tx);
+  // When full the oldest frame is thrown away.
+  void push(uint8_t hwsync, byte *payload, uint8_t bit_length);
+  private:
+    // Position in index of the oldest frame, -1 when empty.
+    int8_t oldest() const;
+};
+#endif

test/test_txbuffer/test_txbuffer.cpp

@@ -0,0 +1,114 @@
+#include <unity.h>
+#include "TxBuffer.h"
+
+void setUp() {}
+void tearDown() {}
+
+// Pushes a frame whose payload is all `tag` so frames can be told apart.
+static void pushTagged(somfy_tx_queue_t &q, uint8_t tag, uint8_t bitLength = 56) {
+  uint8_t payload[10];
+  memset(payload, tag, sizeof(payload));
+  q.push(bitLength == 56 ? 2 : 12, payload, bitLength);
+}
+static void assertTagged(const somfy_tx_t &tx, uint8_t tag) {
+  uint8_t expected[10];
+  memset(expected, tag, sizeof(expected));
+  TEST_ASSERT_EQUAL_HEX8_ARRAY(expected, tx.payload, sizeof(expected));
+}
+
+static void test_push_then_pop_returns_the_frame() {
+  somfy_tx_queue_t q;
+  pushTagged(q, 0x11, 80);
+  TEST_ASSERT_EQUAL_UINT8(1, q.length);
+  somfy_tx_t tx;
+  TEST_ASSERT_TRUE(q.pop(&tx));
+  TEST_ASSERT_EQUAL_UINT8(12, tx.hwsync);
+  TEST_ASSERT_EQUAL_UINT8(80, tx.bit_length);
+  assertTagged(tx, 0x11);
+  TEST_ASSERT_EQUAL_UINT8(0, q.length);
+}
+static void test_frames_come_out_oldest_first() {
+  somfy_tx_queue_t q;
+  pushTagged(q, 1);
+  pushTagged(q, 2);
+  pushTagged(q, 3);
+  somfy_tx_t tx;
+  for(uint8_t tag = 1; tag <= 3; tag++) {
+    TEST_ASSERT_TRUE(q.pop(&tx));
+    assertTagged(tx, tag);
+  }
+  TEST_ASSERT_FALSE(q.pop(&tx));
+}
+static void test_peek_leaves_the_frame_queued() {
+  somfy_tx_queue_t q;
+  pushTagged(q, 1);
+  pushTagged(q, 2);
+  somfy_tx_t tx;
+  TEST_ASSERT_TRUE(q.peek(&tx));
+  assertTagged(tx, 1);
+  TEST_ASSERT_TRUE(q.peek(&tx));
+  assertTagged(tx, 1);
+  TEST_ASSERT_EQUAL_UINT8(2, q.length);
+  TEST_ASSERT_TRUE(q.pop(&tx));
+  assertTagged(tx, 1);
+  TEST_ASSERT_TRUE(q.peek(&tx));
+  assertTagged(tx, 2);
+}
+static void test_empty_buffer_has_nothing() {
+  somfy_tx_queue_t q;
+  somfy_tx_t tx;
+  TEST_ASSERT_FALSE(q.peek(&tx));
+  TEST_ASSERT_FALSE(q.pop(&tx));
+  TEST_ASSERT_EQUAL_UINT8(0, q.length);
+}
+static void test_overflow_drops_the_oldest() {
+  somfy_tx_queue_t q;
+  for(uint8_t tag = 1; tag <= MAX_TX_BUFFER + 2; tag++) pushTagged(q, tag);
+  TEST_ASSERT_EQUAL_UINT8(MAX_TX_BUFFER, q.length);
+  somfy_tx_t tx;
+  // The first two fell off.
+  for(uint8_t tag = 3; tag <= MAX_TX_BUFFER + 2; tag++) {
+    TEST_ASSERT_TRUE(q.pop(&tx));
+    assertTagged(tx, tag);
+  }
+  TEST_ASSERT_FALSE(q.pop(&tx));
+}
+static void test_slots_are_reused_after_pop() {
+  somfy_tx_queue_t q;
+  somfy_tx_t tx;
+  // Keep two frames in flight so freed slots are filled again many times over.
+  for(uint8_t tag = 1; tag <= 4 * MAX_TX_BUFFER; tag++) {
+    pushTagged(q, tag);
+    if(tag >= 2) {
+      TEST_ASSERT_TRUE(q.pop(&tx));
+      assertTagged(tx, tag - 1);
+      TEST_ASSERT_EQUAL_UINT8(1, q.length);
+    }
+  }
+}
+static void test_clear_empties_it() {
+  somfy_tx_queue_t q;
+  pushTagged(q, 1);
+  pushTagged(q, 2);
+  q.clear();
+  TEST_ASSERT_EQUAL_UINT8(0, q.length);
+  somfy_tx_t tx;
+  TEST_ASSERT_FALSE(q.peek(&tx));
+  pushTagged(q, 3);
+  TEST_ASSERT_TRUE(q.pop(&tx));
+  assertTagged(tx, 3);
+}
+
+int main(int argc, char **argv) {
+  (void)argc;
+  (void)argv;
+  UNITY_BEGIN();
+  RUN_TEST(test_push_then_pop_returns_the_frame);
+  RUN_TEST(test_frames_come_out_oldest_first);
+  RUN_TEST(test_peek_leaves_the_frame_queued);
+  RUN_TEST(test_empty_buffer_has_nothing);
+  RUN_TEST(test_overflow_drops_the_oldest);
+  RUN_TEST(test_slots_are_reused_after_pop);
+  RUN_TEST(test_clear_empties_it);
+  return UNITY_END();
+}