Make EStopManager Interrupt based

include/EStopManager.h

@@ -3,14 +3,7 @@
 #include <cstdint>
 
 namespace OpenShock::EStopManager {
-  enum class EStopStatus : std::uint8_t {
-    ALL_CLEAR,          // The initial, idle state
-    ESTOPPED_AND_HELD,  // The EStop has been pressed and has not yet been released
-    ESTOPPED,           // Idle EStopped state
-    ESTOPPED_CLEARED    // The EStop has been cleared by the user, but we're waiting for the user to release the button (to avoid incidental estops)
-  };
-
-  void Init(std::uint16_t updateIntervalMs);
+  void Init();
   bool IsEStopped();
-  std::int64_t WhenEStopped();
+  std::int64_t LastEStopped();
 }  // namespace OpenShock::EStopManager

include/VisualStateManager.h

@@ -8,6 +8,6 @@ namespace OpenShock::VisualStateManager {
 
   void SetCriticalError();
   void SetScanningStarted();
-  void SetEmergencyStop(OpenShock::EStopManager::EStopStatus status);
+  void SetEmergencyStopStatus(bool isActive, bool isAwaitingRelease);
   void SetWebSocketConnected(bool isConnected);
 }  // namespace OpenShock::VisualStateManager

src/EStopManager.cpp

@@ -6,100 +6,158 @@
 #include "Time.h"
 #include "VisualStateManager.h"
 
-#include <Arduino.h>
+#include <driver/gpio.h>
+#include <freertos/FreeRTOS.h>
+#include <freertos/timers.h>
 
 const char* const TAG = "EStopManager";
 
 using namespace OpenShock;
 
-static EStopManager::EStopStatus s_estopStatus   = EStopManager::EStopStatus::ALL_CLEAR;
-static std::uint32_t s_estopHoldToClearTime      = 5000;
-static std::int64_t s_lastEStopButtonStateChange = 0;
-static std::int64_t s_estoppedAt                 = 0;
-static int s_lastEStopButtonState                = HIGH;
+const std::uint32_t k_estopHoldToClearTime = 5000;
+const std::uint32_t k_estopDebounceTime    = 100;
+
+static TaskHandle_t s_estopEventHandlerTask;
+static QueueHandle_t s_estopEventQueue;
+
+static bool s_estopActive              = false;
+static bool s_estopAwaitingRelease     = false;
+static std::int64_t s_estopActivatedAt = 0;
+
+static gpio_num_t s_estopPin;
+
+struct EstopEventQueueMessage {
+  bool pressed              : 1;
+  bool deactivatesAtChanged : 1;
+  std::int64_t deactivatesAt;
+};
+
+// This high-priority task is usually idling, waiting for
+// messages from the EStop interrupt or it's hold timer
+void _estopEventHandler(void* pvParameters) {
+  std::int64_t deactivatesAt = 0;
+  for (;;) {
+    // Wait indefinitely for a message from the EStop interrupt routine
+    TickType_t waitTime = portMAX_DELAY;
+
+    // If the EStop is being deactivated, wait for the hold timer to trigger
+    if (deactivatesAt != 0) {
+      std::int64_t now = OpenShock::millis();
+      if (now >= deactivatesAt) {
+        waitTime = 0;
+      } else {
+        waitTime = pdMS_TO_TICKS(deactivatesAt - OpenShock::millis());
+      }
+    }
+
+    // Wait for a message from the EStop interrupt routine
+    EstopEventQueueMessage message;
+    if (xQueueReceive(s_estopEventQueue, &message, waitTime) == pdTRUE) {
+      if (message.pressed) {
+        ESP_LOGI(TAG, "EStop pressed");
+      } else {
+        ESP_LOGI(TAG, "EStop released");
+      }
+
+      if (message.deactivatesAtChanged) {
+        ESP_LOGI(TAG, "EStop deactivation time changed");
+        deactivatesAt = message.deactivatesAt;
+      }
 
-static std::uint8_t s_estopPin;
+      OpenShock::VisualStateManager::SetEmergencyStopStatus(s_estopActive, s_estopAwaitingRelease);
+      OpenShock::CommandHandler::SetKeepAlivePaused(EStopManager::IsEStopped());
+    } else if (deactivatesAt != 0 && OpenShock::millis() >= deactivatesAt) {  // If we didn't get a message, the time probably expired, check if the estop is pending deactivation and if we have reached that time
+      // Reset the deactivation time
+      deactivatesAt = 0;
 
-void _estopManagerTask(TimerHandle_t xTimer) {
-  configASSERT(xTimer);
+      // If the button is held for the specified time, clear the EStop
+      s_estopAwaitingRelease = true;
+      OpenShock::VisualStateManager::SetEmergencyStopStatus(s_estopActive, s_estopAwaitingRelease);
 
-  int buttonState = digitalRead(s_estopPin);
-  if (buttonState != s_lastEStopButtonState) {
-    s_lastEStopButtonStateChange = OpenShock::millis();
+      ESP_LOGI(TAG, "EStop cleared, awaiting release");
+    }
   }
-  switch (s_estopStatus) {
-    case EStopManager::EStopStatus::ALL_CLEAR:
-      if (buttonState == LOW) {
-        s_estopStatus = EStopManager::EStopStatus::ESTOPPED_AND_HELD;
-        s_estoppedAt  = s_lastEStopButtonStateChange;
-        ESP_LOGI(TAG, "EStop triggered");
-        OpenShock::VisualStateManager::SetEmergencyStop(s_estopStatus);
-        OpenShock::CommandHandler::SetKeepAlivePaused(true);
-      }
-      break;
-    case EStopManager::EStopStatus::ESTOPPED_AND_HELD:
-      if (buttonState == HIGH) {
-        // User has released the button, now we can trust them holding to clear it.
-        s_estopStatus = EStopManager::EStopStatus::ESTOPPED;
-        OpenShock::VisualStateManager::SetEmergencyStop(s_estopStatus);
-      }
-      break;
-    case EStopManager::EStopStatus::ESTOPPED:
-      // If the button is held again for the specified time after being released, clear the EStop
-      if (buttonState == LOW && s_lastEStopButtonState == LOW && s_lastEStopButtonStateChange + s_estopHoldToClearTime <= OpenShock::millis()) {
-        s_estopStatus = EStopManager::EStopStatus::ESTOPPED_CLEARED;
-        ESP_LOGI(TAG, "EStop cleared");
-        OpenShock::VisualStateManager::SetEmergencyStop(s_estopStatus);
-      }
-      break;
-    case EStopManager::EStopStatus::ESTOPPED_CLEARED:
-      // If the button is released, report as ALL_CLEAR
-      if (buttonState == HIGH) {
-        s_estopStatus = EStopManager::EStopStatus::ALL_CLEAR;
-        ESP_LOGI(TAG, "EStop cleared, all clear");
-        OpenShock::VisualStateManager::SetEmergencyStop(s_estopStatus);
-        OpenShock::CommandHandler::SetKeepAlivePaused(false);
-      }
-      break;
+}
 
-    default:
-      break;
+// Interrupt should only be a dumb sender of the GPIO change, additionally triggering if needed
+// Clearing and debouncing is handled by the task.
+void _estopEdgeInterrupt(void* arg) {
+  std::int64_t now = OpenShock::millis();
+
+  // TODO: Allow active HIGH EStops?
+  bool pressed = gpio_get_level(s_estopPin) == 0;
+
+  bool deactivatesAtChanged  = false;
+  std::int64_t deactivatesAt = 0;
+
+  if (!s_estopActive && pressed) {
+    s_estopActive      = true;
+    s_estopActivatedAt = now;
+  } else if (s_estopActive && pressed && (now - s_estopActivatedAt >= k_estopDebounceTime)) {
+    deactivatesAtChanged = true;
+    deactivatesAt        = now + k_estopHoldToClearTime;
+  } else if (s_estopActive && !pressed) {
+    deactivatesAtChanged = true;
+    deactivatesAt        = 0;
+  } else if (s_estopActive && s_estopAwaitingRelease) {
+    s_estopActive          = false;
+    s_estopAwaitingRelease = false;
   }
 
-  s_lastEStopButtonState = buttonState;
+  BaseType_t higherPriorityTaskWoken = pdFALSE;
+  EstopEventQueueMessage message     = {
+        .pressed              = pressed,
+        .deactivatesAtChanged = deactivatesAtChanged,
+        .deactivatesAt        = deactivatesAt,
+  };
+
+  xQueueSendToBackFromISR(s_estopEventQueue, &message, &higherPriorityTaskWoken);  // TODO: Check if queue is full?
+
+  if (higherPriorityTaskWoken) {
+    portYIELD_FROM_ISR();
+  }
 }
 
-// TODO?: Allow active HIGH EStops?
-void EStopManager::Init(std::uint16_t updateIntervalMs) {
+void EStopManager::Init() {
 #ifdef OPENSHOCK_ESTOP_PIN
-  s_estopPin = OPENSHOCK_ESTOP_PIN;
-  pinMode(s_estopPin, INPUT_PULLUP);
-  ESP_LOGI(TAG, "Initializing on pin %u", s_estopPin);
-
-  // Start the repeating task, 10Hz may seem slow, but it's plenty fast for an EStop
-  TimerHandle_t timer = xTimerCreate(TAG, pdMS_TO_TICKS(updateIntervalMs), pdTRUE, nullptr, _estopManagerTask);
-  if (timer == nullptr) {
-    ESP_LOGE(TAG, "Failed to create timer!!! Triggering EStop.");
-    s_estopStatus = EStopManager::EStopStatus::ESTOPPED;
-  } else {
-    xTimerStart(timer, 0);
+  s_estopPin = static_cast<gpio_num_t>(OPENSHOCK_ESTOP_PIN);
+
+  ESP_LOGI(TAG, "Initializing on pin %hhi", static_cast<std::int8_t>(s_estopPin));
+
+  gpio_config_t io_conf;
+  io_conf.pin_bit_mask = 1ULL << s_estopPin;
+  io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
+  io_conf.pull_up_en   = GPIO_PULLUP_ENABLE;
+  io_conf.mode         = GPIO_MODE_INPUT;
+  io_conf.intr_type    = GPIO_INTR_ANYEDGE;
+  gpio_config(&io_conf);
+
+  // TODO?: Should we maybe use statically allocated queues and timers? See CreateStatic for both.
+  s_estopEventQueue = xQueueCreate(8, sizeof(EstopEventQueueMessage));
+
+  esp_err_t err = gpio_install_isr_service(ESP_INTR_FLAG_EDGE);
+  if (err != ESP_OK && err != ESP_ERR_INVALID_STATE) {  // ESP_ERR_INVALID_STATE is fine, it just means the ISR service is already installed
+    ESP_PANIC(TAG, "Failed to install EStop ISR service");
   }
 
-#else
-  (void)updateIntervalMs;
+  err = gpio_isr_handler_add(s_estopPin, _estopEdgeInterrupt, nullptr);
+  if (err != ESP_OK) {
+    ESP_PANIC(TAG, "Failed to add EStop ISR handler");
+  }
 
+  if (xTaskCreate(_estopEventHandler, TAG, 4096, nullptr, 5, &s_estopEventHandlerTask) != pdPASS) {
+    ESP_PANIC(TAG, "Failed to create EStop event handler task");
+  }
+
+#else
   ESP_LOGI(TAG, "EStopManager disabled, no pin defined");
 #endif
 }
 
 bool EStopManager::IsEStopped() {
-  return (s_estopStatus != EStopManager::EStopStatus::ALL_CLEAR);
+  return s_estopActive;
 }
 
-std::int64_t EStopManager::WhenEStopped() {
-  if (IsEStopped()) {
-    return s_estoppedAt;
-  }
-
-  return 0;
+std::int64_t EStopManager::LastEStopped() {
+  return s_estopActivatedAt;
 }

src/VisualStateManager.cpp

@@ -10,12 +10,12 @@
 
 const char* const TAG = "VisualStateManager";
 
-const std::uint64_t kCriticalErrorFlag        = 1 << 0;
-const std::uint64_t kEmergencyStoppedFlag     = 1 << 1;
-const std::uint64_t kEmergencyStopClearedFlag = 1 << 2;
-const std::uint64_t kWebSocketConnectedFlag   = 1 << 3;
-const std::uint64_t kWiFiConnectedFlag        = 1 << 4;
-const std::uint64_t kWiFiScanningFlag         = 1 << 5;
+const std::uint64_t kCriticalErrorFlag                = 1 << 0;
+const std::uint64_t kEmergencyStoppedFlag             = 1 << 1;
+const std::uint64_t kEmergencyStopAwaitingReleaseFlag = 1 << 2;
+const std::uint64_t kWebSocketConnectedFlag           = 1 << 3;
+const std::uint64_t kWiFiConnectedFlag                = 1 << 4;
+const std::uint64_t kWiFiScanningFlag                 = 1 << 5;
 
 // Bitmask of when the system is in a "all clear" state.
 const std::uint64_t kStatusOKMask = kWebSocketConnectedFlag | kWiFiConnectedFlag;
@@ -44,11 +44,11 @@ const RGBPatternManager::RGBState kEmergencyStoppedRGBPattern[] = {
   {  0, 0, 0, 500}
 };
 
-const PinPatternManager::State kEmergencyStopClearedPattern[] = {
+const PinPatternManager::State kEmergencyStopAwaitingReleasePattern[] = {
   { true, 150},
   {false, 150}
 };
-const RGBPatternManager::RGBState kEmergencyStopClearedRGBPattern[] = {
+const RGBPatternManager::RGBState kEmergencyStopAwaitingReleaseRGBPattern[] = {
   {0, 255, 0, 150},
   {0,   0, 0, 150}
 };
@@ -162,8 +162,8 @@ void _updateVisualStateGPIO() {
     return;
   }
 
-  if (s_stateFlags & kEmergencyStopClearedFlag) {
-    s_builtInLedManager->SetPattern(kEmergencyStopClearedPattern);
+  if (s_stateFlags & kEmergencyStopAwaitingReleaseFlag) {
+    s_builtInLedManager->SetPattern(kEmergencyStopAwaitingReleasePattern);
     return;
   }
 
@@ -196,8 +196,8 @@ void _updateVisualStateRGB() {
     return;
   }
 
-  if (s_stateFlags & kEmergencyStopClearedFlag) {
-    s_RGBLedManager->SetPattern(kEmergencyStopClearedRGBPattern);
+  if (s_stateFlags & kEmergencyStopAwaitingReleaseFlag) {
+    s_RGBLedManager->SetPattern(kEmergencyStopAwaitingReleaseRGBPattern);
     return;
   }
 
@@ -344,30 +344,19 @@ void VisualStateManager::SetScanningStarted() {
   }
 }
 
-void VisualStateManager::SetEmergencyStop(OpenShock::EStopManager::EStopStatus status) {
+void VisualStateManager::SetEmergencyStopStatus(bool isActive, bool isAwaitingRelease) {
   std::uint64_t oldState = s_stateFlags;
 
-  switch (status) {
-    // When there is no EStop currently active.
-    case OpenShock::EStopManager::EStopStatus::ALL_CLEAR:
-      s_stateFlags &= ~kEmergencyStoppedFlag;
-      s_stateFlags &= ~kEmergencyStopClearedFlag;
-      break;
-    // EStop has been triggered!
-    case OpenShock::EStopManager::EStopStatus::ESTOPPED_AND_HELD:
-    // EStop still active, and user has released the button from the initial trigger.
-    case OpenShock::EStopManager::EStopStatus::ESTOPPED:
-      s_stateFlags |= kEmergencyStoppedFlag;
-      s_stateFlags &= ~kEmergencyStopClearedFlag;
-      break;
-    // User has held and cleared the EStop, now we're waiting for them to release the button.
-    case OpenShock::EStopManager::EStopStatus::ESTOPPED_CLEARED:
-      s_stateFlags &= ~kEmergencyStoppedFlag;
-      s_stateFlags |= kEmergencyStopClearedFlag;
-      break;
-    default:
-      ESP_LOGE(TAG, "Unhandled EStop status: %d", status);
-      break;
+  if (isActive) {
+    s_stateFlags |= kEmergencyStoppedFlag;
+  } else {
+    s_stateFlags &= ~kEmergencyStoppedFlag;
+  }
+
+  if (isAwaitingRelease) {
+    s_stateFlags |= kEmergencyStopAwaitingReleaseFlag;
+  } else {
+    s_stateFlags &= ~kEmergencyStopAwaitingReleaseFlag;
   }
 
   if (oldState != s_stateFlags) {

src/main.cpp

@@ -27,7 +27,7 @@ bool trySetup() {
     ESP_PANIC(TAG, "Unable to initialize VisualStateManager");
   }
 
-  OpenShock::EStopManager::Init(100);  // 100ms update interval
+  OpenShock::EStopManager::Init();
 
   if (!OpenShock::SerialInputHandler::Init()) {
     ESP_LOGE(TAG, "Unable to initialize SerialInputHandler");

src/radio/RFTransmitter.cpp

@@ -181,7 +181,7 @@ void RFTransmitter::TransmitTask(void* arg) {
 
     if(OpenShock::EStopManager::IsEStopped()) {
 
-      std::int64_t whenEStoppedTime = EStopManager::WhenEStopped();
+      std::int64_t whenEStoppedTime = EStopManager::LastEStopped();
 
       for (auto it = commands.begin(); it != commands.end(); ++it) {
         cmd = *it;