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 static void updateEdges(sdft_t *sdft, const float value, const int batchIdx);
  37
  38
  39 void sdftInit(sdft_t *sdft, const int startBin, const int endBin, const int numBatches)
  40 {
  41     if (!isInitialized) {
  42         rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE);
  43         const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
  44         float phi = 0.0f;
  45         for (int i = 0; i < SDFT_BIN_COUNT; i++) {
  46             phi = c * i;
  47             twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi));
  48         }
  49         isInitialized = true;
  50     }
  51
  52     sdft->idx = 0;
  53
  54     sdft->startBin = constrain(startBin, 0, SDFT_BIN_COUNT - 1);
  55     sdft->endBin = constrain(endBin, 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     updateEdges(sdft, delta, 0);
  83 }
  84
  85
  86 // Add new sample to frequency spectrum in parts
  87 FAST_CODE void sdftPushBatch(sdft_t *sdft, const float sample, const int batchIdx)
  88 {
  89     const int batchStart = sdft->batchSize * batchIdx + sdft->startBin;
  90     int batchEnd = batchStart;
  91
  92     const float delta = sample - rPowerN * sdft->samples[sdft->idx];
  93
  94     if (batchIdx == sdft->numBatches - 1) {
  95         sdft->samples[sdft->idx] = sample;
  96         sdft->idx = (sdft->idx + 1) % SDFT_SAMPLE_SIZE;
  97         batchEnd += sdft->endBin - batchStart + 1;
  98     } else {
  99         batchEnd += sdft->batchSize;
 100     }
 101
 102     for (int i = batchStart; i < batchEnd; i++) {
 103         sdft->data[i] = twiddle[i] * (sdft->data[i] + delta);
 104     }
 105
 106     updateEdges(sdft, delta, batchIdx);
 107 }
 108
 109
 110 // Get squared magnitude of frequency spectrum
 111 FAST_CODE void sdftMagSq(const sdft_t *sdft, float *output)
 112 {
 113     float re;
 114     float im;
 115
 116     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
 117         re = crealf(sdft->data[i]);
 118         im = cimagf(sdft->data[i]);
 119         output[i] = re * re + im * im;
 120     }
 121 }
 122
 123
 124 // Get magnitude of frequency spectrum (slower)
 125 FAST_CODE void sdftMagnitude(const sdft_t *sdft, float *output)
 126 {
 127     sdftMagSq(sdft, output);
 128     applySqrt(sdft, output);
 129 }
 130
 131
 132 // Get squared magnitude of frequency spectrum with Hann window applied
 133 // Hann window in frequency domain: X[k] = -0.25 * X[k-1] +0.5 * X[k] -0.25 * X[k+1]
 134 FAST_CODE void sdftWinSq(const sdft_t *sdft, float *output)
 135 {
 136     complex_t val;
 137     float re;
 138     float im;
 139
 140     // Apply window at the lower edge of active range
 141     if (sdft->startBin == 0) {
 142         val = sdft->data[sdft->startBin] - sdft->data[sdft->startBin + 1];
 143     } else {
 144         val = sdft->data[sdft->startBin] - 0.5f * (sdft->data[sdft->startBin - 1] + sdft->data[sdft->startBin + 1]);
 145     }
 146     re = crealf(val);
 147     im = cimagf(val);
 148     output[sdft->startBin] = re * re + im * im;
 149
 150     for (int i = (sdft->startBin + 1); i < sdft->endBin; i++) {
 151         val = sdft->data[i] - 0.5f * (sdft->data[i - 1] + sdft->data[i + 1]); // multiply by 2 to save one multiplication
 152         re = crealf(val);
 153         im = cimagf(val);
 154         output[i] = re * re + im * im;
 155     }
 156
 157     // Apply window at the upper edge of active range
 158     if (sdft->endBin == SDFT_BIN_COUNT - 1) {
 159         val = sdft->data[sdft->endBin] - sdft->data[sdft->endBin - 1];
 160     } else {
 161         val = sdft->data[sdft->endBin] - 0.5f * (sdft->data[sdft->endBin - 1] + sdft->data[sdft->endBin + 1]);
 162     }
 163     re = crealf(val);
 164     im = cimagf(val);
 165     output[sdft->endBin] = re * re + im * im;
 166 }
 167
 168
 169 // Get magnitude of frequency spectrum with Hann window applied (slower)
 170 FAST_CODE void sdftWindow(const sdft_t *sdft, float *output)
 171 {
 172     sdftWinSq(sdft, output);
 173     applySqrt(sdft, output);
 174 }
 175
 176
 177 // Apply square root to the whole sdft range
 178 static FAST_CODE void applySqrt(const sdft_t *sdft, float *data)
 179 {
 180     for (int i = sdft->startBin; i <= sdft->endBin; i++) {
 181         data[i] = sqrtf(data[i]);
 182     }
 183 }
 184
 185
 186 // Needed for proper windowing at the edges of startBin and endBin
 187 static FAST_CODE void updateEdges(sdft_t *sdft, const float value, const int batchIdx)
 188 {
 189     // First bin outside of lower range
 190     if (sdft->startBin > 0 && batchIdx == 0) {
 191         const unsigned idx = sdft->startBin - 1;
 192         sdft->data[idx] = twiddle[idx] * (sdft->data[idx] + value);
 193     }
 194
 195     // First bin outside of upper range
 196     if (sdft->endBin < SDFT_BIN_COUNT - 1 && batchIdx == sdft->numBatches - 1) {
 197         const unsigned idx = sdft->endBin + 1;
 198         sdft->data[idx] = twiddle[idx] * (sdft->data[idx] + value);
 199     }
 200 }