From afc1591956b50b2c577c0107af498e6a36d5f8a5 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 8 Nov 2018 14:54:16 -0500 Subject: [PATCH] Dual gyro debugging optimize Bypass the numerous dual-gyro DEBUG_SET statements unless a related debug_mode is set. The dual-gyro debugging is rarely used so in most cases all of the statements can be bypassed. Saves 1-2% CPU load on SPRACINGF7DUAL running in `gyro_to_use = BOTH` and at 32K. --- src/main/sensors/gyro.c | 69 ++++++++++++++++++++++++----------------- 1 file changed, 41 insertions(+), 28 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 84e658b57b..ab1d8b093a 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -105,6 +105,8 @@ static FAST_RAM_ZERO_INIT int16_t gyroSensorTemperature; static bool gyroHasOverflowProtection = true; +static FAST_RAM_ZERO_INIT bool useDualGyroDebugging; + typedef struct gyroCalibration_s { float sum[XYZ_AXIS_COUNT]; stdev_t var[XYZ_AXIS_COUNT]; @@ -488,6 +490,9 @@ bool gyroInit(void) } #endif + gyroDebugMode = DEBUG_NONE; + useDualGyroDebugging = false; + switch (debugMode) { case DEBUG_FFT: case DEBUG_FFT_FREQ: @@ -496,9 +501,11 @@ bool gyroInit(void) case DEBUG_GYRO_FILTERED: gyroDebugMode = debugMode; break; - default: - // debugMode is not gyro-related - gyroDebugMode = DEBUG_NONE; + case DEBUG_DUAL_GYRO: + case DEBUG_DUAL_GYRO_COMBINE: + case DEBUG_DUAL_GYRO_DIFF: + case DEBUG_DUAL_GYRO_RAW: + useDualGyroDebugging = true; break; } firstArmingCalibrationWasStarted = false; @@ -1044,12 +1051,14 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) yawSpinDetected = gyroSensor1.yawSpinDetected; #endif } - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y])); - DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y])); + if (useDualGyroDebugging) { + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 0, lrintf(gyro.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[Y])); + } break; #ifdef USE_MULTI_GYRO case GYRO_CONFIG_USE_GYRO_2: @@ -1065,12 +1074,14 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) yawSpinDetected = gyroSensor2.yawSpinDetected; #endif } - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y])); - DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y])); + if (useDualGyroDebugging) { + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 3, lrintf(gyro.gyroADCf[Y])); + } break; case GYRO_CONFIG_USE_GYRO_BOTH: gyroUpdateSensor(&gyroSensor1, currentTimeUs); @@ -1087,19 +1098,21 @@ FAST_CODE void gyroUpdate(timeUs_t currentTimeUs) #endif } - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y])); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); - DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); - DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y])); - DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y])); - DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X])); - DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y])); - DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z])); + if (useDualGyroDebugging) { + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyroSensor1.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyroSensor1.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyroSensor2.gyroDev.gyroADCRaw[X]); + DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyroSensor2.gyroDev.gyroADCRaw[Y]); + DEBUG_SET(DEBUG_DUAL_GYRO, 2, lrintf(gyroSensor2.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO, 3, lrintf(gyroSensor2.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 1, lrintf(gyro.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_COMBINE, 2, lrintf(gyro.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf(gyroSensor1.gyroDev.gyroADCf[X] - gyroSensor2.gyroDev.gyroADCf[X])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf(gyroSensor1.gyroDev.gyroADCf[Y] - gyroSensor2.gyroDev.gyroADCf[Y])); + DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf(gyroSensor1.gyroDev.gyroADCf[Z] - gyroSensor2.gyroDev.gyroADCf[Z])); + } break; #endif }