diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index bf996d0d1..dd0fc5bf5 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -45,6 +45,8 @@ #include +#include + #include #include #include @@ -101,7 +103,7 @@ int32_t AltitudeHoldInitialize() // Create object queue - altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, STACK_SIZE_BYTES); + altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); return 0; diff --git a/flight/modules/CallbackTest/callbacktest.c b/flight/modules/CallbackTest/callbacktest.c index 0b4ccf474..56d8243a2 100644 --- a/flight/modules/CallbackTest/callbacktest.c +++ b/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[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); + cbinfo[0] = DelayedCallbackCreate(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[1] = DelayedCallbackCreate(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[2] = DelayedCallbackCreate(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[3] = DelayedCallbackCreate(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[4] = DelayedCallbackCreate(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[5] = DelayedCallbackCreate(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[6] = DelayedCallbackCreate(&DelayedCb6, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 20, -1, STACK_SIZE); return 0; diff --git a/flight/modules/Example/examplemodcallback.c b/flight/modules/Example/examplemodcallback.c index 349b80934..8bd052588 100644 --- a/flight/modules/Example/examplemodcallback.c +++ b/flight/modules/Example/examplemodcallback.c @@ -45,6 +45,7 @@ */ #include "openpilot.h" +#include "callbackinfo.h" // object needed for callback id macro CALLBACKINFO_RUNNING_ #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 = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_EXAMPLE, STACK_SIZE); return 0; } diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 6c548b6b7..4e337e10d 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -31,6 +31,7 @@ #include "openpilot.h" +#include "callbackinfo.h" #include "pathplan.h" #include "flightstatus.h" #include "airspeedstate.h" @@ -115,8 +116,8 @@ int32_t PathPlannerInitialize() WaypointInitialize(); WaypointActiveInitialize(); - pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, STACK_SIZE_BYTES); - pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, STACK_SIZE_BYTES); + pathPlannerHandle = DelayedCallbackCreate(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES); + pathDesiredUpdaterHandle = DelayedCallbackCreate(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES); return 0; } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 3c142cdf5..587070547 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -31,6 +31,8 @@ #include "inc/stateestimation.h" +#include + #include #include #include @@ -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 = DelayedCallbackCreate(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required); return 0; } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index df2bf7e15..88b4fd1fe 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include #include #if defined(PIOS_INCLUDE_RFM22B) @@ -147,6 +147,7 @@ int32_t SystemModInitialize(void) ObjectPersistenceInitialize(); #ifdef DIAG_TASKS TaskInfoInitialize(); + CallbackInfoInitialize(); #endif #ifdef DIAG_I2C_WDG_STATS I2CStatsInitialize(); diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index f8534da9a..1f0790eb0 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -112,6 +112,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 diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 90bc9618e..3d66daec2 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -84,6 +84,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 diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index a0e668138..8e33bb8c1 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/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 diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index f93429e80..a925f36d4 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/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 diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 44a9828c6..318137d47 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/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 diff --git a/flight/uavobjects/callbackscheduler.c b/flight/uavobjects/callbackscheduler.c index f0f666a15..d31ec3202 100644 --- a/flight/uavobjects/callbackscheduler.c +++ b/flight/uavobjects/callbackscheduler.c @@ -52,6 +52,7 @@ struct DelayedCallbackTaskStruct { */ struct DelayedCallbackInfoStruct { DelayedCallback cb; + int16_t callbackID; bool volatile waiting; uint32_t volatile scheduletime; struct DelayedCallbackTaskStruct *task; @@ -241,6 +242,7 @@ DelayedCallbackInfo *DelayedCallbackCreate( DelayedCallback cb, DelayedCallbackPriority priority, DelayedCallbackPriorityTask priorityTask, + int16_t callbackID, uint32_t stacksize) { xSemaphoreTakeRecursive(mutex, portMAX_DELAY); @@ -318,11 +320,12 @@ DelayedCallbackInfo *DelayedCallbackCreate( xSemaphoreGiveRecursive(mutex); return NULL; // error - not enough memory } - info->next = NULL; - info->waiting = false; + info->next = NULL; + info->waiting = false; info->scheduletime = 0; - info->task = task; + info->task = task; info->cb = cb; + info->callbackID = callbackID; // add to scheduling queue LL_APPEND(task->callbackQueue[priority], info); diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index d9518d620..94564fc54 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -26,7 +26,7 @@ #include -#include +#include // Private constants #if defined(PIOS_EVENTDISAPTCHER_QUEUE) @@ -103,7 +103,7 @@ int32_t EventDispatcherInitialize() mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); // Create callback - eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, STACK_SIZE * 4); + eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_EVENTDISPATCHER, STACK_SIZE * 4); DelayedCallbackDispatch(eventSchedulerCallback); // Done diff --git a/flight/uavobjects/inc/callbackscheduler.h b/flight/uavobjects/inc/callbackscheduler.h index 8dfd7b682..3ec3fd648 100644 --- a/flight/uavobjects/inc/callbackscheduler.h +++ b/flight/uavobjects/inc/callbackscheduler.h @@ -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( DelayedCallback cb, DelayedCallbackPriority priority, DelayedCallbackPriorityTask priorityTask, + int16_t callbackID, uint32_t stacksize); /** diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index e5208baa9..74388f4b8 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/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 \