espressif · hreintke · Apr 22, 2018 · Jun 14, 2018 · Jun 19, 2018 · Jun 22, 2018
diff --git a/cores/esp32/FunctionQueue.cpp b/cores/esp32/FunctionQueue.cpp
@@ -0,0 +1,109 @@
+/*
+ * FunctionQueue.cpp
+ *
+ *  Created on: 9 jun. 2018
+ *      Author: Herman
+ */
+
+#include <FunctionQueue.h>
+#include <functional>
+#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<void(void)> 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<FunctionQueue*>(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
@@ -0,0 +1,53 @@
+/*
+ * FunctionQueue.h
+ *
+ *  Created on: 9 jun. 2018
+ *      Author: Herman
+ */
+
+#ifndef CORE_CORE_FUNCTIONQUEUE_H_
+#define CORE_CORE_FUNCTIONQUEUE_H_
+
+#include <functional>
+
+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<void(void)> 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<void(void)>);
+
+	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
@@ -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
@@ -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
@@ -25,6 +25,9 @@
 #ifndef TICKER_H
 #define TICKER_H
 
+#include <functional>
+#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<void(void)> 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_with_arg_t>(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_with_arg_t>(callback), 0);
+	_callback_function = callback;
+	attach_ms(milliseconds, _static_callback, (void*)this);
   }
 
   template<typename TArg>
@@ -66,14 +82,26 @@ class Ticker
     _attach_ms(milliseconds, true, reinterpret_cast<callback_with_arg_t>(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_with_arg_t>(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_with_arg_t>(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<typename TArg>
@@ -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;
 };