fft_controller.c

#include "esp_dsp.h"
#include <stdint.h>

#ifdef __cplusplus
extern "C"
{
#endif

#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "esp_log.h"

#include "esp_dsp.h"
#include "fft_controller.h"

#define BUF_SIZE 4096
#define USE_WINDOW false

  static uint8_t fft_buffer[BUF_SIZE];
  static fft_running = false;
  static float x2[BUF_SIZE];
  static float y_cf[BUF_SIZE * 2];
  static float *y1_cf = &y_cf[0];

  void init_fft()
  {
    esp_err_t ret = dsps_fft2r_init_fc32(NULL, BUF_SIZE);
    if (ret != ESP_OK)
    {
      ESP_LOGE(FFT_TAG, "Not possible to initialize FFT. Error = %i", ret);
      return;
    }
  }

  void calculate_fft()
  {
    fft_running = true;
    for (int i = 0; i < BUF_SIZE; i++)
    {
      float f1 = fft_buffer[i];
#if USE_WINDOW
      y_cf[i * 2 + 0] = f1 * wind[i];
      y_cf[i * 2 + 1] = x2[i] * wind[i];
#else
    y_cf[i * 2 + 0] = f1;
    y_cf[i * 2 + 1] = x2[i];
#endif
    }

    dsps_fft2r_fc32(y_cf, BUF_SIZE);
    dsps_bit_rev_fc32(y_cf, BUF_SIZE);
    dsps_cplx2reC_fc32(y_cf, BUF_SIZE);

    for (int i = 0; i < BUF_SIZE / 2; i++)
    {
      y1_cf[i] = 10 * log10f((y1_cf[i * 2 + 0] * y1_cf[i * 2 + 0] + y1_cf[i * 2 + 1] * y1_cf[i * 2 + 1]) / BUF_SIZE);
    }

    ESP_LOGW(FFT_TAG, "Signal x1");
    dsps_view(y1_cf, BUF_SIZE / 2, 128, 30, -20, 100, '|');
    fft_running = false;
  }

  void copy_a2dp_buffer_for_fft(const uint8_t *data, uint32_t len)
  {
    if (!fft_running) {
      memcpy(fft_buffer, data, len);
    }
  }

#ifdef __cplusplus
}
#endif