Merge branch 'corvuscorax/callbackeventdispatcher2' into next

flight/modules/System/systemmod.c

@@ -169,12 +169,14 @@ MODULE_INITCALL(SystemModInitialize, 0);
  */
 static void systemTask(__attribute__((unused)) void *parameters)
 {
-    /* start the delayed callback scheduler */
-    CallbackSchedulerStart();
     uint8_t cycleCount = 0;
+
     /* 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/boards/coptercontrol/firmware/pios_board.c

@@ -197,6 +197,9 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -237,9 +240,6 @@ void PIOS_Board_Init(void)
         AlarmsSet(SYSTEMALARMS_ALARM_BOOTFAULT, SYSTEMALARMS_ALARM_CRITICAL);
     }
 
-    /* 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

@@ -38,6 +38,9 @@ void PIOS_Board_Init(void)
     /* Delay system */
     PIOS_DELAY_Init();
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -50,9 +53,6 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
-    /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
-
     /* Initialize the PiOS library */
     PIOS_COM_Init();
 }

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

@@ -105,6 +105,9 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -145,9 +148,6 @@ void PIOS_Board_Init(void)
     PIOS_LED_Init(&pios_led_cfg);
 #endif /* PIOS_INCLUDE_LED */
 
-    /* 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

@@ -38,6 +38,9 @@ void PIOS_Board_Init(void)
     /* Delay system */
     PIOS_DELAY_Init();
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -50,9 +53,6 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
-    /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
-
     /* Initialize the PiOS library */
     PIOS_COM_Init();
 }

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

@@ -177,6 +177,9 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -191,9 +194,6 @@ void PIOS_Board_Init(void)
     /* Initialize the alarms library */
     AlarmsInitialize();
 
-    /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
-
     /* IAP System Setup */
     PIOS_IAP_Init();
     uint16_t boot_count = PIOS_IAP_ReadBootCount();

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

@@ -403,6 +403,9 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -415,9 +418,6 @@ void PIOS_Board_Init(void)
     /* Initialize the alarms library */
     AlarmsInitialize();
 
-    /* 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

@@ -131,6 +131,9 @@ void PIOS_Board_Init(void)
     /* Delay system */
     PIOS_DELAY_Init();
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -150,9 +153,6 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
-    /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
-
 #if defined(PIOS_INCLUDE_COM)
 #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
     {

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

@@ -443,6 +443,9 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -452,9 +455,6 @@ void PIOS_Board_Init(void)
     /* Initialize the alarms library */
     AlarmsInitialize();
 
-    /* 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

@@ -131,6 +131,9 @@ void PIOS_Board_Init(void)
     /* Delay system */
     PIOS_DELAY_Init();
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -151,9 +154,6 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
-    /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
-
 #if defined(PIOS_INCLUDE_COM)
 #if defined(PIOS_INCLUDE_TELEMETRY_RF) && 1
     {

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

@@ -123,6 +123,9 @@ void PIOS_Board_Init(void)
         PIOS_Assert(0);
     }
 
+    /* Initialize the delayed callback library */
+    CallbackSchedulerInitialize();
+
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();
     UAVObjInitialize();
@@ -134,9 +137,6 @@ void PIOS_Board_Init(void)
     /* Initialize the alarms library */
     AlarmsInitialize();
 
-    /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
-
     /* Configure IO ports */
 
     /* Configure Telemetry port */

flight/uavobjects/eventdispatcher.c

@@ -41,7 +41,8 @@
 #define STACK_SIZE           configMINIMAL_STACK_SIZE
 #endif /* PIOS_EVENTDISPATCHER_STACK_SIZE */
 
-#define TASK_PRIORITY        (tskIDLE_PRIORITY + 3)
+#define CALLBACK_PRIORITY    CALLBACK_PRIORITY_CRITICAL
+#define TASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
 #define MAX_UPDATE_PERIOD_MS 1000
 
 // Private types
@@ -70,7 +71,7 @@ typedef struct PeriodicObjectListStruct PeriodicObjectList;
 // Private variables
 static PeriodicObjectList *mObjList;
 static xQueueHandle mQueue;
-static xTaskHandle mEventTaskHandle;
+static DelayedCallbackInfo *eventSchedulerCallback;
 static xSemaphoreHandle mMutex;
 static EventStats mStats;
 
@@ -101,8 +102,8 @@ int32_t EventDispatcherInitialize()
     // Create event queue
     mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
 
-    // Create task
+    // Create callback
-    xTaskCreate(eventTask, (signed char *)"Event", STACK_SIZE, NULL, TASK_PRIORITY, &mEventTaskHandle);
+    eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4);
 
     // Done
     return 0;
@@ -145,7 +146,9 @@ int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb)
     evInfo.cb    = cb;
     evInfo.queue = 0;
     // Push to queue
-    return xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full
+    int32_t result = xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full
+    DelayedCallbackDispatch(eventSchedulerCallback);
+    return result;
 }
 
 /**
@@ -276,38 +279,33 @@ static int32_t eventPeriodicUpdate(UAVObjEvent *ev, UAVObjEventCallback cb, xQue
 }
 
 /**
- * Event task, responsible of invoking callbacks.
+ * Delayed event callback, responsible of invoking (event) callbacks.
  */
 static void eventTask()
 {
-    uint32_t timeToNextUpdateMs;
+    static uint32_t timeToNextUpdateMs = 0;
-    uint32_t delayMs;
     EventCallbackInfo evInfo;
 
-    /* Must do this in task context to ensure that TaskMonitor has already finished its init */
+    // Wait for queue message
-    PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_EVENTDISPATCHER, mEventTaskHandle);
+    int limit = MAX_QUEUE_SIZE;
 
-    // Initialize time
+    while (xQueueReceive(mQueue, &evInfo, 0) == pdTRUE) {
-    timeToNextUpdateMs = xTaskGetTickCount() * portTICK_RATE_MS;
+        // Invoke callback, if any
-
+        if (evInfo.cb != 0) {
-    // Loop forever
+            evInfo.cb(&evInfo.ev); // the function is expected to copy the event information
-    while (1) {
-        // Calculate delay time
-        delayMs = timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS);
-
-        // Wait for queue message
-        if (xQueueReceive(mQueue, &evInfo, delayMs / portTICK_RATE_MS) == pdTRUE) {
-            // Invoke callback, if one
-            if (evInfo.cb != 0) {
-                evInfo.cb(&evInfo.ev); // the function is expected to copy the event information
-            }
         }
-
+        // limit loop to max queue size to slightly reduce the impact of recursive events
-        // Process periodic updates
+        if (!--limit) {
-        if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) {
+            break;
-            timeToNextUpdateMs = processPeriodicUpdates();
         }
     }
+
+    // Process periodic updates
+    if ((xTaskGetTickCount() * portTICK_RATE_MS) >= timeToNextUpdateMs) {
+        timeToNextUpdateMs = processPeriodicUpdates();
+    }
+
+    DelayedCallbackSchedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER);
 }
 
 /**