From b076ba02a18111163ad2a2396301a61021980c86 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Feb 2014 19:54:17 +0100 Subject: [PATCH 01/11] OP-942 API change to CallbackScheduler - include unique ID for each callback for a taskinfo like UAVObject --- flight/modules/AltitudeHold/altitudehold.c | 4 +++- flight/modules/CallbackTest/callbacktest.c | 14 +++++++------- flight/modules/Example/examplemodcallback.c | 3 ++- flight/modules/PathPlanner/pathplanner.c | 5 +++-- flight/modules/StateEstimation/stateestimation.c | 4 +++- flight/modules/System/systemmod.c | 3 ++- .../targets/boards/coptercontrol/firmware/Makefile | 1 + flight/targets/boards/osd/firmware/Makefile | 1 + .../boards/revolution/firmware/UAVObjects.inc | 1 + .../boards/revoproto/firmware/UAVObjects.inc | 1 + .../boards/simposix/firmware/UAVObjects.inc | 1 + flight/uavobjects/callbackscheduler.c | 9 ++++++--- flight/uavobjects/eventdispatcher.c | 4 ++-- flight/uavobjects/inc/callbackscheduler.h | 2 ++ .../src/plugins/uavobjects/uavobjects.pro | 2 ++ 15 files changed, 37 insertions(+), 18 deletions(-) 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 \ From 5b9a17f63092eb5cd9f3ae51a6465ed61f386e50 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Feb 2014 20:31:37 +0100 Subject: [PATCH 02/11] OP-942 moved DelayedCallbackScheduler into pios/common --- .../common/pios_callbackscheduler.c} | 0 .../inc/pios_callbackscheduler.h} | 0 flight/pios/pios.h | 8 ++++++++ flight/pios/pios_sim_posix.h | 8 ++++++++ flight/targets/boards/coptercontrol/firmware/Makefile | 1 - .../targets/boards/coptercontrol/firmware/inc/openpilot.h | 1 - .../boards/coptercontrol/firmware/inc/pios_config.h | 4 ++++ .../boards/coptercontrol/firmware/inc/pios_config_posix.h | 1 + flight/targets/boards/oplinkmini/firmware/Makefile | 1 - flight/targets/boards/oplinkmini/firmware/inc/openpilot.h | 1 - .../targets/boards/oplinkmini/firmware/inc/pios_config.h | 3 +++ .../boards/oplinkmini/firmware/inc/pios_config_posix.h | 1 + flight/targets/boards/osd/firmware/Makefile | 1 - flight/targets/boards/osd/firmware/inc/openpilot.h | 1 - flight/targets/boards/osd/firmware/inc/pios_config.h | 3 +++ flight/targets/boards/revolution/firmware/Makefile | 1 - flight/targets/boards/revolution/firmware/Makefile.osx | 1 - flight/targets/boards/revolution/firmware/inc/openpilot.h | 1 - .../targets/boards/revolution/firmware/inc/pios_config.h | 3 +++ .../boards/revolution/firmware/inc/pios_config_sim.h | 1 + flight/targets/boards/revoproto/firmware/Makefile | 1 - flight/targets/boards/revoproto/firmware/Makefile.osx | 1 - flight/targets/boards/revoproto/firmware/inc/openpilot.h | 1 - .../targets/boards/revoproto/firmware/inc/pios_config.h | 3 +++ .../boards/revoproto/firmware/inc/pios_config_sim.h | 1 + flight/targets/boards/simposix/firmware/Makefile | 2 +- flight/targets/boards/simposix/firmware/inc/openpilot.h | 1 - flight/targets/boards/simposix/firmware/inc/pios_config.h | 1 + make/apps-defs.mk | 1 + 29 files changed, 39 insertions(+), 14 deletions(-) rename flight/{uavobjects/callbackscheduler.c => pios/common/pios_callbackscheduler.c} (100%) rename flight/{uavobjects/inc/callbackscheduler.h => pios/inc/pios_callbackscheduler.h} (100%) diff --git a/flight/uavobjects/callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c similarity index 100% rename from flight/uavobjects/callbackscheduler.c rename to flight/pios/common/pios_callbackscheduler.c diff --git a/flight/uavobjects/inc/callbackscheduler.h b/flight/pios/inc/pios_callbackscheduler.h similarity index 100% rename from flight/uavobjects/inc/callbackscheduler.h rename to flight/pios/inc/pios_callbackscheduler.h diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 6ba15b104..722bb9ed7 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -93,6 +93,14 @@ #include #endif +/* PIOS CallbackScheduler */ +#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER +#ifndef PIOS_INCLUDE_FREERTOS +#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined +#endif +#include +#endif + /* PIOS bootloader helper */ #ifdef PIOS_INCLUDE_BL_HELPER /* #define PIOS_INCLUDE_BL_HELPER_WRITE_SUPPORT */ diff --git a/flight/pios/pios_sim_posix.h b/flight/pios/pios_sim_posix.h index 84fd5440a..5839baf71 100644 --- a/flight/pios/pios_sim_posix.h +++ b/flight/pios/pios_sim_posix.h @@ -47,6 +47,14 @@ #include #endif +/* PIOS CallbackScheduler */ +#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER +#ifndef PIOS_INCLUDE_FREERTOS +#error PiOS CallbackScheduler requires PIOS_INCLUDE_FREERTOS to be defined +#endif +#include +#endif + /* C Lib Includes */ #include #include diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 1f0790eb0..e2fab1dd3 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/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 diff --git a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h index 963014e21..26cfb9a98 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/openpilot.h @@ -36,7 +36,6 @@ #include #include #include -#include #include #include "alarms.h" diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index ea94abb02..2b9417616 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/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 */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h index eda0a2511..4e8aa47c6 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config_posix.h +++ b/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 diff --git a/flight/targets/boards/oplinkmini/firmware/Makefile b/flight/targets/boards/oplinkmini/firmware/Makefile index 3a9c6b113..1768b6ba7 100644 --- a/flight/targets/boards/oplinkmini/firmware/Makefile +++ b/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 diff --git a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h index 3cceef152..89e1e909c 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/openpilot.h @@ -37,7 +37,6 @@ #include #include #include -#include #include #include "alarms.h" diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index ef965b0af..8dd30c0f3 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/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 */ diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h index ecedcf3ce..82712de0d 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config_posix.h +++ b/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 diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 3d66daec2..14d863b1b 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/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 diff --git a/flight/targets/boards/osd/firmware/inc/openpilot.h b/flight/targets/boards/osd/firmware/inc/openpilot.h index 76cc3ccd6..54e6a5d30 100644 --- a/flight/targets/boards/osd/firmware/inc/openpilot.h +++ b/flight/targets/boards/osd/firmware/inc/openpilot.h @@ -36,7 +36,6 @@ #include #include #include -#include #include #include "alarms.h" diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index 1ad6f6629..33542a953 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/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 */ diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 3202b2e1e..26b49d13a 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/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 diff --git a/flight/targets/boards/revolution/firmware/Makefile.osx b/flight/targets/boards/revolution/firmware/Makefile.osx index e5da5da2a..732164a60 100644 --- a/flight/targets/boards/revolution/firmware/Makefile.osx +++ b/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 diff --git a/flight/targets/boards/revolution/firmware/inc/openpilot.h b/flight/targets/boards/revolution/firmware/inc/openpilot.h index 3cceef152..89e1e909c 100644 --- a/flight/targets/boards/revolution/firmware/inc/openpilot.h +++ b/flight/targets/boards/revolution/firmware/inc/openpilot.h @@ -37,7 +37,6 @@ #include #include #include -#include #include #include "alarms.h" diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index fbaff9438..6ad54cc7e 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/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 */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h b/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h index 293516522..c74e0d2f7 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config_sim.h +++ b/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 diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 73dc7acd6..f8f72e0c5 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/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 diff --git a/flight/targets/boards/revoproto/firmware/Makefile.osx b/flight/targets/boards/revoproto/firmware/Makefile.osx index ce2ff7938..d7f29ee4c 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile.osx +++ b/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 diff --git a/flight/targets/boards/revoproto/firmware/inc/openpilot.h b/flight/targets/boards/revoproto/firmware/inc/openpilot.h index 3cceef152..89e1e909c 100644 --- a/flight/targets/boards/revoproto/firmware/inc/openpilot.h +++ b/flight/targets/boards/revoproto/firmware/inc/openpilot.h @@ -37,7 +37,6 @@ #include #include #include -#include #include #include "alarms.h" diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index c8e3d340c..6077225aa 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/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 */ diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h b/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h index 56470b59b..cb2061837 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config_sim.h +++ b/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 diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index 000539682..532b7a4c0 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/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 ## PIOS Hardware include $(PIOS)/posix/library.mk diff --git a/flight/targets/boards/simposix/firmware/inc/openpilot.h b/flight/targets/boards/simposix/firmware/inc/openpilot.h index 3cceef152..89e1e909c 100644 --- a/flight/targets/boards/simposix/firmware/inc/openpilot.h +++ b/flight/targets/boards/simposix/firmware/inc/openpilot.h @@ -37,7 +37,6 @@ #include #include #include -#include #include #include "alarms.h" diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index f3b494e91..9194e68b9 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/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 */ diff --git a/make/apps-defs.mk b/make/apps-defs.mk index b326b218d..a8fae5c35 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -96,6 +96,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 From 49a1c4c28f4485dce8048316de2cffb155104d48 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 2 Feb 2014 22:08:16 +0100 Subject: [PATCH 03/11] OP-942 refactored callback scheduler to fit into PiOS naming scheme --- flight/modules/AltitudeHold/altitudehold.c | 8 +-- flight/modules/CallbackTest/callbacktest.c | 52 +++++++++---------- flight/modules/Example/examplemodcallback.c | 8 +-- flight/modules/OPLink/oplinkmod.c | 2 +- flight/modules/PathPlanner/pathplanner.c | 12 ++--- .../modules/StateEstimation/stateestimation.c | 14 ++--- flight/modules/System/systemmod.c | 2 +- flight/pios/common/pios_callbackscheduler.c | 23 ++++---- flight/pios/inc/pios_callbackscheduler.h | 26 +++++----- .../coptercontrol/firmware/pios_board.c | 2 +- .../coptercontrol/firmware/pios_board_posix.c | 2 +- .../boards/oplinkmini/firmware/pios_board.c | 2 +- .../oplinkmini/firmware/pios_board_posix.c | 2 +- .../targets/boards/osd/firmware/pios_board.c | 2 +- .../boards/revolution/firmware/pios_board.c | 2 +- .../revolution/firmware/pios_board_sim.c | 2 +- .../boards/revoproto/firmware/pios_board.c | 2 +- .../revoproto/firmware/pios_board_sim.c | 2 +- .../boards/simposix/firmware/pios_board.c | 2 +- flight/uavobjects/eventdispatcher.c | 8 +-- 20 files changed, 90 insertions(+), 85 deletions(-) diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index dd0fc5bf5..ce7c556c8 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -86,7 +86,7 @@ int32_t AltitudeHoldStart() { // Start main task SettingsUpdatedCb(NULL); - DelayedCallbackDispatch(altitudeHoldCBInfo); + PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); return 0; } @@ -103,7 +103,7 @@ int32_t AltitudeHoldInitialize() // Create object queue - altitudeHoldCBInfo = DelayedCallbackCreate(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); + altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); return 0; @@ -130,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; @@ -192,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) diff --git a/flight/modules/CallbackTest/callbacktest.c b/flight/modules/CallbackTest/callbacktest.c index 56d8243a2..d5be343de 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, -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); + cbinfo[0] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb0, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[1] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb1, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[2] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb2, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[3] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb3, CALLBACK_PRIORITY_CRITICAL, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[4] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb4, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, STACK_SIZE); + cbinfo[5] = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb5, CALLBACK_PRIORITY_LOW, tskIDLE_PRIORITY + 2, -1, 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]); - DelayedCallbackDispatch(cbinfo[2]); - DelayedCallbackDispatch(cbinfo[1]); - DelayedCallbackDispatch(cbinfo[0]); + PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[3]); + PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[2]); + PIOS_CALLBACKSCHEDULER_Dispatch(cbinfo[1]); + PIOS_CALLBACKSCHEDULER_Dispatch(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); + PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 30000, CALLBACK_UPDATEMODE_NONE); + PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 5000, CALLBACK_UPDATEMODE_OVERRIDE); + PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4000, CALLBACK_UPDATEMODE_SOONER); + PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 10000, CALLBACK_UPDATEMODE_SOONER); + PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 1000, CALLBACK_UPDATEMODE_LATER); + PIOS_CALLBACKSCHEDULER_Schedule(cbinfo[4], 4800, CALLBACK_UPDATEMODE_LATER); + 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]); } } diff --git a/flight/modules/Example/examplemodcallback.c b/flight/modules/Example/examplemodcallback.c index 8bd052588..7c63475fc 100644 --- a/flight/modules/Example/examplemodcallback.c +++ b/flight/modules/Example/examplemodcallback.c @@ -72,7 +72,7 @@ int32_t ExampleModCallbackInitialize() // Listen for ExampleObject1 updates, connect a callback function ExampleObject1ConnectCallback(&ObjectUpdatedCb); - cbinfo = DelayedCallbackCreate(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_EXAMPLE, STACK_SIZE); + cbinfo = PIOS_CALLBACKSCHEDULER_Create(&DelayedCb, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_EXAMPLE, STACK_SIZE); return 0; } @@ -86,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 @@ -136,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); } diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index 994e0d5d9..2ac5e6d74 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/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, diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 4e337e10d..632f4a8e9 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -96,7 +96,7 @@ int32_t PathPlannerStart() PathStatusConnectCallback(statusUpdated); // Start main task callback - DelayedCallbackDispatch(pathPlannerHandle); + PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle); return 0; } @@ -116,8 +116,8 @@ int32_t PathPlannerInitialize() WaypointInitialize(); WaypointActiveInitialize(); - 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); + pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES); + pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES); return 0; } @@ -129,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; @@ -333,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); } diff --git a/flight/modules/StateEstimation/stateestimation.c b/flight/modules/StateEstimation/stateestimation.c index 587070547..bff326c87 100644 --- a/flight/modules/StateEstimation/stateestimation.c +++ b/flight/modules/StateEstimation/stateestimation.c @@ -268,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, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required); + stateEstimationCallback = PIOS_CALLBACKSCHEDULER_Create(&StateEstimationCb, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_STATEESTIMATION, stack_required); return 0; } @@ -307,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; } @@ -390,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: @@ -407,7 +407,7 @@ static void StateEstimationCb(void) if (!current) { runState = RUNSTATE_SAVE; } - DelayedCallbackDispatch(stateEstimationCallback); + PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); break; case RUNSTATE_SAVE: @@ -460,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; } @@ -515,7 +515,7 @@ static void sensorUpdatedCb(UAVObjEvent *ev) updatedSensors |= SENSORUPDATES_airspeed; } - DelayedCallbackDispatch(stateEstimationCallback); + PIOS_CALLBACKSCHEDULER_Dispatch(stateEstimationCallback); } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 88b4fd1fe..010080d66 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -176,7 +176,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, diff --git a/flight/pios/common/pios_callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c index d31ec3202..21ccaff14 100644 --- a/flight/pios/common/pios_callbackscheduler.c +++ b/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,8 +24,11 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#include +#include +#ifdef PIOS_INCLUDE_CALLBACKSCHEDULER +#include +#include #include // Private constants @@ -74,7 +77,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; @@ -100,7 +103,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); @@ -144,7 +147,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) @@ -192,7 +195,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); @@ -216,7 +219,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); @@ -238,7 +241,7 @@ 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, @@ -287,7 +290,7 @@ 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( @@ -414,3 +417,5 @@ static void CallbackSchedulerTask(void *task) } } } + +#endif // ifdef PIOS_INCLUDE_CALLBACKSCHEDULER diff --git a/flight/pios/inc/pios_callbackscheduler.h b/flight/pios/inc/pios_callbackscheduler.h index 3ec3fd648..b72d4d0a4 100644 --- a/flight/pios/inc/pios_callbackscheduler.h +++ b/flight/pios/inc/pios_callbackscheduler.h @@ -1,9 +1,9 @@ /** ****************************************************************************** * - * @file callbackscheduler.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief Include files of the uavobjectlist library + * @file pios_callbackscheduler.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @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 -#define CALLBACKSCHEDULER_H +#ifndef PIOS_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. @@ -138,7 +138,7 @@ int32_t CallbackSchedulerStart(); * \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, @@ -157,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); @@ -168,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 @@ -184,6 +184,6 @@ 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 +#endif // PIOS_CALLBACKSCHEDULER_H diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 194387fdc..4026343ae 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/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(); diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c b/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c index 7487a17aa..7fa958a38 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board_posix.c +++ b/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(); diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 85b3a9939..56adfb4f1 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/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(); diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c b/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c index 7487a17aa..7fa958a38 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board_posix.c +++ b/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(); diff --git a/flight/targets/boards/osd/firmware/pios_board.c b/flight/targets/boards/osd/firmware/pios_board.c index ffaa1dd36..0c45ddfdc 100644 --- a/flight/targets/boards/osd/firmware/pios_board.c +++ b/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(); diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 15b667214..262fcf899 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/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(); diff --git a/flight/targets/boards/revolution/firmware/pios_board_sim.c b/flight/targets/boards/revolution/firmware/pios_board_sim.c index 4a9453efb..2171be22c 100644 --- a/flight/targets/boards/revolution/firmware/pios_board_sim.c +++ b/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(); diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index ed142f951..9206a3f24 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/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(); diff --git a/flight/targets/boards/revoproto/firmware/pios_board_sim.c b/flight/targets/boards/revoproto/firmware/pios_board_sim.c index 401d4be57..a99f0b3e8 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board_sim.c +++ b/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(); diff --git a/flight/targets/boards/simposix/firmware/pios_board.c b/flight/targets/boards/simposix/firmware/pios_board.c index d3dcc3f17..a11316893 100644 --- a/flight/targets/boards/simposix/firmware/pios_board.c +++ b/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(); diff --git a/flight/uavobjects/eventdispatcher.c b/flight/uavobjects/eventdispatcher.c index 94564fc54..e33ac9e30 100644 --- a/flight/uavobjects/eventdispatcher.c +++ b/flight/uavobjects/eventdispatcher.c @@ -103,8 +103,8 @@ int32_t EventDispatcherInitialize() mQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(EventCallbackInfo)); // Create callback - eventSchedulerCallback = DelayedCallbackCreate(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_EVENTDISPATCHER, STACK_SIZE * 4); - DelayedCallbackDispatch(eventSchedulerCallback); + eventSchedulerCallback = PIOS_CALLBACKSCHEDULER_Create(&eventTask, CALLBACK_PRIORITY, TASK_PRIORITY, CALLBACKINFO_RUNNING_EVENTDISPATCHER, STACK_SIZE * 4); + 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); } /** From 418f7e3d0fd70de9215a2da14ba96fc36a145bbc Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 3 Feb 2014 01:00:07 +0100 Subject: [PATCH 04/11] OP-942 task diagnostics for callback scheduler --- flight/pios/common/pios_callbackscheduler.c | 80 +++++++++++++++++++-- 1 file changed, 75 insertions(+), 5 deletions(-) diff --git a/flight/pios/common/pios_callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c index 21ccaff14..4a04e228d 100644 --- a/flight/pios/common/pios_callbackscheduler.c +++ b/flight/pios/common/pios_callbackscheduler.c @@ -27,13 +27,16 @@ #include #ifdef PIOS_INCLUDE_CALLBACKSCHEDULER +#include #include #include #include +#include // Private constants -#define STACK_SIZE 128 -#define MAX_SLEEP 1000 +#define STACK_SIZE 384 +#define STACK_GRANULARITY 16 +#define MAX_SLEEP 1000 // Private types /** @@ -58,6 +61,9 @@ struct DelayedCallbackInfoStruct { int16_t callbackID; bool volatile waiting; uint32_t volatile scheduletime; + uint32_t stackSize; + int32_t stackFree; + uint32_t runCount; struct DelayedCallbackTaskStruct *task; struct DelayedCallbackInfoStruct *next; }; @@ -67,6 +73,7 @@ struct DelayedCallbackInfoStruct { static struct DelayedCallbackTaskStruct *schedulerTasks; static xSemaphoreHandle mutex; static bool schedulerStarted; +static CallbackInfoData callbackInfo; // Private functions static void CallbackSchedulerTask(void *task); @@ -110,6 +117,11 @@ int32_t PIOS_CALLBACKSCHEDULER_Start() // only call once PIOS_Assert(schedulerStarted == false); + #ifdef DIAG_TASKS + CallbackInfoInitialize(); + CallbackInfoGet(&callbackInfo); + #endif + // start tasks struct DelayedCallbackTaskStruct *cursor = NULL; int t = 0; @@ -117,7 +129,7 @@ int32_t PIOS_CALLBACKSCHEDULER_Start() xTaskCreate( CallbackSchedulerTask, cursor->name, - cursor->stackSize / 4, + 1 + (cursor->stackSize / 4), cursor, cursor->priorityTask, &cursor->callbackSchedulerTaskHandle @@ -250,6 +262,9 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( { 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; @@ -276,7 +291,7 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( 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; @@ -296,7 +311,7 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( xTaskCreate( CallbackSchedulerTask, task->name, - task->stackSize / 4, + 1 + (task->stackSize / 4), task, task->priorityTask, &task->callbackSchedulerTaskHandle @@ -329,6 +344,10 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( info->task = task; info->cb = cb; info->callbackID = callbackID; + info->runCount = 0; + // info->stackSize = stacksize - STACK_SIZE; + info->stackFree = stacksize - STACK_SIZE; + info->stackSize = info->stackFree; // add to scheduling queue LL_APPEND(task->callbackQueue[priority], info); @@ -338,6 +357,36 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( return info; } + +#ifdef DIAG_TASKS +/** + * Stack magic, check presence of unique value + */ +__attribute__((optimize("O0"))) static int32_t checkStack(DelayedCallbackInfo *current) +{ +#if 0 + register uint32_t t; + register int32_t result; + volatile unsigned char marker[current->stackSize]; + + result = -1; + for (t = 0; t < current->stackSize; t += STACK_GRANULARITY) { + if (marker[t] != t + '#') { + if (result == -1) { + result = t; + } + } + marker[t] = t + '#'; + } + return result; + +#endif + +// this is not working yet, until a fix is found just return old value + return current->stackFree; +} +#endif /* ifdef DIAG_TASKS */ + /** * Scheduler subtask * \param[in] task The scheduler task in question @@ -390,7 +439,28 @@ 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); + + #ifdef DIAG_TASKS + /* callback gets invoked here - check stack sizes */ + diff = checkStack(current); + #endif + current->cb(); // call the callback + + #ifdef DIAG_TASKS + diff = checkStack(current); + current->stackFree = diff; // < current->stackFree ? diff : current->stackFree; + current->runCount++; + if (current->callbackID >= 0 && current->callbackID < CALLBACKINFO_RUNNING_NUMELEM) { + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to uavobject + ((uint8_t *)&callbackInfo.Running)[current->callbackID] = true; + ((uint32_t *)&callbackInfo.RunningTime)[current->callbackID] = current->runCount; + ((int16_t *)&callbackInfo.StackRemaining)[current->callbackID] = (int16_t)current->stackFree; + CallbackInfoSet(&callbackInfo); + xSemaphoreGiveRecursive(mutex); + } + #endif + return 0; } xSemaphoreGiveRecursive(mutex); From 4ab4e48a98ca29185bf071021dd624bd1007f17a Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 3 Feb 2014 01:05:30 +0100 Subject: [PATCH 05/11] OP-942 added missing new xml file --- shared/uavobjectdefinition/callbackinfo.xml | 40 +++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 shared/uavobjectdefinition/callbackinfo.xml diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml new file mode 100644 index 000000000..a940c88ca --- /dev/null +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -0,0 +1,40 @@ + + + Task information + + + EventDispatcher + StateEstimation + AltitudeHold + PathPlanner0 + PathPlanner1 + + + + + EventDispatcher + StateEstimation + AltitudeHold + PathPlanner0 + PathPlanner1 + + + + + + + + + EventDispatcher + StateEstimation + AltitudeHold + PathPlanner0 + PathPlanner1 + + + + + + + + From cc04b780c51832b9e08fb030c0361b7b82d00720 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 4 Feb 2014 17:53:51 +0100 Subject: [PATCH 06/11] OP-942 one possible but not well working implementation of stack end check --- flight/pios/common/pios_callbackscheduler.c | 84 +++++++++++++++------ 1 file changed, 59 insertions(+), 25 deletions(-) diff --git a/flight/pios/common/pios_callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c index 4a04e228d..40869289b 100644 --- a/flight/pios/common/pios_callbackscheduler.c +++ b/flight/pios/common/pios_callbackscheduler.c @@ -35,7 +35,7 @@ // Private constants #define STACK_SIZE 384 -#define STACK_GRANULARITY 16 +#define STACK_GRANULARITY 4 #define MAX_SLEEP 1000 // Private types @@ -62,6 +62,7 @@ struct DelayedCallbackInfoStruct { bool volatile waiting; uint32_t volatile scheduletime; uint32_t stackSize; + int32_t stackWatermark; int32_t stackFree; uint32_t runCount; struct DelayedCallbackTaskStruct *task; @@ -348,6 +349,7 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( // info->stackSize = stacksize - STACK_SIZE; info->stackFree = stacksize - STACK_SIZE; info->stackSize = info->stackFree; + info->stackWatermark = 0; // add to scheduling queue LL_APPEND(task->callbackQueue[priority], info); @@ -362,28 +364,41 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( /** * Stack magic, check presence of unique value */ -__attribute__((optimize("O0"))) static int32_t checkStack(DelayedCallbackInfo *current) +static int8_t markStack(DelayedCallbackInfo *current) { -#if 0 - register uint32_t t; - register int32_t result; - volatile unsigned char marker[current->stackSize]; - - result = -1; - for (t = 0; t < current->stackSize; t += STACK_GRANULARITY) { - if (marker[t] != t + '#') { - if (result == -1) { - result = t; - } - } - marker[t] = t + '#'; - } - return result; - -#endif - -// this is not working yet, until a fix is found just return old value - return current->stackFree; + volatile unsigned char *marker; + marker = (unsigned char*)(((size_t)&marker) - (size_t)current->stackSize); + // end of stack watermark + *(marker) = '#'; + // shifted watermarks + marker += 16 + (size_t)current->stackWatermark; + *(marker-15) = '#'; + *(marker-14) = '#'; + *(marker-13) = '#'; + *(marker-12) = '#'; + *(marker-3) = '#'; + *(marker-2) = '#'; + *(marker-1) = '#'; + *(marker) = '#'; + return 0; +} +static int8_t checkStack(DelayedCallbackInfo *current) +{ + volatile unsigned char *marker; + marker = (unsigned char*)(((size_t)&marker) - (size_t)current->stackSize); + // end of stack watermark + if (*marker != '#') return -1; + // shifted watermarks + marker += 16 + (size_t)current->stackWatermark; + if (*(marker-15) != '#') return -2; + if (*(marker-14) != '#') return -2; + if (*(marker-13) != '#') return -2; + if (*(marker-12) != '#') return -2; + if (*(marker-3) != '#') return -2; + if (*(marker-2) != '#') return 3; + if (*(marker-1) != '#') return 2; + if (*(marker) != '#') return 1; + return 0; } #endif /* ifdef DIAG_TASKS */ @@ -442,20 +457,39 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa #ifdef DIAG_TASKS /* callback gets invoked here - check stack sizes */ - diff = checkStack(current); + diff = markStack(current); #endif current->cb(); // call the callback #ifdef DIAG_TASKS diff = checkStack(current); - current->stackFree = diff; // < current->stackFree ? diff : current->stackFree; + switch (diff) { + case -2: + current->stackWatermark = -1; + break; + case -1: + current->stackFree = -1; + current->stackWatermark = -1; + break; + case 0: + current->stackWatermark++; + if (current->stackWatermark>current->stackFree) { + current->stackWatermark = current->stackFree; + } + break; + default: + current->stackWatermark -= diff; + current->stackFree = current->stackWatermark; + break; + } + current->runCount++; if (current->callbackID >= 0 && current->callbackID < CALLBACKINFO_RUNNING_NUMELEM) { xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to uavobject ((uint8_t *)&callbackInfo.Running)[current->callbackID] = true; ((uint32_t *)&callbackInfo.RunningTime)[current->callbackID] = current->runCount; - ((int16_t *)&callbackInfo.StackRemaining)[current->callbackID] = (int16_t)current->stackFree; + ((int16_t *)&callbackInfo.StackRemaining)[current->callbackID] = (int16_t)current->stackWatermark; CallbackInfoSet(&callbackInfo); xSemaphoreGiveRecursive(mutex); } From ebe320b435830e07142245bbe5eb9b595529cc9b Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Tue, 4 Feb 2014 23:16:31 +0100 Subject: [PATCH 07/11] OP-942 changed callbackinfo to non acked --- shared/uavobjectdefinition/callbackinfo.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/shared/uavobjectdefinition/callbackinfo.xml b/shared/uavobjectdefinition/callbackinfo.xml index a940c88ca..6850662de 100644 --- a/shared/uavobjectdefinition/callbackinfo.xml +++ b/shared/uavobjectdefinition/callbackinfo.xml @@ -33,8 +33,8 @@ - - + + From 6566a29054ce84a62433c59a9bf9b1b01a10d270 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Thu, 6 Feb 2014 23:54:30 +0100 Subject: [PATCH 08/11] OP-942 reliable and fast free stack test algorithm - will always 'see' overflows --- flight/modules/System/systemmod.c | 4 + flight/pios/common/pios_callbackscheduler.c | 204 +++++++++++--------- flight/pios/inc/pios_callbackscheduler.h | 8 + 3 files changed, 128 insertions(+), 88 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 010080d66..fe5c6a358 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -205,6 +205,7 @@ static void systemTask(__attribute__((unused)) void *parameters) #ifdef DIAG_TASKS TaskInfoData taskInfoData; + CallbackInfoData callbackInfoData; #endif // Main system loop @@ -224,6 +225,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_CallbackInfo(&callbackInfoData); + CallbackInfoSet(&callbackInfoData); #endif // Flash the heartbeat LED diff --git a/flight/pios/common/pios_callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c index 40869289b..bfacc90f1 100644 --- a/flight/pios/common/pios_callbackscheduler.c +++ b/flight/pios/common/pios_callbackscheduler.c @@ -34,8 +34,9 @@ #include // Private constants -#define STACK_SIZE 384 -#define STACK_GRANULARITY 4 +#define STACK_SAFETYCOUNT 16 +#define STACK_SIZE (384 + STACK_SAFETYSIZE) +#define STACK_SAFETYSIZE 8 #define MAX_SLEEP 1000 // Private types @@ -62,8 +63,10 @@ struct DelayedCallbackInfoStruct { bool volatile waiting; uint32_t volatile scheduletime; uint32_t stackSize; - int32_t stackWatermark; int32_t stackFree; + int32_t stackNotFree; + uint16_t stackSafetyCount; + uint16_t currentSafetyCount; uint32_t runCount; struct DelayedCallbackTaskStruct *task; struct DelayedCallbackInfoStruct *next; @@ -74,7 +77,6 @@ struct DelayedCallbackInfoStruct { static struct DelayedCallbackTaskStruct *schedulerTasks; static xSemaphoreHandle mutex; static bool schedulerStarted; -static CallbackInfoData callbackInfo; // Private functions static void CallbackSchedulerTask(void *task); @@ -118,11 +120,6 @@ int32_t PIOS_CALLBACKSCHEDULER_Start() // only call once PIOS_Assert(schedulerStarted == false); - #ifdef DIAG_TASKS - CallbackInfoInitialize(); - CallbackInfoGet(&callbackInfo); - #endif - // start tasks struct DelayedCallbackTaskStruct *cursor = NULL; int t = 0; @@ -339,17 +336,18 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( xSemaphoreGiveRecursive(mutex); return NULL; // error - not enough memory } - info->next = NULL; - info->waiting = false; - info->scheduletime = 0; - info->task = task; + info->next = NULL; + info->waiting = false; + info->scheduletime = 0; + info->task = task; info->cb = cb; - info->callbackID = callbackID; - info->runCount = 0; - // info->stackSize = stacksize - STACK_SIZE; - info->stackFree = stacksize - STACK_SIZE; - info->stackSize = info->stackFree; - info->stackWatermark = 0; + info->callbackID = callbackID; + info->runCount = 0; + info->stackNotFree = stacksize - STACK_SIZE; + info->stackSize = info->stackNotFree; + info->stackFree = 0; + info->stackSafetyCount = STACK_SAFETYCOUNT; + info->currentSafetyCount = 0; // add to scheduling queue LL_APPEND(task->callbackQueue[priority], info); @@ -359,48 +357,109 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( return info; } - -#ifdef DIAG_TASKS /** - * Stack magic, check presence of unique value + * Retrieve callback specific runtime information + * \param[out] *callbackInfoData pointer to CallbackInfoData structure + * \return Success (-1), failure (0) */ -static int8_t markStack(DelayedCallbackInfo *current) +int32_t PIOS_CALLBACKSCHEDULER_CallbackInfo(void *callbackInfoData) { - volatile unsigned char *marker; - marker = (unsigned char*)(((size_t)&marker) - (size_t)current->stackSize); - // end of stack watermark - *(marker) = '#'; - // shifted watermarks - marker += 16 + (size_t)current->stackWatermark; - *(marker-15) = '#'; - *(marker-14) = '#'; - *(marker-13) = '#'; - *(marker-12) = '#'; - *(marker-3) = '#'; - *(marker-2) = '#'; - *(marker-1) = '#'; - *(marker) = '#'; - return 0; + if (!callbackInfoData) { + return 0; + } + + CallbackInfoData *info = (CallbackInfoData *)callbackInfoData; + + + 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) { + if (cbinfo->callbackID >= 0 && cbinfo->callbackID < CALLBACKINFO_RUNNING_NUMELEM) { + xSemaphoreTakeRecursive(mutex, portMAX_DELAY); + ((uint8_t *)&info->Running)[cbinfo->callbackID] = true; + ((uint32_t *)&info->RunningTime)[cbinfo->callbackID] = cbinfo->runCount; + ((int16_t *)&info->StackRemaining)[cbinfo->callbackID] = (int16_t)cbinfo->stackNotFree; + xSemaphoreGiveRecursive(mutex); + } + } + } + } + return -1; } -static int8_t checkStack(DelayedCallbackInfo *current) + +/** + * Stack magic, find how much stack is being used without affecting performance + */ +static void markStack(DelayedCallbackInfo *current) { - volatile unsigned char *marker; - marker = (unsigned char*)(((size_t)&marker) - (size_t)current->stackSize); - // end of stack watermark - if (*marker != '#') return -1; - // shifted watermarks - marker += 16 + (size_t)current->stackWatermark; - if (*(marker-15) != '#') return -2; - if (*(marker-14) != '#') return -2; - if (*(marker-13) != '#') return -2; - if (*(marker-12) != '#') return -2; - if (*(marker-3) != '#') return -2; - if (*(marker-2) != '#') return 3; - if (*(marker-1) != '#') return 2; - if (*(marker) != '#') return 1; - return 0; + register int8_t t; + register int32_t halfWayMark; + volatile unsigned char *marker; + + if (current->stackFree < 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->stackFree < 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->stackFree = -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; + } + } } -#endif /* ifdef DIAG_TASKS */ /** * Scheduler subtask @@ -455,45 +514,14 @@ static int32_t runNextCallback(struct DelayedCallbackTaskStruct *task, DelayedCa current->waiting = false; // the flag is reset just before execution. xSemaphoreGiveRecursive(mutex); - #ifdef DIAG_TASKS /* callback gets invoked here - check stack sizes */ - diff = markStack(current); - #endif + markStack(current); current->cb(); // call the callback - #ifdef DIAG_TASKS - diff = checkStack(current); - switch (diff) { - case -2: - current->stackWatermark = -1; - break; - case -1: - current->stackFree = -1; - current->stackWatermark = -1; - break; - case 0: - current->stackWatermark++; - if (current->stackWatermark>current->stackFree) { - current->stackWatermark = current->stackFree; - } - break; - default: - current->stackWatermark -= diff; - current->stackFree = current->stackWatermark; - break; - } + checkStack(current); current->runCount++; - if (current->callbackID >= 0 && current->callbackID < CALLBACKINFO_RUNNING_NUMELEM) { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // access to uavobject - ((uint8_t *)&callbackInfo.Running)[current->callbackID] = true; - ((uint32_t *)&callbackInfo.RunningTime)[current->callbackID] = current->runCount; - ((int16_t *)&callbackInfo.StackRemaining)[current->callbackID] = (int16_t)current->stackWatermark; - CallbackInfoSet(&callbackInfo); - xSemaphoreGiveRecursive(mutex); - } - #endif return 0; } diff --git a/flight/pios/inc/pios_callbackscheduler.h b/flight/pios/inc/pios_callbackscheduler.h index b72d4d0a4..dccc13e02 100644 --- a/flight/pios/inc/pios_callbackscheduler.h +++ b/flight/pios/inc/pios_callbackscheduler.h @@ -186,4 +186,12 @@ int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo); */ int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); +/** + * Retrieve callback specific runtime information + * \param[out] *callbackInfoData pointer to CallbackInfoData structure + * \return Success (-1), failure (0) + */ +int32_t PIOS_CALLBACKSCHEDULER_CallbackInfo(void *callbackInfoData); + + #endif // PIOS_CALLBACKSCHEDULER_H From 7ec21a8f36a07b30ed4e1a04ac985e9a2203c69e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 7 Feb 2014 22:34:27 +0100 Subject: [PATCH 09/11] OP-942 duplicated exact API of task_monitor to also monitor callbacks This allows cleaner and fine grained handling of STACKOVERFLOW alarms --- flight/modules/System/systemmod.c | 41 +++++++++++++++++---- flight/pios/common/pios_callbackscheduler.c | 32 +++++++--------- flight/pios/inc/pios_callbackscheduler.h | 30 +++++++++++++-- 3 files changed, 73 insertions(+), 30 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index fe5c6a358..0e6ef84db 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -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_HARMLESS = 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); @@ -226,7 +227,7 @@ static void systemTask(__attribute__((unused)) void *parameters) PIOS_TASK_MONITOR_ForEachTask(taskMonitorForEachCallback, &taskInfoData); TaskInfoSet(&taskInfoData); // Update the callback status object - PIOS_CALLBACKSCHEDULER_CallbackInfo(&callbackInfoData); + PIOS_CALLBACKSCHEDULER_ForEachCallback(callbackSchedulerForEachCallback, &callbackInfoData); CallbackInfoSet(&callbackInfoData); #endif @@ -445,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_HARMLESS; + } + // 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 @@ -607,10 +627,15 @@ static void updateSystemAlarms() } // Check for stack overflow - if (stackOverflow) { - AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); - } else { + switch (stackOverflow) { + case STACKOVERFLOW_NONE: AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); + break; + case STACKOVERFLOW_HARMLESS: + AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING); + break; + default: + AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_CRITICAL); } // Check for event errors @@ -655,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) { diff --git a/flight/pios/common/pios_callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c index bfacc90f1..1d3c9d830 100644 --- a/flight/pios/common/pios_callbackscheduler.c +++ b/flight/pios/common/pios_callbackscheduler.c @@ -27,11 +27,9 @@ #include #ifdef PIOS_INCLUDE_CALLBACKSCHEDULER -#include #include #include #include -#include // Private constants #define STACK_SAFETYCOUNT 16 @@ -358,18 +356,18 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( } /** - * Retrieve callback specific runtime information - * \param[out] *callbackInfoData pointer to CallbackInfoData structure - * \return Success (-1), failure (0) + * 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. */ -int32_t PIOS_CALLBACKSCHEDULER_CallbackInfo(void *callbackInfoData) +void PIOS_CALLBACKSCHEDULER_ForEachCallback(CallbackSchedulerCallbackInfoCallback callback, void *context) { - if (!callbackInfoData) { - return 0; + if (!callback) { + return; } - CallbackInfoData *info = (CallbackInfoData *)callbackInfoData; - + struct pios_callback_info info; struct DelayedCallbackTaskStruct *task = NULL; LL_FOREACH(schedulerTasks, task) { @@ -378,17 +376,15 @@ int32_t PIOS_CALLBACKSCHEDULER_CallbackInfo(void *callbackInfoData) for (prio = 0; prio < (CALLBACK_PRIORITY_LOW + 1); prio++) { struct DelayedCallbackInfoStruct *cbinfo; LL_FOREACH(task->callbackQueue[prio], cbinfo) { - if (cbinfo->callbackID >= 0 && cbinfo->callbackID < CALLBACKINFO_RUNNING_NUMELEM) { - xSemaphoreTakeRecursive(mutex, portMAX_DELAY); - ((uint8_t *)&info->Running)[cbinfo->callbackID] = true; - ((uint32_t *)&info->RunningTime)[cbinfo->callbackID] = cbinfo->runCount; - ((int16_t *)&info->StackRemaining)[cbinfo->callbackID] = (int16_t)cbinfo->stackNotFree; - xSemaphoreGiveRecursive(mutex); - } + 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); } } } - return -1; } /** diff --git a/flight/pios/inc/pios_callbackscheduler.h b/flight/pios/inc/pios_callbackscheduler.h index dccc13e02..7fc883cad 100644 --- a/flight/pios/inc/pios_callbackscheduler.h +++ b/flight/pios/inc/pios_callbackscheduler.h @@ -187,11 +187,33 @@ int32_t PIOS_CALLBACKSCHEDULER_Dispatch(DelayedCallbackInfo *cbinfo); int32_t PIOS_CALLBACKSCHEDULER_DispatchFromISR(DelayedCallbackInfo *cbinfo, long *pxHigherPriorityTaskWoken); /** - * Retrieve callback specific runtime information - * \param[out] *callbackInfoData pointer to CallbackInfoData structure - * \return Success (-1), failure (0) + * Information about a running callback that has been registered + * via a call to PIOS_CALLBACKSCHEDULER_Create(). */ -int32_t PIOS_CALLBACKSCHEDULER_CallbackInfo(void *callbackInfoData); +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 From c74790b8233f86ef28f1d703f0fc5df3caabe13e Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Fri, 7 Feb 2014 22:57:31 +0100 Subject: [PATCH 10/11] OP-942 correct reporting of callback stack overflows --- flight/pios/common/pios_callbackscheduler.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/flight/pios/common/pios_callbackscheduler.c b/flight/pios/common/pios_callbackscheduler.c index 1d3c9d830..9c13138c8 100644 --- a/flight/pios/common/pios_callbackscheduler.c +++ b/flight/pios/common/pios_callbackscheduler.c @@ -341,8 +341,8 @@ DelayedCallbackInfo *PIOS_CALLBACKSCHEDULER_Create( info->cb = cb; info->callbackID = callbackID; info->runCount = 0; - info->stackNotFree = stacksize - STACK_SIZE; - info->stackSize = info->stackNotFree; + info->stackSize = stacksize - STACK_SIZE; + info->stackNotFree = info->stackSize; info->stackFree = 0; info->stackSafetyCount = STACK_SAFETYCOUNT; info->currentSafetyCount = 0; @@ -396,7 +396,7 @@ static void markStack(DelayedCallbackInfo *current) register int32_t halfWayMark; volatile unsigned char *marker; - if (current->stackFree < 0) { + if (current->stackNotFree < 0) { return; } @@ -418,7 +418,7 @@ static void checkStack(DelayedCallbackInfo *current) register int32_t halfWayMark; volatile unsigned char *marker; - if (current->stackFree < 0) { + if (current->stackNotFree < 0) { return; } @@ -426,7 +426,7 @@ static void checkStack(DelayedCallbackInfo *current) marker = (unsigned char *)(((size_t)&marker) - (size_t)current->stackSize); for (t = -STACK_SAFETYSIZE; t < 0; t++) { if (*(marker + t) != '#') { - current->stackFree = -1; // stack overflow, disable all further checks + current->stackNotFree = -1; // stack overflow, disable all further checks return; } } From f066bcb2d9a535ea84bc923ec800c660209c1223 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 12:50:40 +0100 Subject: [PATCH 11/11] OP-942 changed enum term from HARMLESS to WARNING as requested --- flight/modules/System/systemmod.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 0e6ef84db..7849211cd 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -96,7 +96,7 @@ static uint32_t idleCounter; static uint32_t idleCounterClear; static xTaskHandle systemTaskHandle; static xQueueHandle objectPersistenceQueue; -static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_HARMLESS = 1, STACKOVERFLOW_CRITICAL = 3 } 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; @@ -456,7 +456,7 @@ static void callbackSchedulerForEachCallback(int16_t callback_id, const struct p } // delayed callback scheduler reports callback stack overflows as remaininng: -1 if (callback_info->stack_remaining < 0 && stackOverflow == STACKOVERFLOW_NONE) { - stackOverflow = STACKOVERFLOW_HARMLESS; + stackOverflow = STACKOVERFLOW_WARNING; } // By convention, there is a direct mapping between (not negative) callback scheduler callback_id's and members // of the CallbackInfoXXXXElem enums @@ -631,7 +631,7 @@ static void updateSystemAlarms() case STACKOVERFLOW_NONE: AlarmsClear(SYSTEMALARMS_ALARM_STACKOVERFLOW); break; - case STACKOVERFLOW_HARMLESS: + case STACKOVERFLOW_WARNING: AlarmsSet(SYSTEMALARMS_ALARM_STACKOVERFLOW, SYSTEMALARMS_ALARM_WARNING); break; default: