├── README.md ├── library.properties ├── library.json ├── LICENSE ├── src ├── fft.h └── fft.cpp └── examples └── PDM_SPM1423 └── PDM_SPM1423.ino /README.md: -------------------------------------------------------------------------------- 1 | # FFT 2 | 3 | ## License 4 | 5 | [MIT](LICENSE) 6 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=FFT 2 | version=0.0.1 3 | author=Robin Scheibler 4 | maintainer=Robin Scheibler 5 | sentence=Library for FFT 6 | paragraph=See more on http://www.robinscheibler.org 7 | category=Device Control 8 | url=https://github.com/Tinyu-Zhao/FFT 9 | architectures=esp32 10 | includes=fft.h 11 | -------------------------------------------------------------------------------- /library.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "FFT", 3 | "description": "FFT", 4 | "keywords": "FFT", 5 | "authors": { 6 | "name": "Robin Scheibler", 7 | "url": "http://www.robinscheibler.org" 8 | }, 9 | "repository": { 10 | "type": "git", 11 | "url": "https://github.com/Tinyu-Zhao/FFT.git" 12 | }, 13 | "version": "0.0.1", 14 | "framework": "arduino", 15 | "platforms": "espressif32" 16 | } 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | ESP32 FFT 3 | ========= 4 | 5 | This provides a vanilla radix-2 FFT implementation and a test example. 6 | 7 | Author 8 | ------ 9 | 10 | This code was written by [Robin Scheibler](http://www.robinscheibler.org) during rainy days in October 2017. 11 | 12 | License 13 | ------- 14 | 15 | Copyright (c) 2017 Robin Scheibler 16 | 17 | Permission is hereby granted, free of charge, to any person obtaining a copy 18 | of this software and associated documentation files (the "Software"), to deal 19 | in the Software without restriction, including without limitation the rights 20 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 21 | copies of the Software, and to permit persons to whom the Software is 22 | furnished to do so, subject to the following conditions: 23 | 24 | The above copyright notice and this permission notice shall be included in all 25 | copies or substantial portions of the Software. 26 | 27 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 28 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 29 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 30 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 31 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 32 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 33 | SOFTWARE. -------------------------------------------------------------------------------- /src/fft.h: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | ESP32 FFT 4 | ========= 5 | 6 | This provides a vanilla radix-2 FFT implementation and a test example. 7 | 8 | Author 9 | ------ 10 | 11 | This code was written by [Robin Scheibler](http://www.robinscheibler.org) during rainy days in October 2017. 12 | 13 | License 14 | ------- 15 | 16 | Copyright (c) 2017 Robin Scheibler 17 | 18 | Permission is hereby granted, free of charge, to any person obtaining a copy 19 | of this software and associated documentation files (the "Software"), to deal 20 | in the Software without restriction, including without limitation the rights 21 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 22 | copies of the Software, and to permit persons to whom the Software is 23 | furnished to do so, subject to the following conditions: 24 | 25 | The above copyright notice and this permission notice shall be included in all 26 | copies or substantial portions of the Software. 27 | 28 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 29 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 30 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 31 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 32 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 33 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 34 | SOFTWARE. 35 | 36 | */ 37 | #ifndef __FFT_H__ 38 | #define __FFT_H__ 39 | 40 | typedef enum 41 | { 42 | FFT_REAL, 43 | FFT_COMPLEX 44 | } fft_type_t; 45 | 46 | typedef enum 47 | { 48 | FFT_FORWARD, 49 | FFT_BACKWARD 50 | } fft_direction_t; 51 | 52 | #define FFT_OWN_INPUT_MEM 1 53 | #define FFT_OWN_OUTPUT_MEM 2 54 | 55 | typedef struct 56 | { 57 | int size; // FFT size 58 | float *input; // pointer to input buffer 59 | float *output; // pointer to output buffer 60 | float *twiddle_factors; // pointer to buffer holding twiddle factors 61 | fft_type_t type; // real or complex 62 | fft_direction_t direction; // forward or backward 63 | unsigned int flags; // FFT flags 64 | } fft_config_t; 65 | 66 | fft_config_t *fft_init(int size, fft_type_t type, fft_direction_t direction, float *input, float *output); 67 | void fft_destroy(fft_config_t *config); 68 | void fft_execute(fft_config_t *config); 69 | void fft(float *input, float *output, float *twiddle_factors, int n); 70 | void ifft(float *input, float *output, float *twiddle_factors, int n); 71 | void rfft(float *x, float *y, float *twiddle_factors, int n); 72 | void irfft(float *x, float *y, float *twiddle_factors, int n); 73 | void fft_primitive(float *x, float *y, int n, int stride, float *twiddle_factors, int tw_stride); 74 | void split_radix_fft(float *x, float *y, int n, int stride, float *twiddle_factors, int tw_stride); 75 | void ifft_primitive(float *input, float *output, int n, int stride, float *twiddle_factors, int tw_stride); 76 | void fft8(float *input, int stride_in, float *output, int stride_out); 77 | void fft4(float *input, int stride_in, float *output, int stride_out); 78 | 79 | #endif // __FFT_H__ 80 | -------------------------------------------------------------------------------- /examples/PDM_SPM1423/PDM_SPM1423.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Description: Read the microphone data of the PDM Unit and display the audio frequency spectrum. 3 | Note: Remove the M5GO base when using this example, otherwise it will not work properly,PDM Unit connected PORT-A 4 | */ 5 | 6 | #include 7 | #include 8 | #include "fft.h" 9 | 10 | #define PIN_CLK 22 11 | #define PIN_DATA 21 12 | 13 | #define MODE_MIC 0 14 | 15 | TFT_eSprite DisFFTbuff = TFT_eSprite(&M5.Lcd); 16 | static QueueHandle_t fftvalueQueue = nullptr; 17 | static QueueHandle_t i2sstateQueue = nullptr; 18 | 19 | void header(const char *string, uint16_t color) 20 | { 21 | M5.Lcd.fillScreen(color); 22 | M5.Lcd.setTextSize(1); 23 | M5.Lcd.setTextColor(WHITE, BLACK); 24 | M5.Lcd.fillRect(0, 0, 320, 30, BLACK); 25 | M5.Lcd.setTextDatum(TC_DATUM); 26 | M5.Lcd.drawString(string, 160, 3, 4); 27 | 28 | } 29 | 30 | typedef struct 31 | { 32 | uint8_t state; 33 | void* audioPtr; 34 | uint32_t audioSize; 35 | }i2sQueueMsg_t; 36 | 37 | bool InitI2SSpakerOrMic(int mode) 38 | { 39 | 40 | 41 | // i2s_driver_uninstall(I2S_NUM_0); 42 | i2s_config_t i2s_config = { 43 | .mode = (i2s_mode_t)(I2S_MODE_MASTER), 44 | .sample_rate = 44100, 45 | .bits_per_sample = I2S_BITS_PER_SAMPLE_16BIT, // is fixed at 12bit, stereo, MSB 46 | .channel_format = I2S_CHANNEL_FMT_ONLY_RIGHT, 47 | .communication_format = I2S_COMM_FORMAT_I2S, 48 | .intr_alloc_flags = ESP_INTR_FLAG_LEVEL1, 49 | .dma_buf_count = 2, 50 | .dma_buf_len = 128, 51 | }; 52 | if (mode == MODE_MIC) 53 | { 54 | i2s_config.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_RX | I2S_MODE_PDM); 55 | } 56 | 57 | //Serial.println("Init i2s_driver_install"); 58 | 59 | i2s_driver_install(I2S_NUM_0, &i2s_config, 0, NULL); 60 | i2s_pin_config_t pin_config; 61 | pin_config.bck_io_num = I2S_PIN_NO_CHANGE; 62 | pin_config.ws_io_num = PIN_CLK; 63 | pin_config.data_out_num = I2S_PIN_NO_CHANGE; 64 | pin_config.data_in_num = PIN_DATA; 65 | 66 | //Serial.println("Init i2s_set_pin"); 67 | i2s_set_pin(I2S_NUM_0, &pin_config); 68 | //Serial.println("Init i2s_set_clk"); 69 | i2s_set_clk(I2S_NUM_0, 44100, I2S_BITS_PER_SAMPLE_16BIT, I2S_CHANNEL_MONO); 70 | 71 | return true; 72 | } 73 | 74 | static void i2sMicroFFTtask(void *arg) 75 | { 76 | uint8_t FFTDataBuff[128]; 77 | uint8_t FFTValueBuff[24]; 78 | uint8_t* microRawData = (uint8_t*)calloc(2048,sizeof(uint8_t)); 79 | size_t bytesread; 80 | int16_t* buffptr; 81 | double data = 0; 82 | float adc_data; 83 | uint16_t ydata; 84 | uint32_t subData; 85 | 86 | uint8_t state = MODE_MIC; 87 | i2sQueueMsg_t QueueMsg; 88 | while(1) 89 | { 90 | if( xQueueReceive(i2sstateQueue,&QueueMsg,(TickType_t)0) == pdTRUE) 91 | { 92 | //Serial.println("Queue Now"); 93 | if( QueueMsg.state == MODE_MIC ) 94 | { 95 | InitI2SSpakerOrMic(MODE_MIC); 96 | state = MODE_MIC; 97 | } 98 | } 99 | else if( state == MODE_MIC ) 100 | { 101 | fft_config_t *real_fft_plan = fft_init(1024, FFT_REAL, FFT_FORWARD, NULL, NULL); 102 | i2s_read(I2S_NUM_0, (char *)microRawData, 2048, &bytesread, (100 / portTICK_RATE_MS)); 103 | buffptr = ( int16_t*)microRawData; 104 | 105 | for ( int count_n = 0; count_n < real_fft_plan->size; count_n++) 106 | { 107 | adc_data = (float)map(buffptr[count_n], INT16_MIN, INT16_MAX, -2000, 2000); 108 | real_fft_plan->input[count_n] = adc_data; 109 | } 110 | fft_execute(real_fft_plan); 111 | 112 | for ( int count_n = 1; count_n < real_fft_plan->size / 4; count_n++) 113 | { 114 | data = sqrt(real_fft_plan->output[2 * count_n] * real_fft_plan->output[2 * count_n] + real_fft_plan->output[2 * count_n + 1] * real_fft_plan->output[2 * count_n + 1]); 115 | if ((count_n - 1) < 128) 116 | { 117 | data = ( data > 2000 ) ? 2000 : data; 118 | ydata = map(data, 0, 2000, 0, 255); 119 | FFTDataBuff[128 - count_n] = ydata; 120 | } 121 | } 122 | 123 | for( int count = 0; count < 24; count++ ) 124 | { 125 | subData = 0; 126 | for( int count_i = 0; count_i < 5; count_i++ ) 127 | { 128 | subData += FFTDataBuff[count * 5 + count_i ]; 129 | } 130 | subData /= 5; 131 | FFTValueBuff[count] = map(subData,0,255,0,8); 132 | } 133 | xQueueSend( fftvalueQueue, (void * )&FFTValueBuff, 0 ); 134 | fft_destroy(real_fft_plan); 135 | //Serial.printf("mmp\r\n"); 136 | } 137 | else 138 | { 139 | delay(10); 140 | } 141 | } 142 | } 143 | 144 | void microPhoneSetup() 145 | { 146 | fftvalueQueue = xQueueCreate(5, 24 * sizeof(uint8_t)); 147 | if( fftvalueQueue == 0 ) 148 | { 149 | return; 150 | } 151 | 152 | i2sstateQueue = xQueueCreate(5, sizeof(i2sQueueMsg_t)); 153 | if( i2sstateQueue == 0 ) 154 | { 155 | return; 156 | } 157 | 158 | InitI2SSpakerOrMic(MODE_MIC); 159 | xTaskCreatePinnedToCore(i2sMicroFFTtask, "microPhoneTask", 4096, NULL, 3, NULL, 0); 160 | 161 | DisFFTbuff.createSprite(320,54); 162 | } 163 | 164 | void MicroPhoneFFT() 165 | { 166 | uint8_t FFTValueBuff[24]; 167 | xQueueReceive( fftvalueQueue, (void * )&FFTValueBuff, portMAX_DELAY ); 168 | DisFFTbuff.fillRect(0,0,320,54,DisFFTbuff.color565(0x00,0x00,0x00)); 169 | uint32_t colorY = DisFFTbuff.color565(0xff,0x9c,0x00); 170 | uint32_t colorG = DisFFTbuff.color565(0x66,0xff,0x00); 171 | uint32_t colorRect; 172 | for( int x = 0; x < 24; x++ ) 173 | { 174 | for( int y = 0; y < 9; y++ ) 175 | { 176 | if( y < FFTValueBuff[23-x] ) 177 | { 178 | colorRect = colorY; 179 | } 180 | else if( y == FFTValueBuff[23-x] ) 181 | { 182 | colorRect = colorG; 183 | } 184 | else 185 | { 186 | continue; 187 | } 188 | DisFFTbuff.fillRect(x*12,54-y*6 - 5,5,5,colorRect); 189 | } 190 | } 191 | DisFFTbuff.pushSprite(20, 120); 192 | } 193 | 194 | 195 | void setup() { 196 | M5.begin(true, true, true, true); 197 | header("PDM Unit", BLACK); 198 | microPhoneSetup(); 199 | 200 | } 201 | 202 | void loop() { 203 | MicroPhoneFFT(); 204 | } 205 | -------------------------------------------------------------------------------- /src/fft.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "fft.h" 8 | 9 | #define TWO_PI 6.28318530 10 | #define USE_SPLIT_RADIX 1 11 | #define LARGE_BASE_CASE 1 12 | 13 | fft_config_t *fft_init(int size, fft_type_t type, fft_direction_t direction, float *input, float *output) 14 | { 15 | /* 16 | * Prepare an FFT of correct size and types. 17 | * 18 | * If no input or output buffers are provided, they will be allocated. 19 | */ 20 | int k,m; 21 | 22 | fft_config_t *config = (fft_config_t *)malloc(sizeof(fft_config_t)); 23 | 24 | // Check if the size is a power of two 25 | if ((size & (size-1)) != 0) // tests if size is a power of two 26 | return NULL; 27 | 28 | // start configuration 29 | config->flags = 0; 30 | config->type = type; 31 | config->direction = direction; 32 | config->size = size; 33 | 34 | // Allocate and precompute twiddle factors 35 | config->twiddle_factors = (float *)malloc(2 * config->size * sizeof(float)); 36 | 37 | float two_pi_by_n = TWO_PI / config->size; 38 | 39 | for (k = 0, m = 0 ; k < config->size ; k++, m+=2) 40 | { 41 | config->twiddle_factors[m] = cosf(two_pi_by_n * k); // real 42 | config->twiddle_factors[m+1] = sinf(two_pi_by_n * k); // imag 43 | } 44 | 45 | // Allocate input buffer 46 | if (input != NULL) 47 | config->input = input; 48 | else 49 | { 50 | if (config->type == FFT_REAL) 51 | config->input = (float *)malloc(config->size * sizeof(float)); 52 | else if (config->type == FFT_COMPLEX) 53 | config->input = (float *)malloc(2 * config->size * sizeof(float)); 54 | 55 | config->flags |= FFT_OWN_INPUT_MEM; 56 | } 57 | 58 | if (config->input == NULL) 59 | return NULL; 60 | 61 | // Allocate output buffer 62 | if (output != NULL) 63 | config->output = output; 64 | else 65 | { 66 | if (config->type == FFT_REAL) 67 | config->output = (float *)malloc(config->size * sizeof(float)); 68 | else if (config->type == FFT_COMPLEX) 69 | config->output = (float *)malloc(2 * config->size * sizeof(float)); 70 | 71 | config->flags |= FFT_OWN_OUTPUT_MEM; 72 | } 73 | 74 | if (config->output == NULL) 75 | return NULL; 76 | 77 | return config; 78 | } 79 | 80 | void fft_destroy(fft_config_t *config) 81 | { 82 | if (config->flags & FFT_OWN_INPUT_MEM) 83 | free(config->input); 84 | 85 | if (config->flags & FFT_OWN_OUTPUT_MEM) 86 | free(config->output); 87 | 88 | free(config->twiddle_factors); 89 | free(config); 90 | } 91 | 92 | void fft_execute(fft_config_t *config) 93 | { 94 | if (config->type == FFT_REAL && config->direction == FFT_FORWARD) 95 | rfft(config->input, config->output, config->twiddle_factors, config->size); 96 | else if (config->type == FFT_REAL && config->direction == FFT_BACKWARD) 97 | irfft(config->input, config->output, config->twiddle_factors, config->size); 98 | else if (config->type == FFT_COMPLEX && config->direction == FFT_FORWARD) 99 | fft(config->input, config->output, config->twiddle_factors, config->size); 100 | else if (config->type == FFT_COMPLEX && config->direction == FFT_BACKWARD) 101 | ifft(config->input, config->output, config->twiddle_factors, config->size); 102 | } 103 | 104 | void fft(float *input, float *output, float *twiddle_factors, int n) 105 | { 106 | /* 107 | * Forward fast Fourier transform 108 | * DIT, radix-2, out-of-place implementation 109 | * 110 | * Parameters 111 | * ---------- 112 | * input (float *) 113 | * The input array containing the complex samples with 114 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 115 | * output (float *) 116 | * The output array containing the complex samples with 117 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 118 | * n (int) 119 | * The FFT size, should be a power of 2 120 | */ 121 | 122 | #if USE_SPLIT_RADIX 123 | split_radix_fft(input, output, n, 2, twiddle_factors, 2); 124 | #else 125 | fft_primitive(input, output, n, 2, twiddle_factors, 2); 126 | #endif 127 | } 128 | 129 | void ifft(float *input, float *output, float *twiddle_factors, int n) 130 | { 131 | /* 132 | * Inverse fast Fourier transform 133 | * DIT, radix-2, out-of-place implementation 134 | * 135 | * Parameters 136 | * ---------- 137 | * input (float *) 138 | * The input array containing the complex samples with 139 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 140 | * output (float *) 141 | * The output array containing the complex samples with 142 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 143 | * n (int) 144 | * The FFT size, should be a power of 2 145 | */ 146 | ifft_primitive(input, output, n, 2, twiddle_factors, 2); 147 | } 148 | 149 | void rfft(float *x, float *y, float *twiddle_factors, int n) 150 | { 151 | 152 | // This code uses the two-for-the-price-of-one strategy 153 | #if USE_SPLIT_RADIX 154 | split_radix_fft(x, y, n / 2, 2, twiddle_factors, 4); 155 | #else 156 | fft_primitive(x, y, n / 2, 2, twiddle_factors, 4); 157 | #endif 158 | 159 | // Now apply post processing to recover positive 160 | // frequencies of the real FFT 161 | float t = y[0]; 162 | y[0] = t + y[1]; // DC coefficient 163 | y[1] = t - y[1]; // Center coefficient 164 | 165 | // Apply post processing to quarter element 166 | // this boils down to taking complex conjugate 167 | y[n/2+1] = -y[n/2+1]; 168 | 169 | // Now process all the other frequencies 170 | int k; 171 | for (k = 2 ; k < n / 2 ; k += 2) 172 | { 173 | float xer, xei, xor_t, xoi, c, s, tr, ti; 174 | 175 | c = twiddle_factors[k]; 176 | s = twiddle_factors[k+1]; 177 | 178 | // even half coefficient 179 | xer = 0.5 * (y[k] + y[n-k]); 180 | xei = 0.5 * (y[k+1] - y[n-k+1]); 181 | 182 | // odd half coefficient 183 | xor_t = 0.5 * (y[k+1] + y[n-k+1]); 184 | xoi = - 0.5 * (y[k] - y[n-k]); 185 | 186 | tr = c * xor_t + s * xoi; 187 | ti = -s * xor_t + c * xoi; 188 | 189 | y[k] = xer + tr; 190 | y[k+1] = xei + ti; 191 | 192 | y[n-k] = xer - tr; 193 | y[n-k+1] = -(xei - ti); 194 | } 195 | } 196 | 197 | void irfft(float *x, float *y, float *twiddle_factors, int n) 198 | { 199 | /* 200 | * Destroys content of input vector 201 | */ 202 | int k; 203 | 204 | // Here we need to apply a pre-processing first 205 | float t = x[0]; 206 | x[0] = 0.5 * (t + x[1]); 207 | x[1] = 0.5 * (t - x[1]); 208 | 209 | x[n/2+1] = -x[n/2+1]; 210 | 211 | for (k = 2 ; k < n / 2 ; k += 2) 212 | { 213 | float xer, xei, xor_t, xoi, c, s, tr, ti; 214 | 215 | c = twiddle_factors[k]; 216 | s = twiddle_factors[k+1]; 217 | 218 | xer = 0.5 * (x[k] + x[n-k]); 219 | tr = 0.5 * (x[k] - x[n-k]); 220 | 221 | xei = 0.5 * (x[k+1] - x[n-k+1]); 222 | ti = 0.5 * (x[k+1] + x[n-k+1]); 223 | 224 | xor_t = c * tr - s * ti; 225 | xoi = s * tr + c * ti; 226 | 227 | x[k] = xer - xoi; 228 | x[k+1] = xor_t + xei; 229 | 230 | x[n-k] = xer + xoi; 231 | x[n-k+1] = xor_t - xei; 232 | } 233 | 234 | ifft_primitive(x, y, n / 2, 2, twiddle_factors, 4); 235 | } 236 | 237 | void fft_primitive(float *x, float *y, int n, int stride, float *twiddle_factors, int tw_stride) 238 | { 239 | /* 240 | * This code will compute the FFT of the input vector x 241 | * 242 | * The input data is assumed to be real/imag interleaved 243 | * 244 | * The size n should be a power of two 245 | * 246 | * y is an output buffer of size 2n to accomodate for complex numbers 247 | * 248 | * Forward fast Fourier transform 249 | * DIT, radix-2, out-of-place implementation 250 | * 251 | * For a complex FFT, call first stage as: 252 | * fft(x, y, n, 2, 2); 253 | * 254 | * Parameters 255 | * ---------- 256 | * x (float *) 257 | * The input array containing the complex samples with 258 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 259 | * y (float *) 260 | * The output array containing the complex samples with 261 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 262 | * n (int) 263 | * The FFT size, should be a power of 2 264 | * stride (int) 265 | * The number of elements to skip between two successive samples 266 | * tw_stride (int) 267 | * The number of elements to skip between two successive twiddle factors 268 | */ 269 | int k; 270 | float t; 271 | 272 | #if LARGE_BASE_CASE 273 | // End condition, stop at n=8 to avoid one trivial recursion 274 | if (n == 8) 275 | { 276 | fft8(x, stride, y, 2); 277 | return; 278 | } 279 | #else 280 | // End condition, stop at n=2 to avoid one trivial recursion 281 | if (n == 2) 282 | { 283 | y[0] = x[0] + x[stride]; 284 | y[1] = x[1] + x[stride + 1]; 285 | y[2] = x[0] - x[stride]; 286 | y[3] = x[1] - x[stride + 1]; 287 | return; 288 | } 289 | #endif 290 | 291 | // Recursion -- Decimation In Time algorithm 292 | fft_primitive(x, y, n / 2, 2 * stride, twiddle_factors, 2 * tw_stride); // even half 293 | fft_primitive(x + stride, y+n, n / 2, 2 * stride, twiddle_factors, 2 * tw_stride); // odd half 294 | 295 | // Stitch back together 296 | 297 | // We can a few multiplications in the first step 298 | t = y[0]; 299 | y[0] = t + y[n]; 300 | y[n] = t - y[n]; 301 | 302 | t = y[1]; 303 | y[1] = t + y[n+1]; 304 | y[n+1] = t - y[n+1]; 305 | 306 | for (k = 1 ; k < n / 2 ; k++) 307 | { 308 | float x1r, x1i, x2r, x2i, c, s; 309 | c = twiddle_factors[k * tw_stride]; 310 | s = twiddle_factors[k * tw_stride + 1]; 311 | 312 | x1r = y[2 * k]; 313 | x1i = y[2 * k + 1]; 314 | x2r = c * y[n + 2 * k] + s * y[n + 2 * k + 1]; 315 | x2i = -s * y[n + 2 * k] + c * y[n + 2 * k + 1]; 316 | 317 | y[2 * k] = x1r + x2r; 318 | y[2 * k + 1] = x1i + x2i; 319 | 320 | y[n + 2 * k] = x1r - x2r; 321 | y[n + 2 * k + 1] = x1i - x2i; 322 | } 323 | 324 | } 325 | 326 | void split_radix_fft(float *x, float *y, int n, int stride, float *twiddle_factors, int tw_stride) 327 | { 328 | /* 329 | * This code will compute the FFT of the input vector x 330 | * 331 | * The input data is assumed to be real/imag interleaved 332 | * 333 | * The size n should be a power of two 334 | * 335 | * y is an output buffer of size 2n to accomodate for complex numbers 336 | * 337 | * Forward fast Fourier transform 338 | * Split-Radix 339 | * DIT, radix-2, out-of-place implementation 340 | * 341 | * For a complex FFT, call first stage as: 342 | * fft(x, y, n, 2, 2); 343 | * 344 | * Parameters 345 | * ---------- 346 | * x (float *) 347 | * The input array containing the complex samples with 348 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 349 | * y (float *) 350 | * The output array containing the complex samples with 351 | * real/imaginary parts interleaved [Re(x0), Im(x0), ..., Re(x_n-1), Im(x_n-1)] 352 | * n (int) 353 | * The FFT size, should be a power of 2 354 | * stride (int) 355 | * The number of elements to skip between two successive samples 356 | * twiddle_factors (float *) 357 | * The array of twiddle factors 358 | * tw_stride (int) 359 | * The number of elements to skip between two successive twiddle factors 360 | */ 361 | int k; 362 | 363 | #if LARGE_BASE_CASE 364 | // End condition, stop at n=2 to avoid one trivial recursion 365 | if (n == 8) 366 | { 367 | fft8(x, stride, y, 2); 368 | return; 369 | } 370 | else if (n == 4) 371 | { 372 | fft4(x, stride, y, 2); 373 | return; 374 | } 375 | #else 376 | // End condition, stop at n=2 to avoid one trivial recursion 377 | if (n == 2) 378 | { 379 | y[0] = x[0] + x[stride]; 380 | y[1] = x[1] + x[stride + 1]; 381 | y[2] = x[0] - x[stride]; 382 | y[3] = x[1] - x[stride + 1]; 383 | return; 384 | } 385 | else if (n == 1) 386 | { 387 | y[0] = x[0]; 388 | y[1] = x[1]; 389 | return; 390 | } 391 | #endif 392 | 393 | // Recursion -- Decimation In Time algorithm 394 | split_radix_fft(x, y, n / 2, 2 * stride, twiddle_factors, 2 * tw_stride); 395 | split_radix_fft(x + stride, y + n, n / 4, 4 * stride, twiddle_factors, 4 * tw_stride); 396 | split_radix_fft(x + 3 * stride, y + n + n / 2, n / 4, 4 * stride, twiddle_factors, 4 * tw_stride); 397 | 398 | // Stitch together the output 399 | float u1r, u1i, u2r, u2i, x1r, x1i, x2r, x2i; 400 | float t; 401 | 402 | // We can save a few multiplications in the first step 403 | u1r = y[0]; 404 | u1i = y[1]; 405 | u2r = y[n / 2]; 406 | u2i = y[n / 2 + 1]; 407 | 408 | x1r = y[n]; 409 | x1i = y[n + 1]; 410 | x2r = y[n / 2 + n]; 411 | x2i = y[n / 2 + n + 1]; 412 | 413 | t = x1r + x2r; 414 | y[0] = u1r + t; 415 | y[n] = u1r - t; 416 | 417 | t = x1i + x2i; 418 | y[1] = u1i + t; 419 | y[n + 1] = u1i - t; 420 | 421 | t = x2i - x1i; 422 | y[n / 2] = u2r - t; 423 | y[n + n / 2] = u2r + t; 424 | 425 | t = x1r - x2r; 426 | y[n / 2 + 1] = u2i - t; 427 | y[n + n / 2 + 1] = u2i + t; 428 | 429 | for (k = 1 ; k < n / 4 ; k++) 430 | { 431 | float u1r, u1i, u2r, u2i, x1r, x1i, x2r, x2i, c1, s1, c2, s2; 432 | c1 = twiddle_factors[k * tw_stride]; 433 | s1 = twiddle_factors[k * tw_stride + 1]; 434 | c2 = twiddle_factors[3 * k * tw_stride]; 435 | s2 = twiddle_factors[3 * k * tw_stride + 1]; 436 | 437 | u1r = y[2 * k]; 438 | u1i = y[2 * k + 1]; 439 | u2r = y[2 * k + n / 2]; 440 | u2i = y[2 * k + n / 2 + 1]; 441 | 442 | x1r = c1 * y[n + 2 * k] + s1 * y[n + 2 * k + 1]; 443 | x1i = -s1 * y[n + 2 * k] + c1 * y[n + 2 * k + 1]; 444 | x2r = c2 * y[n / 2 + n + 2 * k] + s2 * y[n / 2 + n + 2 * k + 1]; 445 | x2i = -s2 * y[n / 2 + n + 2 * k] + c2 * y[n / 2 + n + 2 * k + 1]; 446 | 447 | t = x1r + x2r; 448 | y[2 * k] = u1r + t; 449 | y[2 * k + n] = u1r - t; 450 | 451 | t = x1i + x2i; 452 | y[2 * k + 1] = u1i + t; 453 | y[2 * k + n + 1] = u1i - t; 454 | 455 | t = x2i - x1i; 456 | y[2 * k + n / 2] = u2r - t; 457 | y[2 * k + n + n / 2] = u2r + t; 458 | 459 | t = x1r - x2r; 460 | y[2 * k + n / 2 + 1] = u2i - t; 461 | y[2 * k + n + n / 2 + 1] = u2i + t; 462 | } 463 | 464 | } 465 | 466 | 467 | void ifft_primitive(float *input, float *output, int n, int stride, float *twiddle_factors, int tw_stride) 468 | { 469 | 470 | #if USE_SPLIT_RADIX 471 | split_radix_fft(input, output, n, stride, twiddle_factors, tw_stride); 472 | #else 473 | fft_primitive(input, output, n, stride, twiddle_factors, tw_stride); 474 | #endif 475 | 476 | int ks; 477 | 478 | int ns = n * stride; 479 | 480 | // reverse all coefficients from 1 to n / 2 - 1 481 | for (ks = stride ; ks < ns / 2 ; ks += stride) 482 | { 483 | float t; 484 | 485 | t = output[ks]; 486 | output[ks] = output[ns-ks]; 487 | output[ns-ks] = t; 488 | 489 | t = output[ks+1]; 490 | output[ks+1] = output[ns-ks+1]; 491 | output[ns-ks+1] = t; 492 | } 493 | 494 | // Apply normalization 495 | float norm = 1. / n; 496 | for (ks = 0 ; ks < ns ; ks += stride) 497 | { 498 | output[ks] *= norm; 499 | output[ks+1] *= norm; 500 | } 501 | 502 | } 503 | 504 | inline void fft8(float *input, int stride_in, float *output, int stride_out) 505 | { 506 | /* 507 | * Unrolled implementation of FFT8 for a little more performance 508 | */ 509 | float a0r, a1r, a2r, a3r, a4r, a5r, a6r, a7r; 510 | float a0i, a1i, a2i, a3i, a4i, a5i, a6i, a7i; 511 | float b0r, b1r, b2r, b3r, b4r, b5r, b6r, b7r; 512 | float b0i, b1i, b2i, b3i, b4i, b5i, b6i, b7i; 513 | float t; 514 | float sin_pi_4 = 0.7071067812; 515 | 516 | a0r = input[0]; 517 | a0i = input[1]; 518 | a1r = input[stride_in]; 519 | a1i = input[stride_in+1]; 520 | a2r = input[2*stride_in]; 521 | a2i = input[2*stride_in+1]; 522 | a3r = input[3*stride_in]; 523 | a3i = input[3*stride_in+1]; 524 | a4r = input[4*stride_in]; 525 | a4i = input[4*stride_in+1]; 526 | a5r = input[5*stride_in]; 527 | a5i = input[5*stride_in+1]; 528 | a6r = input[6*stride_in]; 529 | a6i = input[6*stride_in+1]; 530 | a7r = input[7*stride_in]; 531 | a7i = input[7*stride_in+1]; 532 | 533 | // Stage 1 534 | 535 | b0r = a0r + a4r; 536 | b0i = a0i + a4i; 537 | 538 | b1r = a1r + a5r; 539 | b1i = a1i + a5i; 540 | 541 | b2r = a2r + a6r; 542 | b2i = a2i + a6i; 543 | 544 | b3r = a3r + a7r; 545 | b3i = a3i + a7i; 546 | 547 | b4r = a0r - a4r; 548 | b4i = a0i - a4i; 549 | 550 | b5r = a1r - a5r; 551 | b5i = a1i - a5i; 552 | // W_8^1 = 1/sqrt(2) - j / sqrt(2) 553 | t = b5r + b5i; 554 | b5i = (b5i - b5r) * sin_pi_4; 555 | b5r = t * sin_pi_4; 556 | 557 | // W_8^2 = -j 558 | b6r = a2i - a6i; 559 | b6i = a6r - a2r; 560 | 561 | b7r = a3r - a7r; 562 | b7i = a3i - a7i; 563 | // W_8^3 = -1 / sqrt(2) + j / sqrt(2) 564 | t = sin_pi_4 * (b7i - b7r); 565 | b7i = - (b7r + b7i) * sin_pi_4; 566 | b7r = t; 567 | 568 | // Stage 2 569 | 570 | a0r = b0r + b2r; 571 | a0i = b0i + b2i; 572 | 573 | a1r = b1r + b3r; 574 | a1i = b1i + b3i; 575 | 576 | a2r = b0r - b2r; 577 | a2i = b0i - b2i; 578 | 579 | // * j 580 | a3r = b1i - b3i; 581 | a3i = b3r - b1r; 582 | 583 | a4r = b4r + b6r; 584 | a4i = b4i + b6i; 585 | 586 | a5r = b5r + b7r; 587 | a5i = b5i + b7i; 588 | 589 | a6r = b4r - b6r; 590 | a6i = b4i - b6i; 591 | 592 | // * j 593 | a7r = b5i - b7i; 594 | a7i = b7r - b5r; 595 | 596 | // Stage 3 597 | 598 | // X[0] 599 | output[0] = a0r + a1r; 600 | output[1] = a0i + a1i; 601 | 602 | // X[4] 603 | output[4*stride_out] = a0r - a1r; 604 | output[4*stride_out+1] = a0i - a1i; 605 | 606 | // X[2] 607 | output[2*stride_out] = a2r + a3r; 608 | output[2*stride_out+1] = a2i + a3i; 609 | 610 | // X[6] 611 | output[6*stride_out] = a2r - a3r; 612 | output[6*stride_out+1] = a2i - a3i; 613 | 614 | // X[1] 615 | output[stride_out] = a4r + a5r; 616 | output[stride_out+1] = a4i + a5i; 617 | 618 | // X[5] 619 | output[5*stride_out] = a4r - a5r; 620 | output[5*stride_out+1] = a4i - a5i; 621 | 622 | // X[3] 623 | output[3*stride_out] = a6r + a7r; 624 | output[3*stride_out+1] = a6i + a7i; 625 | 626 | // X[7] 627 | output[7*stride_out] = a6r - a7r; 628 | output[7*stride_out+1] = a6i - a7i; 629 | 630 | } 631 | 632 | inline void fft4(float *input, int stride_in, float *output, int stride_out) 633 | { 634 | /* 635 | * Unrolled implementation of FFT4 for a little more performance 636 | */ 637 | float t1, t2; 638 | 639 | t1 = input[0] + input[2*stride_in]; 640 | t2 = input[stride_in] + input[3*stride_in]; 641 | output[0] = t1 + t2; 642 | output[2*stride_out] = t1 - t2; 643 | 644 | t1 = input[1] + input[2*stride_in+1]; 645 | t2 = input[stride_in+1] + input[3*stride_in+1]; 646 | output[1] = t1 + t2; 647 | output[2*stride_out+1] = t1 - t2; 648 | 649 | t1 = input[0] - input[2*stride_in]; 650 | t2 = input[stride_in+1] - input[3*stride_in+1]; 651 | output[stride_out] = t1 + t2; 652 | output[3*stride_out] = t1 - t2; 653 | 654 | t1 = input[1] - input[2*stride_in+1]; 655 | t2 = input[3*stride_in] - input[stride_in]; 656 | output[stride_out+1] = t1 + t2; 657 | output[3*stride_out+1] = t1 - t2; 658 | } 659 | --------------------------------------------------------------------------------