Merge branch 'corvuscorax/OP-942_task-diagnostics-for-callbacks' into next

flight/modules/AltitudeHold/altitudehold.c

@@ -45,6 +45,8 @@
 
 #include <openpilot.h>
 
+#include <callbackinfo.h>
+
 #include <math.h>
 #include <pid.h>
 #include <CoordinateConversions.h>
@@ -84,7 +86,7 @@ int32_t AltitudeHoldStart()
 {
     // Start main task
     SettingsUpdatedCb(NULL);
-    DelayedCallbackDispatch(altitudeHoldCBInfo);
+    PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo);
 
     return 0;
 }
@@ -101,7 +103,7 @@ int32_t AltitudeHoldInitialize()
 
     // Create object queue
 
-    altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES);
+    altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
     AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb);
 
     return 0;
@@ -128,7 +130,7 @@ static void altitudeHoldTask(void)
         pid_zero(&pid0);
         pid_zero(&pid1);
         StabilizationDesiredThrottleGet(&startThrottle);
-        DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
+        PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
         return;
 
         break;
@@ -190,7 +192,7 @@ static void altitudeHoldTask(void)
 
     StabilizationDesiredSet(&stab);
 
-    DelayedCallbackSchedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(altitudeHoldCBInfo, DESIRED_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 }
 
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)

flight/modules/CallbackTest/callbacktest.c

@@ -65,13 +65,13 @@ int32_t CallbackTestInitialize()
 {
     mutex     = xSemaphoreCreateRecursiveMutex();
 
-    cbinfo[0] = DelayedCallbackCreate(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[0] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[1] = DelayedCallbackCreate(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[1] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[2] = DelayedCallbackCreate(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[2] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[3] = DelayedCallbackCreate(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[3] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[4] = DelayedCallbackCreate(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[4] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[5] = DelayedCallbackCreate(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, STACK_SIZE);
+    cbinfo[5] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE);
-    cbinfo[6] = DelayedCallbackCreate(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, STACK_SIZE);
+    cbinfo[6] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, -1, STACK_SIZE);
 
 
     return 0;
@@ -79,22 +79,22 @@ int32_t CallbackTestInitialize()
 int32_t CallbackTestStart()
 {
     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
-    DelayedCallbackDispatch(cbinfo[3]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
-    DelayedCallbackDispatch(cbinfo[2]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
-    DelayedCallbackDispatch(cbinfo[1]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
-    DelayedCallbackDispatch(cbinfo[0]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
     // different callback priorities within a taskpriority
-    DelayedCallbackSchedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE);
-    DelayedCallbackSchedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE);
-    DelayedCallbackSchedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER);
-    DelayedCallbackSchedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER);
-    DelayedCallbackSchedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER);
-    DelayedCallbackSchedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER);
-    DelayedCallbackSchedule(cbinfo[4], 48000, CALLBACK_UPDATEMODE_NONE);
+    PIOS_CALLBACKSCHEDULER_Schedule(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);
+    PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
     // delayed counter with 500 ms
-    DelayedCallbackDispatch(cbinfo[6]);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
     // high task prio
     xSemaphoreGiveRecursive(mutex);
     return 0;
@@ -104,28 +104,28 @@ static void DelayedCb0()
 {
     DEBUGPRINT("delayed counter low prio 0 updated: %i\n", counter[0]);
     if (++counter[0] < 10) {
-        DelayedCallbackDispatch(cbinfo[0]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[0]);
     }
 }
 static void DelayedCb1()
 {
     DEBUGPRINT("delayed counter low prio 1 updated: %i\n", counter[1]);
     if (++counter[1] < 10) {
-        DelayedCallbackDispatch(cbinfo[1]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]);
     }
 }
 static void DelayedCb2()
 {
     DEBUGPRINT("delayed counter high prio 2 updated: %i\n", counter[2]);
     if (++counter[2] < 10) {
-        DelayedCallbackDispatch(cbinfo[2]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]);
     }
 }
 static void DelayedCb3()
 {
     DEBUGPRINT("delayed counter high prio 3 updated: %i\n", counter[3]);
     if (++counter[3] < 10) {
-        DelayedCallbackDispatch(cbinfo[3]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]);
     }
 }
 static void DelayedCb4()
@@ -137,7 +137,7 @@ static void DelayedCb5()
 {
     DEBUGPRINT("delayed scheduled counter 5 updated: %i\n", counter[5]);
     if (++counter[5] < 10) {
-        DelayedCallbackSchedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
+        PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[5], 500, CALLBACK_UPDATEMODE_NONE);
     }
     // it will likely only reach 8 due to cb4 aborting the run
 }
@@ -145,6 +145,6 @@ static void DelayedCb6()
 {
     DEBUGPRINT("delayed counter 6 (high task prio) updated: %i\n", counter[6]);
     if (++counter[6] < 10) {
-        DelayedCallbackDispatch(cbinfo[6]);
+        PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[6]);
     }
 }

flight/modules/Example/examplemodcallback.c

@@ -45,6 +45,7 @@
  */
 
 #include "openpilot.h"
+#include "callbackinfo.h" // object needed for callback id macro CALLBACKINFO_RUNNING_<MODULENAME>
 #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)
@@ -71,7 +72,7 @@ int32_t ExampleModCallbackInitialize()
     // Listen for ExampleObject1 updates, connect a callback function
     ExampleObject1ConnectCallback(&ObjectUpdatedCb);
 
-    cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE);
+    cbinfo = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_EXAMPLE, STACK_SIZE);
 
     return 0;
 }
@@ -85,11 +86,11 @@ int32_t ExampleModCallbackInitialize()
  */
 static void ObjectUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 {
-    DelayedCallbackDispatch(cbinfo);
+    PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo);
 }
 
 /**
- * This function is called by the DelayedCallbackScheduler when its execution
+ * This function is called by the PIOS_CALLBACKSCHEDULER_Scheduler 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
@@ -135,5 +136,5 @@ ExampleObject2Set(&data2);
 
 // call the module again 10 seconds later,
 // even if the exampleobject has not been updated
-DelayedCallbackSchedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE);
+PIOS_CALLBACKSCHEDULER_Schedule(cbinfo, 10 * 1000, CALLBACK_UPDATEMODE_NONE);
 }

flight/modules/OPLink/oplinkmod.c

@@ -119,7 +119,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
     MODULE_TASKCREATE_ALL;
 
     /* start the delayed callback scheduler */
-    CallbackSchedulerStart();
+    PIOS_CALLBACKSCHEDULER_Start();
 
     if (mallocFailed) {
         /* We failed to malloc during task creation,

flight/modules/PathPlanner/pathplanner.c

@@ -31,6 +31,7 @@
 
 #include "openpilot.h"
 
+#include "callbackinfo.h"
 #include "pathplan.h"
 #include "flightstatus.h"
 #include "airspeedstate.h"
@@ -95,7 +96,7 @@ int32_t PathPlannerStart()
     PathStatusConnectCallback(statusUpdated);
 
     // Start main task callback
-    DelayedCallbackDispatch(pathPlannerHandle);
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 
     return 0;
 }
@@ -115,8 +116,8 @@ int32_t PathPlannerInitialize()
     WaypointInitialize();
     WaypointActiveInitialize();
 
-    pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES);
+    pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
-    pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES);
+    pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
 
     return 0;
 }
@@ -128,7 +129,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
  */
 static void pathPlannerTask()
 {
-    DelayedCallbackSchedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 
     bool endCondition = false;
 
@@ -332,13 +333,13 @@ static uint8_t checkPathPlan()
 // callback function when status changed, issue execution of state machine
 void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
 {
-    DelayedCallbackDispatch(pathDesiredUpdaterHandle);
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
 }
 
 // callback function when waypoints changed in any way, update pathDesired
 void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
 {
-    DelayedCallbackDispatch(pathPlannerHandle);
+    PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 }
 
 

flight/modules/StateEstimation/stateestimation.c

@@ -31,6 +31,8 @@
 
 #include "inc/stateestimation.h"
 
+#include <callbackinfo.h>
+
 #include <gyrosensor.h>
 #include <accelsensor.h>
 #include <magsensor.h>
@@ -266,7 +268,7 @@ int32_t StateEstimationInitialize(void)
     stack_required = maxint32_t(stack_required, filterEKF13iInitialize(&ekf13iFilter));
     stack_required = maxint32_t(stack_required, filterEKF13Initialize(&ekf13Filter));
 
-    stateEstimationCallback = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, stack_required);
+    stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required);
 
     return 0;
 }
@@ -305,7 +307,7 @@ static void StateEstimationCb(void)
     // after system startup, first few sensor readings might be messed up, delay until everything has settled
     if (bootDelay) {
         bootDelay--;
-        DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
+        PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
         return;
     }
 
@@ -388,7 +390,7 @@ static void StateEstimationCb(void)
 
         // we are not done, re-dispatch self execution
         runState = RUNSTATE_FILTER;
-        DelayedCallbackDispatch(stateEstimationCallback);
+        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
         break;
 
     case RUNSTATE_FILTER:
@@ -405,7 +407,7 @@ static void StateEstimationCb(void)
         if (!current) {
             runState = RUNSTATE_SAVE;
         }
-        DelayedCallbackDispatch(stateEstimationCallback);
+        PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
         break;
 
     case RUNSTATE_SAVE:
@@ -458,9 +460,9 @@ static void StateEstimationCb(void)
         // we are done, re-schedule next self execution
         runState = RUNSTATE_LOAD;
         if (updatedSensors) {
-            DelayedCallbackDispatch(stateEstimationCallback);
+            PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
         } else {
-            DelayedCallbackSchedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
+            PIOS_CALLBACKSCHEDULER_Schedule(stateEstimationCallback, TIMEOUT_MS, CALLBACK_UPDATEMODE_SOONER);
         }
         break;
     }
@@ -513,7 +515,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev)
         updatedSensors |= SENSORUPDATES_airspeed;
     }
 
-    DelayedCallbackDispatch(stateEstimationCallback);
+    PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback);
 }
 
 

flight/modules/System/systemmod.c

@@ -52,7 +52,7 @@
 #include <i2cstats.h>
 #include <taskinfo.h>
 #include <watchdogstatus.h>
-#include <taskinfo.h>
+#include <callbackinfo.h>
 #include <hwsettings.h>
 #include <pios_flashfs.h>
 #if defined(PIOS_INCLUDE_RFM22B)
@@ -96,7 +96,7 @@ static uint32_t idleCounter;
 static uint32_t idleCounterClear;
 static xTaskHandle systemTaskHandle;
 static xQueueHandle objectPersistenceQueue;
-static bool stackOverflow;
+static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_CRITICAL = 3 } stackOverflow;
 static bool mallocFailed;
 static HwSettingsData bootHwSettings;
 static struct PIOS_FLASHFS_Stats fsStats;
@@ -105,6 +105,7 @@ static void objectUpdatedCb(UAVObjEvent *ev);
 static void hwSettingsUpdatedCb(UAVObjEvent *ev);
 #ifdef DIAG_TASKS
 static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_info *task_info, void *context);
+static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context);
 #endif
 static void updateStats();
 static void updateSystemAlarms();
@@ -124,7 +125,7 @@ extern uintptr_t pios_user_fs_id;
 int32_t SystemModStart(void)
 {
     // Initialize vars
-    stackOverflow = false;
+    stackOverflow = STACKOVERFLOW_NONE;
     mallocFailed  = false;
     // Create system task
     xTaskCreate(systemTask, (signed char *)"System", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &systemTaskHandle);
@@ -147,6 +148,7 @@ int32_t SystemModInitialize(void)
     ObjectPersistenceInitialize();
 #ifdef DIAG_TASKS
     TaskInfoInitialize();
+    CallbackInfoInitialize();
 #endif
 #ifdef DIAG_I2C_WDG_STATS
     I2CStatsInitialize();
@@ -175,7 +177,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
     MODULE_TASKCREATE_ALL;
 
     /* start the delayed callback scheduler */
-    CallbackSchedulerStart();
+    PIOS_CALLBACKSCHEDULER_Start();
 
     if (mallocFailed) {
         /* We failed to malloc during task creation,
@@ -204,6 +206,7 @@ static void systemTask(__attribute__((unused)) void *parameters)
 
 #ifdef DIAG_TASKS
     TaskInfoData taskInfoData;
+    CallbackInfoData callbackInfoData;
 #endif
 
     // Main system loop
@@ -223,6 +226,9 @@ static void systemTask(__attribute__((unused)) void *parameters)
         // Update the task status object
         PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData);
         TaskInfoSet(&taskInfoData);
+        // Update the callback status object
+        PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData);
+        CallbackInfoSet(&callbackInfoData);
 #endif
 
         // Flash the heartbeat LED
@@ -440,7 +446,26 @@ static void taskMonitorForEachCallback(uint16_t task_id, const struct pios_task_
     ((uint16_t *)&taskData->StackRemaining)[task_id] = task_info->stack_remaining;
     ((uint8_t *)&taskData->RunningTime)[task_id]     = task_info->running_time_percentage;
 }
-#endif
+
+static void callbackSchedulerForEachCallback(int16_t callback_id, const struct pios_callback_info *callback_info, void *context)
+{
+    CallbackInfoData *callbackData = (CallbackInfoData *)context;
+
+    if (callback_id < 0) {
+        return;
+    }
+    // delayed callback scheduler reports callback stack overflows as remaininng: -1
+    if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) {
+        stackOverflow = STACKOVERFLOW_WARNING;
+    }
+    // By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members
+    // of the CallbackInfoXXXXElem enums
+    PIOS_DEBUG_Assert(callback_id < CALLBACKINFO_RUNNING_NUMELEM);
+    ((uint8_t *)&callbackData->Running)[callback_id] = callback_info->is_running;
+    ((uint32_t *)&callbackData->RunningTime)[callback_id]   = callback_info->running_time_count;
+    ((int16_t *)&callbackData->StackRemaining)[callback_id] = callback_info->stack_remaining;
+}
+#endif /* ifdef DIAG_TASKS */
 
 /**
  * Called periodically to update the I2C statistics
@@ -602,10 +627,15 @@ static void updateSystemAlarms()
     }
 
     // Check for stack overflow
-    if (stackOverflow) {
+    switch (stackOverflow) {
-        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
+    case STACKOVERFLOW_NONE:
-    } else {
         AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW);
+        break;
+    case STACKOVERFLOW_WARNING:
+        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING);
+        break;
+    default:
+        AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL);
     }
 
     // Check for event errors
@@ -650,7 +680,7 @@ void vApplicationIdleHook(void)
 void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask,
                                    __attribute__((unused)) signed portCHAR *pcTaskName)
 {
-    stackOverflow = true;
+    stackOverflow = STACKOVERFLOW_CRITICAL;
 #if DEBUG_STACK_OVERFLOW
     static volatile bool wait_here = true;
     while (wait_here) {

flight/pios/common/pios_callbackscheduler.c

@@ -1,7 +1,7 @@
 /**
  ******************************************************************************
  *
- * @file       callbackscheduler.c
+ * @file       pios_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.
  *
@@ -24,12 +24,17 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#include <openpilot.h>
+#include <pios.h>
+#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
 
+#include <utlist.h>
+#include <uavobjectmanager.h>
 #include <taskinfo.h>
 
 // Private constants
-#define STACK_SIZE 128
+#define STACK_SAFETYCOUNT 16
+#define STACK_SIZE        (384 + STACK_SAFETYSIZE)
+#define STACK_SAFETYSIZE  8
 #define MAX_SLEEP         1000
 
 // Private types
@@ -52,8 +57,15 @@ struct DelayedCallbackTaskStruct {
  */
 struct DelayedCallbackInfoStruct {
     DelayedCallback   cb;
+    int16_t callbackID;
     bool volatile     waiting;
     uint32_t volatile scheduletime;
+    uint32_t stackSize;
+    int32_t  stackFree;
+    int32_t  stackNotFree;
+    uint16_t stackSafetyCount;
+    uint16_t currentSafetyCount;
+    uint32_t runCount;
     struct DelayedCallbackTaskStruct *task;
     struct DelayedCallbackInfoStruct *next;
 };
@@ -73,7 +85,7 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa
  * must be called before any other functions are called
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerInitialize()
+int32_t PIOS_CALLBACKSCHEDULER_Initialize()
 {
     // Initialize variables
     schedulerTasks   = NULL;
@@ -99,7 +111,7 @@ int32_t CallbackSchedulerInitialize()
  * they can be marked for later execution by executing the dispatch function.
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerStart()
+int32_t PIOS_CALLBACKSCHEDULER_Start()
 {
     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
@@ -113,7 +125,7 @@ int32_t CallbackSchedulerStart()
         xTaskCreate(
             CallbackSchedulerTask,
             cursor->name,
-            cursor->stackSize / 4,
+            1 + (cursor->stackSize / 4),
             cursor,
             cursor->priorityTask,
             &cursor->callbackSchedulerTaskHandle
@@ -143,7 +155,7 @@ int32_t CallbackSchedulerStart()
  * 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(
+int32_t PIOS_CALLBACKSCHEDULER_Schedule(
     DelayedCallbackInfo *cbinfo,
     int32_t milliseconds,
     DelayedCallbackUpdateMode updatemode)
@@ -191,7 +203,7 @@ int32_t DelayedCallbackSchedule(
  * \param[in] cbinfo the callback handle
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
+int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo)
 {
     PIOS_Assert(cbinfo);
 
@@ -215,7 +227,7 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo)
  * Check the demo task for your port to find the syntax required.
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
+int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken)
 {
     PIOS_Assert(cbinfo);
 
@@ -237,14 +249,18 @@ int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigh
  * \param[in] stacksize The stack requirements of the callback when called by the scheduler.
  * \return CallbackInfo Pointer on success, NULL if failed.
  */
-DelayedCallbackInfo *DelayedCallbackCreate(
+DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
     DelayedCallback cb,
     DelayedCallbackPriority priority,
     DelayedCallbackPriorityTask priorityTask,
+    int16_t callbackID,
     uint32_t stacksize)
 {
     xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
 
+    // add callback schedulers own stack requirements
+    stacksize += STACK_SIZE;
+
     // find appropriate scheduler task matching priorityTask
     struct DelayedCallbackTaskStruct *task = NULL;
     int t = 0;
@@ -271,7 +287,7 @@ DelayedCallbackInfo *DelayedCallbackCreate(
         task->name[0]      = 'C';
         task->name[1]      = 'a' + t;
         task->name[2]      = 0;
-        task->stackSize    = ((STACK_SIZE > stacksize) ? STACK_SIZE : stacksize);
+        task->stackSize    = stacksize;
         task->priorityTask = priorityTask;
         task->next = NULL;
 
@@ -285,13 +301,13 @@ DelayedCallbackInfo *DelayedCallbackCreate(
         // add to list of scheduler tasks
         LL_APPEND(schedulerTasks, task);
 
-        // Previously registered tasks are spawned when CallbackSchedulerStart() is called.
+        // Previously registered tasks are spawned when PIOS_CALLBACKSCHEDULER_Start() is called.
         // Tasks registered afterwards need to spawn upon creation.
         if (schedulerStarted) {
             xTaskCreate(
                 CallbackSchedulerTask,
                 task->name,
-                task->stackSize / 4,
+                1 + (task->stackSize / 4),
                 task,
                 task->priorityTask,
                 &task->callbackSchedulerTaskHandle
@@ -323,6 +339,13 @@ DelayedCallbackInfo *DelayedCallbackCreate(
     info->scheduletime       = 0;
     info->task               = task;
     info->cb = cb;
+    info->callbackID         = callbackID;
+    info->runCount           = 0;
+    info->stackSize          = stacksize - STACK_SIZE;
+    info->stackNotFree       = info->stackSize;
+    info->stackFree          = 0;
+    info->stackSafetyCount   = STACK_SAFETYCOUNT;
+    info->currentSafetyCount = 0;
 
     // add to scheduling queue
     LL_APPEND(task->callbackQueue[priority], info);
@@ -332,6 +355,108 @@ DelayedCallbackInfo *DelayedCallbackCreate(
     return info;
 }
 
+/**
+ * Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
+ *
+ * @param[in] callback  Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
+ * @param     context   Context information optionally provided to the callback.
+ */
+void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context)
+{
+    if (!callback) {
+        return;
+    }
+
+    struct pios_callback_info info;
+
+    struct DelayedCallbackTaskStruct *task = NULL;
+    LL_FOREACH(schedulerTasks, task) {
+        int prio;
+
+        for (prio = 0; prio < (CALLBACK_PRIORITY_LOW + 1); prio++) {
+            struct DelayedCallbackInfoStruct *cbinfo;
+            LL_FOREACH(task->callbackQueue[prio], cbinfo) {
+                xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
+                info.is_running = true;
+                info.stack_remaining    = cbinfo->stackNotFree;
+                info.running_time_count = cbinfo->runCount;
+                xSemaphoreGiveRecursive(mutex);
+                callback(cbinfo->callbackID, &info, context);
+            }
+        }
+    }
+}
+
+/**
+ * Stack magic, find how much stack is being used without affecting performance
+ */
+static void markStack(DelayedCallbackInfo *current)
+{
+    register int8_t t;
+    register int32_t halfWayMark;
+    volatile unsigned char *marker;
+
+    if (current->stackNotFree < 0) {
+        return;
+    }
+
+    // end of stack watermark
+    marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        *(marker + t) = '#';
+    }
+    // shifted watermarks
+    halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
+    marker = (unsigned char *)((size_t)marker + halfWayMark);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        *(marker + t) = '#';
+    }
+}
+static void checkStack(DelayedCallbackInfo *current)
+{
+    register int8_t t;
+    register int32_t halfWayMark;
+    volatile unsigned char *marker;
+
+    if (current->stackNotFree < 0) {
+        return;
+    }
+
+    // end of stack watermark
+    marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        if (*(marker + t) != '#') {
+            current->stackNotFree = -1; // stack overflow, disable all further checks
+            return;
+        }
+    }
+    // shifted watermarks
+    halfWayMark = current->stackFree + (current->stackNotFree - current->stackFree) / 2;
+    marker = (unsigned char *)((size_t)marker + halfWayMark);
+    for (t = -STACK_SAFETYSIZE; t < 0; t++) {
+        if (*(marker + t) != '#') {
+            current->stackNotFree = halfWayMark; // tainted mark, this place is definitely used stack
+            current->currentSafetyCount = 0;
+            if (current->stackNotFree <= current->stackFree) {
+                current->stackFree = 0; // if it was supposed to be free, restart search between here and bottom of stack
+            }
+            return;
+        }
+    }
+
+    if (current->currentSafetyCount < 0xffff) {
+        current->currentSafetyCount++; // mark has not been tainted, increase safety counter
+    }
+    if (current->currentSafetyCount >= current->stackSafetyCount) { // if the safety counter is above the limit, then
+        if (halfWayMark == current->stackFree) { // check if search already converged, if so increase the limit to find very rare stack usage incidents
+            current->stackSafetyCount = current->currentSafetyCount;
+        } else {
+            current->stackFree = halfWayMark; // otherwise just mark this position as free stack to narrow search
+            current->currentSafetyCount = 0;
+        }
+    }
+}
+
 /**
  * Scheduler subtask
  * \param[in] task The scheduler task in question
@@ -384,7 +509,16 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa
                 current->scheduletime = 0; // any schedules are reset
                 current->waiting = false; // the flag is reset just before execution.
                 xSemaphoreGiveRecursive(mutex);
+
+                /* callback gets invoked here - check stack sizes */
+                markStack(current);
+
                 current->cb(); // call the callback
+
+                checkStack(current);
+
+                current->runCount++;
+
                 return 0;
             }
             xSemaphoreGiveRecursive(mutex);
@@ -411,3 +545,5 @@ static void CallbackSchedulerTask(void *task)
         }
     }
 }
+
+#endif // ifdef PIOS_INCLUDE_CALLBACKSCHEDULER

flight/pios/inc/pios_callbackscheduler.h

@@ -1,9 +1,9 @@
 /**
  ******************************************************************************
  *
- * @file       callbackscheduler.h
+ * @file       pios_callbackscheduler.h
- * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
- * @brief      Include files of the uavobjectlist library
+ * @brief      Include files of the PIOS_CALLBACKSCHEDULER
  * @see        The GNU Public License (GPL) Version 3
  *
  *****************************************************************************/
@@ -23,8 +23,8 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  */
 
-#ifndef CALLBACKSCHEDULER_H
+#ifndef PIOS_CALLBACKSCHEDULER_H
-#define CALLBACKSCHEDULER_H
+#define PIOS_CALLBACKSCHEDULER_H
 
 // Public types
 typedef enum {
@@ -48,7 +48,7 @@ typedef enum {
 // ...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:
+// And if only A and y need execution it will be:
 // ...AyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAyAy...
 // despite their different priority they would get treated equally in this case.
 //
@@ -113,7 +113,7 @@ typedef struct DelayedCallbackInfoStruct DelayedCallbackInfo;
  * must be called before any other functions are called
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerInitialize();
+int32_t PIOS_CALLBACKSCHEDULER_Initialize();
 
 /**
  * Start all scheduler tasks
@@ -125,7 +125,7 @@ int32_t CallbackSchedulerInitialize();
  * they can be marked for later execution by executing the dispatch function.
  * \return Success (0), failure (-1)
  */
-int32_t CallbackSchedulerStart();
+int32_t PIOS_CALLBACKSCHEDULER_Start();
 
 /**
  * Register a new callback to be called by a delayed callback scheduler task.
@@ -135,12 +135,14 @@ int32_t CallbackSchedulerStart();
  * \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.
+ * \param[in] callbackID - CallbackInfoRunningElem from CallbackInfo UAVObject, unique identified to collect stats for the callback, -1 to ignore!
  * \return CallbackInfo Pointer on success, NULL if failed.
  */
-DelayedCallbackInfo *DelayedCallbackCreate(
+DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create(
     DelayedCallback cb,
     DelayedCallbackPriority priority,
     DelayedCallbackPriorityTask priorityTask,
+    int16_t callbackID,
     uint32_t stacksize);
 
 /**
@@ -155,7 +157,7 @@ DelayedCallbackInfo *DelayedCallbackCreate(
  * 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(
+int32_t PIOS_CALLBACKSCHEDULER_Schedule(
     DelayedCallbackInfo *cbinfo,
     int32_t milliseconds,
     DelayedCallbackUpdateMode updatemode);
@@ -166,7 +168,7 @@ int32_t DelayedCallbackSchedule(
  * \param[in] *cbinfo the callback handle
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
+int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo);
 
 /**
  * Dispatch an event by invoking the supplied callback. The function
@@ -182,6 +184,36 @@ int32_t DelayedCallbackDispatch(DelayedCallbackInfo *cbinfo);
  * Check the demo task for your port to find the syntax required.
  * \return Success (-1), failure (0)
  */
-int32_t DelayedCallbackDispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken);
+int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken);
 
-#endif // CALLBACKSCHEDULER_H
+/**
+ * Information about a running callback that has been registered
+ * via a call to PIOS_CALLBACKSCHEDULER_Create().
+ */
+struct pios_callback_info {
+    /** Remaining task stack in bytes -1 for detected stack overflow. */
+    int32_t  stack_remaining;
+    /** Flag indicating whether or not the task is running. */
+    bool     is_running;
+    /** Count of executions of the callback since system start */
+    uint32_t running_time_count;
+};
+
+/**
+ * Iterator callback, called for each monitored callback by PIOS_CALLBACKSCHEDULER_ForEachCallback().
+ *
+ * @param task_id   The id of the task the task_info refers to.
+ * @param task_info Information about the task identified by task_id.
+ * @param context   Context information optionally provided by the caller to PIOS_TASK_MONITOR_TasksIterate()
+ */
+typedef void (*CallbackSchedulerCallbackInfoCallback)(int16_t task_id, const struct pios_callback_info *callback_info, void *context);
+
+/**
+ * Iterator. Iterates over all callbacks and all scheduler tasks and retrieves information
+ *
+ * @param[in] callback  Callback function to receive the data - will be called in same task context as the callerThe id of the task the task_info refers to.
+ * @param     context   Context information optionally provided to the callback.
+ */
+void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context);
+
+#endif // PIOS_CALLBACKSCHEDULER_H

flight/pios/pios.h

@@ -93,6 +93,14 @@
 #include <pios_task_monitor.h>
 #endif
 
+/* PIOS CallbackScheduler */
+#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
+#ifndef PIOS_INCLUDE_FREERTOS
+#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
+#endif
+#include <pios_callbackscheduler.h>
+#endif
+
 /* PIOS bootloader helper */
 #ifdef PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

flight/pios/pios_sim_posix.h

@@ -47,6 +47,14 @@
 #include <pios_task_monitor.h>
 #endif
 
+/* PIOS CallbackScheduler */
+#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER
+#ifndef PIOS_INCLUDE_FREERTOS
+#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined
+#endif
+#include <pios_callbackscheduler.h>
+#endif
+
 /* C Lib Includes */
 #include <stdio.h>
 #include <stdlib.h>

flight/targets/boards/coptercontrol/firmware/Makefile

@@ -70,7 +70,6 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
-    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## UAVObjects
     SRC += $(OPUAVSYNTHDIR)/accessorydesired.c
@@ -112,6 +111,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/relaytuningsettings.c
     SRC += $(OPUAVSYNTHDIR)/relaytuning.c
     SRC += $(OPUAVSYNTHDIR)/taskinfo.c
+    SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
     SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
     SRC += $(OPUAVSYNTHDIR)/ratedesired.c
     SRC += $(OPUAVSYNTHDIR)/barosensor.c

flight/targets/boards/coptercontrol/firmware/inc/openpilot.h

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

flight/targets/boards/coptercontrol/firmware/inc/pios_config.h

@@ -43,6 +43,10 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+
+/* PIOS CallbackScheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h

@@ -34,6 +34,7 @@
 #define PIOS_INCLUDE_DELAY
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_FREERTOS
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
 #define PIOS_INCLUDE_TASK_MONITOR
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_UDP

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

@@ -198,7 +198,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

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

@@ -39,7 +39,7 @@ void PIOS_Board_Init(void)
     PIOS_DELAY_Init();
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/oplinkmini/firmware/Makefile

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

flight/targets/boards/oplinkmini/firmware/inc/openpilot.h

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

flight/targets/boards/oplinkmini/firmware/inc/pios_config.h

@@ -43,6 +43,9 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+/* PIOS CallbackScheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h

@@ -34,6 +34,7 @@
 #define PIOS_INCLUDE_DELAY
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_FREERTOS
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
 #define PIOS_INCLUDE_TASK_MONITOR
 #define PIOS_INCLUDE_COM
 #define PIOS_INCLUDE_UDP

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

@@ -106,7 +106,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

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

@@ -39,7 +39,7 @@ void PIOS_Board_Init(void)
     PIOS_DELAY_Init();
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/osd/firmware/Makefile

@@ -55,7 +55,6 @@ ifndef TESTAPP
     SRC += $(OPUAVTALK)/uavtalk.c
     SRC += $(OPUAVOBJ)/uavobjectmanager.c
     SRC += $(OPUAVOBJ)/eventdispatcher.c
-    SRC += $(OPUAVOBJ)/callbackscheduler.c
 
     ## OSD fonts
     SRC += $(OPSYSTEM)/fonts.c
@@ -84,6 +83,7 @@ ifndef TESTAPP
     SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
     SRC += $(OPUAVSYNTHDIR)/hwsettings.c
     SRC += $(OPUAVSYNTHDIR)/taskinfo.c
+    SRC += $(OPUAVSYNTHDIR)/callbackinfo.c
     SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
     SRC += $(OPUAVSYNTHDIR)/homelocation.c
     SRC += $(OPUAVSYNTHDIR)/gpspositionsensor.c

flight/targets/boards/osd/firmware/inc/openpilot.h

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

flight/targets/boards/osd/firmware/inc/pios_config.h

@@ -43,6 +43,9 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+/* PIOS CallbackScheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

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

@@ -178,7 +178,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/revolution/firmware/Makefile

@@ -73,7 +73,6 @@ 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,7 +141,6 @@ 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/UAVObjects.inc

@@ -90,6 +90,7 @@ UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
 UAVOBJSRCFILENAMES += systemstats
 UAVOBJSRCFILENAMES += taskinfo
+UAVOBJSRCFILENAMES += callbackinfo
 UAVOBJSRCFILENAMES += velocitystate
 UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus

flight/targets/boards/revolution/firmware/inc/openpilot.h

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

flight/targets/boards/revolution/firmware/inc/pios_config.h

@@ -43,6 +43,9 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+/* PIOS Callback Scheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

flight/targets/boards/revolution/firmware/inc/pios_config_sim.h

@@ -35,6 +35,7 @@
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_FREERTOS
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
 #define PIOS_INCLUDE_TASK_MONITOR
 #define PIOS_INCLUDE_COM
 // #define PIOS_INCLUDE_GPS

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

@@ -404,7 +404,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

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

@@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
     PIOS_DELAY_Init();
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/revoproto/firmware/Makefile

@@ -71,7 +71,6 @@ 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,7 +145,6 @@ 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/UAVObjects.inc

@@ -90,6 +90,7 @@ UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
 UAVOBJSRCFILENAMES += systemstats
 UAVOBJSRCFILENAMES += taskinfo
+UAVOBJSRCFILENAMES += callbackinfo
 UAVOBJSRCFILENAMES += velocitystate
 UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus

flight/targets/boards/revoproto/firmware/inc/openpilot.h

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

flight/targets/boards/revoproto/firmware/inc/pios_config.h

@@ -43,6 +43,9 @@
 /* PIOS FreeRTOS support */
 #define PIOS_INCLUDE_FREERTOS
 
+/* PIOS Callback Scheduler support */
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
+
 /* PIOS bootloader helper */
 #define PIOS_INCLUDE_BL_HELPER
 /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */

flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h

@@ -35,6 +35,7 @@
 #define PIOS_INCLUDE_LED
 #define PIOS_INCLUDE_SDCARD
 #define PIOS_INCLUDE_FREERTOS
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
 #define PIOS_INCLUDE_TASK_MONITOR
 #define PIOS_INCLUDE_COM
 // #define PIOS_INCLUDE_GPS

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

@@ -444,7 +444,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

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

@@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
     PIOS_DELAY_Init();
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/targets/boards/simposix/firmware/Makefile

@@ -85,7 +85,6 @@ 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
@@ -101,6 +100,7 @@ SRC += $(MATHLIB)/pid.c
 SRC += $(PIOSCORECOMMON)/pios_task_monitor.c
 SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c
 SRC += $(PIOSCORECOMMON)/pios_debuglog.c
+SRC += $(PIOSCORECOMMON)/pios_callbackscheduler.c
 SRC += $(PIOSCORECOMMON)/pios_deltatime.c
 
 ## PIOS Hardware

flight/targets/boards/simposix/firmware/UAVObjects.inc

@@ -90,6 +90,7 @@ UAVOBJSRCFILENAMES += systemalarms
 UAVOBJSRCFILENAMES += systemsettings
 UAVOBJSRCFILENAMES += systemstats
 UAVOBJSRCFILENAMES += taskinfo
+UAVOBJSRCFILENAMES += callbackinfo
 UAVOBJSRCFILENAMES += velocitystate
 UAVOBJSRCFILENAMES += velocitydesired
 UAVOBJSRCFILENAMES += watchdogstatus

flight/targets/boards/simposix/firmware/inc/openpilot.h

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

flight/targets/boards/simposix/firmware/inc/pios_config.h

@@ -36,6 +36,7 @@
 
 /* Major features */
 #define PIOS_INCLUDE_FREERTOS
+#define PIOS_INCLUDE_CALLBACKSCHEDULER
 #define PIOS_INCLUDE_BL_HELPER
 
 /* Enable/Disable PiOS Modules */

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

@@ -132,7 +132,7 @@ void PIOS_Board_Init(void)
     }
 
     /* Initialize the delayed callback library */
-    CallbackSchedulerInitialize();
+    PIOS_CALLBACKSCHEDULER_Initialize();
 
     /* Initialize UAVObject libraries */
     EventDispatcherInitialize();

flight/uavobjects/eventdispatcher.c

@@ -26,7 +26,7 @@
 
 #include <openpilot.h>
 
-#include <taskinfo.h>
+#include <callbackinfo.h>
 
 // Private constants
 #if defined(PIOS_EVENTDISAPTCHER_QUEUE)
@@ -103,8 +103,8 @@ int32_t EventDispatcherInitialize()
     mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo));
 
     // Create callback
-    eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4);
+    eventSchedulerCallback = PIOS_CALLBACKSCHEDULER_Create(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_EVENTDISPATCHER, STACK_SIZE * 4);
-    DelayedCallbackDispatch(eventSchedulerCallback);
+    PIOS_CALLBACKSCHEDULER_Dispatch(eventSchedulerCallback);
 
     // Done
     return 0;
@@ -148,7 +148,7 @@ int32_t EventCallbackDispatch(UAVObjEvent *ev, UAVObjEventCallback cb)
     evInfo.queue = 0;
     // Push to queue
     int32_t result = xQueueSend(mQueue, &evInfo, 0); // will not block if queue is full
-    DelayedCallbackDispatch(eventSchedulerCallback);
+    PIOS_CALLBACKSCHEDULER_Dispatch(eventSchedulerCallback);
     return result;
 }
 
@@ -306,7 +306,7 @@ static void eventTask()
         timeToNextUpdateMs = processPeriodicUpdates();
     }
 
-    DelayedCallbackSchedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER);
+    PIOS_CALLBACKSCHEDULER_Schedule(eventSchedulerCallback, timeToNextUpdateMs - (xTaskGetTickCount() * portTICK_RATE_MS), CALLBACK_UPDATEMODE_SOONER);
 }
 
 /**

ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro

@@ -102,6 +102,7 @@ HEADERS += \
     $$UAVOBJECT_SYNTHETICS/i2cstats.h \
     $$UAVOBJECT_SYNTHETICS/flightbatterysettings.h \
     $$UAVOBJECT_SYNTHETICS/taskinfo.h \
+    $$UAVOBJECT_SYNTHETICS/callbackinfo.h \
     $$UAVOBJECT_SYNTHETICS/flightplanstatus.h \
     $$UAVOBJECT_SYNTHETICS/flightplansettings.h \
     $$UAVOBJECT_SYNTHETICS/flightplancontrol.h \
@@ -200,6 +201,7 @@ SOURCES += \
     $$UAVOBJECT_SYNTHETICS/i2cstats.cpp \
     $$UAVOBJECT_SYNTHETICS/flightbatterysettings.cpp \
     $$UAVOBJECT_SYNTHETICS/taskinfo.cpp \
+    $$UAVOBJECT_SYNTHETICS/callbackinfo.cpp \
     $$UAVOBJECT_SYNTHETICS/flightplanstatus.cpp \
     $$UAVOBJECT_SYNTHETICS/flightplansettings.cpp \
     $$UAVOBJECT_SYNTHETICS/flightplancontrol.cpp \

make/apps-defs.mk

@@ -97,6 +97,7 @@ SRC += $(PIOSCOMMON)/pios_usb_util.c
 
 ## PIOS system code
 SRC += $(PIOSCOMMON)/pios_task_monitor.c
+SRC += $(PIOSCOMMON)/pios_callbackscheduler.c
 
 ## Misc library functions
 SRC += $(FLIGHTLIB)/fifo_buffer.c

shared/uavobjectdefinition/callbackinfo.xml

@@ -0,0 +1,40 @@
+<xml>
+    <object name="CallbackInfo" singleinstance="true" settings="false" category="System">
+        <description>Task information</description>
+        <field name="StackRemaining" units="bytes" type="int16">
+		<elementnames>
+			<elementname>EventDispatcher</elementname>
+			<elementname>StateEstimation</elementname>
+			<elementname>AltitudeHold</elementname>
+			<elementname>PathPlanner0</elementname>
+			<elementname>PathPlanner1</elementname>
+		</elementnames>
+	</field> 
+	<field name="Running" units="bool" type="enum">
+		<elementnames>
+			<elementname>EventDispatcher</elementname>
+			<elementname>StateEstimation</elementname>
+			<elementname>AltitudeHold</elementname>
+			<elementname>PathPlanner0</elementname>
+			<elementname>PathPlanner1</elementname>
+		</elementnames>
+		<options>
+			<option>False</option>
+			<option>True</option>
+		</options>
+	</field>
+	<field name="RunningTime" units="#" type="uint32">
+		<elementnames>
+			<elementname>EventDispatcher</elementname>
+			<elementname>StateEstimation</elementname>
+			<elementname>AltitudeHold</elementname>
+			<elementname>PathPlanner0</elementname>
+			<elementname>PathPlanner1</elementname>
+		</elementnames>
+	</field> 
+        <access gcs="readonly" flight="readwrite"/>
+        <telemetrygcs acked="false" updatemode="onchange" period="0"/>
+        <telemetryflight acked="false" updatemode="periodic" period="10000"/>
+	<logging updatemode="manual" period="0"/>
+    </object>
+</xml>

shared/uavobjectdefinition/taskinfo.xml

@@ -108,8 +108,8 @@
 		</elementnames>
 	</field> 
         <access gcs="readonly" flight="readwrite"/>
-        <telemetrygcs acked="true" updatemode="onchange" period="0"/>
+        <telemetrygcs acked="false" updatemode="onchange" period="0"/>
-        <telemetryflight acked="true" updatemode="periodic" period="10000"/>
+        <telemetryflight acked="false" updatemode="periodic" period="10000"/>
 	<logging updatemode="manual" period="0"/>
     </object>
 </xml>