src/main/sensors/gyroanalyse.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdint.h>
  22
  23 #include "platform.h"
  24
  25 #ifdef USE_GYRO_DATA_ANALYSE
  26 #include "arm_math.h"
  27
  28 #include "build/debug.h"
  29
  30 #include "common/filter.h"
  31 #include "common/maths.h"
  32 #include "common/time.h"
  33 #include "common/utils.h"
  34
  35 #include "drivers/accgyro/accgyro.h"
  36 #include "drivers/time.h"
  37
  38 #include "sensors/gyro.h"
  39 #include "sensors/gyroanalyse.h"
  40
  41 // The FFT splits the frequency domain into an number of bins
  42 // A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz
  43 // Eg [0,31), [31,62), [62, 93) etc
  44
  45 #define FFT_WINDOW_SIZE       32  // max for f3 targets
  46 #define FFT_BIN_COUNT         (FFT_WINDOW_SIZE / 2)
  47 #define FFT_MIN_FREQ          100  // not interested in filtering frequencies below 100Hz
  48 #define FFT_SAMPLING_RATE     1000  // allows analysis up to 500Hz which is more than motors create
  49 #define FFT_MAX_FREQUENCY     (FFT_SAMPLING_RATE / 2) // nyquist rate
  50 #define FFT_BPF_HZ            200  // use a bandpass on gyro data to ignore extreme low and extreme high frequencies
  51 #define FFT_RESOLUTION        ((float)FFT_SAMPLING_RATE / FFT_WINDOW_SIZE) // hz per bin
  52 #define DYN_NOTCH_WIDTH       100  // just an orientation and start value
  53 #define DYN_NOTCH_CHANGERATE  60  // lower cut does not improve the performance much, higher cut makes it worse...
  54 #define DYN_NOTCH_MIN_CUTOFF  120  // don't cut too deep into low frequencies
  55 #define DYN_NOTCH_MAX_CUTOFF  200  // don't go above this cutoff (better filtering with "constant" delay at higher center frequencies)
  56 #define DYN_NOTCH_CALC_TICKS  (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis
  57
  58 #define BIQUAD_Q 1.0f / sqrtf(2.0f)         // quality factor - butterworth
  59
  60 static FAST_RAM uint16_t fftSamplingScale;
  61
  62 // gyro data used for frequency analysis
  63 static float FAST_RAM gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
  64
  65 static FAST_RAM arm_rfft_fast_instance_f32 fftInstance;
  66 static FAST_RAM float fftData[FFT_WINDOW_SIZE];
  67 static FAST_RAM float rfftData[FFT_WINDOW_SIZE];
  68 static FAST_RAM gyroFftData_t fftResult[XYZ_AXIS_COUNT];
  69
  70 // use a circular buffer for the last FFT_WINDOW_SIZE samples
  71 static FAST_RAM uint16_t fftIdx;
  72
  73 // bandpass filter gyro data
  74 static FAST_RAM biquadFilter_t fftGyroFilter[XYZ_AXIS_COUNT];
  75
  76 // filter for smoothing frequency estimation
  77 static FAST_RAM biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT];
  78
  79 // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
  80 static FAST_RAM float hanningWindow[FFT_WINDOW_SIZE];
  81
  82 void initHanning(void)
  83 {
  84     for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
  85         hanningWindow[i] = (0.5 - 0.5 * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
  86     }
  87 }
  88
  89 void initGyroData(void)
  90 {
  91     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  92         for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
  93             gyroData[axis][i] = 0;
  94         }
  95     }
  96 }
  97
  98 void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
  99 {
 100     // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
 101     const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
 102     fftSamplingScale = samplingFrequency / FFT_SAMPLING_RATE;
 103     arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
 104
 105     initGyroData();
 106     initHanning();
 107
 108     // recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
 109     // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
 110     // for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
 111     const float looptime = MAX(1000000u / FFT_SAMPLING_RATE, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
 112     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 113         fftResult[axis].centerFreq = 200; // any init value
 114         biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_CHANGERATE, looptime);
 115         biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE, BIQUAD_Q, FILTER_BPF);
 116     }
 117 }
 118
 119 // used in OSD
 120 const gyroFftData_t *gyroFftData(int axis)
 121 {
 122     return &fftResult[axis];
 123 }
 124
 125 /*
 126  * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
 127  */
 128 void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
 129 {
 130     // accumulator for oversampled data => no aliasing and less noise
 131     static FAST_RAM float fftAcc[XYZ_AXIS_COUNT];
 132     static FAST_RAM uint32_t fftAccCount;
 133
 134     static FAST_RAM uint32_t gyroDataAnalyseUpdateTicks;
 135
 136     // if gyro sampling is > 1kHz, accumulate multiple samples
 137     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 138         fftAcc[axis] += gyroDev->gyroADC[axis];
 139     }
 140     fftAccCount++;
 141
 142     // this runs at 1kHz
 143     if (fftAccCount == fftSamplingScale) {
 144         fftAccCount = 0;
 145
 146         //calculate mean value of accumulated samples
 147         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 148             float sample = fftAcc[axis] / fftSamplingScale;
 149             sample = biquadFilterApply(&fftGyroFilter[axis], sample);
 150             gyroData[axis][fftIdx] = sample;
 151             if (axis == 0)
 152                 DEBUG_SET(DEBUG_FFT, 2, lrintf(sample * gyroDev->scale));
 153             fftAcc[axis] = 0;
 154         }
 155
 156         fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE;
 157
 158         // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
 159         gyroDataAnalyseUpdateTicks = DYN_NOTCH_CALC_TICKS;
 160     }
 161
 162     // calculate FFT and update filters
 163     if (gyroDataAnalyseUpdateTicks > 0) {
 164         gyroDataAnalyseUpdate(notchFilterDyn);
 165         --gyroDataAnalyseUpdateTicks;
 166     }
 167 }
 168
 169 void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut);
 170 void arm_cfft_radix8by2_f32( arm_cfft_instance_f32 * S, float32_t * p1);
 171 void arm_cfft_radix8by4_f32( arm_cfft_instance_f32 * S, float32_t * p1);
 172 void arm_radix8_butterfly_f32(float32_t * pSrc, uint16_t fftLen, const float32_t * pCoef, uint16_t twidCoefModifier);
 173 void arm_bitreversal_32(uint32_t * pSrc, const uint16_t bitRevLen, const uint16_t * pBitRevTable);
 174
 175 typedef enum {
 176     STEP_ARM_CFFT_F32,
 177     STEP_BITREVERSAL,
 178     STEP_STAGE_RFFT_F32,
 179     STEP_ARM_CMPLX_MAG_F32,
 180     STEP_CALC_FREQUENCIES,
 181     STEP_UPDATE_FILTERS,
 182     STEP_HANNING,
 183     STEP_COUNT
 184 } UpdateStep_e;
 185
 186 /*
 187  * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
 188  */
 189 void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
 190 {
 191     static int axis;
 192     static int step;
 193     arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
 194
 195     uint32_t startTime = 0;
 196     if (debugMode == (DEBUG_FFT_TIME))
 197         startTime = micros();
 198
 199     DEBUG_SET(DEBUG_FFT_TIME, 0, step);
 200     switch (step) {
 201         case STEP_ARM_CFFT_F32:
 202         {
 203             switch (FFT_BIN_COUNT) {
 204             case 16:
 205                 // 16us
 206                 arm_cfft_radix8by2_f32(Sint, fftData);
 207                 break;
 208             case 32:
 209                 // 35us
 210                 arm_cfft_radix8by4_f32(Sint, fftData);
 211                 break;
 212             case 64:
 213                 // 70us
 214                 arm_radix8_butterfly_f32(fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
 215                 break;
 216             }
 217             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 218             break;
 219         }
 220         case STEP_BITREVERSAL:
 221         {
 222             // 6us
 223             arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable);
 224             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 225             step++;
 226             FALLTHROUGH;
 227         }
 228         case STEP_STAGE_RFFT_F32:
 229         {
 230             // 14us
 231             // this does not work in place => fftData AND rfftData needed
 232             stage_rfft_f32(&fftInstance, fftData, rfftData);
 233             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 234             break;
 235         }
 236         case STEP_ARM_CMPLX_MAG_F32:
 237         {
 238             // 8us
 239             arm_cmplx_mag_f32(rfftData, fftData, FFT_BIN_COUNT);
 240             DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
 241             step++;
 242             FALLTHROUGH;
 243         }
 244         case STEP_CALC_FREQUENCIES:
 245         {
 246             // 13us
 247             float fftSum = 0;
 248             float fftWeightedSum = 0;
 249
 250             fftResult[axis].maxVal = 0;
 251             // iterate over fft data and calculate weighted indexes
 252             float squaredData;
 253             for (int i = 0; i < FFT_BIN_COUNT; i++) {
 254                 squaredData = fftData[i] * fftData[i];  //more weight on higher peaks
 255                 fftResult[axis].maxVal = MAX(fftResult[axis].maxVal, squaredData);
 256                 fftSum += squaredData;
 257                 fftWeightedSum += squaredData * (i + 1); // calculate weighted index starting at 1, not 0
 258             }
 259
 260             // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
 261             if (fftSum > 0) {
 262                 // idx was shifted by 1 to start at 1, not 0
 263                 float fftMeanIndex = (fftWeightedSum / fftSum) - 1;
 264                 // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
 265                 // fftMeanIndex += 0.5;
 266
 267                 // don't go below the minimal cutoff frequency + 10 and don't jump around too much
 268                 float centerFreq;
 269                 centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY);
 270                 centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
 271                 centerFreq = constrain(centerFreq, DYN_NOTCH_MIN_CUTOFF + 10, FFT_MAX_FREQUENCY);
 272                 fftResult[axis].centerFreq = centerFreq;
 273                 if (axis == 0) {
 274                     DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
 275                 }
 276             }
 277
 278             DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq);
 279             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 280             break;
 281         }
 282         case STEP_UPDATE_FILTERS:
 283         {
 284             // 7us
 285             // calculate new filter coefficients
 286             float cutoffFreq = constrain(fftResult[axis].centerFreq - DYN_NOTCH_WIDTH, DYN_NOTCH_MIN_CUTOFF, DYN_NOTCH_MAX_CUTOFF);
 287             float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq);
 288             biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 289             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 290
 291             axis = (axis + 1) % 3;
 292             step++;
 293             FALLTHROUGH;
 294         }
 295         case STEP_HANNING:
 296         {
 297             // 5us
 298             // apply hanning window to gyro samples and store result in fftData
 299             // hanning starts and ends with 0, could be skipped for minor speed improvement
 300             uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx;
 301             arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx);
 302             if (fftIdx > 0)
 303                 arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx);
 304
 305             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 306         }
 307     }
 308
 309     step = (step + 1) % STEP_COUNT;
 310 }
 311
 312 #endif // USE_GYRO_DATA_ANALYSE