diff --git a/cores/esp32/FunctionQueue.cpp b/cores/esp32/FunctionQueue.cpp new file mode 100644 index 00000000000..5d1c297c7cc --- /dev/null +++ b/cores/esp32/FunctionQueue.cpp @@ -0,0 +1,109 @@ +/* + * FunctionQueue.cpp + * + * Created on: 9 jun. 2018 + * Author: Herman + */ + +#include +#include +#include "Arduino.h" +#include "freertos/event_groups.h" + +EventGroupHandle_t FunctionQueue::loopEventHandle = xEventGroupCreate(); +uint8_t FunctionQueue::allSynced = 0; // Loop is always in the sync group +const uint8_t FunctionQueue::loopIndex = 1; +SemaphoreHandle_t FunctionQueue::syncedMutex = xSemaphoreCreateMutex(); + +FunctionQueue::FunctionQueue() +:FunctionQueue(false) +{ +} + +FunctionQueue::FunctionQueue(bool loopSynced) +{ + if (loopSynced) + { + syncIndex = 0; + for (int ix = loopIndex;ix < 8;ix++) + { + if (((allSynced >> ix) & 1U) == 0) + { + syncIndex = ix; + break; + } + } + if (syncIndex) + { + xSemaphoreTake(syncedMutex, portMAX_DELAY); + allSynced |= (1u << syncIndex); + xSemaphoreGive(syncedMutex); + } + else + { + // To many synced FQ's + } + } + + functionQueue = xQueueCreate( 10,sizeof(QueuedItem*) ); + // ARDUINO_RUNNING_CORE is defined in main.cpp core is here hardcoded to 1 + // Priority 1 is identical to the prio of the looptask + xTaskCreatePinnedToCore(staticFunction, "FunctionQueue", 8192, this, 1, &this->functionTask, 1); +} + +FunctionQueue::~FunctionQueue() +{ + if (syncIndex != 0) + { + xSemaphoreTake(syncedMutex, portMAX_DELAY); + allSynced &= ~(1u << syncIndex); + xSemaphoreGive(syncedMutex); + } + xEventGroupSetBits(loopEventHandle, (1u << syncIndex)); //Loop still may be waiting on this + vQueueDelete(functionQueue); + vTaskDelete(functionTask); +} + +bool FunctionQueue::scheduleFunction(std::function sf) +{ + QueuedItem* queuedItem = new QueuedItem; + queuedItem->queuedFunction = sf; + xQueueSendToBack(functionQueue,&queuedItem,portMAX_DELAY); + + return true; +} + +void FunctionQueue::processQueueItem() +{ + QueuedItem* queuedItem = nullptr; + if (xQueueReceive(functionQueue, &queuedItem, portMAX_DELAY) == pdTRUE){ + queuedItem->queuedFunction(); + delete queuedItem; + } +} + +void FunctionQueue::staticFunction(void* pvParameters) +{ + FunctionQueue* _this = static_cast(pvParameters); + for (;;) { + if (_this->syncIndex != 0) + { + uint16_t er = xEventGroupSync(loopEventHandle,(1u << _this->syncIndex),0x01,portMAX_DELAY); + + int mc = uxQueueMessagesWaiting(_this->functionQueue); + // only run already queued functions to allow recursive + while (mc-- > 0) + { + _this->processQueueItem(); // Will always return + } + } + else + { + _this->processQueueItem(); // Will block when queue is empty + } + } + vTaskDelete(NULL); +} + +FunctionQueue FQ; + diff --git a/cores/esp32/FunctionQueue.h b/cores/esp32/FunctionQueue.h new file mode 100644 index 00000000000..db001116f04 --- /dev/null +++ b/cores/esp32/FunctionQueue.h @@ -0,0 +1,53 @@ +/* + * FunctionQueue.h + * + * Created on: 9 jun. 2018 + * Author: Herman + */ + +#ifndef CORE_CORE_FUNCTIONQUEUE_H_ +#define CORE_CORE_FUNCTIONQUEUE_H_ + +#include + +extern "C" +{ +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/queue.h" +#include "freertos/semphr.h" +#include "freertos/event_groups.h" +} + +class FunctionQueue { +public: + + struct QueuedItem + { + std::function queuedFunction; + }; + + FunctionQueue(); + FunctionQueue(bool); + virtual ~FunctionQueue(); + + static void staticFunction(void* pvParameters); + static EventGroupHandle_t loopEventHandle; + static uint8_t allSynced; + static SemaphoreHandle_t syncedMutex; + static const uint8_t loopIndex; + + void processQueueItem(); + + bool scheduleFunction(std::function); + + uint8_t syncIndex; + + QueueHandle_t functionQueue; + TaskHandle_t functionTask; + +}; + +extern FunctionQueue FQ; + +#endif /* CORE_CORE_FUNCTIONQUEUE_H_ */ diff --git a/cores/esp32/main.cpp b/cores/esp32/main.cpp index 771da0452fa..9bbcab9f71e 100644 --- a/cores/esp32/main.cpp +++ b/cores/esp32/main.cpp @@ -1,6 +1,8 @@ #include "freertos/FreeRTOS.h" #include "freertos/task.h" +#include "freertos/event_groups.h" #include "Arduino.h" +#include "FunctionQueue.h" #if CONFIG_AUTOSTART_ARDUINO @@ -10,12 +12,15 @@ #define ARDUINO_RUNNING_CORE 1 #endif + void loopTask(void *pvParameters) { setup(); for(;;) { - loop(); - } + xEventGroupSync(FunctionQueue::loopEventHandle,0x00,FunctionQueue::allSynced,portMAX_DELAY); + loop(); + xEventGroupSetBits(FunctionQueue::loopEventHandle, FunctionQueue::loopIndex);// enable synced fq's to run + } } extern "C" void app_main() diff --git a/libraries/Ticker/src/Ticker.cpp b/libraries/Ticker/src/Ticker.cpp index ce5cf69332c..8cbf432c377 100644 --- a/libraries/Ticker/src/Ticker.cpp +++ b/libraries/Ticker/src/Ticker.cpp @@ -54,5 +54,18 @@ void Ticker::detach() { esp_timer_stop(_timer); esp_timer_delete(_timer); _timer = nullptr; + _callback_function = nullptr; } } + +void Ticker::_static_callback(void* arg){ + Ticker* _this = (Ticker*)arg; + if (_this == nullptr) + { + return; + } + if (_this->_callback_function) + { + _this->_callback_function(); + } +} diff --git a/libraries/Ticker/src/Ticker.h b/libraries/Ticker/src/Ticker.h index 82804e0f37d..69c7a48a85e 100644 --- a/libraries/Ticker/src/Ticker.h +++ b/libraries/Ticker/src/Ticker.h @@ -25,6 +25,9 @@ #ifndef TICKER_H #define TICKER_H +#include +#include "FunctionQueue.h" + extern "C" { #include "esp_timer.h" } @@ -36,15 +39,28 @@ class Ticker ~Ticker(); typedef void (*callback_t)(void); typedef void (*callback_with_arg_t)(void*); + typedef std::function callback_function_t; + + void attach_scheduled(float seconds, callback_function_t callback) + { + attach(seconds,std::bind(&FunctionQueue::scheduleFunction, &FQ , callback)); + } + + void attach(float seconds, callback_function_t callback) + { + _callback_function = callback; + attach(seconds, _static_callback, (void*)this); + } - void attach(float seconds, callback_t callback) + void attach_ms_scheduled(uint32_t milliseconds, callback_function_t callback) { - _attach_ms(seconds * 1000, true, reinterpret_cast(callback), 0); + attach_ms(milliseconds, std::bind(&FunctionQueue::scheduleFunction, &FQ , callback)); } - void attach_ms(uint32_t milliseconds, callback_t callback) + void attach_ms(uint32_t milliseconds, callback_function_t callback) { - _attach_ms(milliseconds, true, reinterpret_cast(callback), 0); + _callback_function = callback; + attach_ms(milliseconds, _static_callback, (void*)this); } template @@ -66,14 +82,26 @@ class Ticker _attach_ms(milliseconds, true, reinterpret_cast(callback), arg32); } - void once(float seconds, callback_t callback) + void once_scheduled(float seconds, callback_function_t callback) { - _attach_ms(seconds * 1000, false, reinterpret_cast(callback), 0); + once(seconds, std::bind(&FunctionQueue::scheduleFunction, &FQ , callback)); } - void once_ms(uint32_t milliseconds, callback_t callback) + void once(float seconds, callback_function_t callback) { - _attach_ms(milliseconds, false, reinterpret_cast(callback), 0); + _callback_function = callback; + once(seconds, _static_callback, (void*)this); + } + + void once_ms_scheduled(uint32_t milliseconds, callback_function_t callback) + { + once_ms(milliseconds, std::bind(&FunctionQueue::scheduleFunction, &FQ , callback)); + } + + void once_ms(uint32_t milliseconds, callback_function_t callback) + { + _callback_function = callback; + once_ms(milliseconds, _static_callback, (void*)this); } template @@ -97,10 +125,11 @@ class Ticker protected: void _attach_ms(uint32_t milliseconds, bool repeat, callback_with_arg_t callback, uint32_t arg); - + static void _static_callback (void* arg); protected: esp_timer_handle_t _timer; + callback_function_t _callback_function = nullptr; };