src/main/common/sdft.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 <math.h>
  22 #include <stdbool.h>
  23
  24 #include "platform.h"
  25
  26 #include "common/maths.h"
  27 #include "common/sdft.h"
  28
  29 #define SDFT_R 0.9999f  // damping factor for guaranteed SDFT stability (r < 1.0f)
  30
  31 static FAST_DATA_ZERO_INIT float     rPowerN;  // SDFT_R to the power of SDFT_SAMPLE_SIZE
  32 static FAST_DATA_ZERO_INIT bool      isInitialized;
  33 static FAST_DATA_ZERO_INIT complex_t twiddle[SDFT_BIN_COUNT];
  34
  35 static void applySqrt(const sdft_t *sdft, float *data);
  36
  37
  38 void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches)
  39 {
  40     if (!isInitialized) {
  41         rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE);
  42         const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
  43         float phi = 0.0f;
  44         for (int i = 0; i < SDFT_BIN_COUNT; i++) {
  45             phi = c * i;
  46             twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi));
  47         }
  48         isInitialized = true;
  49     }
  50
  51     sdft->idx = 0;
  52
  53     // Add 1 bin on either side outside of range (if possible) to get proper windowing up to range limits
  54     sdft->startBin = constrain(startBin - 1, 0, SDFT_BIN_COUNT - 1);
  55     sdft->endBin = constrain(endBin + 1, sdft->startBin, SDFT_BIN_COUNT - 1);
  56
  57     sdft->numBatches = MAX(numBatches, 1);
  58     sdft->batchSize = (sdft->endBin - sdft->startBin) / sdft->numBatches + 1;  // batchSize = ceil(numBins / numBatches)
  59
  60     for (int i = 0; i < SDFT_SAMPLE_SIZE; i++) {
  61         sdft->samples[i] = 0.0f;
  62     }
  63
  64     for (int i = 0; i < SDFT_BIN_COUNT; i++) {
  65         sdft->data[i] = 0.0f;
  66     }
  67 }
  68
  69
  70 // Add new sample to frequency spectrum
  71 FAST_CODE void sdftPush(sdft_t *sdft, const float sample)
  72 {
  73     const float delta = sample - rPowerN * sdft->samples[sdft->idx];
  74
  75     sdft->samples[sdft->idx] = sample;
  76     sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
  77
  78     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
  79         sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
  80     }
  81 }
  82
  83
  84 // Add new sample to frequency spectrum in parts
  85 FAST_CODE void sdftPushBatch(sdft_t* sdft, const float sample, const int batchIdx)
  86 {
  87     const int batchStart = sdft->batchSize * batchIdx;
  88     int batchEnd = batchStart;
  89
  90     const float delta = sample - rPowerN * sdft->samples[sdft->idx];
  91
  92     if (batchIdx == sdft->numBatches - 1) {
  93         sdft->samples[sdft->idx] = sample;
  94         sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
  95         batchEnd += sdft->endBin - batchStart + 1;
  96     } else {
  97         batchEnd += sdft->batchSize;
  98     }
  99
 100     for (int i = batchStart; i < batchEnd; i++) {
 101         sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
 102     }
 103 }
 104
 105
 106 // Get squared magnitude of frequency spectrum
 107 FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
 108 {
 109     float re;
 110     float im;
 111
 112     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
 113         re = crealf(sdft->data[i]);
 114         im = cimagf(sdft->data[i]);
 115         output[i] = re * re + im * im;
 116     }
 117 }
 118
 119
 120 // Get magnitude of frequency spectrum (slower)
 121 FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output)
 122 {
 123     sdftMagSq(sdft, output);
 124     applySqrt(sdft, output);
 125 }
 126
 127
 128 // Get squared magnitude of frequency spectrum with Hann window applied
 129 // Hann window in frequency domain: X[k] = -0.25 * X[k-1] +0.5 * X[k] -0.25 * X[k+1]
 130 FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
 131 {
 132     complex_t val;
 133     float re;
 134     float im;
 135
 136     for (int i = (sdft->startBin + 1); i < sdft->endBin; i++) {
 137         val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication
 138         re = crealf(val);
 139         im = cimagf(val);
 140         output[i] = re * re + im * im;
 141     }
 142 }
 143
 144
 145 // Get magnitude of frequency spectrum with Hann window applied (slower)
 146 FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
 147 {
 148     sdftWinSq(sdft, output);
 149     applySqrt(sdft, output);
 150 }
 151
 152
 153 // Apply square root to the whole sdft range
 154 static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
 155 {
 156     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
 157         data[i] = sqrtf(data[i]);
 158     }
 159 }