Steven/3d_audio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

remove some eq type

Browse Source
This commit is contained in:
Steven Dan
2026-03-20 22:25:39 +08:00
parent 3f89011703
commit fd163f1537
6 changed files with 76 additions and 845 deletions
Download Patch File Download Diff File Expand all files Collapse all files

285
sw_usb_audio/app_usb_aud_phaten_golden/src/extensions/biquad_standalone.c
View File

@@ -39,11 +39,6 @@ static inline float biquad_cos(float x) {
return cosf(x);
}
// 双曲正弦函数 - 重命名以避免冲突
static inline float biquad_sinhf(float x) {
return (expf(x) - expf(-x)) / 2.0f;
}
static inline float _check_fc(float fc, float fs) {
float fc_sat = fc;
// saturate if > fs/2
@@ -131,109 +126,6 @@ void eq_biquad_highpass(q2_30 coeffs[5], const float fc, const float fs, const f
float a1 = 2.0f * (KK - 1.0f) * norm;
float a2 = (1.0f - KQ + KK) * norm;
//DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0, Q_factor);
coeffs[1] = _float2fixed_assert(b1, Q_factor);
coeffs[2] = _float2fixed_assert(b2, Q_factor);
coeffs[3] = _float2fixed_assert(-a1, Q_factor);
coeffs[4] = _float2fixed_assert(-a2, Q_factor);
}
void eq_biquad_bandpass(q2_30 coeffs[5], const float fc, const float fs, const float bandwidth) {
float fc_sat = _check_fc(fc, fs);
// 使用lib_audio_dsp中biquad.c的算法，但添加参数保护
float w0 = 2.0f * pi * fc_sat / fs;
float sin_w0 = biquad_sin(w0);
// 限制bandwidth参数范围，避免sinh溢出
float bw_sat = bandwidth;
if (bw_sat > 100.0f) bw_sat = 100.0f; // 限制最大bandwidth
if (bw_sat < 0.01f) bw_sat = 0.01f; // 限制最小bandwidth
float sinh_arg = log_2 / 2.0f * bw_sat * w0 / sin_w0;
// 限制sinh参数范围，避免溢出
if (sinh_arg > 10.0f) sinh_arg = 10.0f;
if (sinh_arg < -10.0f) sinh_arg = -10.0f;
float alpha = sin_w0 * biquad_sinhf(sinh_arg);
// Compute coeffs - 与lib_audio_dsp完全一致
float b0 = alpha;
float b1 = 0.0f;
float b2 = -alpha;
float a0 = 1.0f + alpha;
float a1 = -2.0f * biquad_cos(w0);
float a2 = 1.0f - alpha;
float inv_a0 = 1.0f / a0;
//DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0 * inv_a0, b1 * inv_a0, b2 * inv_a0, a1 * inv_a0, a2 * inv_a0);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0 * inv_a0, Q_factor);
coeffs[1] = _float2fixed_assert(b1 * inv_a0, Q_factor);
coeffs[2] = _float2fixed_assert(b2 * inv_a0, Q_factor);
coeffs[3] = _float2fixed_assert(-a1 * inv_a0, Q_factor);
coeffs[4] = _float2fixed_assert(-a2 * inv_a0, Q_factor);
}
void eq_biquad_bandstop(q2_30 coeffs[5], const float fc, const float fs, const float bandwidth) {
float fc_sat = _check_fc(fc, fs);
// 使用lib_audio_dsp中biquad.c的算法，但添加参数保护
float w0 = 2.0f * pi * fc_sat / fs;
float sin_w0 = biquad_sin(w0);
// 限制bandwidth参数范围，避免sinh溢出
float bw_sat = bandwidth;
if (bw_sat > 100.0f) bw_sat = 100.0f; // 限制最大bandwidth
if (bw_sat < 0.01f) bw_sat = 0.01f; // 限制最小bandwidth
float sinh_arg = log_2 / 2.0f * bw_sat * w0 / sin_w0;
// 限制sinh参数范围，避免溢出
if (sinh_arg > 10.0f) sinh_arg = 10.0f;
if (sinh_arg < -10.0f) sinh_arg = -10.0f;
float alpha = sin_w0 * biquad_sinhf(sinh_arg);
// Compute coeffs - 与lib_audio_dsp完全一致
float b0 = 1.0f;
float b1 = -2.0f * biquad_cos(w0);
float b2 = 1.0f;
float a0 = 1.0f + alpha;
float a1 = b1;
float a2 = 1.0f - alpha;
float inv_a0 = 1.0f / a0;
//DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0 * inv_a0, b1 * inv_a0, b2 * inv_a0, a1 * inv_a0, a2 * inv_a0);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0 * inv_a0, Q_factor);
coeffs[1] = _float2fixed_assert(b1 * inv_a0, Q_factor);
coeffs[2] = _float2fixed_assert(b2 * inv_a0, Q_factor);
coeffs[3] = _float2fixed_assert(-a1 * inv_a0, Q_factor);
coeffs[4] = _float2fixed_assert(-a2 * inv_a0, Q_factor);
}
void eq_biquad_notch(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q) {
float fc_sat = _check_fc(fc, fs);
// 使用与Python端BiquadFilterCalculator.calculate("notch")完全一致的算法
float K = tanf(pi * fc_sat / fs); // 与Python端一致：K = tan(π * Fc / Fs)
float norm = 1.0f / (1.0f + K / filter_Q + K * K); // 与Python端一致
// Compute coeffs - 与Python端完全一致
float b0 = (1.0f + K * K) * norm;
float b1 = 2.0f * (K * K - 1.0f) * norm;
float b2 = b0;
float a1 = b1;
float a2 = (1.0f - K / filter_Q + K * K) * norm;
//DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0, Q_factor);
coeffs[1] = _float2fixed_assert(b1, Q_factor);
@@ -245,21 +137,15 @@ void eq_biquad_notch(q2_30 coeffs[5], const float fc, const float fs, const floa
void eq_biquad_allpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q) {
float fc_sat = _check_fc(fc, fs);
// 使用与Python端BiquadFilterCalculator.calculate("allpass")完全一致的算法
// 注意：Python端没有allpass类型，但我们可以使用类似的算法
float K = tanf(pi * fc_sat / fs); // 与Python端一致：K = tan(π * Fc / Fs)
float norm = 1.0f / (1.0f + K / filter_Q + K * K); // 与Python端一致
float K = tanf(pi * fc_sat / fs);
float norm = 1.0f / (1.0f + K / filter_Q + K * K);
// Compute coeffs - 全通滤波器的标准算法
float b0 = (1.0f - K / filter_Q + K * K) * norm;
float b1 = 2.0f * (K * K - 1.0f) * norm;
float b2 = 1.0f;
float a1 = b1;
float a2 = b0;
//DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0, Q_factor);
coeffs[1] = _float2fixed_assert(b1, Q_factor);
coeffs[2] = _float2fixed_assert(b2, Q_factor);
@@ -267,131 +153,53 @@ void eq_biquad_allpass(q2_30 coeffs[5], const float fc, const float fs, const fl
coeffs[4] = _float2fixed_assert(-a2, Q_factor);
}
// 动态计算bshift的函数，与Python端calculate_bshift算法一致
// 动态计算bshift的函数
static left_shift_t calculate_bshift(float b0, float b1, float b2) {
float max_b = fabsf(b0);
if (fabsf(b1) > max_b) max_b = fabsf(b1);
if (fabsf(b2) > max_b) max_b = fabsf(b2);
if (max_b == 0.0f) return 0;
// 计算bshift = floor(log2(max_b))
int bshift = 0;
if (max_b >= 1.0f) {
float temp = max_b;
while (temp >= 2.0f) {
temp /= 2.0f;
bshift++;
}
while (temp >= 2.0f) { temp /= 2.0f; bshift++; }
} else {
float temp = max_b;
while (temp < 1.0f) {
temp *= 2.0f;
while (temp < 1.0f) { temp *= 2.0f; bshift--; }
bshift--;
}
bshift--; // 调整到正确的值
}
// 确保bshift >= 0
return (bshift >= 0) ? bshift : 0;
}
left_shift_t eq_biquad_peaking(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db) {
float gain_db_sat = _check_gain(gain_db, 20.0f * db_2); // 增加最大增益限制
float fc_sat = _check_fc(fc, fs);
// 使用与Python端BiquadFilterCalculator.calculate("peak")完全一致的算法
float V = powf(10.0f, fabsf(gain_db_sat) / 20.0f); // 与Python端一致
float K = tanf(pi * fc_sat / fs); // 与Python端一致：K = tan(π * Fc / Fs)
float norm = 0.0f;
// 根据增益正负选择不同的计算方式，与Python端完全一致
if (gain_db_sat >= 0) {
norm = 1.0f / (1.0f + 1.0f/filter_Q * K + K * K);
float b0 = (1.0f + V/filter_Q * K + K * K) * norm;
float b1 = 2.0f * (K * K - 1.0f) * norm;
float b2 = (1.0f - V/filter_Q * K + K * K) * norm;
float a1 = b1; // 与Python端一致
float a2 = (1.0f - 1.0f/filter_Q * K + K * K) * norm;
// DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
// 动态计算bshift
left_shift_t bshift = calculate_bshift(b0, b1, b2);
//DPRINTF("Calculated bshift: %d\n", bshift);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
coeffs[3] = _float2fixed_assert(-a1, Q_factor);
coeffs[4] = _float2fixed_assert(-a2, Q_factor);
return bshift;
} else {
norm = 1.0f / (1.0f + V/filter_Q * K + K * K);
float b0 = (1.0f + 1.0f/filter_Q * K + K * K) * norm;
float b1 = 2.0f * (K * K - 1.0f) * norm;
float b2 = (1.0f - 1.0f/filter_Q * K + K * K) * norm;
float a1 = b1; // 与Python端一致
float a2 = (1.0f - V/filter_Q * K + K * K) * norm;
// DPRINTF("b0: %f \n b1: %f \n b2: %f \n a1: %f \n a2: %f\n", b0, b1, b2, a1, a2);
// 动态计算bshift
left_shift_t bshift = calculate_bshift(b0, b1, b2);
//DPRINTF("Calculated bshift: %d\n", bshift);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
coeffs[3] = _float2fixed_assert(-a1, Q_factor);
coeffs[4] = _float2fixed_assert(-a2, Q_factor);
return bshift;
}
}
left_shift_t eq_biquad_const_q(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db) {
float gain_db_sat = _check_gain(gain_db, 20.0f * db_2);
float fc_sat = _check_fc(fc, fs);
// Compute common factors
float V = powf(10.0f, (gain_db_sat * (1.0f / 20.0f)));
// w0 is only needed for calculating K
float V = powf(10.0f, fabsf(gain_db_sat) / 20.0f);
float K = tanf(pi * fc_sat / fs);
float norm, b0, b1, b2, a1, a2;
float factor_a = K / filter_Q;
float factor_b = 0;
float K_pow2 = K * K;
if (gain_db_sat > 0) {
factor_b = V * factor_a;
if (gain_db_sat >= 0) {
norm = 1.0f / (1.0f + 1.0f/filter_Q * K + K * K);
b0 = (1.0f + V/filter_Q * K + K * K) * norm;
b1 = 2.0f * (K * K - 1.0f) * norm;
b2 = (1.0f - V/filter_Q * K + K * K) * norm;
a1 = b1;
a2 = (1.0f - 1.0f/filter_Q * K + K * K) * norm;
} else {
factor_b = factor_a;
factor_a = factor_b / V;
norm = 1.0f / (1.0f + V/filter_Q * K + K * K);
b0 = (1.0f + 1.0f/filter_Q * K + K * K) * norm;
b1 = 2.0f * (K * K - 1.0f) * norm;
b2 = (1.0f - 1.0f/filter_Q * K + K * K) * norm;
a1 = b1;
a2 = (1.0f - V/filter_Q * K + K * K) * norm;
}
// Compute coeffs
float b0 = 1.0f + factor_b + K_pow2;
float b1 = 2.0f * (K_pow2 - 1.0f);
float b2 = 1.0f - factor_b + K_pow2;
float a0 = 1.0f + factor_a + K_pow2;
float a1 = b1;
float a2 = 1.0f - factor_a + K_pow2;
float inv_a0 = 1.0f / a0;
// 动态计算bshift
left_shift_t bshift = calculate_bshift(b0 * inv_a0, b1 * inv_a0, b2 * inv_a0);
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0 * inv_a0, Q_factor - bshift);
coeffs[1] = _float2fixed_assert(b1 * inv_a0, Q_factor - bshift);
coeffs[2] = _float2fixed_assert(b2 * inv_a0, Q_factor - bshift);
coeffs[3] = _float2fixed_assert(-a1 * inv_a0, Q_factor);
coeffs[4] = _float2fixed_assert(-a2 * inv_a0, Q_factor);
left_shift_t bshift = calculate_bshift(b0, b1, b2);
coeffs[0] = _float2fixed_assert(b0, Q_factor - bshift);
coeffs[1] = _float2fixed_assert(b1, Q_factor - bshift);
coeffs[2] = _float2fixed_assert(b2, Q_factor - bshift);
coeffs[3] = _float2fixed_assert(-a1, Q_factor);
coeffs[4] = _float2fixed_assert(-a2, Q_factor);
return bshift;
}
@@ -505,43 +313,4 @@ left_shift_t eq_biquad_highshelf(q2_30 coeffs[5], const float fc, const float fs
}
}
void eq_biquad_linkwitz(q2_30 coeffs[5], const float f0, const float fs, const float q0, const float fp, const float qp) {
float f0_sat = _check_fc(f0, fs);
float fp_sat = _check_fc(fp, fs);
// Compute common factors
float fc = (f0_sat + fp_sat) / 2.0f;
float w_f0 = 2.0f * pi * f0_sat;
float half_w_fc = pi * fc;
float w_fp = 2.0f * pi * fp_sat;
float w_f0_pow2 = w_f0 * w_f0;
float d1i = w_f0 / q0;
float w_fp_pow2 = w_fp * w_fp;
float c1i = w_fp / qp;
float gn = 2.0f * half_w_fc * (1.0f / (tanf(half_w_fc / fs)));
float gn_pow2 = gn * gn;
float factor_b = gn * d1i;
float factor_a = gn * c1i;
// Compute coeffs
float a0 = w_fp_pow2 + factor_a + gn_pow2;
float a1 = 2.0f * (w_fp_pow2 - gn_pow2);
float a2 = w_fp_pow2 - factor_a + gn_pow2;
float b0 = w_f0_pow2 + factor_b + gn_pow2;
float b1 = 2.0f * (w_f0_pow2 - gn_pow2);
float b2 = w_f0_pow2 - factor_b + gn_pow2;
float inv_a0 = 1.0f / a0;
// Store as fixed-point values
coeffs[0] = _float2fixed_assert(b0 * inv_a0, Q_factor);
coeffs[1] = _float2fixed_assert(b1 * inv_a0, Q_factor);
coeffs[2] = _float2fixed_assert(b2 * inv_a0, Q_factor);
coeffs[3] = _float2fixed_assert(-a1 * inv_a0, Q_factor);
coeffs[4] = _float2fixed_assert(-a2 * inv_a0, Q_factor);
}
/* Removed unused filters: bandpass, bandstop, notch, const_q, linkwitz */

6
sw_usb_audio/app_usb_aud_phaten_golden/src/extensions/biquad_standalone.h
View File

@@ -12,21 +12,15 @@ void eq_biquad_mute(q2_30 coeffs[5]);
left_shift_t eq_biquad_gain(q2_30 coeffs[5], const float gain_db);
void eq_biquad_lowpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q);
void eq_biquad_highpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q);
void eq_biquad_bandpass(q2_30 coeffs[5], const float fc, const float fs, const float bandwidth);
void eq_biquad_bandstop(q2_30 coeffs[5], const float fc, const float fs, const float bandwidth);
void eq_biquad_notch(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q);
void eq_biquad_allpass(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q);
left_shift_t eq_biquad_peaking(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db);
left_shift_t eq_biquad_const_q(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db);
left_shift_t eq_biquad_lowshelf(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db);
left_shift_t eq_biquad_highshelf(q2_30 coeffs[5], const float fc, const float fs, const float filter_Q, const float gain_db);
void eq_biquad_linkwitz(q2_30 coeffs[5], const float f0, const float fs, const float q0, const float fp, const float qp);
// 辅助函数声明 - 重命名以避免与系统库冲突
static inline int32_t _float2fixed(float x, int32_t q);
static inline int32_t _float2fixed_assert(float x, int32_t q);
static inline float biquad_sin(float x); // 重命名为 biquad_sin
static inline float biquad_cos(float x); // 重命名为 biquad_cos
static inline float biquad_sinhf(float x); // 重命名为 biquad_sinhf
#endif // BIQUAD_STANDALONE_H

11
sw_usb_audio/app_usb_aud_phaten_golden/src/extensions/dsp.c
View File

@@ -85,10 +85,6 @@ static unsigned frame_index = 0;
//static unsigned dnr_frame_idx = 0;
static int32_t ubm_egress[UBM_TO_EX3D_CHANS][DSP_BLOCK_LENGTH]; //leaving ubm
static int32_t ubm_ingress[EX3D_TO_UBM_CHANS][DSP_BLOCK_LENGTH]; //entering ubm
#if AIZIP_DNR == 1
static int32_t ubm_micIn[DSP_CH_NUM][DNR_DSP_FRAME_SIZE]; //leaving ubm
static int32_t ubm_micOut[DSP_CH_NUM][DNR_DSP_FRAME_SIZE]; //entering ubm
#endif
chanend_t uc_dsp_to_dnr_t1;
//chanend_t uc_dsp_to_dnr_t0;
chanend_t uc_key_to_ubm_t0;
@@ -272,16 +268,9 @@ void UserBufferManagement(unsigned sampsFromUsbToAudio[], unsigned sampsFromAudi
chan_out_buf_word(uc_ex3d_to_ubm, (const uint32_t *)ubm_egress, UBM_TO_EX3D_CHANS * DSP_BLOCK_LENGTH);
// And get a stereo frames worth of data back from the mixer process
chan_in_buf_word(uc_ex3d_to_ubm, (uint32_t *)ubm_ingress, EX3D_TO_UBM_CHANS * DSP_BLOCK_LENGTH);
#if AIZIP_DNR == 1
chan_out_buf_word(uc_dsp_to_dnr_t1, (const uint32_t *)(&ubm_micIn[0]), DSP_BLOCK_LENGTH);
chan_out_buf_word(uc_dsp_to_dnr_t1, (const uint32_t *)(&ubm_micIn[1]), DSP_BLOCK_LENGTH);
chan_in_buf_word(uc_dsp_to_dnr_t1, (uint32_t *)(&ubm_micOut[0]), DSP_BLOCK_LENGTH);
chan_in_buf_word(uc_dsp_to_dnr_t1, (uint32_t *)(&ubm_micOut[1]), DSP_BLOCK_LENGTH);
#endif
};
buffer_exchange(uc_eq_data, sampsFromUsbToAudio, sampsFromAudioToUsb);
// sampsFromAudioToUsb[0] = sampsFromAudioToUsb[1];
}
#ifndef HID_MAX_DATA_BYTES

407
sw_usb_audio/app_usb_aud_phaten_golden/src/extensions/eq.c
View File

@@ -74,40 +74,6 @@ static struct {
#if EQ_EN
#if 0
void print_float(float value, int precision)
{
// 处理负数
if (value < 0) {
DPRINTF('-');
value = -value;
}
// 整数部分
int32_t int_part = (int32_t)value;
DPRINTF("%d", int_part);
// 打印小数点
DPRINTF('.');
// 小数部分
float frac = value - (float)int_part;
for (int i = 0; i < precision; i++) {
frac *= 10.0f;
int digit = (int)frac;
DPRINTF("%d", digit);
frac -= digit;
}
}
#else
void print_float(float value, int precision)
{
}
#endif
unsigned int g_current_eq_mode = 0;
unsigned int g_request_eq_mode = 0;
unsigned int g_new_eq_mode = 9;
@@ -145,7 +111,7 @@ static struct {
int32_t post_gain_db;
} read_request = {0};
static const int32_t attenuation_lookup_table[255] = {
static const int32_t attenuation_lookup_table[75] = {
0x46CB697E, // 0 37.0
0x42D59AB2, // 1 36.5
0x3F18820D, // 2 36.0
@@ -223,7 +189,7 @@ static const int32_t attenuation_lookup_table[255] = {
0x01000000, // 74 0.0
};
static const int32_t attenuation_lookup_table_minus[255] = {
static const int32_t attenuation_lookup_table_minus[183] = {
0x01000000, // 74 0.0
0x00F1ADF9, // 75 -0.5
0x00E42905, // 76 -1.0
@@ -405,7 +371,6 @@ static const int32_t attenuation_lookup_table_minus[255] = {
0x00000253, // 252 -89.0
0x00000231, // 253 -89.5
0x00000212, // 254 -90.0
0x00000000 // 255 -90.5
};
static inline int get_gain_q24(int input, unsigned multipler){
@@ -569,19 +534,7 @@ int calculate_current_mode_coefficients(uint32_t sample_rate, uint8_t mode) {
coeffs[3] = 0;
coeffs[4] = 0;
calculated_bshift = 0;
} else {
debug_printf("Band %d: Type=%d, fc=%.2f, q=%.3f, gain=%.2f, bshift=%d\n",
band_index, band->type, band->fc, band->q, band->gain, calculated_bshift);
debug_printf("fc: ");
print_float(band->fc, 6);
debug_printf("q: ");
print_float(band->q, 6);
debug_printf("bw: ");
print_float(band->bw, 6);
debug_printf("gain: ");
print_float(band->gain, 6);
}
// 累加非bypass滤波器的bshift
total_bshift += calculated_bshift;
}
@@ -781,95 +734,6 @@ static void repair_eq_data(eq_mode_data_t *eq_data) {
}
}
void debug_print_eq_mode(eq_mode_data_t *eq_data) {
debug_printf("*******************************\n");
DPRINTF("Total bshift: 0x%d\n", eq_data->total_bshift);
DPRINTF("Post gain: 0x%d\n", eq_data->post_gain_db);
DPRINTF("Filter Parameters:\n");
for (int i = 0; i < MAX_EQ_BANDS; i++) {
filter_params_t *band = &eq_data->bands[i];
// 添加指针有效性检查
if (band == NULL) {
DPRINTF(" Band %d: ERROR - band pointer is NULL\n", i);
continue;
}
DPRINTF(" Band %d:\n", i);
DPRINTF(" Index: %d\n", band->index);
// Print filter type
DPRINTF(" Type: ");
switch(band->type) {
case FILTER_TYPE_BYPASS:
DPRINTF("Bypass\n");
break;
case FILTER_TYPE_ALLPASS:
DPRINTF("All Pass\n");
break;
case FILTER_TYPE_PEAKING:
DPRINTF("Peaking\n");
break;
case FILTER_TYPE_LOWPASS:
DPRINTF("Low Pass\n");
break;
case FILTER_TYPE_HIGHPASS:
DPRINTF("High Pass\n");
break;
case FILTER_TYPE_BANDPASS:
DPRINTF("Band Pass\n");
break;
case FILTER_TYPE_BANDSTOP:
DPRINTF("Band Stop\n");
break;
case FILTER_TYPE_NOTCH:
DPRINTF("Notch\n");
break;
case FILTER_TYPE_CONST_Q:
DPRINTF("Constant Q\n");
break;
case FILTER_TYPE_LOWSHELF:
DPRINTF("Low Shelf\n");
break;
case FILTER_TYPE_HIGHSHELF:
DPRINTF("High Shelf\n");
break;
default:
DPRINTF("Unknown Type (%d)\n", band->type);
}
// 安全的浮点打印方法 - 避免%f崩溃
DPRINTF(" fc=");
print_float(band->fc, 6);
DPRINTF(" q=");
print_float(band->q, 6);
DPRINTF(" bw=");
print_float(band->bw, 6);
DPRINTF(" gain=");
print_float(band->gain, 6);
}
DPRINTF(" Filter Coefficients:\n");
DPRINTF(" Biquad Count: %d\n", eq_data->filter.biquad_count);
// Print coefficients with sign handling
for (int i = 0; i < 5; i++) {
DPRINTF(" Coefficient Group %d: ", i);
for (int j = 0; j < MAX_EQ_BANDS; j++) {
int32_t coeff = eq_data->filter.coef[i][j];
if (coeff < 0) {
DPRINTF("-0x%08x ", -coeff);
} else {
DPRINTF("0x%08x ", coeff);
}
}
DPRINTF("\n");
}
DPRINTF("*******************************\n");
}
int32_t handler_eq_filter(unsigned sample_freq, uint32_t ch, int32_t new_sample) {
int32_t temp_sample = new_sample;
@@ -924,118 +788,6 @@ int32_t handler_eq_filter(unsigned sample_freq, uint32_t ch, int32_t new_sample)
void debug_print_eq_params(unsigned sample_freq) {
unsigned mode = g_current_eq_mode;
// 改动原因：使用公用数组，通过函数获取对应采样率的数组指针
eq_mode_data_t (*eq_data)[NUM_EQ_MODES][NUM_EQ_CHANS] = get_eq_array_ptr_common(sample_freq);
if (eq_data == NULL) {
debug_printf("Error: Unsupported sample rate %u\n", sample_freq);
return;
}
// 修复可能损坏的数据
for (int ch = 0; ch < NUM_EQ_CHANS; ch++) {
repair_eq_data(&(*eq_data)[mode][ch]);
}
debug_printf("===== EQ Parameters Debug Info =====\n");
debug_printf("Sample Rate: %u Hz, Mode: %u\n", sample_freq, mode);
for (int ch = 0; ch < NUM_EQ_CHANS; ch++) {
DPRINTF("\nChannel %d:\n", ch);
DPRINTF("Total bshift: 0x%d\n", (*eq_data)[mode][ch].total_bshift);
DPRINTF("Post gain: 0x%d\n", (*eq_data)[mode][ch].post_gain_db);
DPRINTF("Filter Parameters:\n");
for (int i = 0; i < MAX_EQ_BANDS; i++) {
filter_params_t *band = &(*eq_data)[mode][ch].bands[i];
// 添加指针有效性检查
if (band == NULL) {
DPRINTF(" Band %d: ERROR - band pointer is NULL\n", i);
continue;
}
// 检查指针是否在有效内存范围内
uintptr_t band_addr = (uintptr_t)band;
uintptr_t eq_data_addr = (uintptr_t)eq_data;
size_t eq_data_size = sizeof(eq_mode_data_t);
DPRINTF(" Band %d:\n", i);
DPRINTF(" Index: %d\n", band->index);
// Print filter type
DPRINTF(" Type: ");
switch(band->type) {
case FILTER_TYPE_BYPASS:
DPRINTF("Bypass\n");
break;
case FILTER_TYPE_ALLPASS:
DPRINTF("All Pass\n");
break;
case FILTER_TYPE_PEAKING:
DPRINTF("Peaking\n");
break;
case FILTER_TYPE_LOWPASS:
DPRINTF("Low Pass\n");
break;
case FILTER_TYPE_HIGHPASS:
DPRINTF("High Pass\n");
break;
case FILTER_TYPE_BANDPASS:
DPRINTF("Band Pass\n");
break;
case FILTER_TYPE_BANDSTOP:
DPRINTF("Band Stop\n");
break;
case FILTER_TYPE_NOTCH:
DPRINTF("Notch\n");
break;
case FILTER_TYPE_CONST_Q:
DPRINTF("Constant Q\n");
break;
case FILTER_TYPE_LOWSHELF:
DPRINTF("Low Shelf\n");
break;
case FILTER_TYPE_HIGHSHELF:
DPRINTF("High Shelf\n");
break;
default:
DPRINTF("Unknown Type (%d)\n", band->type);
}
DPRINTF(" fc=");
print_float(band->fc, 6);
DPRINTF(", q=");
print_float(band->q, 6);
DPRINTF(", bw=");
print_float(band->bw, 6);
DPRINTF(", gain=");
print_float(band->gain, 6);
DPRINTF("\n");
}
DPRINTF(" Filter Coefficients:\n");
DPRINTF(" Biquad Count: %d\n", (*eq_data)[mode][ch].filter.biquad_count);
// Print coefficients with sign handling
for (int i = 0; i < 5; i++) {
DPRINTF(" Coefficient Group %d: ", i);
for (int j = 0; j < MAX_EQ_BANDS; j++) {
int32_t coeff = (*eq_data)[mode][ch].filter.coef[i][j];
if (coeff < 0) {
DPRINTF("-0x%08x ", -coeff);
} else {
DPRINTF("0x%08x ", coeff);
}
}
DPRINTF("\n");
}
}
DPRINTF("===== Debug Info End =====\n");
}
#endif
@@ -1810,138 +1562,7 @@ unsigned char process_send_params(uint8_t data[], uint16_t len) {
return true;
}
#if 0
// 旧代码已重构，以下代码不再使用
uint32_t all_sample_rates[] = {44100, 48000, 88200, 96000, 176400, 192000};
int num_rates = sizeof(all_sample_rates) / sizeof(all_sample_rates[0]);
for (int rate_idx = 0; rate_idx < num_rates; rate_idx++) {
uint32_t fs = all_sample_rates[rate_idx];
// 查找对应采样率和模式的eq参数数组
eq_mode_data_t *eq_data = NULL;
eq_mode_data_t *eq_data_2 = NULL;
if (fs == 44100) {
eq_data = &sEQ_data_44100HZ[mode][0];
eq_data_2 = &sEQ_data_44100HZ[mode][1];
} else {
// 改动原因：所有采样率共用44100Hz数组
eq_data = &sEQ_data_44100HZ[mode][0];
eq_data_2 = &sEQ_data_44100HZ[mode][1];
}
// 检查指针有效性和mode范围
if (eq_data != NULL && eq_data_2 != NULL && mode < NUM_EQ_MODES) {
// 更新对应band的参数到eq参数数组
filter_params_t *band = &eq_data->bands[band_index];
filter_params_t new_params;
// 初始化结构体
memset(&new_params, 0, sizeof(filter_params_t));
new_params.index = band_index;
new_params.type = (filter_type_t)data[4]; // 位置从8移到4
// 打印原始数据字节
DPRINTF(" Raw data bytes: ");
for (int i = 0; i < 25; i++) {
DPRINTF("%02x ", data[i]);
}
DPRINTF("\n");
// 手动组合字节解析小端序浮点数
// Python发送小端序: 44 16 00 00 (600.0)
// C接收小端序: 00 00 16 44，需要按小端序解析
// fc: 00 00 16 44 -> 0x44160000
uint32_t fc_bytes = data[5] | (data[6] << 8) | (data[7] << 16) | (data[8] << 24);
memcpy(&new_params.fc, &fc_bytes, 4);
DPRINTF("fc: ");
print_float(new_params.fc, 6);
// q: cd cc cc 3e -> 0x3ecccccd (小端序)
uint32_t q_bytes = data[9] | (data[10] << 8) | (data[11] << 16) | (data[12] << 24);
memcpy(&new_params.q, &q_bytes, 4);
DPRINTF("q: ");
print_float(new_params.q, 6);
// bw: 00 00 80 3f -> 0x3f800000
uint32_t bw_bytes = data[13] | (data[14] << 8) | (data[15] << 16) | (data[16] << 24);
memcpy(&new_params.bw, &bw_bytes, 4);
DPRINTF("bw: ");
print_float(new_params.bw, 6);
// gain: 00 00 10 c1 -> 0xc1100000
uint32_t gain_bytes = data[17] | (data[18] << 8) | (data[19] << 16) | (data[20] << 24);
memcpy(&new_params.gain, &gain_bytes, 4);
DPRINTF("gain: ");
print_float(new_params.gain, 6);
DPRINTF("\n");
// 双通道也要同步
// 检查参数有效性
if ((isnan(new_params.fc) || isnan(new_params.q) || isnan(new_params.bw) || isnan(new_params.gain)) || new_params.fc == 0) {
DPRINTF(" ERROR: Invalid float values detected!\n");
return false;
}
if (eq_single_params_different(band, &new_params)) {
DPRINTF(" Single parameter changed for sample rate %d Hz, adding to database\n", fs);
band->type = new_params.type;
band->fc = new_params.fc;
band->q = new_params.q;
band->bw = new_params.bw;
band->gain = new_params.gain;
// 改动原因：当EQ禁用时，不设置强制模式切换标志，避免实际切换模式
if (g_eq_enable == 1) {
SET_SHARED_GLOBAL(g_force_request_eq_mode_change, 1);
} else {
debug_printf("EQ disabled: Parameter updated for saved mode %d (not switching current mode)\n", mode);
}
}
else
{
DPRINTF(" Single parameter unchanged for sample rate %d Hz, no action needed\n", fs);
continue;
}
// 自动计算并存储系数到eq参数数组
q2_30 coeffs[5];
int calculated_bshift;
#if 1
DPRINTF(" Calculating coefficients for sample rate %d Hz, type=%d, fc=", fs, new_params.type);
print_float(new_params.fc, 6);
eq_data->bands[band_index].fc = new_params.fc;
DPRINTF(", q=");
print_float(new_params.q, 6);
eq_data->bands[band_index].q = new_params.q;
DPRINTF(", bw=");
print_float(new_params.bw, 6);
eq_data->bands[band_index].bw = new_params.bw;
DPRINTF(", gain=");
print_float(new_params.gain, 6);
eq_data->bands[band_index].gain = new_params.gain;
DPRINTF("\n");
#endif
if (fs == 44100) {
if (eq_process_single_param_change(fs, mode, 0, band_index, band) == 0) {
debug_printf("Successfully processed 0x8d single parameter change for 44100Hz channel 0\n");
} else {
debug_printf("Error processing 0x8d single parameter change for 44100Hz channel 0\n");
}
// 标记参数已修改，需要同步到Flash
eq_flash_mark_dirty(mode);
debug_printf("Marked mode %d as dirty for Flash sync\n", mode);
}
} else {
debug_printf("Error: eq_data pointer NULL or mode out of range (mode=%d, fs=%d)\n", mode, fs);
}
}
debug_printf("Parameter processing completed for all sample rates\n");
}
#endif // #if 0
/* Dead code removed - old 0x8d handler was refactored above */
#endif // #if EQ_EN (for process_send_params EQ commands section starting at line 1228)
return true;
@@ -2159,18 +1780,6 @@ uint8_t process_uart_set_eq_params(uint8_t *data)
bw = *(float*)(&data[16]);
gain = *(float*)(&data[20]);
DPRINTF("mode: %d, band_index: %d, filter_type: %d\n", mode, band_index, filter_type);
DPRINTF("fc: ");
print_float(fc, 6);
DPRINTF("q: ");
print_float(q, 6);
DPRINTF("bw: ");
print_float(bw, 6);
print_float(gain, 6);
DPRINTF("gain: ");
print_float(gain, 6);
DPRINTF("\n");
// 验证参数范围（band_index检查）
if ((mode > (EQ_MODE_MAX - 1)) || (band_index >= MAX_EQ_BANDS)) {
debug_printf("Invalid parameters: mode=%d, band=%d\n", mode, band_index);
@@ -2253,16 +1862,6 @@ uint8_t process_uart_get_eq_params(uint8_t *data, uint8_t *response, uint8_t res
memcpy(&response[12], &band->q, 4);
memcpy(&response[16], &band->bw, 4);
memcpy(&response[20], &band->gain, 4);
DPRINTF("mode: %d, band_index: %d\n", mode, band_index);
DPRINTF("fc: ");
print_float(band->fc, 6);
DPRINTF("q: ");
print_float(band->q, 6);
DPRINTF("bw: ");
print_float(band->bw, 6);
DPRINTF("gain: ");
print_float(band->gain, 6);
DPRINTF("\n");
} else {
// 返回默认值

46
sw_usb_audio/app_usb_aud_phaten_golden/src/extensions/eq_coefficient_calculator.c
View File

@@ -3,8 +3,6 @@
#include "biquad_standalone.h"
#include <math.h>
extern void print_float(float value, int precision);
#if 0 // DEBUG_PRINT_ENABLE
#define DPRINTF(...) printf(__VA_ARGS__)
#else
@@ -22,20 +20,6 @@ int eq_calculate_coefficients_from_params(filter_params_t *params,
return -1;
}
#if 1 // DEBUG_PRINT_ENABLE == 1
DPRINTF("eq_calculate_coefficients_from_params\n");
DPRINTF("params->type: %d\n", params->type);
DPRINTF("params->fc: ");
print_float(params->fc, 6);
DPRINTF(", q=");
print_float(params->q, 6);
DPRINTF(", bw=");
print_float(params->bw, 6);
DPRINTF(", gain=");
print_float(params->gain, 6);
DPRINTF("\n");
#endif
// 根据滤波器类型调用相应的biquad计算函数
switch (params->type) {
case FILTER_TYPE_BYPASS:
@@ -60,26 +44,12 @@ int eq_calculate_coefficients_from_params(filter_params_t *params,
break;
case FILTER_TYPE_BANDPASS:
eq_biquad_bandpass(coeffs,
params->fc,
(float)sample_rate,
params->bw);
*bshift = 0;
break;
case FILTER_TYPE_BANDSTOP:
eq_biquad_bandstop(coeffs,
params->fc,
(float)sample_rate,
params->bw);
*bshift = 0;
break;
case FILTER_TYPE_NOTCH:
eq_biquad_notch(coeffs,
params->fc,
(float)sample_rate,
params->q);
case FILTER_TYPE_CONST_Q:
// These filter types have been removed to save memory.
// Fall through to bypass behavior.
eq_biquad_bypass(coeffs);
*bshift = 0;
break;
@@ -99,14 +69,6 @@ int eq_calculate_coefficients_from_params(filter_params_t *params,
params->gain);
break;
case FILTER_TYPE_CONST_Q:
*bshift = eq_biquad_const_q(coeffs,
params->fc,
(float)sample_rate,
params->q,
params->gain);
break;
case FILTER_TYPE_LOWSHELF:
*bshift = eq_biquad_lowshelf(coeffs,
params->fc,

142
sw_usb_audio/app_usb_aud_phaten_golden/src/extensions/eq_flash_storage.c
View File

@@ -21,7 +21,6 @@
extern uint32_t get_reference_time(void);
extern lfs_t lfs;
void delay_milliseconds(unsigned int);
extern void print_float(float value, int precision);
#define FILE_PATH_MAX_LENGTH 30
// EQ Flash存储目录和文件名全局变量定义（默认值）
@@ -213,6 +212,33 @@ void eq_flash_deinit(void) {
* @param mode EQ模式
* @return 0: 成功, -1: 失败
*/
/**
* @brief Initialize eq_mode_data_t with default bypass values
* @param eq_data Target data to initialize
* @param sample_rate Sample rate to set
*/
static void init_eq_data_defaults(eq_mode_data_t *eq_data, uint32_t sample_rate) {
memset(eq_data, 0, sizeof(eq_mode_data_t));
eq_data->sample_rate = sample_rate;
eq_data->total_bshift = 0;
eq_data->post_gain_db = 0;
for (int i = 0; i < MAX_EQ_BANDS; i++) {
eq_data->bands[i].index = i;
eq_data->bands[i].type = FILTER_TYPE_BYPASS;
eq_data->bands[i].fc = 1000.0f;
eq_data->bands[i].q = 0.707f;
eq_data->bands[i].bw = 1.0f;
eq_data->bands[i].gain = 0.0f;
}
for (int i = 0; i < 8; i++) {
eq_data->filter.coef[0][i] = 1 << 30;
eq_data->filter.coef[1][i] = 0;
eq_data->filter.coef[2][i] = 0;
eq_data->filter.coef[3][i] = 0;
eq_data->filter.coef[4][i] = 0;
}
}
int eq_flash_load_mode(uint8_t mode) {
//DPRINTF("Loading EQ mode %d from Flash\n", mode);
@@ -244,32 +270,7 @@ int eq_flash_load_mode(uint8_t mode) {
// 检查文件是否存在
if (!lfs_file_exists(file_path)) {
DPRINTF(" ****************** file is not exist (ch %d), initializing with default values\n", ch);
// 文件不存在，使用默认值初始化当前通道
eq_mode_data_t *eq_data = &(*eq_array)[mode][ch];
memset(eq_data, 0, sizeof(eq_mode_data_t));
eq_data->sample_rate = g_eq_sample_rate;
eq_data->total_bshift = 0;
eq_data->post_gain_db = 0;
// 初始化bands数组为默认值
for (int i = 0; i < MAX_EQ_BANDS; i++) {
eq_data->bands[i].index = i;
eq_data->bands[i].type = FILTER_TYPE_BYPASS;
eq_data->bands[i].fc = 1000.0f;
eq_data->bands[i].q = 0.707f;
eq_data->bands[i].bw = 1.0f;
eq_data->bands[i].gain = 0.0f;
}
// 初始化filter系数为默认值
for (int i = 0; i < 8; i++) {
eq_data->filter.coef[0][i] = 1 << 30; // b0 = 1.0 in Q30
eq_data->filter.coef[1][i] = 0; // b1 = 0
eq_data->filter.coef[2][i] = 0; // b2 = 0
eq_data->filter.coef[3][i] = 0; // a1 = 0
eq_data->filter.coef[4][i] = 0; // a2 = 0
}
init_eq_data_defaults(&(*eq_array)[mode][ch], g_eq_sample_rate);
DPRINTF(" Initialized with default values for mode %d, channel %d\n", mode, ch);
continue; // 继续处理下一个通道
}
@@ -300,32 +301,7 @@ int eq_flash_load_mode(uint8_t mode) {
if (!verify_file_header(&header, mode, ch)) {
DPRINTF(" Invalid file header for mode %d, channel %d\n", mode, ch);
// 文件头验证失败，使用默认值初始化当前通道
eq_mode_data_t *eq_data = &(*eq_array)[mode][ch];
memset(eq_data, 0, sizeof(eq_mode_data_t));
eq_data->sample_rate = g_eq_sample_rate;
eq_data->total_bshift = 0;
eq_data->post_gain_db = 0;
// 初始化bands数组为默认值
for (int i = 0; i < MAX_EQ_BANDS; i++) {
eq_data->bands[i].index = i;
eq_data->bands[i].type = FILTER_TYPE_BYPASS;
eq_data->bands[i].fc = 1000.0f;
eq_data->bands[i].q = 0.707f;
eq_data->bands[i].bw = 1.0f;
eq_data->bands[i].gain = 0.0f;
}
// 初始化filter系数为默认值
for (int i = 0; i < 8; i++) {
eq_data->filter.coef[0][i] = 1 << 30; // b0 = 1.0 in Q30
eq_data->filter.coef[1][i] = 0; // b1 = 0
eq_data->filter.coef[2][i] = 0; // b2 = 0
eq_data->filter.coef[3][i] = 0; // a1 = 0
eq_data->filter.coef[4][i] = 0; // a2 = 0
}
init_eq_data_defaults(&(*eq_array)[mode][ch], g_eq_sample_rate);
DPRINTF(" Initialized with default values for mode %d, channel %d\n", mode, ch);
continue; // 继续处理下一个通道
}
@@ -334,32 +310,7 @@ int eq_flash_load_mode(uint8_t mode) {
uint32_t calculated_crc = eq_flash_calculate_crc32((uint8_t*)&flash_data, sizeof(flash_data));
if (calculated_crc != header.crc32) {
DPRINTF(" CRC32 mismatch for mode %d, sample rate %lu, channel %d\n", mode, (unsigned long)g_eq_sample_rate, ch);
// CRC32验证失败，使用默认值初始化当前通道
eq_mode_data_t *eq_data = &(*eq_array)[mode][ch];
memset(eq_data, 0, sizeof(eq_mode_data_t));
eq_data->sample_rate = g_eq_sample_rate;
eq_data->total_bshift = 0;
eq_data->post_gain_db = 0;
// 初始化bands数组为默认值
for (int i = 0; i < MAX_EQ_BANDS; i++) {
eq_data->bands[i].index = i;
eq_data->bands[i].type = FILTER_TYPE_BYPASS;
eq_data->bands[i].fc = 1000.0f;
eq_data->bands[i].q = 0.707f;
eq_data->bands[i].bw = 1.0f;
eq_data->bands[i].gain = 0.0f;
}
// 初始化filter系数为默认值
for (int i = 0; i < 8; i++) {
eq_data->filter.coef[0][i] = 1 << 30; // b0 = 1.0 in Q30
eq_data->filter.coef[1][i] = 0; // b1 = 0
eq_data->filter.coef[2][i] = 0; // b2 = 0
eq_data->filter.coef[3][i] = 0; // a1 = 0
eq_data->filter.coef[4][i] = 0; // a2 = 0
}
init_eq_data_defaults(&(*eq_array)[mode][ch], g_eq_sample_rate);
DPRINTF(" Initialized with default values due to CRC32 mismatch for channel %d\n", ch);
continue; // 继续处理下一个通道
}
@@ -539,32 +490,13 @@ int eq_single_param_database_init(void) {
* @return 0: 成功, -1: 失败
*/
int eq_process_single_param_change(uint32_t sample_rate, uint8_t mode, uint8_t channel, uint8_t band_index, filter_params_t *band) {
DPRINTF("Processing single parameter change: rate=%u, mode=%d, ch=%d, band=%d\n",
sample_rate, mode, channel, band_index);
DPRINTF("fc=");
print_float(band->fc, 6);
DPRINTF(", q=");
print_float(band->q, 6);
DPRINTF(", bw=");
print_float(band->bw, 6);
DPRINTF(", gain=");
print_float(band->gain, 6);
DPRINTF("\n");
// 比较参数是否不同
DPRINTF(" Single parameter changed, adding to database\n");
// 添加到单参数数据库
if (eq_add_single_param(sample_rate, mode, channel, band_index, band) == 0) {
DPRINTF(" Single parameter change recorded successfully\n");
// 标记为脏数据
dirty_flags[mode] = 1;
DPRINTF(" Marked mode %d as dirty\n", mode);
return 0;
} else {
DPRINTF(" Error: Failed to add single parameter change to database\n");
return -1;
}
}
@@ -779,23 +711,9 @@ int eq_load_single_param(uint32_t sample_rate, uint8_t mode, uint8_t channel, ui
// 应用到两个通道
(*eq_array)[mode][0].bands[band_index] = param;
(*eq_array)[mode][1].bands[band_index] = param;
DPRINTF(" Applied parameter to sample rate %u (both channels)\n", fs);
DPRINTF(" fc=");
print_float(param.fc, 6);
DPRINTF(", q=");
print_float(param.q, 6);
DPRINTF(", bw=");
print_float(param.bw, 6);
DPRINTF(", gain=");
print_float(param.gain, 6);
DPRINTF("\n");
} else {
DPRINTF(" Error: Failed to get EQ array for sample rate %u\n", fs);
}
}
DPRINTF(" Successfully loaded single parameter: type=%d\n", param.type);
return 0;
}