From e14e5febe2c67fa0755ef92febf81b6235b86be1 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Tue, 3 May 2016 01:00:28 +0200 Subject: [PATCH] LP-150 - prevent stab error conditions --- flight/modules/Stabilization/inc/stabilization.h | 5 ++++- flight/modules/Stabilization/outerloop.c | 13 +++++++++---- .../boards/coptercontrol/firmware/inc/pios_config.h | 1 + 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/flight/modules/Stabilization/inc/stabilization.h b/flight/modules/Stabilization/inc/stabilization.h index b1ff2fc13..fbf8dbf9b 100644 --- a/flight/modules/Stabilization/inc/stabilization.h +++ b/flight/modules/Stabilization/inc/stabilization.h @@ -80,9 +80,12 @@ extern StabilizationData stabSettings; // must be same as eventdispatcher to avoid needing additional mutexes #define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED // outer loop only executes every 4th uavobject update to save CPU #define OUTERLOOP_SKIPCOUNT 4 - +#else +#define OUTERLOOP_SKIPCOUNT ATTITUDE_SENSORS_DOWNSAMPLE +#endif // STABILIZATION_ATTITUDE_DOWNSAMPLED #endif // STABILIZATION_H /** diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 31c81e86c..9fc3c6e4a 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -365,14 +365,19 @@ static void stabilizationOuterloopTask() static void AttitudeStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { +#ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED // to reduce CPU utilization, outer loop is not executed on every state update static uint8_t cpusaver = 0; if ((cpusaver++ % 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); - } +#endif + // this does not need mutex protection as both eventdispatcher and stabi run in same callback task! + AttitudeStateGet(&attitude); + PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle); + +#ifndef STABILIZATION_ATTITUDE_DOWNSAMPLED +} +#endif } /** diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 122b0fcc8..6c617d06f 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -94,6 +94,7 @@ /* #define PIOS_INCLUDE_HCSR04 */ #define PIOS_SENSOR_RATE 500.0f +#define STABILIZATION_ATTITUDE_DOWNSAMPLED #define ATTITUDE_SENSORS_DOWNSAMPLE 4 /* PIOS receiver drivers */