291 lines
6.8 KiB
C
291 lines
6.8 KiB
C
// 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;
|
|
}
|