From f2cc9acc1fbb6264b1b8a93492bc998abfbedb92 Mon Sep 17 00:00:00 2001 From: AJ Christensen Date: Thu, 7 Jun 2018 17:17:26 +1200 Subject: [PATCH 1/4] gyro sensors/debug: standardize gyro debugging enum elements * will require coordination with BFC of course. * rationale: previously `DEBUG_GYRO_NOTCH` debugging was used to grab the scaled, unfiltered gyro readings, prior to the FFT running. This has been updated to `DEBUG_GYRO_SCALED`. similarly, `DEBUG_GYRO` debugging was used to record the filtered gyro. This is updated to `GYRO_FILTERED`. interestingly, `DEBUG_GYRO` was also used for movement threshold calibration. This has been updated to be `DEBUG_GYRO_CALIBRATION` and also now stores per-axis standard deviation. Application of filter position moved slightly for logical grouping, has no effect due to LTI. --- src/main/build/debug.h | 4 ++-- src/main/sensors/gyro.c | 27 +++++++++++---------------- 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index c83c0ef45..bc29f3462 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -52,10 +52,10 @@ typedef enum { DEBUG_NONE, DEBUG_CYCLETIME, DEBUG_BATTERY, - DEBUG_GYRO, + DEBUG_GYRO_FILTERED, DEBUG_ACCELEROMETER, DEBUG_PIDLOOP, - DEBUG_GYRO_NOTCH, + DEBUG_GYRO_SCALED, DEBUG_RC_INTERPOLATION, DEBUG_ANGLERATE, DEBUG_ESC_SENSOR, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 63c3277e8..e169619b5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -520,9 +520,9 @@ bool gyroInit(void) switch (debugMode) { case DEBUG_FFT: - case DEBUG_GYRO_NOTCH: - case DEBUG_GYRO: case DEBUG_GYRO_RAW: + case DEBUG_GYRO_SCALED: + case DEBUG_GYRO_FILTERED: gyroDebugMode = debugMode; break; default: @@ -864,8 +864,8 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]); - - DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); + // DEBUG_GYRO_CALIBRATION records per-axis standard deviation + DEBUG_SET(DEBUG_GYRO_CALIBRATION, axis, lrintf(stddev)); // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { @@ -1054,14 +1054,13 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; - gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); - #ifdef USE_GYRO_DATA_ANALYSE gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); #endif gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); + gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; if (!gyroSensor->overflowDetected) { @@ -1075,11 +1074,8 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens DEBUG_SET(DEBUG_GYRO_RAW, axis, gyroSensor->gyroDev.gyroADCRaw[axis]); // scale gyro output to degrees per second float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; - // DEBUG_GYRO_NOTCH records the unfiltered gyro output - DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf)); - - // apply lowpass2 filter - gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); + // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output + DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf)); #ifdef USE_GYRO_DATA_ANALYSE // apply dynamic notch filter @@ -1093,14 +1089,13 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens } } #endif - - // apply static notch filters + // apply static notch filters and software lowpass filters gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); - - // apply lowpass2 filter - DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); + gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); + // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied. + DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf)); gyroSensor->gyroDev.gyroADCf[axis] = gyroADCf; if (!gyroSensor->overflowDetected) { From f8ebdf06f1ea73d74065bc9d9f68455f8867bc80 Mon Sep 17 00:00:00 2001 From: AJ Christensen Date: Sat, 9 Jun 2018 14:12:32 +1200 Subject: [PATCH 2/4] debug: update debugModeNames * TBC --- src/main/build/debug.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index f4534753b..bc002e68c 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -34,10 +34,10 @@ const char * const debugModeNames[DEBUG_COUNT] = { "NONE", "CYCLETIME", "BATTERY", - "GYRO", + "GYRO_FILTERED", "ACCELEROMETER", "PIDLOOP", - "NOTCH", + "GYRO_SCALED", "RC_INTERPOLATION", "ANGLERATE", "ESC_SENSOR", From 03b480a8ef8000c43e71a60160dd63fb917fc4b3 Mon Sep 17 00:00:00 2001 From: AJ Christensen Date: Sat, 9 Jun 2018 14:18:47 +1200 Subject: [PATCH 3/4] gyro sensors/debug: record standard deviation for roll *only* * visibile in the DEBUG_GYRO_RAW[3] --- src/main/sensors/gyro.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index e169619b5..b033fb1b0 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -864,8 +864,11 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) { const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]); - // DEBUG_GYRO_CALIBRATION records per-axis standard deviation - DEBUG_SET(DEBUG_GYRO_CALIBRATION, axis, lrintf(stddev)); + // DEBUG_GYRO_CALIBRATION records the standard deviation of roll + // into the spare field - debug[3], in DEBUG_GYRO_RAW + if (axis == X) { + DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev)); + } // check deviation and startover in case the model was moved if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) { From b1069027c3108c1920749b5521a57aeb96e6c68b Mon Sep 17 00:00:00 2001 From: AJ Christensen Date: Sat, 9 Jun 2018 14:25:52 +1200 Subject: [PATCH 4/4] gyro sensors: use `axis_e` enum for consistency --- src/main/sensors/gyro.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b033fb1b0..568ba0046 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -1083,11 +1083,11 @@ static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSens #ifdef USE_GYRO_DATA_ANALYSE // apply dynamic notch filter if (isDynamicFilterActive()) { - if (axis == 0) { + if (axis == X) { DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data } gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); - if (axis == 0) { + if (axis == X) { DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch } }