diff --git a/flight/modules/Stabilization/cruisecontrol.c b/flight/modules/Stabilization/cruisecontrol.c index 9b86608c8..c7444faa9 100644 --- a/flight/modules/Stabilization/cruisecontrol.c +++ b/flight/modules/Stabilization/cruisecontrol.c @@ -38,6 +38,12 @@ static float cruisecontrol_factor = 1.0f; void cruisecontrol_compute_factor(AttitudeStateData *attitude) { + // safe CPU, only execute every 8th attitude update + static uint8_t cpusafer = 0; + + if (cpusafer++ % (8 / OUTERLOOP_SKIPCOUNT)) { + return; + } float angleCosine; // get attitude state and calculate angle diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index 3ac8e9092..a857fba9b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -68,6 +68,17 @@ extern StabilizationData stabSettings; #define AXES 4 #define FAILSAFE_TIMEOUT_MS 30 +#ifndef PIOS_STABILIZATION_STACK_SIZE +#define STACK_SIZE_BYTES 800 +#else +#define STACK_SIZE_BYTES PIOS_STABILIZATION_STACK_SIZE +#endif + +// must be same as eventdispatcher to avoid needing additional mutexes +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL + +// outer loop only executes every 4th uavobject update to safe CPU +#define OUTERLOOP_SKIPCOUNT 4 #endif // STABILIZATION_H diff --git a/flight/modules/Stabilization/innerloop.c b/flight/modules/Stabilization/innerloop.c index 95373b48d..b04556cb6 100644 --- a/flight/modules/Stabilization/innerloop.c +++ b/flight/modules/Stabilization/innerloop.c @@ -50,9 +50,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 256 -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define CALLBACK_PRIORITY CALLBACK_PRIORITY_CRITICAL #define UPDATE_EXPECTED (1.0f / 666.0f) @@ -116,12 +114,12 @@ static void stabilizationInnerloopTask() if (stabSettings.monitor.rateupdates > -64) { stabSettings.monitor.rateupdates--; } - if (stabSettings.monitor.rateupdates < -3) { - // warning if rate loop skipped more than 2 beats + if (stabSettings.monitor.rateupdates < -(2 * OUTERLOOP_SKIPCOUNT)) { + // warning if rate loop skipped more than 2 execution warn = true; } - if (stabSettings.monitor.rateupdates < -7) { - // error if rate loop skipped more than 7 beats + if (stabSettings.monitor.rateupdates < -(4 * OUTERLOOP_SKIPCOUNT)) { + // error if rate loop skipped more than 4 executions error = true; } // check if gyro keeps updating diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index c10077a10..853684634 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -49,9 +49,7 @@ #include // Private constants -#define STACK_SIZE_BYTES 256 -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL #define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR #define UPDATE_EXPECTED (1.0f / 666.0f) @@ -260,10 +258,14 @@ static void stabilizationOuterloopTask() static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { - // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! - AttitudeStateGet(&attitude); + // to reduce CPU utilisation, outer loop is not executed every state update + static uint8_t cpusafer = 0; - PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + if ((cpusafer++ % OUTERLOOP_SKIPCOUNT) == 0) { + // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! + AttitudeStateGet(&attitude); + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + } } /** diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 13697942d..d3a1aa2f9 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -160,7 +160,7 @@ #define PIOS_ACTUATOR_STACK_SIZE 820 #define PIOS_MANUAL_STACK_SIZE 635 #define PIOS_RECEIVER_STACK_SIZE 620 -#define PIOS_STABILIZATION_STACK_SIZE 780 +#define PIOS_STABILIZATION_STACK_SIZE 400 #ifdef DIAG_TASKS #define PIOS_SYSTEM_STACK_SIZE 740 diff --git a/shared/uavobjectdefinition/stabilizationstatus.xml b/shared/uavobjectdefinition/stabilizationstatus.xml index f58200853..0a3788b76 100644 --- a/shared/uavobjectdefinition/stabilizationstatus.xml +++ b/shared/uavobjectdefinition/stabilizationstatus.xml @@ -22,7 +22,7 @@ - +