diff --git a/include/EStopManager.h b/include/EStopManager.h index fd89b4ed..077db205 100644 --- a/include/EStopManager.h +++ b/include/EStopManager.h @@ -3,14 +3,7 @@ #include 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 diff --git a/include/VisualStateManager.h b/include/VisualStateManager.h index 0d717ec0..b7a56198 100644 --- a/include/VisualStateManager.h +++ b/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 diff --git a/src/CommandHandler.cpp b/src/CommandHandler.cpp index 21af002b..7b72e020 100644 --- a/src/CommandHandler.cpp +++ b/src/CommandHandler.cpp @@ -104,7 +104,6 @@ bool _internalSetKeepAliveEnabled(bool enabled) { bool wasEnabled = s_keepAliveQueue != nullptr && s_keepAliveTaskHandle != nullptr; if (enabled == wasEnabled) { - ESP_LOGV(TAG, "keep-alive task is already %s", enabled ? "enabled" : "disabled"); return true; } diff --git a/src/EStopManager.cpp b/src/EStopManager.cpp index 22e0d7fa..6420aa28 100644 --- a/src/EStopManager.cpp +++ b/src/EStopManager.cpp @@ -6,100 +6,158 @@ #include "Time.h" #include "VisualStateManager.h" -#include +#include +#include +#include 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 && s_estopAwaitingRelease) { + s_estopActive = false; + s_estopAwaitingRelease = false; + } else if (s_estopActive && !pressed) { + deactivatesAtChanged = true; + deactivatesAt = 0; } - 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(OPENSHOCK_ESTOP_PIN); + + ESP_LOGI(TAG, "Initializing on pin %hhi", static_cast(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; } diff --git a/src/VisualStateManager.cpp b/src/VisualStateManager.cpp index 7ab132e0..91cf5d37 100644 --- a/src/VisualStateManager.cpp +++ b/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} }; @@ -157,13 +157,13 @@ void _updateVisualStateGPIO() { return; } - if (s_stateFlags & kEmergencyStoppedFlag) { - s_builtInLedManager->SetPattern(kEmergencyStoppedPattern); + if (s_stateFlags & kEmergencyStopAwaitingReleaseFlag) { + s_builtInLedManager->SetPattern(kEmergencyStopAwaitingReleasePattern); return; } - if (s_stateFlags & kEmergencyStopClearedFlag) { - s_builtInLedManager->SetPattern(kEmergencyStopClearedPattern); + if (s_stateFlags & kEmergencyStoppedFlag) { + s_builtInLedManager->SetPattern(kEmergencyStoppedPattern); return; } @@ -191,13 +191,13 @@ void _updateVisualStateRGB() { return; } - if (s_stateFlags & kEmergencyStoppedFlag) { - s_RGBLedManager->SetPattern(kEmergencyStoppedRGBPattern); + if (s_stateFlags & kEmergencyStopAwaitingReleaseFlag) { + s_RGBLedManager->SetPattern(kEmergencyStopAwaitingReleaseRGBPattern); return; } - if (s_stateFlags & kEmergencyStopClearedFlag) { - s_RGBLedManager->SetPattern(kEmergencyStopClearedRGBPattern); + if (s_stateFlags & kEmergencyStoppedFlag) { + s_RGBLedManager->SetPattern(kEmergencyStoppedRGBPattern); 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) { diff --git a/src/main.cpp b/src/main.cpp index d9e190af..f62331e5 100644 --- a/src/main.cpp +++ b/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"); diff --git a/src/radio/RFTransmitter.cpp b/src/radio/RFTransmitter.cpp index df9848f9..f43b77a4 100644 --- a/src/radio/RFTransmitter.cpp +++ b/src/radio/RFTransmitter.cpp @@ -179,9 +179,8 @@ void RFTransmitter::TransmitTask(void* arg) { } } - if(OpenShock::EStopManager::IsEStopped()) { - - std::int64_t whenEStoppedTime = EStopManager::WhenEStopped(); + if (OpenShock::EStopManager::IsEStopped()) { + std::int64_t whenEStoppedTime = EStopManager::LastEStopped(); for (auto it = commands.begin(); it != commands.end(); ++it) { cmd = *it;