add some code
This commit is contained in:
@@ -0,0 +1,87 @@
|
||||
// Copyright 2018-2025 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "dsps_biquad_platform.h"
|
||||
#if (dsps_biquad_f32_ae32_enabled == 1)
|
||||
|
||||
// This is bi quad filter form II for ESP32 processor.
|
||||
.text
|
||||
.align 4
|
||||
.global dsps_biquad_f32_ae32
|
||||
.type dsps_biquad_f32_ae32,@function
|
||||
// The function implements the following C code:
|
||||
//esp_err_t dsps_biquad_f32_ae32(const float* input, float* output, int len, float* coef, float* w)
|
||||
// {
|
||||
// for (int i=0 ; i< len ; i++)
|
||||
// {
|
||||
// float d0 = input[i] - coef[3]*w[0] - coef[4]*w[1]; (input[i] - a[1]*w[0] - a[2]*w[1];)
|
||||
// output[i] = coef[0]*d0 + coef[1]*w[0] + coef[2]*w[1];
|
||||
// w[1] = w[0];
|
||||
// w[0] = d0;
|
||||
// }
|
||||
// return ESP_OK;
|
||||
// }
|
||||
|
||||
dsps_biquad_f32_ae32:
|
||||
// input - a2
|
||||
// output - a3
|
||||
// len - a4
|
||||
// coeffs - a5
|
||||
// w- a6
|
||||
|
||||
// f0 - b0
|
||||
// f1 - b1
|
||||
// f2 - b2
|
||||
// f3 - a1
|
||||
// f4 - a2
|
||||
|
||||
// f5 - w0
|
||||
// f6 - w1
|
||||
|
||||
entry a1, 16
|
||||
// Array increment for floating point data should be 4
|
||||
lsi f0, a5, 0
|
||||
lsi f1, a5, 4
|
||||
lsi f2, a5, 8
|
||||
lsi f3, a5, 12
|
||||
lsi f4, a5, 16
|
||||
|
||||
|
||||
neg.s f5, f3 // -a[1]
|
||||
neg.s f6, f4 // -a[2]
|
||||
|
||||
lsi f7, a6, 0 // w[0]
|
||||
lsi f8, a6, 4 // w[1]
|
||||
|
||||
lsip f9, a2, 4 // f9 = x[i]
|
||||
loopnez a4, .loop_bq_end_m_ae32
|
||||
madd.s f9, f7, f5 // f9 += -a1*w0
|
||||
mul.s f10, f1, f7 // f10 = b1*w0
|
||||
madd.s f9, f8, f6 // f9 += -a2*w1
|
||||
madd.s f10, f9, f0 // f10 += b0*d0
|
||||
addi a2, a2, 4 // in++;
|
||||
madd.s f10, f2, f8 // f10+= b2*w1, f10 - result
|
||||
mov.s f8, f7 // w1 = w0
|
||||
mov.s f7, f9 // w0 = d0
|
||||
lsip f9, a2, 4 // f9 = x[i]
|
||||
ssip f10, a3, 4 // y[i] = result
|
||||
.loop_bq_end_m_ae32:
|
||||
// Store delay line
|
||||
ssi f7, a6, 0
|
||||
ssi f8, a6, 4
|
||||
|
||||
movi.n a2, 0 // return status ESP_OK
|
||||
retw.n
|
||||
|
||||
#endif // dsps_biquad_f32_ae32_enabled
|
||||
@@ -0,0 +1,89 @@
|
||||
// Copyright 2025 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "dsps_biquad_platform.h"
|
||||
#if (dsps_biquad_f32_aes3_enabled == 1)
|
||||
|
||||
// This is bi quad filter form II for ESP32 processor.
|
||||
.text
|
||||
.align 4
|
||||
.global dsps_biquad_f32_aes3
|
||||
.type dsps_biquad_f32_aes3,@function
|
||||
// The function implements the following C code:
|
||||
//esp_err_t dsps_biquad_f32_aes3(const float* input, float* output, int len, float* coef, float* w)
|
||||
// {
|
||||
// for (int i=0 ; i< len ; i++)
|
||||
// {
|
||||
// float d0 = input[i] - coef[3]*w[0] - coef[4]*w[1]; (input[i] - a[1]*w[0] - a[2]*w[1];)
|
||||
// output[i] = coef[0]*d0 + coef[1]*w[0] + coef[2]*w[1];
|
||||
// w[1] = w[0];
|
||||
// w[0] = d0;
|
||||
// }
|
||||
// return ESP_OK;
|
||||
// }
|
||||
|
||||
dsps_biquad_f32_aes3:
|
||||
// input - a2
|
||||
// output - a3
|
||||
// len - a4
|
||||
// coeffs - a5
|
||||
// w- a6
|
||||
|
||||
// f0 - b0
|
||||
// f1 - b1
|
||||
// f2 - b2
|
||||
// f3 - a1
|
||||
// f4 - a2
|
||||
|
||||
// f5 - w0
|
||||
// f6 - w1
|
||||
|
||||
entry a1, 16
|
||||
// Array increment for floating point data should be 4
|
||||
lsi f0, a5, 0
|
||||
lsi f1, a5, 4
|
||||
lsi f2, a5, 8
|
||||
lsi f3, a5, 12
|
||||
lsi f4, a5, 16
|
||||
|
||||
|
||||
neg.s f5, f3 // -a[1]
|
||||
neg.s f6, f4 // -a[2]
|
||||
|
||||
lsi f7, a6, 0 // w[0]
|
||||
lsi f8, a6, 4 // w[1]
|
||||
|
||||
addi a3, a3, -4 // i-- // preset a3
|
||||
lsi f9, a2, 0 // f9 = x[i]
|
||||
loopnez a4, .loop_bq_end_m_aes3
|
||||
madd.s f9, f7, f5 // f9 += -a1*w0
|
||||
addi a3, a3, 4 // out++;
|
||||
mul.s f10, f1, f7 // f10 = b1*w0
|
||||
madd.s f9, f8, f6 // f9 += -a2*w1
|
||||
madd.s f10, f9, f0 // f10 += b0*d0
|
||||
addi a2, a2, 4 // in++;
|
||||
madd.s f10, f2, f8 // f10+= b2*w1, f10 - result
|
||||
mov.s f8, f7 // w1 = w0
|
||||
mov.s f7, f9 // w0 = d0
|
||||
lsi f9, a2, 0 // f9 = x[i]
|
||||
ssi f10, a3, 0 // y[i] = result
|
||||
.loop_bq_end_m_aes3:
|
||||
// Store delay line
|
||||
ssi f7, a6, 0
|
||||
ssi f8, a6, 4
|
||||
|
||||
movi.n a2, 0 // return status ESP_OK
|
||||
retw.n
|
||||
|
||||
#endif // dsps_biquad_f32_aes3_enabled
|
||||
@@ -0,0 +1,28 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#include "dsps_biquad.h"
|
||||
|
||||
|
||||
esp_err_t dsps_biquad_f32_ansi(const float *input, float *output, int len, float *coef, float *w)
|
||||
{
|
||||
for (int i = 0 ; i < len ; i++) {
|
||||
float d0 = input[i] - coef[3] * w[0] - coef[4] * w[1];
|
||||
output[i] = coef[0] * d0 + coef[1] * w[0] + coef[2] * w[1];
|
||||
w[1] = w[0];
|
||||
w[0] = d0;
|
||||
}
|
||||
return ESP_OK;
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
// Copyright 2018-2025 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "dsps_biquad_platform.h"
|
||||
#if (dsps_biquad_f32_arp4_enabled == 1)
|
||||
|
||||
// This is bi quad filter form II for ESP32 processor.
|
||||
.text
|
||||
.align 4
|
||||
.global dsps_biquad_f32_arp4
|
||||
.type dsps_biquad_f32_arp4,@function
|
||||
// The function implements the following C code:
|
||||
//esp_err_t dsps_biquad_f32_arp4(const float* input, float* output, int len, float* coef, float* w)
|
||||
// {
|
||||
// for (int i=0 ; i< len ; i++)
|
||||
// {
|
||||
// float d0 = input[i] - coef[3]*w[0] - coef[4]*w[1]; (input[i] - a[1]*w[0] - a[2]*w[1];)
|
||||
// output[i] = coef[0]*d0 + coef[1]*w[0] + coef[2]*w[1];
|
||||
// w[1] = w[0];
|
||||
// w[0] = d0;
|
||||
// }
|
||||
// return ESP_OK;
|
||||
// }
|
||||
|
||||
dsps_biquad_f32_arp4:
|
||||
// input - a0
|
||||
// output - a1
|
||||
// len - a2
|
||||
// coeffs - a3
|
||||
// w- a4
|
||||
|
||||
// fa0 - b0
|
||||
// fa1 - b1
|
||||
// fa2 - b2
|
||||
// fa3 - a1
|
||||
// fa4 - a2
|
||||
|
||||
// fa5 - w0
|
||||
// fa6 - w1
|
||||
|
||||
add sp,sp,-16
|
||||
|
||||
flw fa0, 0(a3)
|
||||
flw fa1, 4(a3)
|
||||
flw fa2, 8(a3)
|
||||
flw fa3, 12(a3)
|
||||
flw fa4, 16(a3)
|
||||
|
||||
fneg.S fa5, fa3 // -a[1]
|
||||
fneg.S fa6, fa4 // -a[2]
|
||||
|
||||
flw ft0, 0(a4) // ft0 - w0
|
||||
flw ft1, 4(a4) // ft1 - w1
|
||||
|
||||
flw ft2, 0(a0) // ft2 - f9 = x[i]
|
||||
esp.lp.setup 0, a2, .iir_loop_end // label to the last executed instruction
|
||||
fmadd.S ft2, ft0, fa5, ft2
|
||||
fmul.s ft3, fa1, ft0
|
||||
fmadd.s ft2, ft1, fa6, ft2 // f9 += -a2*w1
|
||||
|
||||
fmadd.s ft3, ft2, fa0, ft3 // f10 += b0*d0
|
||||
addi a0, a0, 4 // in++;
|
||||
fmadd.s ft3, fa2, ft1, ft3 // f10+= b2*w1, f10 - result
|
||||
fmv.s ft1, ft0 // w1 = w0
|
||||
fmv.s ft0, ft2 // w0 = d0
|
||||
|
||||
flw ft2, 0(a0)
|
||||
fsw ft3, 0(a1)
|
||||
addi a1, a1, 4 // out++;
|
||||
|
||||
.iir_loop_end: nop
|
||||
fsw ft0, 0(a4) // ft0 - f7
|
||||
fsw ft1, 4(a4) // ft1 - f8
|
||||
|
||||
mv a0, a6
|
||||
add sp,sp,16
|
||||
ret
|
||||
|
||||
#endif // dsps_biquad_f32_aes3_enabled
|
||||
@@ -0,0 +1,290 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "dsps_biquad_gen.h"
|
||||
#include <math.h>
|
||||
#include "esp_log.h"
|
||||
|
||||
esp_err_t dsps_biquad_gen_lpf_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = (1 - c) / 2;
|
||||
float b1 = 1 - c;
|
||||
float b2 = b0;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_hpf_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = (1 + c) / 2;
|
||||
float b1 = -(1 + c);
|
||||
float b2 = b0;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_bpf_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = s / 2;
|
||||
float b1 = 0;
|
||||
float b2 = -b0;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_bpf0db_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = alpha;
|
||||
float b1 = 0;
|
||||
float b2 = -alpha;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_notch_f32(float *coeffs, float f, float gain, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float A = sqrtf(pow(10, (double)gain / 20.0));
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = 1 + alpha * A;
|
||||
float b1 = -2 * c;
|
||||
float b2 = 1 - alpha * A;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_allpass360_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = 1 - alpha;
|
||||
float b1 = -2 * c;
|
||||
float b2 = 1 + alpha;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_allpass180_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = 1 - alpha;
|
||||
float b1 = -2 * c;
|
||||
float b2 = 1 + alpha;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_peakingEQ_f32(float *coeffs, float f, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = alpha;
|
||||
float b1 = 0;
|
||||
float b2 = -alpha;
|
||||
float a0 = 1 + alpha;
|
||||
float a1 = -2 * c;
|
||||
float a2 = 1 - alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_lowShelf_f32(float *coeffs, float f, float gain, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float A = sqrtf(pow(10, (double)gain / 20.0));
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = A * ((A + 1) - (A - 1) * c + 2 * sqrtf(A) * alpha);
|
||||
float b1 = 2 * A * ((A - 1) - (A + 1) * c);
|
||||
float b2 = A * ((A + 1) - (A - 1) * c - 2 * sqrtf(A) * alpha);
|
||||
float a0 = (A + 1) + (A - 1) * c + 2 * sqrtf(A) * alpha;
|
||||
float a1 = -2 * ((A - 1) + (A + 1) * c);
|
||||
float a2 = (A + 1) + (A - 1) * c - 2 * sqrtf(A) * alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t dsps_biquad_gen_highShelf_f32(float *coeffs, float f, float gain, float qFactor)
|
||||
{
|
||||
if (qFactor <= 0.0001) {
|
||||
qFactor = 0.0001;
|
||||
}
|
||||
float Fs = 1;
|
||||
|
||||
float A = sqrtf(pow(10, (double)gain / 20.0));
|
||||
float w0 = 2 * M_PI * f / Fs;
|
||||
float c = cosf(w0);
|
||||
float s = sinf(w0);
|
||||
float alpha = s / (2 * qFactor);
|
||||
|
||||
float b0 = A * ((A + 1) + (A - 1) * c + 2 * sqrtf(A) * alpha);
|
||||
float b1 = -2 * A * ((A - 1) + (A + 1) * c);
|
||||
float b2 = A * ((A + 1) + (A - 1) * c - 2 * sqrtf(A) * alpha);
|
||||
float a0 = (A + 1) - (A - 1) * c + 2 * sqrtf(A) * alpha;
|
||||
float a1 = 2 * ((A - 1) - (A + 1) * c);
|
||||
float a2 = (A + 1) - (A - 1) * c - 2 * sqrtf(A) * alpha;
|
||||
|
||||
coeffs[0] = b0 / a0;
|
||||
coeffs[1] = b1 / a0;
|
||||
coeffs[2] = b2 / a0;
|
||||
coeffs[3] = a1 / a0;
|
||||
coeffs[4] = a2 / a0;
|
||||
return ESP_OK;
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
// Copyright 2018-2025 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "dsps_biquad_platform.h"
|
||||
#if (dsps_biquad_f32_ae32_enabled == 1)
|
||||
|
||||
// This is bi quad filter form II for ESP32 processor.
|
||||
.text
|
||||
.align 4
|
||||
.global dsps_biquad_sf32_ae32
|
||||
.type dsps_biquad_sf32_ae32,@function
|
||||
// The function implements the following C code:
|
||||
//esp_err_t dsps_biquad_f32_ae32(const float* input, float* output, int len, float* coef, float* w)
|
||||
// {
|
||||
// for (int i=0 ; i< len ; i++)
|
||||
// {
|
||||
// float d0 = input[i] - coef[3]*w[0] - coef[4]*w[1]; (input[i] - a[1]*w[0] - a[2]*w[1];)
|
||||
// output[i] = coef[0]*d0 + coef[1]*w[0] + coef[2]*w[1];
|
||||
// w[1] = w[0];
|
||||
// w[0] = d0;
|
||||
// }
|
||||
// return ESP_OK;
|
||||
// }
|
||||
|
||||
dsps_biquad_sf32_ae32:
|
||||
// input - a2
|
||||
// output - a3
|
||||
// len - a4
|
||||
// coeffs - a5
|
||||
// w- a6
|
||||
|
||||
// f0 - b0
|
||||
// f1 - b1
|
||||
// f2 - b2
|
||||
// f3 - a1
|
||||
// f4 - a2
|
||||
|
||||
// f5 - w0
|
||||
// f6 - w1
|
||||
|
||||
entry a1, 16
|
||||
// Array increment for floating point data should be 4
|
||||
lsi f0, a5, 0
|
||||
lsi f1, a5, 4
|
||||
lsi f2, a5, 8
|
||||
lsi f3, a5, 12
|
||||
lsi f4, a5, 16
|
||||
|
||||
|
||||
neg.s f5, f3 // -a[1]
|
||||
neg.s f6, f4 // -a[2]
|
||||
|
||||
lsi f7, a6, 0 // w[0]
|
||||
lsi f8, a6, 4 // w[1]
|
||||
lsi f11, a6, 8 // w[0]
|
||||
lsi f12, a6, 12 // w[1]
|
||||
|
||||
//addi a3, a3, -4 // i-- // preset a3
|
||||
lsip f9, a2, 4 // f9 = x[i]
|
||||
loopnez a4, .loop_bq_end_m_ae32
|
||||
madd.s f9, f7, f5 // f9 += -a1*w0
|
||||
mul.s f10, f1, f7 // f10 = b1*w0
|
||||
madd.s f9, f8, f6 // f9 += -a2*w1
|
||||
madd.s f10, f9, f0 // f10 += b0*d0
|
||||
madd.s f10, f2, f8 // f10+= b2*w1, f10 - result
|
||||
mov.s f8, f7 // w1 = w0
|
||||
mov.s f7, f9 // w0 = d0
|
||||
lsip f9, a2, 4 // f9 = x[i]
|
||||
ssip f10, a3, 4 // y[i] = result
|
||||
|
||||
|
||||
madd.s f9, f11, f5 // f9 += -a1*w0
|
||||
mul.s f10, f1, f11 // f10 = b1*w0
|
||||
madd.s f9, f12, f6 // f9 += -a2*w1
|
||||
madd.s f10, f9, f0 // f10 += b0*d0
|
||||
madd.s f10, f2, f12 // f10+= b2*w1, f10 - result
|
||||
mov.s f12, f11 // w1 = w0
|
||||
mov.s f11, f9 // w0 = d0
|
||||
lsip f9, a2, 4 // f9 = x[i]
|
||||
ssip f10, a3, 4 // y[i] = result
|
||||
.loop_bq_end_m_ae32:
|
||||
// Store delay line
|
||||
ssi f7, a6, 0
|
||||
ssi f8, a6, 4
|
||||
ssi f11, a6, 8
|
||||
ssi f12, a6, 12
|
||||
|
||||
movi.n a2, 0 // return status ESP_OK
|
||||
retw.n
|
||||
|
||||
#endif // dsps_biquad_f32_ae32_enabled
|
||||
@@ -0,0 +1,33 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#include "dsps_biquad.h"
|
||||
|
||||
|
||||
esp_err_t dsps_biquad_sf32_ansi(const float *input, float *output, int len, float *coef, float *w)
|
||||
{
|
||||
for (int i = 0 ; i < len ; i++) {
|
||||
float d0 = input[i * 2 + 0] - coef[3] * w[0] - coef[4] * w[1];
|
||||
output[i * 2 + 0] = coef[0] * d0 + coef[1] * w[0] + coef[2] * w[1];
|
||||
w[1] = w[0];
|
||||
w[0] = d0;
|
||||
|
||||
d0 = input[i * 2 + 1] - coef[3] * w[2] - coef[4] * w[3];
|
||||
output[i * 2 + 1] = coef[0] * d0 + coef[1] * w[2] + coef[2] * w[3];
|
||||
w[3] = w[2];
|
||||
w[2] = d0;
|
||||
}
|
||||
return ESP_OK;
|
||||
}
|
||||
@@ -0,0 +1,116 @@
|
||||
// Copyright 2018-2025 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include "dsps_biquad_platform.h"
|
||||
#if (dsps_biquad_f32_arp4_enabled == 1)
|
||||
|
||||
// This is bi quad filter form II for ESP32 processor.
|
||||
.text
|
||||
.align 4
|
||||
.global dsps_biquad_sf32_arp4
|
||||
.type dsps_biquad_sf32_arp4,@function
|
||||
// The function implements the following C code:
|
||||
//esp_err_t dsps_biquad_f32_arp4(const float* input, float* output, int len, float* coef, float* w)
|
||||
// {
|
||||
// for (int i = 0 ; i < len ; i++) {
|
||||
// float d0 = input[i*2 + 0] - coef[3] * w[0] - coef[4] * w[1];
|
||||
// output[i*2 + 0] = coef[0] * d0 + coef[1] * w[0] + coef[2] * w[1];
|
||||
// w[1] = w[0];
|
||||
// w[0] = d0;
|
||||
//
|
||||
// d0 = input[i*2 + 1] - coef[3] * w[2] - coef[4] * w[3];
|
||||
// output[i*2 + 1] = coef[0] * d0 + coef[1] * w[2] + coef[2] * w[3];
|
||||
// w[3] = w[2];
|
||||
// w[2] = d0;
|
||||
// }
|
||||
// return ESP_OK;
|
||||
// }
|
||||
|
||||
dsps_biquad_sf32_arp4:
|
||||
// input - a0
|
||||
// output - a1
|
||||
// len - a2
|
||||
// coeffs - a3
|
||||
// w- a4
|
||||
|
||||
// fa0 - b0
|
||||
// fa1 - b1
|
||||
// fa2 - b2
|
||||
// fa3 - a1
|
||||
// fa4 - a2
|
||||
|
||||
// fa5 - w0
|
||||
// fa6 - w1
|
||||
|
||||
add sp,sp,-16
|
||||
|
||||
flw fa0, 0(a3) // coeff[0] : b0
|
||||
flw fa1, 4(a3) // coeff[1] : b1
|
||||
flw fa2, 8(a3) // coeff[2] : b2
|
||||
flw fa3, 12(a3) // coeff[3] : a1
|
||||
flw fa4, 16(a3) // coeff[4] : a2
|
||||
|
||||
fneg.S fa5, fa3 // -a[1]
|
||||
fneg.S fa6, fa4 // -a[2]
|
||||
|
||||
flw ft0, 0(a4) // ft0 - f7 w0
|
||||
flw ft1, 4(a4) // ft1 - f8 w1
|
||||
flw ft5, 8(a4) // ft0 - f12 w2
|
||||
flw ft6, 12(a4) // ft1 - f13 w3
|
||||
|
||||
flw ft2, 0(a0) // ft2 - f9 = x[i]
|
||||
esp.lp.setup 0, a2, .iir_loop_end // label to the last executed instruction
|
||||
fmadd.S ft2, ft0, fa5, ft2 // ft2 = x[i] - a1*w0
|
||||
|
||||
fmul.s ft3, fa1, ft0 // ft3 = w0*b1
|
||||
fmadd.s ft2, ft1, fa6, ft2 // ft2 += -a2*w1 = d0
|
||||
|
||||
fmadd.s ft3, ft2, fa0, ft3 // f10 += b0*d0
|
||||
addi a0, a0, 4 // in++;
|
||||
fmadd.s ft3, fa2, ft1, ft3 // f10+= b2*w1, f10 - result
|
||||
|
||||
fmv.s ft1, ft0 // w1 = w0
|
||||
fmv.s ft0, ft2 // w0 = d0
|
||||
|
||||
flw ft2, 0(a0)
|
||||
fsw ft3, 0(a1)
|
||||
addi a1, a1, 4 // out++;
|
||||
|
||||
fmadd.S ft2, ft5, fa5, ft2 // ft2 = x[i] - a1*w0
|
||||
fmul.s ft3, fa1, ft5 // ft3 = w0*b1
|
||||
fmadd.s ft2, ft6, fa6, ft2 // ft2 += -a2*w1 = d0
|
||||
|
||||
|
||||
fmadd.s ft3, ft2, fa0, ft3 // f10 += b0*d0
|
||||
addi a0, a0, 4 // in++;
|
||||
fmadd.s ft3, fa2, ft6, ft3 // f10+= b2*w1, f10 - result
|
||||
|
||||
fmv.s ft6, ft5 // w1 = w0
|
||||
fmv.s ft5, ft2 // w0 = d0
|
||||
|
||||
flw ft2, 0(a0)
|
||||
fsw ft3, 0(a1)
|
||||
addi a1, a1, 4 // out++;
|
||||
|
||||
.iir_loop_end: nop
|
||||
fsw ft0, 0(a4) // ft0 - f7
|
||||
fsw ft1, 4(a4) // ft1 - f8
|
||||
fsw ft5, 8(a4) // ft5 - f12
|
||||
fsw ft6, 12(a4) // ft6 - f13
|
||||
|
||||
mv a0, a6
|
||||
add sp,sp,16
|
||||
ret
|
||||
|
||||
#endif // dsps_biquad_f32_aes3_enabled
|
||||
@@ -0,0 +1,105 @@
|
||||
// Copyright 2018-2025 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
|
||||
#ifndef _dsps_biquad_H_
|
||||
#define _dsps_biquad_H_
|
||||
|
||||
#include "dsp_err.h"
|
||||
|
||||
#include "dsps_biquad_platform.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
/**@{*/
|
||||
/**
|
||||
* @brief IIR filter
|
||||
*
|
||||
* IIR filter 2nd order direct form II (bi quad)
|
||||
* The extension (_ansi) use ANSI C and could be compiled and run on any platform.
|
||||
* The extension (_ae32) is optimized for ESP32 chip.
|
||||
*
|
||||
* @param[in] input: input array
|
||||
* @param output: output array
|
||||
* @param len: length of input and output vectors
|
||||
* @param coef: array of coefficients. b0,b1,b2,a1,a2
|
||||
* expected that a0 = 1. b0..b2 - numerator, a0..a2 - denominator
|
||||
* @param w: delay line w0,w1. Length of 2.
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_f32_ansi(const float *input, float *output, int len, float *coef, float *w);
|
||||
esp_err_t dsps_biquad_f32_ae32(const float *input, float *output, int len, float *coef, float *w);
|
||||
esp_err_t dsps_biquad_f32_aes3(const float *input, float *output, int len, float *coef, float *w);
|
||||
esp_err_t dsps_biquad_f32_arp4(const float *input, float *output, int len, float *coef, float *w);
|
||||
/**@}*/
|
||||
|
||||
/**@{*/
|
||||
/**
|
||||
* @brief IIR filter for stereo data
|
||||
*
|
||||
* IIR filter 2nd order direct form II (bi quad)
|
||||
* The extension (_ansi) use ANSI C and could be compiled and run on any platform.
|
||||
* The extension (_ae32) is optimized for ESP32 chip.
|
||||
*
|
||||
* @param[in] input: input array of two channels: L/R/L/R/L/R
|
||||
* @param output: output array of two channels: L/R/L/R/L/R
|
||||
* @param len: number of samples in one channel
|
||||
* @param coef: array of coefficients. b0,b1,b2,a1,a2
|
||||
* expected that a0 = 1. b0..b2 - numerator, a0..a2 - denominator
|
||||
* @param w: delay line w0,w1,w2,w3. Length of 4. w0,w1 - channel0, w2,w3 - channel1
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_sf32_ansi(const float *input, float *output, int len, float *coef, float *w);
|
||||
esp_err_t dsps_biquad_sf32_ae32(const float *input, float *output, int len, float *coef, float *w);
|
||||
esp_err_t dsps_biquad_sf32_aes3(const float *input, float *output, int len, float *coef, float *w);
|
||||
esp_err_t dsps_biquad_sf32_arp4(const float *input, float *output, int len, float *coef, float *w);
|
||||
/**@}*/
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#if CONFIG_DSP_OPTIMIZED
|
||||
|
||||
#if (dsps_biquad_f32_ae32_enabled == 1)
|
||||
#define dsps_biquad_f32 dsps_biquad_f32_ae32
|
||||
#define dsps_biquad_sf32 dsps_biquad_sf32_ae32
|
||||
#elif (dsps_biquad_f32_aes3_enabled == 1)
|
||||
#define dsps_biquad_f32 dsps_biquad_f32_aes3
|
||||
#define dsps_biquad_sf32 dsps_biquad_sf32_ae32
|
||||
#elif (dsps_biquad_f32_arp4_enabled == 1)
|
||||
#define dsps_biquad_f32 dsps_biquad_f32_arp4
|
||||
#define dsps_biquad_sf32 dsps_biquad_sf32_arp4
|
||||
#else
|
||||
#define dsps_biquad_f32 dsps_biquad_f32_ansi
|
||||
#define dsps_biquad_sf32 dsps_biquad_sf32_ansi
|
||||
#endif
|
||||
|
||||
#else // CONFIG_DSP_OPTIMIZED
|
||||
|
||||
#define dsps_biquad_f32 dsps_biquad_f32_ansi
|
||||
#define dsps_biquad_sf32 dsps_biquad_sf32_ansi
|
||||
|
||||
#endif // CONFIG_DSP_OPTIMIZED
|
||||
|
||||
|
||||
#endif // _dsps_biquad_H_
|
||||
@@ -0,0 +1,200 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#ifndef _dsps_biquad_gen_H_
|
||||
#define _dsps_biquad_gen_H_
|
||||
|
||||
#include "dsp_err.h"
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C"
|
||||
{
|
||||
#endif
|
||||
|
||||
// Common rules for all generated coefficients.
|
||||
// The coefficients placed to the array as follows:
|
||||
// coeffs[0] = b0;
|
||||
// coeffs[1] = b1;
|
||||
// coeffs[2] = b2;
|
||||
// coeffs[3] = a1;
|
||||
// coeffs[4] = a2;
|
||||
// a0 - are not placed and expected always as == 1
|
||||
|
||||
/**
|
||||
* @brief LPF IIR filter coefficients
|
||||
* Coefficients for low pass 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter cut off frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_lpf_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief HPF IIR filter coefficients
|
||||
*
|
||||
* Coefficients for high pass 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter cut off frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_hpf_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief BPF IIR filter coefficients
|
||||
*
|
||||
* Coefficients for band pass 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter center frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_bpf_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief 0 dB BPF IIR filter coefficients
|
||||
*
|
||||
* Coefficients for band pass 2nd order IIR filter (bi-quad) with 0 dB gain in passband
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter center frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_bpf0db_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief Notch IIR filter coefficients
|
||||
*
|
||||
* Coefficients for notch 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter notch frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param gain: gain in stopband in dB
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_notch_f32(float *coeffs, float f, float gain, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief Allpass 360 degree IIR filter coefficients
|
||||
*
|
||||
* Coefficients for all pass 2nd order IIR filter (bi-quad) with 360 degree phase shift
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter notch frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_allpass360_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief Allpass 180 degree IIR filter coefficients
|
||||
*
|
||||
* Coefficients for all pass 2nd order IIR filter (bi-quad) with 180 degree phase shift
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter notch frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_allpass180_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief peak IIR filter coefficients
|
||||
*
|
||||
* Coefficients for peak 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter notch frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_peakingEQ_f32(float *coeffs, float f, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief low shelf IIR filter coefficients
|
||||
*
|
||||
* Coefficients for low pass Shelf 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter notch frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param gain: gain in stopband in dB
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_lowShelf_f32(float *coeffs, float f, float gain, float qFactor);
|
||||
|
||||
/**
|
||||
* @brief high shelf IIR filter coefficients
|
||||
*
|
||||
* Coefficients for high pass Shelf 2nd order IIR filter (bi-quad)
|
||||
* The implementation use ANSI C and could be compiled and run on any platform
|
||||
*
|
||||
* @param coeffs: result coefficients. b0,b1,b2,a1,a2, a0 are not placed to the array and expected by IIR as 1
|
||||
* @param f: filter notch frequency in range of 0..0.5 (normalized to sample frequency)
|
||||
* @param gain: gain in stopband in dB
|
||||
* @param qFactor: Q factor of filter
|
||||
*
|
||||
* @return
|
||||
* - ESP_OK on success
|
||||
* - One of the error codes from DSP library
|
||||
*/
|
||||
esp_err_t dsps_biquad_gen_highShelf_f32(float *coeffs, float f, float gain, float qFactor);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif // _dsps_biquad_gen_H_
|
||||
@@ -0,0 +1,34 @@
|
||||
#ifndef _dsps_biquad_platform_H_
|
||||
#define _dsps_biquad_platform_H_
|
||||
|
||||
#include "sdkconfig.h"
|
||||
|
||||
#ifdef __XTENSA__
|
||||
#include <xtensa/config/core-isa.h>
|
||||
#include <xtensa/config/core-matmap.h>
|
||||
|
||||
#if ((XCHAL_HAVE_FP == 1) && (XCHAL_HAVE_LOOPS == 1))
|
||||
|
||||
#define dsps_biquad_f32_ae32_enabled 1
|
||||
|
||||
#endif
|
||||
|
||||
#if CONFIG_IDF_TARGET_ESP32S3
|
||||
#define dsps_biquad_f32_aes3_enabled 1
|
||||
#else
|
||||
#define dsps_biquad_f32_aes3_enabled 0
|
||||
#endif
|
||||
|
||||
#endif // __XTENSA__
|
||||
|
||||
#ifdef CONFIG_IDF_TARGET_ESP32P4
|
||||
#ifdef CONFIG_DSP_OPTIMIZED
|
||||
#define dsps_biquad_f32_arp4_enabled 1
|
||||
#else
|
||||
#define dsps_biquad_f32_arp4_enabled 0
|
||||
#endif // CONFIG_DSP_OPTIMIZED
|
||||
#else
|
||||
#define dsps_biquad_f32_arp4_enabled 0
|
||||
#endif
|
||||
|
||||
#endif // _dsps_biquad_platform_H_
|
||||
@@ -0,0 +1,103 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string.h>
|
||||
#include "unity.h"
|
||||
#include "dsp_platform.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
#include "dsp_tests.h"
|
||||
#include "dsps_tone_gen.h"
|
||||
#include "dsps_d_gen.h"
|
||||
#include "dsps_biquad_gen.h"
|
||||
#include "dsps_biquad.h"
|
||||
|
||||
static const char *TAG = "dsps_biquad_f32";
|
||||
static const int bq_len = 1024;
|
||||
TEST_CASE("dsps_biquad_f32 functionality", "[dsps]")
|
||||
{
|
||||
float *x = calloc(bq_len, sizeof(float));
|
||||
float *y = calloc(bq_len, sizeof(float));
|
||||
float *z = calloc(bq_len, sizeof(float));
|
||||
|
||||
// In the test we generate filter with cutt off frequency 0.1
|
||||
// and then filtering 0.1 and 0.3 frequencis.
|
||||
// Result must be better then 24 dB
|
||||
int len = bq_len;
|
||||
|
||||
dsps_d_gen_f32(x, len, 0);
|
||||
float coeffs[5];
|
||||
float w1[2] = {0};
|
||||
float w2[2] = {0};
|
||||
dsps_biquad_gen_lpf_f32(coeffs, 0.1, 1);
|
||||
dsps_biquad_f32(x, y, len, coeffs, w1);
|
||||
dsps_biquad_f32_ansi(x, z, len, coeffs, w2);
|
||||
|
||||
for (int i = 0 ; i < 32 ; i++) {
|
||||
if (y[i] != z[i]) {
|
||||
ESP_LOGI(TAG, "[%i]calc = %f, expected=%f", i, y[i], z[i]);
|
||||
TEST_ASSERT_EQUAL( y[i] * 100000, z[i] * 100000);
|
||||
}
|
||||
}
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
}
|
||||
|
||||
TEST_CASE("dsps_biquad_f32 benchmark", "[dsps]")
|
||||
{
|
||||
float *x = calloc(bq_len, sizeof(float));
|
||||
float *y = calloc(bq_len, sizeof(float));
|
||||
float *z = calloc(bq_len, sizeof(float));
|
||||
|
||||
float w1[2] = {0};
|
||||
int len = bq_len;
|
||||
int repeat_count = 1024;
|
||||
dsps_d_gen_f32(x, len, 0);
|
||||
float coeffs[5];
|
||||
dsps_biquad_gen_lpf_f32(coeffs, 0.1, 1);
|
||||
|
||||
|
||||
unsigned int start_b = dsp_get_cpu_cycle_count();
|
||||
for (int i = 0 ; i < repeat_count ; i++) {
|
||||
dsps_biquad_f32(x, y, len, coeffs, w1);
|
||||
}
|
||||
unsigned int end_b = dsp_get_cpu_cycle_count();
|
||||
|
||||
float total_b = end_b - start_b;
|
||||
float cycles = total_b / (len * repeat_count);
|
||||
|
||||
start_b = dsp_get_cpu_cycle_count();
|
||||
for (int i = 0 ; i < repeat_count ; i++) {
|
||||
dsps_biquad_f32_ansi(x, y, len, coeffs, w1);
|
||||
}
|
||||
end_b = dsp_get_cpu_cycle_count();
|
||||
float total_b_ansi = end_b - start_b;
|
||||
float cycles_ansi = total_b_ansi / (len * repeat_count);
|
||||
|
||||
ESP_LOGI(TAG, "dsps_biquad_f32 - %f per sample\n", cycles);
|
||||
ESP_LOGI(TAG, "dsps_biquad_f32_ansi - %f per sample\n", cycles_ansi);
|
||||
// float min_exec = 10;
|
||||
// float max_exec = 20;
|
||||
// if (cycles >= max_exec) {
|
||||
// TEST_ASSERT_MESSAGE (false, "Exec time takes more than expected!");
|
||||
// }
|
||||
// if (cycles < min_exec) {
|
||||
// TEST_ASSERT_MESSAGE (false, "Exec time takes less then expected!");
|
||||
// }
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,63 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string.h>
|
||||
#include "unity.h"
|
||||
#include "dsp_platform.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
#include "dsps_tone_gen.h"
|
||||
#include "dsps_d_gen.h"
|
||||
#include "dsps_biquad_gen.h"
|
||||
#include "dsps_biquad.h"
|
||||
|
||||
static const char *TAG = "dsps_biquad_f32_ansi";
|
||||
|
||||
float x[1024];
|
||||
float y[1024];
|
||||
|
||||
TEST_CASE("dsps_biquad_f32_ansi functionality", "[dsps]")
|
||||
{
|
||||
// In the test we generate filter with cutt off frequency 0.1
|
||||
// and then filtering 0.1 and 0.3 frequencis.
|
||||
// Result must be better then 24 dB
|
||||
int len = sizeof(x) / sizeof(float);
|
||||
|
||||
dsps_tone_gen_f32(x, len, 1, 0.1, 0);
|
||||
|
||||
float coeffs[5];
|
||||
float w1[2] = {0};
|
||||
float w2[2] = {0};
|
||||
dsps_biquad_gen_lpf_f32(coeffs, 0.1, 1);
|
||||
dsps_biquad_f32_ansi(x, y, len, coeffs, w1);
|
||||
float pow_band = 0;
|
||||
for (int i = len / 2 ; i < len ; i++) {
|
||||
pow_band += y[i] * y[i];
|
||||
}
|
||||
float pow_out_band = 0;
|
||||
dsps_tone_gen_f32(x, len, 1, 0.3, 0);
|
||||
dsps_biquad_f32_ansi(x, y, len, coeffs, w2);
|
||||
for (int i = len / 2 ; i < len ; i++) {
|
||||
pow_out_band += y[i] * y[i];
|
||||
}
|
||||
pow_band = 2 * pow_band / (float)len;
|
||||
pow_out_band = 2 * pow_out_band / (float)len;
|
||||
float diff_db = -10 * log10f(0.000000001 + pow_out_band / pow_band);
|
||||
ESP_LOGI(TAG, "Power: pass =%f, stop= %f, diff = %f dB", pow_band, pow_out_band, diff_db);
|
||||
|
||||
if (diff_db < 24) {
|
||||
ESP_LOGE(TAG, "Attenuation for LPF must be not less then 24! Now it is: %f", diff_db);
|
||||
TEST_ASSERT_MESSAGE (false, "LPF attenuation is less then expected");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string.h>
|
||||
#include "unity.h"
|
||||
#include "dsp_platform.h"
|
||||
#include "esp_log.h"
|
||||
|
||||
#include "dsp_tests.h"
|
||||
#include "dsps_tone_gen.h"
|
||||
#include "dsps_d_gen.h"
|
||||
#include "dsps_biquad_gen.h"
|
||||
#include "dsps_biquad.h"
|
||||
|
||||
static const char *TAG = "dsps_biquad_sf32";
|
||||
static const int bq_len = 1024;
|
||||
TEST_CASE("dsps_biquad_sf32 functionality", "[dsps]")
|
||||
{
|
||||
float *x = calloc(bq_len * 2, sizeof(float));
|
||||
float *y = calloc(bq_len * 2, sizeof(float));
|
||||
float *z = calloc(bq_len * 2, sizeof(float));
|
||||
|
||||
// In the test we generate filter with cutt off frequency 0.1
|
||||
// and then filtering 0.1 and 0.3 frequencis.
|
||||
// Result must be better then 24 dB
|
||||
int len = bq_len;
|
||||
|
||||
dsps_d_gen_f32(x, len * 2, 0);
|
||||
x[1] = 1;
|
||||
float coeffs[5];
|
||||
float w1[4] = {0};
|
||||
float w2[4] = {0};
|
||||
dsps_biquad_gen_lpf_f32(coeffs, 0.1, 1);
|
||||
dsps_biquad_sf32(x, y, len, coeffs, w1);
|
||||
dsps_biquad_sf32_ansi(x, z, len, coeffs, w2);
|
||||
|
||||
for (int i = 0 ; i < 32 * 2 ; i++) {
|
||||
if (y[i] != z[i]) {
|
||||
ESP_LOGI(TAG, "[%i]calc = %f, expected=%f", i, y[i], z[i]);
|
||||
//TEST_ASSERT_EQUAL( y[i] * 100000, z[i] * 100000);
|
||||
}
|
||||
}
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
}
|
||||
|
||||
TEST_CASE("dsps_biquad_sf32 benchmark", "[dsps]")
|
||||
{
|
||||
float *x = calloc(bq_len * 2, sizeof(float));
|
||||
float *y = calloc(bq_len * 2, sizeof(float));
|
||||
float *z = calloc(bq_len * 2, sizeof(float));
|
||||
|
||||
float w1[4] = {0};
|
||||
int len = bq_len;
|
||||
int repeat_count = 1024;
|
||||
dsps_d_gen_f32(x, len * 2, 0);
|
||||
float coeffs[5];
|
||||
dsps_biquad_gen_lpf_f32(coeffs, 0.1, 1);
|
||||
|
||||
|
||||
unsigned int start_b = dsp_get_cpu_cycle_count();
|
||||
for (int i = 0 ; i < repeat_count ; i++) {
|
||||
dsps_biquad_sf32(x, y, len, coeffs, w1);
|
||||
}
|
||||
unsigned int end_b = dsp_get_cpu_cycle_count();
|
||||
|
||||
float total_b = end_b - start_b;
|
||||
float cycles = total_b / (len * repeat_count);
|
||||
|
||||
start_b = dsp_get_cpu_cycle_count();
|
||||
for (int i = 0 ; i < repeat_count ; i++) {
|
||||
dsps_biquad_sf32_ansi(x, y, len, coeffs, w1);
|
||||
}
|
||||
end_b = dsp_get_cpu_cycle_count();
|
||||
float total_b_ansi = end_b - start_b;
|
||||
float cycles_ansi = total_b_ansi / (len * repeat_count);
|
||||
|
||||
ESP_LOGI(TAG, "dsps_biquad_f32 - %f per sample\n", cycles);
|
||||
ESP_LOGI(TAG, "dsps_biquad_f32_ansi - %f per sample\n", cycles_ansi);
|
||||
// float min_exec = 10;
|
||||
// float max_exec = 20;
|
||||
// if (cycles >= max_exec) {
|
||||
// TEST_ASSERT_MESSAGE (false, "Exec time takes more than expected!");
|
||||
// }
|
||||
// if (cycles < min_exec) {
|
||||
// TEST_ASSERT_MESSAGE (false, "Exec time takes less then expected!");
|
||||
// }
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
// Copyright 2018-2019 Espressif Systems (Shanghai) PTE LTD
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#include <string.h>
|
||||
#include "unity.h"
|
||||
#include "dsp_platform.h"
|
||||
#include "esp_log.h"
|
||||
#include "dsp_tests.h"
|
||||
|
||||
#include "dsps_tone_gen.h"
|
||||
#include "dsps_d_gen.h"
|
||||
#include "dsps_biquad_gen.h"
|
||||
#include "dsps_biquad.h"
|
||||
#include <malloc.h>
|
||||
|
||||
static const char *TAG = "dsps_biquad_sf32_ansi";
|
||||
|
||||
TEST_CASE("dsps_biquad_sf32_ansi functionality", "[dsps]")
|
||||
{
|
||||
|
||||
float *x = (float *)memalign(16, sizeof(float) * 1024 * 2);
|
||||
float *y = (float *)memalign(16, sizeof(float) * 1024 * 2);
|
||||
|
||||
// In the test we generate filter with cutt off frequency 0.1
|
||||
// and then filtering 0.1 and 0.3 frequencis.
|
||||
// Result must be better then 24 dB
|
||||
int len = 1024;
|
||||
|
||||
dsps_tone_gen_f32(x, len * 2, 1, 0.1, 0);
|
||||
for (int i = len / 2 ; i < len ; i++) {
|
||||
y[i * 2 + 0] *= 10;
|
||||
y[i * 2 + 1] *= 0.1;
|
||||
}
|
||||
|
||||
float coeffs[5];
|
||||
float w1[4] = {0};
|
||||
float w2[4] = {0};
|
||||
dsps_biquad_gen_lpf_f32(coeffs, 0.1, 1);
|
||||
dsps_biquad_sf32_ansi(x, y, len, coeffs, w1);
|
||||
float pow_band0 = 0;
|
||||
float pow_band1 = 0;
|
||||
for (int i = len / 2 ; i < len ; i++) {
|
||||
pow_band0 += y[i * 2 + 0] * y[i * 2 + 0];
|
||||
pow_band1 += y[i * 2 + 1] * y[i * 2 + 1];
|
||||
}
|
||||
float pow_out_band0 = 0;
|
||||
float pow_out_band1 = 0;
|
||||
dsps_tone_gen_f32(x, len * 2, 1, 0.3, 0);
|
||||
for (int i = len / 2 ; i < len ; i++) {
|
||||
y[i * 2 + 0] *= 10;
|
||||
y[i * 2 + 1] *= 0.1;
|
||||
}
|
||||
dsps_biquad_sf32_ansi(x, y, len, coeffs, w2);
|
||||
for (int i = len / 2 ; i < len ; i++) {
|
||||
pow_out_band0 += y[i * 2 + 0] * y[i * 2 + 0];
|
||||
pow_out_band1 += y[i * 2 + 1] * y[i * 2 + 1];
|
||||
}
|
||||
pow_band0 = 2 * pow_band0 / (float)len;
|
||||
pow_band1 = 2 * pow_band1 / (float)len;
|
||||
pow_out_band0 = 2 * pow_out_band0 / (float)len;
|
||||
pow_out_band1 = 2 * pow_out_band1 / (float)len;
|
||||
float diff0_db = -10 * log10f(0.000000001 + pow_out_band0 / pow_band0);
|
||||
float diff1_db = -10 * log10f(0.000000001 + pow_out_band1 / pow_band1);
|
||||
ESP_LOGI(TAG, "Power0: pass =%f, stop= %f, diff = %f dB", pow_band0, pow_out_band0, diff0_db);
|
||||
ESP_LOGI(TAG, "Power1: pass =%f, stop= %f, diff = %f dB", pow_band1, pow_out_band1, diff1_db);
|
||||
|
||||
if (diff0_db < 24) {
|
||||
ESP_LOGE(TAG, "Attenuation for LPF must be not less then 24! Now it is: %f", diff0_db);
|
||||
TEST_ASSERT_MESSAGE (false, "LPF attenuation is less then expected");
|
||||
}
|
||||
if (diff1_db < 24) {
|
||||
ESP_LOGE(TAG, "Attenuation for HPF must be not less then 24! Now it is: %f", diff1_db);
|
||||
TEST_ASSERT_MESSAGE (false, "HPF attenuation is less then expected");
|
||||
}
|
||||
|
||||
|
||||
free(x);
|
||||
free(y);
|
||||
}
|
||||
@@ -0,0 +1,12 @@
|
||||
|
||||
void test_iir_biquad();
|
||||
|
||||
int main(void)
|
||||
{
|
||||
printf("main starts!\n");
|
||||
// xt_iss_profile_enable();
|
||||
test_iir_biquad();
|
||||
// xt_iss_profile_disable();
|
||||
|
||||
printf("Test done\n");
|
||||
}
|
||||
@@ -0,0 +1,56 @@
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "dsp_common.h"
|
||||
|
||||
#include "dsps_biquad.h"
|
||||
|
||||
#define N_SAMPLES 1024
|
||||
int N = N_SAMPLES;
|
||||
// Input test array
|
||||
float d[N_SAMPLES];
|
||||
// output array
|
||||
float y[N_SAMPLES];
|
||||
float y_ref[N_SAMPLES];
|
||||
|
||||
extern void xt_iss_profile_disable();
|
||||
extern void xt_iss_profile_enable();
|
||||
esp_err_t dsps_biquad_f32_aes3(const float *input, float *output, int len, float *coef, float *w);
|
||||
|
||||
void test_iir_biquad()
|
||||
{
|
||||
float coeffs_lpf[5] = {0.073802, 0.147603, 0.073802, -1.250516, 0.545723};
|
||||
float w_lpf[5] = {0, 0};
|
||||
float w_lpf_ref[5] = {0, 0};
|
||||
esp_err_t ret = ESP_OK;
|
||||
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
d[i] = 0;
|
||||
}
|
||||
d[0] = 1;
|
||||
xt_iss_profile_enable();
|
||||
ret = dsps_biquad_f32_ansi(d, y_ref, N, coeffs_lpf, w_lpf_ref);
|
||||
xt_iss_profile_disable();
|
||||
if (ret != ESP_OK) {
|
||||
printf("dsps_biquad_f32 error = %i\n", ret);
|
||||
return;
|
||||
}
|
||||
xt_iss_profile_enable();
|
||||
ret = dsps_biquad_f32_aes3(d, y, N, coeffs_lpf, w_lpf);
|
||||
xt_iss_profile_disable();
|
||||
if (ret != ESP_OK) {
|
||||
printf("dsps_biquad_f32 error = %i\n", ret);
|
||||
return;
|
||||
}
|
||||
for (size_t i = 0; i < N; i++) {
|
||||
if (((y[i] - y_ref[i]) > 0.0000001) || (y[i] - y_ref[i]) < -0.0000001) {
|
||||
printf("ERROR result[%i]: %f, expect = %f, diff=%f\n", i, y[i], y_ref[i], y[i] - y_ref[i]);
|
||||
return;
|
||||
|
||||
}
|
||||
}
|
||||
printf("Test Correct!\n");
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user