rebased delayed callback scheduler

flight/Modules/System/systemmod.c

@@ -151,6 +151,9 @@ static void systemTask(void *parameters)
 	/* create all modules thread */
 	MODULE_TASKCREATE_ALL;
 
+	/* start the delayed callback scheduler */
+	CallbackSchedulerStart();
+
 	if (mallocFailed) {
 		/* We failed to malloc during task creation,
 		 * system behaviour is undefined.  Reset and let

flight/targets/CopterControl/Makefile

@@ -66,6 +66,7 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/accessorydesired.c

flight/targets/CopterControl/System/inc/openpilot.h

@@ -38,6 +38,7 @@
 #include "utlist.h"
 #include "uavobjectmanager.h"
 #include "eventdispatcher.h"
+#include "callbackscheduler.h"
 #include "alarms.h"
 #include "taskmonitor.h"
 #include "uavtalk.h"

flight/targets/CopterControl/System/pios_board.c

@@ -222,6 +222,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* Set up pulse timers */
 	PIOS_TIM_InitClock(&tim_1_cfg);
 	PIOS_TIM_InitClock(&tim_2_cfg);

flight/targets/CopterControl/System/pios_board_posix.c

@@ -49,6 +49,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* Initialize the PiOS library */
 	PIOS_COM_Init();
 

flight/targets/OSD/Makefile

@@ -52,6 +52,7 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## OSD fonts
     SRC += $(OPSYSTEM)/fonts.c

flight/targets/OSD/System/inc/openpilot.h

@@ -38,6 +38,7 @@
 #include "utlist.h"
 #include "uavobjectmanager.h"
 #include "eventdispatcher.h"
+#include "callbackscheduler.h"
 #include "alarms.h"
 #include "taskmonitor.h"
 #include "uavtalk.h"

flight/targets/OSD/System/pios_board.c

@@ -136,6 +136,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 	
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* IAP System Setup */
 	PIOS_IAP_Init();
 	uint16_t boot_count = PIOS_IAP_ReadBootCount();

flight/targets/PipXtreme/Makefile

@@ -43,6 +43,7 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c

flight/targets/PipXtreme/System/inc/openpilot.h

@@ -38,6 +38,7 @@
 #include "utlist.h"
 #include "uavobjectmanager.h"
 #include "eventdispatcher.h"
+#include "callbackscheduler.h"
 #include "alarms.h"
 #include "taskmonitor.h"
 #include "uavtalk.h"

flight/targets/PipXtreme/System/pios_board.c

@@ -115,6 +115,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 #if defined(PIOS_INCLUDE_TIM)
 	/* Set up pulse timers */
 	PIOS_TIM_InitClock(&tim_1_cfg);

flight/targets/PipXtreme/System/pios_board_posix.c

@@ -49,6 +49,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* Initialize the PiOS library */
 	PIOS_COM_Init();
 

flight/targets/RevoMini/Makefile

@@ -70,6 +70,7 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     #ifeq ($(DEBUG), YES)
     SRC += $(OPSYSTEM)/dcc_stdio.c

flight/targets/RevoMini/Makefile.osx

@@ -141,6 +141,7 @@ SRC += $(OPSYSTEM)/alarms.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
+SRC += $(OPUAVOBJ)/callbackscheduler.c
 SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
 else
 ## TESTCODE

flight/targets/RevoMini/System/inc/openpilot.h

@@ -38,6 +38,7 @@
 #include "utlist.h"
 #include "uavobjectmanager.h"
 #include "eventdispatcher.h"
+#include "callbackscheduler.h"
 #include "alarms.h"
 #include "taskmonitor.h"
 #include "uavtalk.h"

flight/targets/RevoMini/System/pios_board.c

@@ -370,6 +370,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* Set up pulse timers */
 	PIOS_TIM_InitClock(&tim_1_cfg);
 	PIOS_TIM_InitClock(&tim_3_cfg);

flight/targets/RevoMini/System/pios_board_sim.c

@@ -154,6 +154,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 #if defined(PIOS_INCLUDE_COM)
 #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
 	{

flight/targets/Revolution/Makefile

@@ -70,6 +70,7 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
+    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     #ifeq ($(DEBUG), YES)
     SRC += $(OPSYSTEM)/dcc_stdio.c

flight/targets/Revolution/Makefile.osx

@@ -145,6 +145,7 @@ SRC += $(OPSYSTEM)/alarms.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
+SRC += $(OPUAVOBJ)/callbackscheduler.c
 SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
 
 

flight/targets/Revolution/System/inc/openpilot.h

@@ -38,6 +38,7 @@
 #include "utlist.h"
 #include "uavobjectmanager.h"
 #include "eventdispatcher.h"
+#include "callbackscheduler.h"
 #include "alarms.h"
 #include "taskmonitor.h"
 #include "uavtalk.h"

flight/targets/Revolution/System/pios_board.c

@@ -417,6 +417,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* Set up pulse timers */
 	PIOS_TIM_InitClock(&tim_1_cfg);
 	PIOS_TIM_InitClock(&tim_2_cfg);

flight/targets/Revolution/System/pios_board_sim.c

@@ -154,6 +154,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 #if defined(PIOS_INCLUDE_COM)
 #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
 	{

flight/targets/SimPosix/Makefile

@@ -82,6 +82,7 @@ SRC += $(OPSYSTEM)/alarms.c
 SRC += $(OPUAVTALK)/uavtalk.c
 SRC += $(OPUAVOBJ)/uavobjectmanager.c
 SRC += $(OPUAVOBJ)/eventdispatcher.c
+SRC += $(OPUAVOBJ)/callbackscheduler.c
 SRC += $(UAVOBJSYNTHDIR)/uavobjectsinit.c
 
 SRC += $(FLIGHTLIB)/CoordinateConversions.c

flight/targets/SimPosix/System/inc/openpilot.h

@@ -38,6 +38,7 @@
 #include "utlist.h"
 #include "uavobjectmanager.h"
 #include "eventdispatcher.h"
+#include "callbackscheduler.h"
 #include "alarms.h"
 #include "taskmonitor.h"
 #include "uavtalk.h"

flight/targets/SimPosix/System/pios_board.c

@@ -122,6 +122,9 @@ void PIOS_Board_Init(void) {
 	/* Initialize the task monitor library */
 	TaskMonitorInitialize();
 
+	/* Initialize the delayed callback library */
+	CallbackSchedulerInitialize();
+
 	/* Configure IO ports */
 	
 	/* Configure Telemetry port */

flight/targets/UAVObjects/callbackscheduler.c

@@ -0,0 +1,193 @@
+/**
+ ******************************************************************************
+ *
+ * @file       callbackscheduler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Scheduler to run callback functions from a shared context with given priorities.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include "openpilot.h"
+
+// Private constants
+#define MAX_QUEUE_SIZE 1
+
+#define STACK_SIZE 128
+
+#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // we run at regular update speed
+#define MAX_UPDATE_PERIOD_MS 1000
+
+// Private types
+/**
+ * callback information
+ */
+struct DelayedCallbackInfoStruct {
+	DelayedCallback cb;
+	bool volatile waiting;
+	struct DelayedCallbackInfoStruct *next;
+};
+
+// Private variables
+static DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW+1];
+static DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW+1];
+static xQueueHandle queue;
+static xTaskHandle callbackSchedulerTaskHandle;
+static xSemaphoreHandle mutex;
+static uint32_t stack_size;
+static bool scheduler_started;
+
+// Private functions
+static void CallbackSchedulerTask();
+static bool runNextCallback(DelayedCallbackPriority priority);
+
+
+/**
+ * Initialize the dispatcher
+ * \return Success (0), failure (-1)
+ */
+int32_t CallbackSchedulerInitialize()
+{
+	// Initialize variables
+	for (DelayedCallbackPriority p=0; p<=CALLBACK_PRIORITY_LOW; p++) callbackQueue[p] = NULL;
+
+	// Create mutex
+	mutex = xSemaphoreCreateRecursiveMutex();
+	if (mutex == NULL)
+		return -1;
+
+	// Create event queue (dummy queue for self signals)
+	queue = xQueueCreate(1,sizeof(DelayedCallbackInfo*));
+
+	//minimal stack_size
+	stack_size = STACK_SIZE; 
+
+	scheduler_started = false;
+
+	// Done
+	return 0;
+}
+
+int32_t CallbackSchedulerStart()
+{
+	// Create task
+	scheduler_started = true;
+	xTaskCreate( CallbackSchedulerTask, (signed char*)"CallbackScheduler", stack_size/4, NULL, TASK_PRIORITY, &callbackSchedulerTaskHandle );
+	TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER, callbackSchedulerTaskHandle);
+	return 0;
+}
+
+/**
+ * Dispatch an event by invoking the supplied callback. The function
+ * returns imidiatelly, the callback is invoked from the event task.
+ * \param[in] cbinfo the callback handle
+ * \return Success (0), failure (-1)
+ */
+int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
+{
+	// no semaphore needed
+	cbinfo->waiting=true;
+	// Push to queue
+	return xQueueSend(queue, &cbinfo, 0); // will not block if queue is full
+}
+
+/**
+ * Dispatch an event at periodic intervals.
+ * \param[in] ev The event to be dispatched
+ * \param[in] cb The callback to be invoked
+ * \param[in] periodMs The period the event is generated
+ * \return Success (0), failure (-1)
+ */
+DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, uint32_t stacksize)
+{
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	if (!scheduler_started && stacksize>stack_size) stack_size=stacksize; // during system initialisation we can adapt to the maximum needed stack
+	xSemaphoreGiveRecursive(mutex);
+
+	if (stacksize>stack_size) return NULL; // error - not enough memory
+
+	// create process info
+	DelayedCallbackInfo *info = (DelayedCallbackInfo*)pvPortMalloc(sizeof(DelayedCallbackInfo));
+	info->next     = NULL;
+	info->waiting  = false;
+	info->cb       = cb;
+
+	// add to scheduling queue
+	LL_APPEND(callbackQueue[priority],info);
+
+	return info;
+}
+
+/**
+ * Scheduler subtask
+ * \param[in] priority The scheduling priority of the callback to search for
+ */
+static bool runNextCallback(DelayedCallbackPriority priority) {
+	// no such queue
+	if (priority>CALLBACK_PRIORITY_LOW) return false;
+
+	// queue is empty, search a lower priority queue
+	if (callbackQueue[priority]==NULL)
+		return runNextCallback(priority+1);
+
+
+	DelayedCallbackInfo *current=queueCursor[priority];
+	DelayedCallbackInfo *next;
+	do {
+		if (current==NULL) {
+			next=callbackQueue[priority]; // loop around the end of the list
+			// also attempt to run a callback that has lower priority
+			// every time the queue is completely traversed
+			if (runNextCallback(priority+1)) {
+				queueCursor[priority]=next;
+				return true;
+			}
+		} else {
+			next=current->next;
+			if (current->waiting) {
+				queueCursor[priority]=next;
+				current->waiting=false; // flag is reset just before execution.
+				current->cb(); // call the callback
+				return true;
+			}
+		}
+		current=next;
+	} while (current!=queueCursor[priority]);
+	// once the list has been traversed entirely once, abort (nothing to do)
+	return false;
+}
+
+
+/**
+ * Scheduler task, responsible of invoking callbacks.
+ */
+static void CallbackSchedulerTask()
+{
+	static DelayedCallbackInfo *dummy;
+	while (1) {
+		
+		if (!runNextCallback(CALLBACK_PRIORITY_CRITICAL)) {
+			// queue has no entries, sleep
+			xQueueReceive(queue, &dummy, MAX_UPDATE_PERIOD_MS/portTICK_RATE_MS);
+		}
+	}
+}
+
+
+

flight/targets/UAVObjects/inc/callbackscheduler.h

@@ -0,0 +1,44 @@
+/**
+ ******************************************************************************
+ *
+ * @file       callbackscheduler.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Include files of the uavobjectlist library
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ *****************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef CALLBACKSCHEDULER_H
+#define CALLBACKSCHEDULER_H
+
+// Public types
+typedef enum{CALLBACK_PRIORITY_CRITICAL=0,CALLBACK_PRIORITY_REGULAR=1,CALLBACK_PRIORITY_LOW=2} DelayedCallbackPriority;
+
+typedef void (*DelayedCallback)(void);
+
+struct DelayedCallbackInfoStruct;
+typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo;
+
+// Public functions
+//
+int32_t CallbackSchedulerInitialize();
+int32_t CallbackSchedulerStart();
+int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
+DelayedCallbackInfo* DelayedCallbackCreate(DelayedCallback cb, DelayedCallbackPriority priority, uint32_t stacksize);
+
+#endif // CALLBACKSCHEDULER_H

shared/uavobjectdefinition/taskinfo.xml

@@ -27,6 +27,7 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>CallbackScheduler</elementname>
 		</elementnames>
 	</field> 
 	<field name="Running" units="bool" type="enum">
@@ -55,6 +56,7 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>CallbackScheduler</elementname>
 		</elementnames>
 		<options>
 			<option>False</option>
@@ -87,6 +89,7 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>CallbackScheduler</elementname>
 		</elementnames>
 	</field> 
         <access gcs="readwrite" flight="readwrite"/>