Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into OP-932-Modularize_Radio_Driver

flight/modules/CallbackTest/callbacktest.c

@@ -0,0 +1,132 @@
+/**
+ ******************************************************************************
+ *
+ * @file       callbacktest.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Example module to be used as a template for actual modules.
+ *             Event callback version.
+ *
+ * @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
+ */
+
+/**
+ *
+ * This is a test suite to test the callback scheduler,
+ * including its forward scheduling ability
+ *
+ */
+
+#include "openpilot.h"
+
+// Private constants
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
+#define TASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY
+// Private types
+
+//#define DEBUGPRINT(...) fprintf (stderr, __VA_ARGS__)
+#define DEBUGPRINT(...) xSemaphoreTakeRecursive(mutex, portMAX_DELAY); fprintf(stderr, __VA_ARGS__ ); xSemaphoreGiveRecursive(mutex);
+
+static xSemaphoreHandle mutex;
+
+// Private variables
+static DelayedCallbackInfo *cbinfo[10];
+
+static volatile int32_t counter[10]={0};
+
+static void DelayedCb0();
+static void DelayedCb1();
+static void DelayedCb2();
+static void DelayedCb3();
+static void DelayedCb4();
+static void DelayedCb5();
+static void DelayedCb6();
+/**
+ * Initialise the module, called on startup.
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t CallbackTestInitialize()
+{
+
+	mutex = xSemaphoreCreateRecursiveMutex();
+
+	cbinfo[0] = DelayedCallbackCreate(&DelayedCb0,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[1] = DelayedCallbackCreate(&DelayedCb1,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[2] = DelayedCallbackCreate(&DelayedCb2,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[3] = DelayedCallbackCreate(&DelayedCb3,CALLBACK_PRIORITY_CRITICAL,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[4] = DelayedCallbackCreate(&DelayedCb4,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[5] = DelayedCallbackCreate(&DelayedCb5,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+2,STACK_SIZE);
+	cbinfo[6] = DelayedCallbackCreate(&DelayedCb6,CALLBACK_PRIORITY_LOW,tskIDLE_PRIORITY+20,STACK_SIZE);
+
+	
+	return 0;
+}
+int32_t CallbackTestStart() {
+	
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+	DelayedCallbackDispatch(cbinfo[3]);
+	DelayedCallbackDispatch(cbinfo[2]);
+	DelayedCallbackDispatch(cbinfo[1]);
+	DelayedCallbackDispatch(cbinfo[0]);
+	// different callback priorities within a taskpriority
+	DelayedCallbackSchedule(cbinfo[4],30000,CALLBACK_UPDATEMODE_NONE);
+	DelayedCallbackSchedule(cbinfo[4],5000,CALLBACK_UPDATEMODE_OVERRIDE);
+	DelayedCallbackSchedule(cbinfo[4],4000,CALLBACK_UPDATEMODE_SOONER);
+	DelayedCallbackSchedule(cbinfo[4],10000,CALLBACK_UPDATEMODE_SOONER);
+	DelayedCallbackSchedule(cbinfo[4],1000,CALLBACK_UPDATEMODE_LATER);
+	DelayedCallbackSchedule(cbinfo[4],4800,CALLBACK_UPDATEMODE_LATER);
+	DelayedCallbackSchedule(cbinfo[4],48000,CALLBACK_UPDATEMODE_NONE);
+	// should be at 4.8 seconds after this, allowing for exactly 9 prints of the following
+	DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE);
+	// delayed counter with 500 ms
+	DelayedCallbackDispatch(cbinfo[6]);
+	// high task prio
+	xSemaphoreGiveRecursive(mutex);
+	return 0;
+}
+
+static void DelayedCb0() {
+	DEBUGPRINT("delayed counter low prio 0 updated: %i\n",counter[0]);
+	if (++counter[0]<10) DelayedCallbackDispatch(cbinfo[0]);
+}
+static void DelayedCb1() {
+	DEBUGPRINT("delayed counter low prio 1 updated: %i\n",counter[1]);
+	if (++counter[1]<10) DelayedCallbackDispatch(cbinfo[1]);
+}
+static void DelayedCb2() {
+	DEBUGPRINT("delayed counter high prio 2 updated: %i\n",counter[2]);
+	if (++counter[2]<10) DelayedCallbackDispatch(cbinfo[2]);
+}
+static void DelayedCb3() {
+	DEBUGPRINT("delayed counter high prio 3 updated: %i\n",counter[3]);
+	if (++counter[3]<10) DelayedCallbackDispatch(cbinfo[3]);
+}
+static void DelayedCb4() {
+	DEBUGPRINT("delayed scheduled callback 4 reached!\n");
+	exit(0);
+}
+static void DelayedCb5() {
+	DEBUGPRINT("delayed scheduled counter 5 updated: %i\n",counter[5]);
+	if (++counter[5]<10) DelayedCallbackSchedule(cbinfo[5],500,CALLBACK_UPDATEMODE_NONE);
+	// it will likely only reach 8 due to cb4 aborting the run
+}
+static void DelayedCb6() {
+	DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n",counter[6]);
+	if (++counter[6]<10) DelayedCallbackDispatch(cbinfo[6]);
+}

flight/modules/Example/examplemodcallback.c

@@ -0,0 +1,140 @@
+/**
+ ******************************************************************************
+ *
+ * @file       examplemodcallback.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @brief      Example module to be used as a template for actual modules.
+ *             Event callback version.
+ *
+ * @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
+ */
+
+/**
+ * Input objects: ExampleObject1, ExampleSettings
+ * Output object: ExampleObject2
+ *
+ * This module executes in response to ExampleObject1 updates. When the
+ * module is triggered it will update the data of ExampleObject2.
+ *
+ * No threads are used in this example.
+ *
+ * UAVObjects are automatically generated by the UAVObjectGenerator from
+ * the object definition XML file.
+ *
+ * Modules have no API, all communication to other modules is done through UAVObjects.
+ * However modules may use the API exposed by shared libraries.
+ * See the OpenPilot wiki for more details.
+ * http://www.openpilot.org/OpenPilot_Application_Architecture
+ *
+ */
+
+#include "openpilot.h"
+#include "exampleobject1.h"	// object the module will listen for updates (input)
+#include "exampleobject2.h"	// object the module will update (output)
+#include "examplesettings.h"	// object holding module settings (input)
+
+// Private constants
+#define STACK_SIZE configMINIMAL_STACK_SIZE
+#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW
+#define CBTASK_PRIORITY CALLBACK_TASKPRIORITY_AUXILIARY
+// Private types
+
+// Private variables
+static DelayedCallbackInfo *cbinfo;
+
+// Private functions
+static void ObjectUpdatedCb(UAVObjEvent *ev);
+
+static void DelayedCb();
+/**
+ * Initialise the module, called on startup.
+ * \returns 0 on success or -1 if initialisation failed
+ */
+int32_t ExampleModCallbackInitialize()
+{
+	// Listen for ExampleObject1 updates, connect a callback function
+	ExampleObject1ConnectCallback(&ObjectUpdatedCb);
+
+	cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE);
+	
+	return 0;
+}
+
+/**
+ * This function is called each time ExampleObject1 is updated, this could be
+ * a local update or a remote update from the GCS. In this example the module
+ * does not have its own thread, the callbacks are executed from within the
+ * event thread. Because of that the callback execution time must be kept to
+ * a minimum.
+ */
+static void ObjectUpdatedCb(UAVObjEvent * ev)
+{
+	DelayedCallbackDispatch(cbinfo);
+}
+
+/**
+ * This function is called by the DelayedCallbackScheduler when its execution
+ * has been requested.  Callbacks scheduled for execution are executed in the
+ * same thread in a round robin fashion. The Dispatch function to reschedule
+ * execution can be called from within the Callback itself, in which case the
+ * re-run will be scheduled after all other callback with equal or higher
+ * priority have been executed.  Like event callbacks, delayed callbacks are
+ * executed in the same thread context one at a time, therefore blocking IO
+ * functions or very long lasting calculations are prohibited. Unlike Event
+ * callbacks these callbacks run with a standard (IDLE+1) thread priority and
+ * do not block regular threads. They are therefore saver to use.
+ */
+static void DelayedCb();
+	ExampleSettingsData settings;
+	ExampleObject1Data data1;
+	ExampleObject2Data data2;
+	int32_t step;
+
+	// Update settings with latest value
+	ExampleSettingsGet(&settings);
+
+	// Get the input object
+	ExampleObject1Get(&data1);
+
+	// Determine how to update the output object
+	if (settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP) {
+		step = settings.StepSize;
+	} else {
+		step = -settings.StepSize;
+	}
+
+	// Update data
+	data2.Field1 = data1.Field1 + step;
+	data2.Field2 = data1.Field2 + step;
+	data2.Field3 = data1.Field3 + step;
+	data2.Field4[0] = data1.Field4[0] + step;
+	data2.Field4[1] = data1.Field4[1] + step;
+
+	// Update the ExampleObject2, after this function is called
+	// notifications to any other modules listening to that object
+	// will be sent and the GCS object will be updated through the
+	// telemetry link. All operations will take place asynchronously
+	// and the following call will return immediately.
+	ExampleObject2Set(&data2);
+
+	//call the module again 10 seconds later,
+	//even if the exampleobject has not been updated
+	DelayedCallbackSchedule(cbinfo, 10*1000, CALLBACK_UPDATEMODE_NONE);
+	
+}

flight/modules/System/systemmod.c

@@ -157,6 +157,9 @@ MODULE_INITCALL(SystemModInitialize, 0)
  */
 static void systemTask(void *parameters)
 {
+	/* start the delayed callback scheduler */
+	CallbackSchedulerStart();
+
 	/* create all modules thread */
 	MODULE_TASKCREATE_ALL;
 

flight/targets/boards/coptercontrol/firmware/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/boards/coptercontrol/firmware/inc/openpilot.h

@@ -36,6 +36,7 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
+#include <callbackscheduler.h>
 #include <taskmonitor.h>
 #include <uavtalk.h>
 

flight/targets/boards/coptercontrol/firmware/pios_board.c

@@ -231,6 +231,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/boards/coptercontrol/firmware/pios_board_posix.c

@@ -48,6 +48,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/boards/oplinkmini/firmware/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/boards/oplinkmini/firmware/inc/openpilot.h

@@ -37,6 +37,7 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
+#include <callbackscheduler.h>
 #include <taskmonitor.h>
 #include <uavtalk.h>
 

flight/targets/boards/oplinkmini/firmware/pios_board.c

@@ -141,6 +141,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/boards/oplinkmini/firmware/pios_board_posix.c

@@ -48,6 +48,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/boards/osd/firmware/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/boards/osd/firmware/inc/openpilot.h

@@ -36,6 +36,7 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
+#include <callbackscheduler.h>
 #include <taskmonitor.h>
 #include <uavtalk.h>
 

flight/targets/boards/osd/firmware/pios_board.c

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

flight/targets/boards/revolution/firmware/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/boards/revolution/firmware/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/boards/revolution/firmware/inc/openpilot.h

@@ -37,6 +37,7 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
+#include <callbackscheduler.h>
 #include <taskmonitor.h>
 #include <uavtalk.h>
 

flight/targets/boards/revolution/firmware/pios_board.c

@@ -382,6 +382,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/boards/revolution/firmware/pios_board_sim.c

@@ -152,6 +152,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/boards/revoproto/firmware/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/boards/revoproto/firmware/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/boards/revoproto/firmware/inc/openpilot.h

@@ -37,6 +37,7 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
+#include <callbackscheduler.h>
 #include <taskmonitor.h>
 #include <uavtalk.h>
 

flight/targets/boards/revoproto/firmware/pios_board.c

@@ -431,6 +431,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/boards/revoproto/firmware/pios_board_sim.c

@@ -152,6 +152,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/boards/simposix/firmware/Makefile

@@ -79,6 +79,7 @@ SRC += $(FLIGHTLIB)/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/boards/simposix/firmware/inc/openpilot.h

@@ -37,6 +37,7 @@
 #include <utlist.h>
 #include <uavobjectmanager.h>
 #include <eventdispatcher.h>
+#include <callbackscheduler.h>
 #include <taskmonitor.h>
 #include <uavtalk.h>
 

flight/targets/boards/simposix/firmware/pios_board.c

@@ -129,6 +129,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/uavobjects/callbackscheduler.c

@@ -0,0 +1,417 @@
+/**
+ ******************************************************************************
+ *
+ * @file       callbackscheduler.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013.
+ * @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 STACK_SIZE 128
+#define MAX_SLEEP 1000
+
+// Private types
+/**
+ * task information
+ */
+struct DelayedCallbackTaskStruct {
+        DelayedCallbackInfo *callbackQueue[CALLBACK_PRIORITY_LOW + 1];
+        DelayedCallbackInfo *queueCursor[CALLBACK_PRIORITY_LOW + 1];
+	xTaskHandle callbackSchedulerTaskHandle;
+	signed char name[3];
+	uint32_t stackSize;
+	DelayedCallbackPriorityTask priorityTask;
+	xSemaphoreHandle signal;
+	struct DelayedCallbackTaskStruct *next;
+};
+
+/**
+ * callback information
+ */
+struct DelayedCallbackInfoStruct {
+	DelayedCallback cb;
+	bool volatile waiting;
+	uint32_t volatile scheduletime;
+	struct DelayedCallbackTaskStruct *task;
+	struct DelayedCallbackInfoStruct *next;
+};
+
+
+
+// Private variables
+static struct DelayedCallbackTaskStruct *schedulerTasks;
+static xSemaphoreHandle mutex;
+static bool schedulerStarted;
+
+// Private functions
+static void CallbackSchedulerTask(void *task);
+static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority);
+
+/**
+ * Initialize the scheduler
+ * must be called before any other functions are called
+ * \return Success (0), failure (-1)
+ */
+int32_t CallbackSchedulerInitialize()
+{
+	// Initialize variables
+	schedulerTasks = NULL;
+	schedulerStarted = false;
+
+	// Create mutex
+	mutex = xSemaphoreCreateRecursiveMutex();
+	if (mutex == NULL) {
+		return -1;
+	}
+
+	// Done
+	return 0;
+}
+
+/**
+ * Start all scheduler tasks
+ * Will instantiate all scheduler tasks registered so far.  Although new
+ * callbacks CAN be registered beyond that point, any further scheduling tasks
+ * will be started the moment of instantiation.  It is not possible to increase
+ * the STACK requirements of a scheduler task after this function has been
+ * run.  No callbacks will be run before this function is called, although
+ * they can be marked for later execution by executing the dispatch function.
+ * \return Success (0), failure (-1)
+ */
+int32_t CallbackSchedulerStart()
+{
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+	// only call once
+	PIOS_Assert(schedulerStarted == false);
+
+	// start tasks
+	struct DelayedCallbackTaskStruct *cursor = NULL;
+	int t = 0;
+	LL_FOREACH(schedulerTasks, cursor) {
+		xTaskCreate(
+			CallbackSchedulerTask,
+			cursor->name,
+			cursor->stackSize/4,
+			cursor,
+			cursor->priorityTask,
+			&cursor->callbackSchedulerTaskHandle
+			);
+		if (TASKINFO_RUNNING_CALLBACKSCHEDULER0+t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) {
+			TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, cursor->callbackSchedulerTaskHandle);
+		}
+		t++;
+	}
+
+	schedulerStarted = true;
+
+	xSemaphoreGiveRecursive(mutex);
+
+	return 0;
+}
+
+/**
+ * Schedule dispatching a callback at some point in the future. The function returns immediately.
+ * \param[in] *cbinfo the callback handle
+ * \param[in] milliseconds How far in the future to dispatch the callback
+ * \param[in] updatemode What to do if the callback is already scheduled but not dispatched yet.
+ * The options are:
+ * UPDATEMODE_NONE: An existing schedule will not be touched, the call will have no effect at all if there's an existing schedule.
+ * UPDATEMODE_SOONER: The callback will be rescheduled only if the new schedule triggers before the original one would have triggered.
+ * UPDATEMODE_LATER: The callback will be rescheduled only if the new schedule triggers after the original one would have triggered.
+ * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
+ * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
+ */
+int32_t DelayedCallbackSchedule(
+	DelayedCallbackInfo *cbinfo,
+	int32_t milliseconds,
+	DelayedCallbackUpdateMode updatemode
+	)
+{
+	int32_t result = 0;
+
+	PIOS_Assert(cbinfo);
+
+	if (milliseconds <= 0) {
+		milliseconds = 0; // we can and will not schedule in the past since that ruins the wraparound of uint32_t
+	}
+
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+	uint32_t new = xTaskGetTickCount() + (milliseconds/portTICK_RATE_MS);
+	if (!new) {
+		new = 1; // zero has a special meaning, schedule at time 1 instead
+	}
+
+	int32_t diff = new - cbinfo->scheduletime;
+	if ( (!cbinfo->scheduletime)
+	   || ((updatemode & CALLBACK_UPDATEMODE_SOONER) && diff < 0)
+	   || ((updatemode & CALLBACK_UPDATEMODE_LATER ) && diff > 0)
+	   ) {
+
+		// the scheduletime may be updated
+		if (!cbinfo->scheduletime) {
+			result = 1;
+		} else {
+			result = 2;
+		}
+		cbinfo->scheduletime = new;
+
+		// scheduler needs to be notified to adapt sleep times
+		xSemaphoreGive(cbinfo->task->signal);
+	}
+
+	xSemaphoreGiveRecursive(mutex);
+
+	return result;
+}
+
+/**
+ * Dispatch an event by invoking the supplied callback. The function
+ * returns immediately, the callback is invoked from the event task.
+ * \param[in] cbinfo the callback handle
+ * \return Success (-1), failure (0)
+ */
+int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
+{
+	PIOS_Assert(cbinfo);
+
+	// no semaphore needed for the callback
+	cbinfo->waiting = true;
+	// but the scheduler as a whole needs to be notified
+	return xSemaphoreGive(cbinfo->task->signal);
+}
+
+/**
+ * Dispatch an event by invoking the supplied callback. The function
+ * returns immediately, the callback is invoked from the event task.
+ * \param[in] cbinfo the callback handle
+ * \param[in] pxHigherPriorityTaskWoken
+ * xSemaphoreGiveFromISR() will set *pxHigherPriorityTaskWoken to pdTRUE if
+ * giving the semaphore caused a task to unblock, and the unblocked task has a
+ * priority higher than the currently running task.  If xSemaphoreGiveFromISR()
+ * sets this value to pdTRUE then a context switch should be requested before
+ * the interrupt is exited.
+ * From FreeRTOS Docu: Context switching from an ISR uses port specific syntax.
+ * Check the demo task for your port to find the syntax required.
+ * \return Success (-1), failure (0)
+ */
+int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
+{
+	PIOS_Assert(cbinfo);
+
+	// no semaphore needed for the callback
+	cbinfo->waiting = true;
+	// but the scheduler as a whole needs to be notified
+	return xSemaphoreGiveFromISR(cbinfo->task->signal, pxHigherPriorityTaskWoken);
+}
+
+/**
+ * Register a new callback to be called by a delayed callback scheduler task.
+ * If a scheduler task with the specified task priority does not exist yet, it
+ * will be created.
+ * \param[in] cb The callback to be invoked
+ * \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task.
+ * \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified, further callbacks created  with the same priorityTask will all be handled by the same delayed callback scheduler task and scheduled according to their individual callback priorities
+ * \param[in] stacksize The stack requirements of the callback when called by the scheduler.
+ * \return CallbackInfo Pointer on success, NULL if failed.
+ */
+DelayedCallbackInfo *DelayedCallbackCreate(
+	DelayedCallback cb,
+	DelayedCallbackPriority priority,
+	DelayedCallbackPriorityTask priorityTask,
+	uint32_t stacksize
+	)
+{
+
+	xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+
+	// find appropriate scheduler task matching priorityTask
+	struct DelayedCallbackTaskStruct *task = NULL;
+	int t = 0;
+	LL_FOREACH(schedulerTasks, task) {
+		if (task->priorityTask == priorityTask) {
+			break; // found
+		}
+		t++;
+	}
+	// if given priorityTask does not exist, create it
+	if (!task) {
+		// allocate memory if possible
+		task = (struct DelayedCallbackTaskStruct*)pvPortMalloc(sizeof(struct DelayedCallbackTaskStruct));
+		if (!task) {
+	                xSemaphoreGiveRecursive(mutex);
+			return NULL;
+		}
+
+		// initialize structure
+		for (DelayedCallbackPriority p = 0; p <= CALLBACK_PRIORITY_LOW; p++) {
+			task->callbackQueue[p] = NULL;
+			task->queueCursor[p]   = NULL;
+		}
+		task->name[0]      = 'C';
+		task->name[1]      = 'a'+t;
+		task->name[2]      = 0;
+		task->stackSize    = ((STACK_SIZE>stacksize)?STACK_SIZE:stacksize);
+		task->priorityTask = priorityTask;
+		task->next         = NULL;
+
+		// create the signaling semaphore
+		vSemaphoreCreateBinary( task->signal );
+		if (!task->signal) {
+	                xSemaphoreGiveRecursive(mutex);
+			return NULL;
+		}
+
+		// add to list of scheduler tasks
+		LL_APPEND(schedulerTasks, task);
+
+		// Previously registered tasks are spawned when CallbackSchedulerStart() is called.
+		// Tasks registered afterwards need to spawn upon creation.
+		if (schedulerStarted) {
+			xTaskCreate( 
+				CallbackSchedulerTask,
+				task->name,
+				task->stackSize/4,
+				task,
+				task->priorityTask,
+				&task->callbackSchedulerTaskHandle
+				);
+			if (TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t <= TASKINFO_RUNNING_CALLBACKSCHEDULER3) {
+				TaskMonitorAdd(TASKINFO_RUNNING_CALLBACKSCHEDULER0 + t, task->callbackSchedulerTaskHandle);
+			}
+		}
+	}
+
+	if (!schedulerStarted && stacksize > task->stackSize) {
+		task->stackSize = stacksize; // previous to task initialisation we can still adapt to the maximum needed stack
+	}
+
+
+	if (stacksize > task->stackSize) {
+		xSemaphoreGiveRecursive(mutex);
+		return NULL; // error - not enough memory
+	}
+
+	// initialize callback scheduling info
+	DelayedCallbackInfo *info = (DelayedCallbackInfo*)pvPortMalloc(sizeof(DelayedCallbackInfo));
+	if (!info) {
+		xSemaphoreGiveRecursive(mutex);
+		return NULL; // error - not enough memory
+	}
+	info->next         = NULL;
+	info->waiting      = false;
+	info->scheduletime = 0;
+	info->task         = task;
+	info->cb           = cb;
+
+	// add to scheduling queue
+	LL_APPEND(task->callbackQueue[priority], info);
+
+	xSemaphoreGiveRecursive(mutex);
+
+	return info;
+}
+
+/**
+ * Scheduler subtask
+ * \param[in] task The scheduler task in question
+ * \param[in] priority The scheduling priority of the callback to search for
+ * \return wait time until next scheduled callback is due - 0 if a callback has just been executed
+ */
+static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCallbackPriority priority)
+{
+
+	int32_t result = MAX_SLEEP;
+	int32_t diff   = 0;
+
+	// no such queue
+	if (priority > CALLBACK_PRIORITY_LOW) {
+		return result;
+	}
+
+	// queue is empty, search a lower priority queue
+	if (task->callbackQueue[priority] == NULL) {
+		return runNextCallback(task, priority + 1);
+	}
+
+	DelayedCallbackInfo *current = task->queueCursor[priority];
+	DelayedCallbackInfo *next;
+	do {
+		if (current == NULL) {
+			next = task->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
+			diff = runNextCallback(task, priority + 1);
+			if (!diff) {
+				task->queueCursor[priority] = next; // the recursive call has executed a callback
+				return 0;
+			}
+			if (diff < result) {
+				result = diff; // adjust sleep time
+			}
+		} else {
+			next = current->next;
+			xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to scheduletime should be mutex protected
+			if (current->scheduletime) {
+				diff = current->scheduletime - xTaskGetTickCount();
+				if (diff <= 0) {
+					current->waiting = true;
+				} else if (diff < result) {
+					result = diff; // adjust sleep time
+				}
+			}
+			if (current->waiting) {
+				task->queueCursor[priority] = next;
+				current->scheduletime = 0; // any schedules are reset
+				current->waiting = false; // the flag is reset just before execution.
+				xSemaphoreGiveRecursive(mutex);
+				current->cb(); // call the callback
+				return 0;
+			}
+			xSemaphoreGiveRecursive(mutex);
+		}
+		current = next;
+	} while (current != task->queueCursor[priority]);
+	// once the list has been traversed entirely without finding any to be executed task, abort (nothing to do)
+	return result;
+}
+
+/**
+ * Scheduler task, responsible of invoking callbacks.
+ * \param[in] task The scheduling task being run
+ */
+static void CallbackSchedulerTask(void *task)
+{
+	uint32_t delay = 0;
+
+	while (1) {
+
+		delay = runNextCallback((struct DelayedCallbackTaskStruct*)task, CALLBACK_PRIORITY_CRITICAL);
+		if (delay) {
+			// nothing to do but sleep
+			xSemaphoreTake(((struct DelayedCallbackTaskStruct*)task)->signal, delay);
+		}
+	}
+}
+

flight/uavobjects/inc/callbackscheduler.h

@@ -0,0 +1,189 @@
+/**
+ ******************************************************************************
+ *
+ * @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;
+// Use the CallbackPriority to define how frequent a callback needs to be
+// called in relation to others in the same callback scheduling task.
+// The scheduler will call callbacks waiting for execution with the same
+// priority in a round robin way. However one slot in this queue is reserved
+// for a chosen member of the next lower priority. This member will also be
+// chosen in a round robin way.
+// Example:
+// Assume you have 6 callbacks in the same PriorityTask, all constantly wanting
+// to be executed.
+// A and B are priority CRITICAL,
+// c and d are priority REGULAR,
+// x and y are priority LOW.
+// Then the execution schedule will look as follows:
+// ...ABcABdABxABcABdAByABcABdABxABcABdABy...
+// However if only the 3 callbacks, A, c and x want to execute, you will get:
+// ...AcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAxAcAx...
+// And if onlz A and y need execution it will be:
+// ...AyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAy...
+// despite their different priority they would get treated equally in this case.
+//
+// WARNING: Callbacks ALWAYS should return as quickly as possible.  Otherwise
+// a low priority callback can block a critical one from being executed.
+// Callbacks MUST NOT block execution!
+
+typedef enum{
+	CALLBACK_TASK_AUXILIARY     = (tskIDLE_PRIORITY + 1),
+	CALLBACK_TASK_NAVIGATION    = (tskIDLE_PRIORITY + 2),
+	CALLBACK_TASK_FLIGHTCONTROL = (tskIDLE_PRIORITY + 3),
+	CALLBACK_TASK_DEVICEDRIVER  = (tskIDLE_PRIORITY + 4),
+	} DelayedCallbackPriorityTask;
+// Use the PriorityTask to define the global importance of callback execution
+// compared to other processes in the system.
+// Callbacks dispatched in a higher PriorityTasks will halt the execution of
+// any lower priority processes, including callbacks and even callback
+// scheduling tasks until they are done!
+// Assume you have two callbacks:
+// A in priorityTask DEVICEDRIVER,
+// b and c in priorityTask AUXILIARY,
+// Then the execution schedule can look as follows: (| marks a task switch)
+// <b ... /b><c ... |<A ... /A>| ... /c><b ...
+// be aware that if A gets constantly dispatched this would look like this:
+// <b ... |<A><A><A><A><A><A><A><A><A><A><A><A><A><A><A><A><A><A><A>...
+//
+// WARNING: Any higher priority task can prevent lower priority code from being
+// executed! (This does not only apply to callbacks but to all FreeRTOS tasks!)
+
+typedef enum{
+	CALLBACK_UPDATEMODE_NONE     = 0,
+	CALLBACK_UPDATEMODE_SOONER   = 1,
+	CALLBACK_UPDATEMODE_LATER    = 2,
+	CALLBACK_UPDATEMODE_OVERRIDE = 3
+	} DelayedCallbackUpdateMode;
+// When scheduling a callback for execution at a time in the future, use the
+// update mode to define what should happen if the callback is already
+// scheduled.
+// With NONE, the schedule will not be updated and the callback will be
+// executed at the original time.
+// With SOONER, the closer of the two schedules will take precedence
+// With LATER, the schedule more distant in the future will be used.
+// With OVERRIDE, the original schedule will be discarded.
+
+typedef void (*DelayedCallback)(void);
+// Use this type for the callback function.
+
+struct DelayedCallbackInfoStruct;
+typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo;
+// Use a pointer to DelayedCallbackInfo as a handle to identify registered callbacks.
+// be aware that the same callback function can be registered as a callback
+// several times, even with different callback priorities and even
+// priorityTasks, using different handles and as such different dispatch calls.
+// Be aware that using different priorityTasks for the same callback function
+// might cause your callback to be executed recursively in different task contexts!
+
+// Public functions
+//
+
+/**
+ * Initialize the scheduler
+ * must be called before any other functions are called
+ * \return Success (0), failure (-1)
+ */
+int32_t CallbackSchedulerInitialize();
+
+/**
+ * Start all scheduler tasks
+ * Will instantiate all scheduler tasks registered so far.  Although new
+ * callbacks CAN be registered beyond that point, any further scheduling tasks
+ * will be started the moment of instantiation.  It is not possible to increase
+ * the STACK requirements of a scheduler task after this function has been
+ * run.  No callbacks will be run before this function is called, although
+ * they can be marked for later execution by executing the dispatch function.
+ * \return Success (0), failure (-1)
+ */
+int32_t CallbackSchedulerStart();
+
+/**
+ * Register a new callback to be called by a delayed callback scheduler task.
+ * If a scheduler task with the specified task priority does not exist yet, it
+ * will be created.
+ * \param[in] cb The callback to be invoked
+ * \param[in] priority Priority of the callback compared to other callbacks scheduled by the same delayed callback scheduler task.
+ * \param[in] priorityTask Task priority of the scheduler task. One scheduler task will be spawned for each distinct value specified, further callbacks created  with the same priorityTask will all be handled by the same delayed callback scheduler task and scheduled according to their individual callback priorities
+ * \param[in] stacksize The stack requirements of the callback when called by the scheduler.
+ * \return CallbackInfo Pointer on success, NULL if failed.
+ */
+DelayedCallbackInfo *DelayedCallbackCreate(
+	DelayedCallback cb,
+	DelayedCallbackPriority priority,
+	DelayedCallbackPriorityTask priorityTask,
+	uint32_t stacksize
+	);
+
+/**
+ * Schedule dispatching a callback at some point in the future. The function returns immediately.
+ * \param[in] *cbinfo the callback handle
+ * \param[in] milliseconds How far in the future to dispatch the callback
+ * \param[in] updatemode What to do if the callback is already scheduled but not dispatched yet.
+ * The options are:
+ * UPDATEMODE_NONE: An existing schedule will not be touched, the call will have no effect at all if there's an existing schedule.
+ * UPDATEMODE_SOONER: The callback will be rescheduled only if the new schedule triggers before the original one would have triggered.
+ * UPDATEMODE_LATER: The callback will be rescheduled only if the new schedule triggers after the original one would have triggered.
+ * UPDATEMODE_OVERRIDE: The callback will be rescheduled in any case, effectively overriding any previous schedule. (sooner+later=override)
+ * \return 0: not scheduled, previous schedule takes precedence, 1: new schedule, 2: previous schedule overridden
+ */
+int32_t DelayedCallbackSchedule(
+	DelayedCallbackInfo *cbinfo,
+	int32_t milliseconds,
+	DelayedCallbackUpdateMode updatemode
+	);
+
+/**
+ * Dispatch an event by invoking the supplied callback. The function
+ * returns immediately, the callback is invoked from the event task.
+ * \param[in] *cbinfo the callback handle
+ * \return Success (-1), failure (0)
+ */
+int32_t DelayedCallbackDispatch( DelayedCallbackInfo *cbinfo );
+
+/**
+ * Dispatch an event by invoking the supplied callback. The function
+ * returns immediately, the callback is invoked from the event task.
+ * \param[in] *cbinfo the callback handle
+ * \param[in] pxHigherPriorityTaskWoken
+ * xSemaphoreGiveFromISR() will set *pxHigherPriorityTaskWoken to pdTRUE if
+ * giving the semaphore caused a task to unblock, and the unblocked task has a
+ * priority higher than the currently running task.  If xSemaphoreGiveFromISR()
+ * sets this value to pdTRUE then a context switch should be requested before
+ * the interrupt is exited.
+ * From FreeRTOS Docu: Context switching from an ISR uses port specific syntax.
+ * Check the demo task for your port to find the syntax required.
+ * \return Success (-1), failure (0)
+ */
+int32_t DelayedCallbackDispatchFromISR( DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken );
+
+#endif // CALLBACKSCHEDULER_H

shared/uavobjectdefinition/taskinfo.xml

@@ -27,6 +27,10 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>CallbackScheduler0</elementname>
+			<elementname>CallbackScheduler1</elementname>
+			<elementname>CallbackScheduler2</elementname>
+			<elementname>CallbackScheduler3</elementname>
 		</elementnames>
 	</field> 
 	<field name="Running" units="bool" type="enum">
@@ -55,6 +59,10 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>CallbackScheduler0</elementname>
+			<elementname>CallbackScheduler1</elementname>
+			<elementname>CallbackScheduler2</elementname>
+			<elementname>CallbackScheduler3</elementname>
 		</elementnames>
 		<options>
 			<option>False</option>
@@ -87,6 +95,10 @@
 			<elementname>ModemStat</elementname>
 			<elementname>Autotune</elementname>
 			<elementname>EventDispatcher</elementname>
+			<elementname>CallbackScheduler0</elementname>
+			<elementname>CallbackScheduler1</elementname>
+			<elementname>CallbackScheduler2</elementname>
+			<elementname>CallbackScheduler3</elementname>
 		</elementnames>
 	</field> 
         <access gcs="readwrite" flight="readwrite"/>