Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #pragma once
- #include "fft.h"
- #include <math.h>
- int fft_rearange(uint16_t n, float* sig)// rearange the input signal to n 2-point signal
- {
- float odd[MAX_POINT_FFT / 2] ; // 128 point fft maximum
- float even[MAX_POINT_FFT / 2] ;
- if (n == 1 || n > MAX_POINT_FFT || ((n & (n - 1)) != 0)) { return -1; } // reach to last stage or too many fft point or is n power of 2
- uint16_t i = 0;
- for (i = 0; i < n; i += 2) { // divide into 2 part, odd and even part
- even[i / 2] = sig[i];
- }
- for (i = 1; i < n; i += 2) {
- odd[i / 2] = sig[i];
- }
- fft_rearange(n / 2, odd); // recursively keep dividing
- fft_rearange(n / 2, even);
- for (i = 0; i < n; i++) {
- if (i >= n / 2) sig[i] = odd[i - n / 2];
- else sig[i] = even[i];
- }
- return 1;
- }
- void get_tw(uint16_t n, uint16_t m, float* r_tw, float* i_tw) { // get twiddle factor n: number of point on circle, m: point to get twiddle factor, r_tw: real part of twiddle factor, i_tw: imagine part of twiddle factor
- const float _r_tw[MAX_POINT_FFT / 2] = { 1.000000f, 0.998795f, 0.995185f, 0.989177f, 0.980785f, 0.970031f, 0.956940f, 0.941544f, 0.923880f, 0.903989f, 0.881921f, 0.857729f, 0.831470f, 0.803208f,
- 0.773010f, 0.740951f, 0.707107f, 0.671559f, 0.634393f, 0.595699f, 0.555570f, 0.514103f, 0.471397f, 0.427555f, 0.382683f, 0.336890f, 0.290285f, 0.242980f, 0.195090f, 0.146730f, 0.098017f,
- 0.049068f, 0.000000f, -0.049068f, -0.098017f, -0.146730f, -0.195090f, -0.242980f, -0.290285f, -0.336890f, -0.382683f, -0.427555f, -0.471397f, -0.514103f, -0.555570f, -0.595699f, -0.634393f,
- -0.671559f, -0.707107f, -0.740951f, -0.773010f, -0.803208f, -0.831470f, -0.857729f, -0.881921f, -0.903989f, -0.923880f, -0.941544f, -0.956940f, -0.970031f, -0.980785f, -0.989177f, -0.995185f, -0.998795f };
- const float _i_tw[MAX_POINT_FFT / 2] = { 0.000000f, -0.049068f, -0.098017f, -0.146730f, -0.195090f, -0.242980f, -0.290285f, -0.336890f, -0.382683f, -0.427555f, -0.471397f, -0.514103f, -0.555570f, -0.595699f,
- -0.634393f, -0.671559f, -0.707107f, -0.740951f, -0.773010f, -0.803208f, -0.831470f, -0.857729f, -0.881921f, -0.903989f, -0.923880f, -0.941544f, -0.956940f, -0.970031f, -0.980785f, -0.989177f, -0.995185f,
- -0.998795f, -1.000000f, -0.998795f, -0.995185f, -0.989177f, -0.980785f, -0.970031f, -0.956940f, -0.941544f, -0.923880f, -0.903989f, -0.881921f, -0.857729f, -0.831470f, -0.803208f, -0.773010f, -0.740951f,
- -0.707107f, -0.671559f, -0.634393f, -0.595699f, -0.555570f, -0.514103f, -0.471397f, -0.427555f, -0.382683f, -0.336890f, -0.290285f, -0.242980f, -0.195090f, -0.146730f, -0.098017f, -0.049068f };
- //GENERATED FROM MATLAB
- // this is half lower of circle, the other half is oppsite value
- if (n > MAX_POINT_FFT) return;
- int ratio = MAX_POINT_FFT / n;
- if (m >= n) {
- int temp = m / n;
- m = m - (n * temp);
- }
- m *= ratio; // scale up the m to fit the table
- if ((m) >= (MAX_POINT_FFT / 2)) { // DETERMINE IF M BELONG TO UPPER OR LOWER CIRCLE
- *r_tw = -_r_tw[(m)-(MAX_POINT_FFT / 2)];
- *i_tw = -_i_tw[(m)-(MAX_POINT_FFT / 2)];
- }
- else
- {
- *r_tw = _r_tw[(m)];
- *i_tw = _i_tw[(m)];
- }
- }
- void complex_mul(float* r_0, float* i_0, float r_1, float i_1) { // function to multiplex 2 complex number,result is store in first number
- float r = (*r_0 * r_1) - (*i_0 * i_1);
- float i = (*i_0 * r_1) + (*r_0 * i_1);
- *i_0 = i;
- *r_0 = r;
- }
- void fft_compute(uint16_t n, uint16_t stage, float* r_sig, float* i_sig) {
- int i = 0;
- float r_temp = 0, i_temp = 0;
- // multiply with twiddle factor
- for (i = stage / 2; i < n; i += stage)
- {
- float r_tw, i_tw; // complex twiddle factor variable
- int j = 0;
- for (j = 0; j < stage / 2; j++) {
- get_tw(stage, j, &r_tw, &i_tw);
- //printf("get tw factor for %d-dft, m=%d: %.f+%.fi \n", n, j, r_tw, i_tw);
- complex_mul(&r_sig[i + j], &i_sig[i + j], r_tw, i_tw);
- //printf("calculate data[%d] with tw factor %.f+%.fi \n", i + j, r_tw, i_tw);
- }
- }
- //calculate stage-dft
- for (i = 0; i < n; i += stage) {
- int j = 0;
- for (j = 0; j < stage / 2; j++) {
- r_temp = r_sig[i + j];
- i_temp = i_sig[i + j];
- //printf("calculate sum sig[%d] and sig[%d] for out[%d] \n", i + j, i + j + stage / 2,i+j);
- r_sig[i + j] = r_sig[i + j] + r_sig[i + j + stage / 2]; // sum
- i_sig[i + j] = i_sig[i + j] + i_sig[i + j + stage / 2];
- //printf("calculate sub sig[%d] and sig[%d] for out[%d] \n", i + j, i + j + stage / 2, i + j+stage/2);
- r_sig[stage / 2 + i + j] = r_temp - r_sig[i + j + stage / 2];
- i_sig[stage / 2 + i + j] = i_temp - i_sig[i + j + stage / 2]; // subtraction
- }
- }
- }
- int fft(uint16_t n, float* r_data, float* i_data) {
- if (fft_rearange(n, r_data) != 1) return -1;
- if (fft_rearange(n, i_data) != 1) return -1;
- int i = 0;
- for (i = 2; i <= n; i *= 2) {
- fft_compute(n, i, r_data, i_data);
- }
- return 1;
- }
- void fft_mag(uint16_t n, float* result, float* r_data, float* i_data) {
- int i = 0;
- double x0 = 0, x1 = 0;
- for (i = 0; i < n; i++) {
- x0 = (double)r_data[i] * (double)r_data[i];
- x1 = (double)i_data[i] * (double)i_data[i];
- result[i] = sqrt(x0 + x1);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement