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     sdft->startBin = startBin;
  53     sdft->endBin = endBin;
  54     sdft->numBatches = numBatches;
  55     sdft->batchSize = (sdft->endBin - sdft->startBin) / sdft->numBatches + 1;  // batchSize = ceil(numBins / numBatches)
  56
  57     for (int i = 0; i < SDFT_SAMPLE_SIZE; i++) {
  58         sdft->samples[i] = 0.0f;
  59     }
  60
  61     for (int i = 0; i < SDFT_BIN_COUNT; i++) {
  62         sdft->data[i] = 0.0f;
  63     }
  64 }
  65
  66
  67 // Add new sample to frequency spectrum
  68 FAST_CODE void sdftPush(sdft_t *sdft, const float sample)
  69 {
  70     const float delta = sample - rPowerN * sdft->samples[sdft->idx];
  71
  72     sdft->samples[sdft->idx] = sample;
  73     sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
  74
  75     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
  76         sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
  77     }
  78 }
  79
  80
  81 // Add new sample to frequency spectrum in parts
  82 FAST_CODE void sdftPushBatch(sdft_t* sdft, const float sample, const int batchIdx)
  83 {
  84     const int batchStart = sdft->batchSize * batchIdx;
  85     int batchEnd = batchStart;
  86
  87     const float delta = sample - rPowerN * sdft->samples[sdft->idx];
  88
  89     if (batchIdx == sdft->numBatches - 1) {
  90         sdft->samples[sdft->idx] = sample;
  91         sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
  92         batchEnd += sdft->endBin - batchStart + 1;
  93     } else {
  94         batchEnd += sdft->batchSize;
  95     }
  96
  97     for (int i = batchStart; i < batchEnd; i++) {
  98         sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
  99     }
 100 }
 101
 102
 103 // Get squared magnitude of frequency spectrum
 104 FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
 105 {
 106     float re;
 107     float im;
 108
 109     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
 110         re = crealf(sdft->data[i]);
 111         im = cimagf(sdft->data[i]);
 112         output[i] = re * re + im * im;
 113     }
 114 }
 115
 116
 117 // Get magnitude of frequency spectrum (slower)
 118 FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output)
 119 {
 120     sdftMagSq(sdft, output);
 121     applySqrt(sdft, output);
 122 }
 123
 124
 125 // Get squared magnitude of frequency spectrum with Hann window applied
 126 // Hann window in frequency domain: X[k] = -0.25 * X[k-1] +0.5 * X[k] -0.25 * X[k+1]
 127 FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
 128 {
 129     complex_t val;
 130     float re;
 131     float im;
 132
 133     for (int i = (sdft->startBin + 1); i < sdft->endBin; i++) {
 134         val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication
 135         re = crealf(val);
 136         im = cimagf(val);
 137         output[i] = re * re + im * im;
 138     }
 139 }
 140
 141
 142 // Get magnitude of frequency spectrum with Hann window applied (slower)
 143 FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
 144 {
 145     sdftWinSq(sdft, output);
 146     applySqrt(sdft, output);
 147 }
 148
 149
 150 // Apply square root to the whole sdft range
 151 static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
 152 {
 153     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
 154         data[i] = sqrtf(data[i]);
 155     }
 156 }