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 /* original work by Rav
  22  * 2018_07 updated by ctzsnooze to post filter, wider Q, different peak detection
  23  * coding assistance and advice from DieHertz, Rav, eTracer
  24  * test pilots icr4sh, UAV Tech, Flint723
  25  */
  26 #include <stdint.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_GYRO_DATA_ANALYSE
  31 #include "arm_math.h"
  32
  33 #include "build/debug.h"
  34
  35 #include "common/filter.h"
  36 #include "common/maths.h"
  37 #include "common/time.h"
  38 #include "common/utils.h"
  39
  40 #include "drivers/accgyro/accgyro.h"
  41 #include "drivers/time.h"
  42
  43 #include "sensors/gyro.h"
  44 #include "sensors/gyroanalyse.h"
  45
  46 // The FFT splits the frequency domain into an number of bins
  47 // 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
  48 // Eg [0,31), [31,62), [62, 93) etc
  49
  50 #define FFT_WINDOW_SIZE           32  // max for f3 targets
  51 #define FFT_BIN_COUNT             (FFT_WINDOW_SIZE / 2)
  52 #define FFT_BIN_OFFSET            1 // compare 1 + this offset FFT bins for peak, ie if 1 start 2.5 * 41.655 or about 104Hz
  53 #define FFT_SAMPLING_RATE_HZ      1333  // analyse up to 666Hz, 16 bins each 41.655z wide
  54 #define FFT_BPF_HZ                (FFT_SAMPLING_RATE_HZ / 4)  // centre frequency of bandpass that constrains input to FFT
  55 #define FFT_BIQUAD_Q              0.05f  // bandpass quality factor, 0.1 for steep sided bandpass
  56 #define FFT_RESOLUTION            ((float)FFT_SAMPLING_RATE_HZ / FFT_WINDOW_SIZE) // hz per bin
  57 #define FFT_MIN_BIN_RISE          2 // following bin must be at least 2 times previous to indicate start of peak
  58 #define DYN_NOTCH_SMOOTH_FREQ_HZ  60  // lowpass frequency for smoothing notch centre point
  59 #define DYN_NOTCH_MIN_CENTRE_HZ   125  // notch centre point will not go below this, must be greater than cutoff, mid of bottom bin
  60 #define DYN_NOTCH_MAX_CENTRE_HZ   (FFT_SAMPLING_RATE_HZ / 2) // maximum notch centre frequency limited by nyquist
  61 #define DYN_NOTCH_WIDTH_PERCENT   25  //  maybe adjustable via CLI?
  62 #define DYN_NOTCH_CUTOFF          ((float)(100 - DYN_NOTCH_WIDTH_PERCENT) / 100)
  63 #define DYN_NOTCH_MIN_CUTOFF_HZ   105  // lowest allowed notch cutoff frequency
  64 #define DYN_NOTCH_CALC_TICKS      (XYZ_AXIS_COUNT * 4) // we need 4 steps for each axis
  65
  66 static FAST_RAM_ZERO_INIT uint16_t fftSamplingCount;
  67
  68 // gyro data used for frequency analysis
  69 static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE];
  70
  71 static FAST_RAM_ZERO_INIT arm_rfft_fast_instance_f32 fftInstance;
  72 static FAST_RAM_ZERO_INIT float fftData[FFT_WINDOW_SIZE];
  73 static FAST_RAM_ZERO_INIT float rfftData[FFT_WINDOW_SIZE];
  74 static FAST_RAM_ZERO_INIT gyroFftData_t fftResult[XYZ_AXIS_COUNT];
  75
  76 // use a circular buffer for the last FFT_WINDOW_SIZE samples
  77 static FAST_RAM_ZERO_INIT uint16_t fftIdx;
  78
  79 // bandpass filter gyro data
  80 static FAST_RAM_ZERO_INIT biquadFilter_t fftGyroFilter[XYZ_AXIS_COUNT];
  81
  82 // filter for smoothing frequency estimation
  83 static FAST_RAM_ZERO_INIT biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT];
  84
  85 // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window
  86 static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE];
  87
  88 void initHanning(void)
  89 {
  90     for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
  91         hanningWindow[i] = (0.5 - 0.5 * cos_approx(2 * M_PIf * i / (FFT_WINDOW_SIZE - 1)));
  92     }
  93 }
  94
  95 void initGyroData(void)
  96 {
  97     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  98         for (int i = 0; i < FFT_WINDOW_SIZE; i++) {
  99             gyroData[axis][i] = 0;
 100         }
 101     }
 102 }
 103
 104 void gyroDataAnalyseInit(uint32_t targetLooptimeUs)
 105 {
 106     // initialise even if FEATURE_DYNAMIC_FILTER not set, since it may be set later
 107     const uint16_t samplingFrequency = 1000000 / targetLooptimeUs;
 108     fftSamplingCount = samplingFrequency / FFT_SAMPLING_RATE_HZ;
 109     arm_rfft_fast_init_f32(&fftInstance, FFT_WINDOW_SIZE);
 110
 111     initGyroData();
 112     initHanning();
 113
 114     // recalculation of filters takes 4 calls per axis => each filter gets updated every DYN_NOTCH_CALC_TICKS calls
 115     // at 4khz gyro loop rate this means 4khz / 4 / 3 = 333Hz => update every 3ms
 116     // for gyro rate > 16kHz, we have update frequency of 1kHz => 1ms
 117     const float looptime = MAX(1000000u / FFT_SAMPLING_RATE_HZ, targetLooptimeUs * DYN_NOTCH_CALC_TICKS);
 118     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 119         fftResult[axis].centerFreq = 200; // any init value
 120         biquadFilterInitLPF(&fftFreqFilter[axis], DYN_NOTCH_SMOOTH_FREQ_HZ, looptime);
 121         biquadFilterInit(&fftGyroFilter[axis], FFT_BPF_HZ, 1000000 / FFT_SAMPLING_RATE_HZ, FFT_BIQUAD_Q, FILTER_BPF);
 122     }
 123 }
 124
 125 // used in OSD
 126 const gyroFftData_t *gyroFftData(int axis)
 127 {
 128     return &fftResult[axis];
 129 }
 130
 131 static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT];
 132 void gyroDataAnalysePush(const int axis, const float sample)
 133 {
 134     fftAcc[axis] += sample;
 135 }
 136
 137 /*
 138  * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
 139  */
 140 void gyroDataAnalyse(biquadFilter_t *notchFilterDyn)
 141 {
 142     // accumulator for oversampled data => no aliasing and less noise
 143     static FAST_RAM_ZERO_INIT uint32_t fftAccCount;
 144     static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks;
 145
 146      // samples should have been pushed by `gyroDataAnalysePush`
 147      // if gyro sampling is > 1kHz, accumulate multiple samples
 148     fftAccCount++;
 149
 150     // this runs at 1kHz
 151     if (fftAccCount == fftSamplingCount) {
 152         fftAccCount = 0;
 153
 154         //calculate mean value of accumulated samples
 155         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 156             float sample = fftAcc[axis] / fftSamplingCount;
 157             sample = biquadFilterApply(&fftGyroFilter[axis], sample);
 158             gyroData[axis][fftIdx] = sample;
 159             if (axis == 0)
 160                 DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
 161             fftAcc[axis] = 0;
 162         }
 163
 164         fftIdx = (fftIdx + 1) % FFT_WINDOW_SIZE;
 165
 166         // We need DYN_NOTCH_CALC_TICKS tick to update all axis with newly sampled value
 167         gyroDataAnalyseUpdateTicks = DYN_NOTCH_CALC_TICKS;
 168     }
 169
 170     // calculate FFT and update filters
 171     if (gyroDataAnalyseUpdateTicks > 0) {
 172         gyroDataAnalyseUpdate(notchFilterDyn);
 173         --gyroDataAnalyseUpdateTicks;
 174     }
 175 }
 176
 177 void stage_rfft_f32(arm_rfft_fast_instance_f32 * S, float32_t * p, float32_t * pOut);
 178 void arm_cfft_radix8by2_f32( arm_cfft_instance_f32 * S, float32_t * p1);
 179 void arm_cfft_radix8by4_f32( arm_cfft_instance_f32 * S, float32_t * p1);
 180 void arm_radix8_butterfly_f32(float32_t * pSrc, uint16_t fftLen, const float32_t * pCoef, uint16_t twidCoefModifier);
 181 void arm_bitreversal_32(uint32_t * pSrc, const uint16_t bitRevLen, const uint16_t * pBitRevTable);
 182
 183 typedef enum {
 184     STEP_ARM_CFFT_F32,
 185     STEP_BITREVERSAL,
 186     STEP_STAGE_RFFT_F32,
 187     STEP_ARM_CMPLX_MAG_F32,
 188     STEP_CALC_FREQUENCIES,
 189     STEP_UPDATE_FILTERS,
 190     STEP_HANNING,
 191     STEP_COUNT
 192 } UpdateStep_e;
 193
 194 /*
 195  * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds
 196  */
 197 void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
 198 {
 199     static int axis;
 200     static int step;
 201     arm_cfft_instance_f32 * Sint = &(fftInstance.Sint);
 202
 203     uint32_t startTime = 0;
 204     if (debugMode == (DEBUG_FFT_TIME))
 205         startTime = micros();
 206
 207     DEBUG_SET(DEBUG_FFT_TIME, 0, step);
 208     switch (step) {
 209         case STEP_ARM_CFFT_F32:
 210         {
 211             switch (FFT_BIN_COUNT) {
 212             case 16:
 213                 // 16us
 214                 arm_cfft_radix8by2_f32(Sint, fftData);
 215                 break;
 216             case 32:
 217                 // 35us
 218                 arm_cfft_radix8by4_f32(Sint, fftData);
 219                 break;
 220             case 64:
 221                 // 70us
 222                 arm_radix8_butterfly_f32(fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
 223                 break;
 224             }
 225             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 226             break;
 227         }
 228         case STEP_BITREVERSAL:
 229         {
 230             // 6us
 231             arm_bitreversal_32((uint32_t*) fftData, Sint->bitRevLength, Sint->pBitRevTable);
 232             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 233             step++;
 234             FALLTHROUGH;
 235         }
 236         case STEP_STAGE_RFFT_F32:
 237         {
 238             // 14us
 239             // this does not work in place => fftData AND rfftData needed
 240             stage_rfft_f32(&fftInstance, fftData, rfftData);
 241             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 242             break;
 243         }
 244         case STEP_ARM_CMPLX_MAG_F32:
 245         {
 246             // 8us
 247             arm_cmplx_mag_f32(rfftData, fftData, FFT_BIN_COUNT);
 248             DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
 249             step++;
 250             FALLTHROUGH;
 251         }
 252         case STEP_CALC_FREQUENCIES:
 253         {
 254             // 13us
 255             // calculate FFT centreFreq
 256             float fftSum = 0;
 257             float fftWeightedSum = 0;
 258             fftResult[axis].maxVal = 0;
 259             bool fftIncreasing = false;
 260             float cubedData;
 261             // iterate over fft data and calculate weighted indexes
 262             for (int i = 1 + FFT_BIN_OFFSET; i < FFT_BIN_COUNT; i++) {
 263                 if (!fftIncreasing && (fftData[i] < fftData[i-1] * FFT_MIN_BIN_RISE)) {
 264                     // do nothing unless has increased at some point
 265                 } else {
 266                     cubedData = fftData[i] * fftData[i] * fftData[i];  //more weight on higher peaks
 267                     if (!fftIncreasing){
 268                         cubedData += fftData[i-1] * fftData[i-1] * fftData[i-1];  //add previous bin before first rise
 269                      }
 270                     fftSum += cubedData;
 271                     fftWeightedSum += cubedData * (i + 1); // calculate weighted index starting at 1, not 0
 272                     fftIncreasing = true;
 273                 }
 274             }
 275             // get weighted center of relevant frequency range (this way we have a better resolution than 31.25Hz)
 276             float centerFreq;
 277             float fftMeanIndex;
 278             if (fftSum > 0) {
 279                 // idx was shifted by 1 to start at 1, not 0
 280                 fftMeanIndex = (fftWeightedSum / fftSum) - 1;
 281                 // the index points at the center frequency of each bin so index 0 is actually 16.125Hz
 282                 centerFreq = constrain(fftMeanIndex * FFT_RESOLUTION, DYN_NOTCH_MIN_CENTRE_HZ, DYN_NOTCH_MAX_CENTRE_HZ);
 283             } else {
 284                 centerFreq = DYN_NOTCH_MAX_CENTRE_HZ;  // if no peak, go to highest point to minimise delay
 285             }
 286             centerFreq = biquadFilterApply(&fftFreqFilter[axis], centerFreq);
 287             fftResult[axis].centerFreq = centerFreq;
 288             if (axis == 0) {
 289                DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
 290             }
 291             DEBUG_SET(DEBUG_FFT_FREQ, axis, fftResult[axis].centerFreq);
 292             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 293             break;
 294         }
 295         case STEP_UPDATE_FILTERS:
 296         {
 297             // 7us
 298             // calculate cutoffFreq and notch Q, update notch filter
 299             float cutoffFreq = fmax(fftResult[axis].centerFreq * DYN_NOTCH_CUTOFF, DYN_NOTCH_MIN_CUTOFF_HZ);
 300             float notchQ = filterGetNotchQ(fftResult[axis].centerFreq, cutoffFreq);
 301             biquadFilterUpdate(&notchFilterDyn[axis], fftResult[axis].centerFreq, gyro.targetLooptime, notchQ, FILTER_NOTCH);
 302             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 303
 304             axis = (axis + 1) % 3;
 305             step++;
 306             FALLTHROUGH;
 307         }
 308         case STEP_HANNING:
 309         {
 310             // 5us
 311             // apply hanning window to gyro samples and store result in fftData
 312             // hanning starts and ends with 0, could be skipped for minor speed improvement
 313             uint8_t ringBufIdx = FFT_WINDOW_SIZE - fftIdx;
 314             arm_mult_f32(&gyroData[axis][fftIdx], &hanningWindow[0], &fftData[0], ringBufIdx);
 315             if (fftIdx > 0)
 316                 arm_mult_f32(&gyroData[axis][0], &hanningWindow[ringBufIdx], &fftData[ringBufIdx], fftIdx);
 317
 318             DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
 319         }
 320     }
 321
 322     step = (step + 1) % STEP_COUNT;
 323 }
 324
 325 #endif // USE_GYRO_DATA_ANALYSE