Espressif DSP Library のFFT関数を使ってみた．ポイント数は4096までサポートされている．信号処理のパフォーマンスを落としたくないのでFFTは信号処理とは別のコアで実行させたい．信号はリングバッファ経由で渡すようにした．
// I2S on ESP32-S3
// T.Uebo October 12 , 2022
// I2S Master MODE 48kHz/32bit
//
// Mclk GPIO 39
// Bclk GPIO 42
// LRclk GPIO 2
// Dout GPIO 41
// Din GPIO 1
#include <esp_dsp.h>
#include <driver/i2s.h>
#define fsample 48000
#define BLOCK_SAMPLES 64
#define MUTE 40 // MUTE control (LOW: Mute)
//buffers
int rxbuf[BLOCK_SAMPLES*2], txbuf[BLOCK_SAMPLES*2];
float Lch_in[BLOCK_SAMPLES], Rch_in[BLOCK_SAMPLES];
float Lch_out[BLOCK_SAMPLES], Rch_out[BLOCK_SAMPLES];
// for Filter work
float zL0[2], zR0[2], coeffs0[5];
float zL1[2], zR1[2], coeffs1[5];
#define Nfir 512
fir_f32_t firL_st, firR_st;
float coeffs_firL[Nfir], coeffs_firR[Nfir];
float z_firL[Nfir], z_firR[Nfir];
#define Nfft 4096
float dat[Nfft*2];
float wf[Nfft]; // 窓関数
float pow_sp[Nfft]; // パワースペクトル
//リングバッファ
#define Ring_Buf_Size (Nfft+256)
volatile float d_Lch[Ring_Buf_Size];
volatile float d_Rch[Ring_Buf_Size];
volatile int pt = 0; // リングバッファのポインタ
TaskHandle_t H_Task;//タスクハンドル
/*--------------------------------------------------
core 0 で処理するTask Alt_Thread()
---------------------------------------------------*/
void Alt_Thread(void *args) {
dsps_fft2r_init_fc32(NULL, Nfft); // 回転因子生成
dsps_wind_hann_f32(wf, Nfft); // 窓関数生成
/*----------------------------------------------------
Loop
------------------------------------------------------*/
while (1) {
// リングバッファから信号をコピー
int ptc = pt;
ptc -= Nfft; if(ptc < 0) ptc += Ring_Buf_Size;
int j=0;
for(int i = 0; i<Nfft; i++){
dat[j] = d_Lch[ptc]; // Re
dat[j+1] = d_Rch[ptc]; //Im
ptc++; if(ptc == Ring_Buf_Size) ptc=0;
j+=2;
}
// 窓関数を掛ける
j=0;
for(int i = 0; i<Nfft; i++){
dat[j] *= wf[i];
dat[j+1] *= wf[i];
j+=2;
}
// FFT
dsps_fft2r_fc32(dat, Nfft);
dsps_bit_rev2r_fc32(dat, Nfft);
// パワースペクトルの計算
float Re, Im;
j=0;
for(int i=0; i<Nfft; i++){
Re=dat[j]; Im=dat[j+1];
pow_sp[i] = Re*Re + Im*Im;
pow_sp[i]=10.0f*log10(pow_sp[i]) -120;
if(pow_sp[i]<0) pow_sp[i]=0;
j+=2;
}
// パワースペクトル[dB] pow_sp[i]
delay(1);
}
}
/*-----------------------------------------------------------------------------------------------
Setup
-------------------------------------------------------------------------------------------------*/
void setup(void) {
Serial.begin(115200);
delay(50);
xTaskCreatePinnedToCore(Alt_Thread, "Alt_Thread", 8192, NULL, 4, &H_Task, 0);
//Mute Control
pinMode(MUTE, OUTPUT);
digitalWrite(MUTE, HIGH); //unmute
pinMode(48, OUTPUT);
//-- Filter settings -------------------------------------
// set IIR Filter Coeffs.
dsps_biquad_gen_lpf_f32(coeffs0, 0.45f, 0.71f);
dsps_biquad_gen_lpf_f32(coeffs1, 0.45f, 0.71f);
// set up FIR
dsps_fir_init_f32(&firL_st, coeffs_firL, z_firL, Nfir);
dsps_fir_init_f32(&firR_st, coeffs_firR, z_firR, Nfir);
// set FIR Filter Coeffs.
dsps_d_gen_f32(coeffs_firL, Nfir, 0);
dsps_d_gen_f32(coeffs_firR, Nfir, 0);
// I2S setup (Lables are defined in i2s_types.h, esp_intr_alloc.h)----------------------------------------
i2s_config_t i2s_config = {
.mode = (i2s_mode_t)(I2S_MODE_MASTER | I2S_MODE_TX | I2S_MODE_RX),
.sample_rate = fsample,
.bits_per_sample = I2S_BITS_PER_SAMPLE_32BIT,
.channel_format = I2S_CHANNEL_FMT_RIGHT_LEFT,
.communication_format = I2S_COMM_FORMAT_STAND_I2S,
.intr_alloc_flags = ESP_INTR_FLAG_LOWMED,
.dma_buf_count = 2,
.dma_buf_len = BLOCK_SAMPLES*4,
.use_apll = false,
.tx_desc_auto_clear = true,
.fixed_mclk = 0,
.mclk_multiple = I2S_MCLK_MULTIPLE_256,
};
i2s_driver_install( I2S_NUM_0, &i2s_config, 0, NULL);
i2s_pin_config_t pin_config = {
.mck_io_num = 39,
.bck_io_num = 42,
.ws_io_num = 2,
.data_out_num = 41,
.data_in_num = 1
};
i2s_set_pin( I2S_NUM_0, &pin_config);
}
/*-----------------------------------------------------------------------------------------------
Signal Process Loop
-------------------------------------------------------------------------------------------------*/
void loop(void) {
size_t readsize = 0;
//Input from I2S codec
esp_err_t rxfb = i2s_read(I2S_NUM_0, &rxbuf[0], BLOCK_SAMPLES*2*4, &readsize, portMAX_DELAY);
if (rxfb == ESP_OK && readsize==BLOCK_SAMPLES*2*4) {
digitalWrite(48, HIGH);
int j=0;
for (int i=0; i<BLOCK_SAMPLES; i++) {
Lch_in[i] = (float) rxbuf[j];
Rch_in[i] = (float) rxbuf[j+1];
j+=2;
}
//-------Signal process -------------------------------
dsps_biquad_f32(Lch_in, Lch_out, BLOCK_SAMPLES, coeffs0, zL0);
dsps_biquad_f32(Rch_in, Rch_out, BLOCK_SAMPLES, coeffs0, zR0);
dsps_biquad_f32(Lch_out, Lch_in, BLOCK_SAMPLES, coeffs1, zL1);
dsps_biquad_f32(Rch_out, Rch_in, BLOCK_SAMPLES, coeffs1, zR1);
dsps_fir_f32(&firL_st, Lch_in, Lch_out, BLOCK_SAMPLES);
dsps_fir_f32(&firR_st, Rch_in, Rch_out, BLOCK_SAMPLES);
//Output to I2S codec
j=0;
for (int i=0; i<BLOCK_SAMPLES; i++) {
txbuf[j] = (int) Lch_out[i];
txbuf[j+1] = (int) Rch_out[i];
j+=2;
}
i2s_write( I2S_NUM_0, &txbuf[0], BLOCK_SAMPLES*2*4, &readsize, portMAX_DELAY);
//core0 のTaskでFFTを実行するために信号をリングバッファにコピー
for (int i=0; i<BLOCK_SAMPLES; i++) {
d_Lch[pt]=Lch_out[i];
d_Rch[pt]=Rch_out[i];
pt++; if( pt == Ring_Buf_Size ) pt=0;
}
}
digitalWrite(48, LOW);
}
FFTの処理をまとめると以下とおり．
１．#define Nfft でポイント数定義
２．配列を準備 float dat[Nfft*2]; float wf[Nfft];
３．dsps_fft2r_init_fc32(NULL, Nfft); で回転因子を生成
４．dsps_wind_hann_f32(wf, Nfft); で窓関数生成．（hann 窓しか用意されてない）
５．処理する信号に窓関数を掛けてdat[ ]に入れる．実部：偶数番目，虚部：奇数番目
６．dsps_fft2r_fc32(dat, Nfft); FFT処理
７．dsps_bit_rev2r_fc32(dat, Nfft); ビットリバース処理
８．結果は，dat[ ] に上書きされる．実部：偶数番目，虚部：奇数番目
あとは必要に応じてパワースペクトルにするなりすればOK．上のコードでは 最終 pow_sp[ ] にパワースペクトルを入れている．
ビットリバース関数が用意されているが，これを使う必要があるかどうかドキュメントからは読み取れなかった．動作させて確認したところ dsps_fft2r_fc32にはビットリバース処理は含まれておらず，この関数は必須だった．また1/Nfftの係数もかかっていないようで，結局 dsps_fft2r_fc32 は純粋にバタフライ演算のみをしていると思われる．いちいち係数をかけるのもめんどうなので上のコードではパワーをdBに変換した後で適当な定数を引いている．
Tj Lab 