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
|
||||
Reference in New Issue
Block a user