Better RMT and RFTX logic (#368)

* Initial proposal

* Less enthusiastic changes

* More tweaking

* Update src/radio/rmt/Sequence.cpp

Co-authored-by: Copilot <[email protected]>

* Update src/radio/RFTransmitter.cpp

Co-authored-by: Copilot <[email protected]>

* Update src/radio/RFTransmitter.cpp

Co-authored-by: Copilot <[email protected]>

---------

Co-authored-by: Copilot <[email protected]>

Author: hhvrc

include/radio/RFTransmitter.h

@@ -32,6 +32,8 @@ namespace OpenShock {
     void destroy();
     void TransmitTask();
 
+    struct Command;
+
     gpio_num_t m_txPin;
     rmt_obj_t* m_rmtHandle;
     QueueHandle_t m_queueHandle;

include/radio/rmt/MainEncoder.h renamed to include/radio/rmt/Sequence.h

@@ -9,62 +9,77 @@
 #include <cstdint>
 
 namespace OpenShock::Rmt {
-  class MainEncoder {
-    DISABLE_COPY(MainEncoder);
+  class Sequence {
+    DISABLE_COPY(Sequence);
 
   public:
-    MainEncoder()
+    Sequence()
       : m_data(nullptr)
       , m_size(0)
-      , m_shockerModel()
+      , m_transmitEnd(0)
       , m_shockerId(0)
+      , m_shockerModel()
     {
     }
-    MainEncoder(ShockerModelType shockerModel, uint16_t shockerId);
-    MainEncoder(MainEncoder&& other) noexcept
+    Sequence(ShockerModelType shockerModel, uint16_t shockerId, int64_t transmitEnd);
+    Sequence(Sequence&& other) noexcept
       : m_data(other.m_data)
       , m_size(other.m_size)
-      , m_shockerModel(other.m_shockerModel)
+      , m_transmitEnd(other.m_transmitEnd)
       , m_shockerId(other.m_shockerId)
+      , m_shockerModel(other.m_shockerModel)
     {
-      other.m_data      = nullptr;
-      other.m_size      = 0;
-      other.m_shockerId = 0;
+      other.reset();
     }
-    ~MainEncoder() { free(m_data); }
+    ~Sequence() { free(m_data); }
 
     inline bool is_valid() const noexcept { return m_data != nullptr && m_size > 0; }
 
     inline ShockerModelType shockerModel() const noexcept { return m_shockerModel; }
     inline uint16_t shockerId() const noexcept { return m_shockerId; }
 
+    inline int64_t transmitEnd() const noexcept { return m_transmitEnd; }
+    inline void setTransmitEnd(int64_t transmitEnd) noexcept { m_transmitEnd = transmitEnd; }
+
     inline rmt_data_t* payload() noexcept { return m_data; }
     inline const rmt_data_t* payload() const noexcept { return m_data; }
     inline rmt_data_t* terminator() noexcept { return m_data + m_size; }
     inline const rmt_data_t* terminator() const noexcept { return m_data + m_size; }
     inline size_t size() const noexcept { return m_size; }
 
-    bool fillSequence(ShockerCommandType commandType, uint8_t intensity);
+    bool fill(ShockerCommandType commandType, uint8_t intensity);
 
-    MainEncoder& operator=(MainEncoder&& other)
+    Sequence& operator=(Sequence&& other)
     {
       if (this == &other) return *this;
 
       free(m_data);
 
-      m_data = other.m_data;
-      m_size = other.m_size;
+      m_data         = other.m_data;
+      m_size         = other.m_size;
+      m_transmitEnd  = other.m_transmitEnd;
+      m_shockerId    = other.m_shockerId;
+      m_shockerModel = other.m_shockerModel;
 
-      other.m_data = nullptr;
-      other.m_size = 0;
+      other.reset();
 
       return *this;
     }
 
   private:
+    void reset()
+    {
+      m_data         = nullptr;
+      m_size         = 0;
+      m_transmitEnd  = 0;
+      m_shockerId    = 0;
+      m_shockerModel = (ShockerModelType)0;
+    }
+
     rmt_data_t* m_data;
     size_t m_size;
-    ShockerModelType m_shockerModel;
+    int64_t m_transmitEnd;
     uint16_t m_shockerId;
+    ShockerModelType m_shockerModel;
   };
 }  // namespace OpenShock::Rmt

src/CommandHandler.cpp

@@ -297,18 +297,6 @@ bool CommandHandler::HandleCommand(ShockerModelType model, uint16_t shockerId, S
     return false;
   }
 
-  // Stop logic
-  if (type == ShockerCommandType::Stop) {
-    OS_LOGV(TAG, "Stop command received, clearing pending commands");
-
-    type       = ShockerCommandType::Vibrate;
-    intensity  = 0;
-    durationMs = 300;
-
-    s_rfTransmitter->ClearPendingCommands();
-  } else {
-    OS_LOGD(TAG, "Command received: %u %u %u %u", model, shockerId, type, intensity);
-  }
 
   bool ok = s_rfTransmitter->SendCommand(model, shockerId, type, intensity, durationMs);
 

src/radio/RFTransmitter.cpp

@@ -7,33 +7,30 @@ const char* const TAG = "RFTransmitter";
 #include "Core.h"
 #include "estop/EStopManager.h"
 #include "Logging.h"
-#include "radio/rmt/MainEncoder.h"
+#include "radio/rmt/Sequence.h"
 #include "util/FnProxy.h"
 #include "util/TaskUtils.h"
 
 #include <freertos/queue.h>
 
-const UBaseType_t RFTRANSMITTER_QUEUE_SIZE   = 64;
-const BaseType_t RFTRANSMITTER_TASK_PRIORITY = 1;
-const uint32_t RFTRANSMITTER_TASK_STACK_SIZE = 4096;  // PROFILED: 1.4KB stack usage
-const float RFTRANSMITTER_TICKRATE_NS        = 1000;
-const int64_t TRANSMIT_END_DURATION          = 300;
-const int64_t SEQUENCE_TIME_TO_LIVE          = 1000;
+const UBaseType_t kQueueSize        = 64;
+const BaseType_t kTaskPriority      = 1;
+const uint32_t kTaskStackSize       = 4096;  // PROFILED: 1.4KB stack usage
+const float kTickrateNs             = 1000;
+const int64_t kTerminatorDurationMs = 300;
+const uint8_t kFlagOverwrite        = 1 << 0;
+const uint8_t kFlagDeleteTask       = 1 << 1;
+const TickType_t kTaskIdleDelay     = pdMS_TO_TICKS(5);
 
 using namespace OpenShock;
 
-struct command_t {
-  int64_t until;
-  ShockerModelType model;
+struct RFTransmitter::Command {
+  int64_t transmitEnd;
+  ShockerModelType modelType;
   ShockerCommandType type;
   uint16_t shockerId;
   uint8_t intensity;
-  bool overwrite : 1;
-  bool destroy   : 1;
-};
-struct sequence_t {
-  int64_t until;
-  Rmt::MainEncoder encoder;
+  uint8_t flags;
 };
 
 RFTransmitter::RFTransmitter(gpio_num_t gpioPin)
@@ -51,10 +48,10 @@ RFTransmitter::RFTransmitter(gpio_num_t gpioPin)
     return;
   }
 
-  float realTick = rmtSetTick(m_rmtHandle, RFTRANSMITTER_TICKRATE_NS);
+  float realTick = rmtSetTick(m_rmtHandle, kTickrateNs);
   OS_LOGD(TAG, "[pin-%hhi] real tick set to: %fns", m_txPin, realTick);
 
-  m_queueHandle = xQueueCreate(RFTRANSMITTER_QUEUE_SIZE, sizeof(command_t));
+  m_queueHandle = xQueueCreate(kQueueSize, sizeof(Command));
   if (m_queueHandle == nullptr) {
     OS_LOGE(TAG, "[pin-%hhi] Failed to create queue", m_txPin);
     destroy();
@@ -64,7 +61,7 @@ RFTransmitter::RFTransmitter(gpio_num_t gpioPin)
   char name[32];
   snprintf(name, sizeof(name), "RFTransmitter-%u", m_txPin);
 
-  if (TaskUtils::TaskCreateExpensive(&Util::FnProxy<&RFTransmitter::TransmitTask>, name, RFTRANSMITTER_TASK_STACK_SIZE, this, RFTRANSMITTER_TASK_PRIORITY, &m_taskHandle) != pdPASS) {
+  if (TaskUtils::TaskCreateExpensive(&Util::FnProxy<&RFTransmitter::TransmitTask>, name, kTaskStackSize, this, kTaskPriority, &m_taskHandle) != pdPASS) {
     OS_LOGE(TAG, "[pin-%hhi] Failed to create task", m_txPin);
     destroy();
     return;
@@ -83,7 +80,19 @@ bool RFTransmitter::SendCommand(ShockerModelType model, uint16_t shockerId, Shoc
     return false;
   }
 
-  command_t cmd = command_t {.until = OpenShock::millis() + durationMs, .model = model, .type = type, .shockerId = shockerId, .intensity = intensity, .overwrite = overwriteExisting, .destroy = false};
+  // Stop logic
+  if (type == ShockerCommandType::Stop) {
+    OS_LOGV(TAG, "Stop command received");
+
+    type              = ShockerCommandType::Vibrate;
+    intensity         = 0;
+    durationMs        = 300;
+    overwriteExisting = true;
+  } else {
+    OS_LOGD(TAG, "Command received: %u %u %u %u", model, shockerId, type, intensity);
+  }
+
+  Command cmd = Command {.transmitEnd = OpenShock::millis() + durationMs, .modelType = model, .type = type, .shockerId = shockerId, .intensity = intensity, .flags = overwriteExisting ? kFlagOverwrite : (uint8_t)0};
 
   // Add the command to the queue, wait max 10 ms (Adjust this)
   if (xQueueSend(m_queueHandle, &cmd, pdMS_TO_TICKS(10)) != pdTRUE) {
@@ -102,7 +111,7 @@ void RFTransmitter::ClearPendingCommands()
 
   OS_LOGI(TAG, "[pin-%hhi] Clearing pending commands", m_txPin);
 
-  command_t command;
+  Command command;
   while (xQueueReceive(m_queueHandle, &command, 0) == pdPASS) {
   }
 }
@@ -113,7 +122,7 @@ void RFTransmitter::destroy()
     OS_LOGD(TAG, "[pin-%hhi] Stopping task", m_txPin);
 
     // Wait for the task to stop
-    command_t cmd {.destroy = true};
+    Command cmd {.flags = kFlagDeleteTask};
     while (eTaskGetState(m_taskHandle) != eDeleted) {
       vTaskDelay(pdMS_TO_TICKS(10));
 
@@ -138,73 +147,98 @@ void RFTransmitter::destroy()
   }
 }
 
+static bool addSequence(std::vector<Rmt::Sequence>& sequences, ShockerModelType modelType, uint16_t shockerId, ShockerCommandType commandType, uint8_t intensity, int64_t transmitEnd)
+{
+  Rmt::Sequence sequence(modelType, shockerId, transmitEnd);
+  if (!sequence.is_valid()) return false;
+
+  if (!sequence.fill(commandType, intensity)) return false;
+
+  sequences.push_back(std::move(sequence));
+
+  return true;
+}
+
+static bool modifySequence(std::vector<Rmt::Sequence>& sequences, ShockerModelType modelType, uint16_t shockerId, ShockerCommandType commandType, uint8_t intensity, int64_t transmitEnd)
+{
+  for (auto& seq : sequences) {
+    if (seq.shockerModel() == modelType && seq.shockerId() == shockerId) {
+      bool ok = seq.fill(commandType, intensity);
+      seq.setTransmitEnd(ok ? transmitEnd : 0);  // Remove this immediately if fill didnt succeed
+      return ok;                                 // Returns whether modification succeeded; caller should generate a new sequence if this fails
+    }
+  }
+
+  return false;
+}
+
+static void writeSequences(rmt_obj_t* rmt_handle, std::vector<Rmt::Sequence>& sequences)
+{
+  // Send queued commands
+  for (auto seq = sequences.begin(); seq != sequences.end();) {
+    int64_t timeToLive = seq->transmitEnd() - OpenShock::millis();
+
+    if (timeToLive > 0) {
+      // Send the command
+      rmtWriteBlocking(rmt_handle, seq->payload(), seq->size());
+    } else {
+      // Remove command if it has sent out its termination sequence for long enough
+      if (timeToLive + kTerminatorDurationMs <= 0) {
+        seq = sequences.erase(seq);
+        continue;
+      }
+
+      // Send the termination sequence to stop the shocker
+      rmtWriteBlocking(rmt_handle, seq->terminator(), seq->size());
+    }
+
+    // Move to the next command
+    ++seq;
+  }
+}
+
 void RFTransmitter::TransmitTask()
 {
   OS_LOGD(TAG, "[pin-%hhi] RMT loop running on core %d", m_txPin, xPortGetCoreID());
 
-  std::vector<sequence_t> sequences;
+  bool wasEstopped = false;
+  std::vector<Rmt::Sequence> sequences;
   while (true) {
     // Receive commands
-    command_t cmd;
+    Command cmd;
     while (xQueueReceive(m_queueHandle, &cmd, sequences.empty() ? portMAX_DELAY : 0) == pdTRUE) {
       // Destroy task if we receive destroy command
-      if (cmd.destroy) {
+      if ((cmd.flags & kFlagDeleteTask) != 0) {
         sequences.clear();
         vTaskDelete(nullptr);
         return;
       }
 
-      if (cmd.overwrite) {
+      if ((cmd.flags & kFlagOverwrite) != 0) {
         // Replace the sequence if it already exists
-        auto it = std::find_if(sequences.begin(), sequences.end(), [&cmd](const sequence_t& seq) { return seq.encoder.shockerId() == cmd.shockerId; });
-        if (it != sequences.end()) {
-          it->encoder.fillSequence(cmd.type, cmd.intensity);
-          it->until = cmd.until;
+        if (modifySequence(sequences, cmd.modelType, cmd.shockerId, cmd.type, cmd.intensity, cmd.transmitEnd)) {
           continue;
         }
       }
 
-      Rmt::MainEncoder encoder(cmd.model, cmd.shockerId);
-      if (encoder.is_valid()) {
-        encoder.fillSequence(cmd.type, cmd.intensity);
-
-        // If the command was not replaced, add it to the queue
-        sequences.push_back(sequence_t {.until = cmd.until, .encoder = std::move(encoder)});
+      if (!addSequence(sequences, cmd.modelType, cmd.shockerId, cmd.type, cmd.intensity, cmd.transmitEnd)) {
+        OS_LOGD(TAG, "[pin-%hhi] Failed to add sequence");
       }
     }
 
-    if (OpenShock::EStopManager::IsEStopped()) {
-      int64_t whenEStoppedTime = EStopManager::LastEStopped();
+    bool isEstopped = OpenShock::EStopManager::IsEStopped();
+    if (isEstopped != wasEstopped) {
+      wasEstopped = isEstopped;
 
-      for (auto seq = sequences.begin(); seq != sequences.end(); ++seq) {
-        seq->until = whenEStoppedTime;
-      }
-    }
-
-    // Send queued commands
-    for (auto seq = sequences.begin(); seq != sequences.end();) {
-      bool expired = seq->until < OpenShock::millis();
-
-      // Remove expired commands, else send the command.
-      // After sending/receiving a command, move to the next one.
-      if (expired) {
-        // Send the zero sequence to stop the shocker
-        rmtWriteBlocking(m_rmtHandle, seq->encoder.terminator(), seq->encoder.size());
-
-        if (seq->until + TRANSMIT_END_DURATION < OpenShock::millis()) {
-          // Remove the command and move to the next one
-          seq = sequences.erase(seq);
-        } else {
-          // Move to the next command
-          ++seq;
+      if (isEstopped) {
+        // Set all sequences to transmit their terminators
+        int64_t now = OpenShock::millis();
+        for (auto seq = sequences.begin(); seq != sequences.end(); ++seq) {
+          seq->setTransmitEnd(now);
         }
-      } else {
-        // Send the command
-        rmtWriteBlocking(m_rmtHandle, seq->encoder.payload(), seq->encoder.size());
-
-        // Move to the next command
-        ++seq;
       }
     }
+
+    writeSequences(m_rmtHandle, sequences);
   }
 }