Advertisement
Guest User

Untitled

a guest
Nov 20th, 2019
122
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 4.98 KB | None | 0 0
  1. #pragma once
  2. #include "fft.h"
  3. #include <math.h>
  4. int fft_rearange(uint16_t n, float* sig)// rearange the input signal to n 2-point signal
  5. {
  6. float odd[MAX_POINT_FFT / 2] ; // 128 point fft maximum
  7. float even[MAX_POINT_FFT / 2] ;
  8. 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
  9. uint16_t i = 0;
  10. for (i = 0; i < n; i += 2) { // divide into 2 part, odd and even part
  11. even[i / 2] = sig[i];
  12. }
  13. for (i = 1; i < n; i += 2) {
  14. odd[i / 2] = sig[i];
  15. }
  16. fft_rearange(n / 2, odd); // recursively keep dividing
  17. fft_rearange(n / 2, even);
  18. for (i = 0; i < n; i++) {
  19. if (i >= n / 2) sig[i] = odd[i - n / 2];
  20. else sig[i] = even[i];
  21. }
  22. return 1;
  23. }
  24. 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
  25. 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,
  26. 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,
  27. 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,
  28. -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 };
  29.  
  30. 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,
  31. -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,
  32. -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,
  33. -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 };
  34. //GENERATED FROM MATLAB
  35. // this is half lower of circle, the other half is oppsite value
  36. if (n > MAX_POINT_FFT) return;
  37. int ratio = MAX_POINT_FFT / n;
  38. if (m >= n) {
  39. int temp = m / n;
  40. m = m - (n * temp);
  41. }
  42. m *= ratio; // scale up the m to fit the table
  43. if ((m) >= (MAX_POINT_FFT / 2)) { // DETERMINE IF M BELONG TO UPPER OR LOWER CIRCLE
  44. *r_tw = -_r_tw[(m)-(MAX_POINT_FFT / 2)];
  45. *i_tw = -_i_tw[(m)-(MAX_POINT_FFT / 2)];
  46. }
  47. else
  48. {
  49. *r_tw = _r_tw[(m)];
  50. *i_tw = _i_tw[(m)];
  51. }
  52. }
  53. 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
  54. float r = (*r_0 * r_1) - (*i_0 * i_1);
  55. float i = (*i_0 * r_1) + (*r_0 * i_1);
  56. *i_0 = i;
  57. *r_0 = r;
  58. }
  59.  
  60. void fft_compute(uint16_t n, uint16_t stage, float* r_sig, float* i_sig) {
  61. int i = 0;
  62. float r_temp = 0, i_temp = 0;
  63. // multiply with twiddle factor
  64. for (i = stage / 2; i < n; i += stage)
  65. {
  66. float r_tw, i_tw; // complex twiddle factor variable
  67. int j = 0;
  68. for (j = 0; j < stage / 2; j++) {
  69. get_tw(stage, j, &r_tw, &i_tw);
  70. //printf("get tw factor for %d-dft, m=%d: %.f+%.fi \n", n, j, r_tw, i_tw);
  71. complex_mul(&r_sig[i + j], &i_sig[i + j], r_tw, i_tw);
  72. //printf("calculate data[%d] with tw factor %.f+%.fi \n", i + j, r_tw, i_tw);
  73. }
  74. }
  75. //calculate stage-dft
  76. for (i = 0; i < n; i += stage) {
  77. int j = 0;
  78. for (j = 0; j < stage / 2; j++) {
  79. r_temp = r_sig[i + j];
  80. i_temp = i_sig[i + j];
  81. //printf("calculate sum sig[%d] and sig[%d] for out[%d] \n", i + j, i + j + stage / 2,i+j);
  82. r_sig[i + j] = r_sig[i + j] + r_sig[i + j + stage / 2]; // sum
  83. i_sig[i + j] = i_sig[i + j] + i_sig[i + j + stage / 2];
  84. //printf("calculate sub sig[%d] and sig[%d] for out[%d] \n", i + j, i + j + stage / 2, i + j+stage/2);
  85. r_sig[stage / 2 + i + j] = r_temp - r_sig[i + j + stage / 2];
  86. i_sig[stage / 2 + i + j] = i_temp - i_sig[i + j + stage / 2]; // subtraction
  87.  
  88. }
  89. }
  90. }
  91. int fft(uint16_t n, float* r_data, float* i_data) {
  92. if (fft_rearange(n, r_data) != 1) return -1;
  93. if (fft_rearange(n, i_data) != 1) return -1;
  94. int i = 0;
  95. for (i = 2; i <= n; i *= 2) {
  96. fft_compute(n, i, r_data, i_data);
  97. }
  98. return 1;
  99. }
  100. void fft_mag(uint16_t n, float* result, float* r_data, float* i_data) {
  101. int i = 0;
  102. double x0 = 0, x1 = 0;
  103. for (i = 0; i < n; i++) {
  104. x0 = (double)r_data[i] * (double)r_data[i];
  105. x1 = (double)i_data[i] * (double)i_data[i];
  106. result[i] = sqrt(x0 + x1);
  107. }
  108. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement