diff --git a/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h b/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h index d59f080c0c..fbd28b5dc6 100644 --- a/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h +++ b/libraries/AP_HAL_ESP32/HAL_ESP32_Namespace.h @@ -15,6 +15,7 @@ class RCInput; class Util; class Semaphore; class Semaphore_Recursive; +class BinarySemaphore; class GPIO; class DigitalSource; class Storage; diff --git a/libraries/AP_HAL_ESP32/Semaphores.cpp b/libraries/AP_HAL_ESP32/Semaphores.cpp index a397a53b18..d76ca820ae 100644 --- a/libraries/AP_HAL_ESP32/Semaphores.cpp +++ b/libraries/AP_HAL_ESP32/Semaphores.cpp @@ -73,3 +73,46 @@ bool Semaphore::check_owner() { return xSemaphoreGetMutexHolder((QueueHandle_t)handle) == xTaskGetCurrentTaskHandle(); } + + +/* + BinarySemaphore implementation + */ +BinarySemaphore::BinarySemaphore(bool initial_state) +{ + _sem = xSemaphoreCreateBinary(); + if (initial_state) { + xSemaphoreGive(_sem); + } +} + +bool BinarySemaphore::wait(uint32_t timeout_us) +{ + TickType_t ticks = pdMS_TO_TICKS(timeout_us / 1000U); + return xSemaphoreTake(_sem, ticks) == pdTRUE; +} + +bool BinarySemaphore::wait_blocking() +{ + return xSemaphoreTake(_sem, portMAX_DELAY) == pdTRUE; +} + +void BinarySemaphore::signal() +{ + xSemaphoreGive(_sem); +} + +void BinarySemaphore::signal_ISR() +{ + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xSemaphoreGiveFromISR(_sem, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +BinarySemaphore::~BinarySemaphore(void) +{ + if (_sem != nullptr) { + vSemaphoreDelete(_sem); + } + _sem = nullptr; +} diff --git a/libraries/AP_HAL_ESP32/Semaphores.h b/libraries/AP_HAL_ESP32/Semaphores.h index 844fd1b271..0a950f7a10 100644 --- a/libraries/AP_HAL_ESP32/Semaphores.h +++ b/libraries/AP_HAL_ESP32/Semaphores.h @@ -20,6 +20,8 @@ #include #include #include "HAL_ESP32_Namespace.h" +#include +#include class ESP32::Semaphore : public AP_HAL::Semaphore { @@ -34,3 +36,20 @@ public: protected: void* handle; }; + +class ESP32::BinarySemaphore : public AP_HAL::BinarySemaphore { +public: + BinarySemaphore(bool initial_state=false); + ~BinarySemaphore(void); + + CLASS_NO_COPY(BinarySemaphore); + + bool wait(uint32_t timeout_us) override; + bool wait_blocking(void) override; + void signal(void) override; + void signal_ISR(void) override; + +protected: + SemaphoreHandle_t _sem; +}; +