diff --git a/components/bl00mbox/plugins/lowpass.c b/components/bl00mbox/plugins/lowpass.c index 16413fa42289ebeca1bca57d7d4ea15ffa01efa7..614eb121ab39d485e6ff67677e8f6e89e4416320 100644 --- a/components/bl00mbox/plugins/lowpass.c +++ b/components/bl00mbox/plugins/lowpass.c @@ -1,5 +1,11 @@ #include "lowpass.h" +//#define LOWPASS_DEBUG_PRINT + +#ifdef LOWPASS_DEBUG_PRINT +#include <stdio.h> +#endif + radspa_t * lowpass_create(uint32_t init_var); radspa_descriptor_t lowpass_desc = { .name = "lowpass", @@ -16,79 +22,67 @@ radspa_descriptor_t lowpass_desc = { #define LOWPASS_Q 3 #define LOWPASS_GAIN 4 -static float apply_lowpass(lowpass_data_t * data, int32_t input){ - data->pos++; - if(data->pos >= 3) data->pos = 0; - - float out = 0; - float in_acc = input; - - for(int8_t i = 0; i<2; i++){ - int8_t pos = data->pos - i - 1; - if(pos < 0) pos += 3; - in_acc += (2-i)*data->in_history[pos]; - - out -= (data->out_history[pos] * data->out_coeff[i]); - } - out += in_acc * data->in_coeff; - - data->in_history[data->pos] = input; - data->out_history[data->pos] = out; +#define LOWPASS_INTERNAL_SHIFT 14 +#define LOWPASS_OUT_COEFF_SHIFT 11 - return out; -} - -static uint8_t coeff_shift(float coeff) { - int32_t ret = 16; +static void coeff_shift(uint64_t coeff, int32_t *shift, int32_t * shifted_coeff) { + int32_t ret = 22; + uint64_t ret_coeff = coeff >> (32-ret); while(1){ - int32_t test_val = (1<<ret) * coeff; - if(test_val < 0) test_val = -test_val; - if(test_val > (1<<12)){ + if(ret > 31){ + ret = 31; + ret_coeff = coeff >> (32-ret); + break; + } + if(ret < (LOWPASS_INTERNAL_SHIFT)){ + ret = (LOWPASS_INTERNAL_SHIFT); + ret_coeff = coeff >> (32-ret); + break; + } + if(ret_coeff > (1UL<<14)){ ret -= 3; - } else if (test_val < (1<<8)){ + ret_coeff = coeff >> (32-ret); + } else if (ret_coeff < (1UL<<11)){ ret += 3; + ret_coeff = coeff >> (32-ret); } else { break; } - if(ret > 28) break; - if(ret < 4) break; } - return ret; + + * shift = ret; + * shifted_coeff = (int32_t) ret_coeff; } -static void set_lowpass_coeffs(lowpass_data_t * data, float freq, float q){ - //molasses, sorry - if(freq == 0) return; - if(q == 0) return; - float K = 2*48000; - float omega = freq * 6.28; - float A[3]; - A[0] = K*K; - A[1] = K*omega/q; +static void set_lowpass_coeffs(lowpass_data_t * data, int32_t freq, int32_t mq){ + if(freq < 15.) freq = 15.; + if(freq > 15000.) freq = 15000.; + if(mq < 130) mq = 130; + int32_t K = 15287; // 2*48000/6.28 + int32_t omega = freq; + int32_t A[3]; + A[0] = K * K; + A[1] = K * 1000; + A[1] = (int64_t) A[1] * omega / (mq+1); A[2] = omega*omega; - float B = omega*omega; - - float sum_a = A[0] + A[1] + A[2]; - - float round; - //int16_t shift; + int32_t sum_a = A[0] + A[1] + A[2]; + int64_t round = A[2]; - round = B / sum_a; - //shift = coeff_shift(round); - data->in_coeff = round;// * (1<<shift); - //data->in_coeff_shift = shift; + round = (round << 32) / sum_a; + int32_t shift; + int32_t shifted_coeff; + coeff_shift(round, &shift, &shifted_coeff); + data->in_coeff_shift = shift - (LOWPASS_INTERNAL_SHIFT); + data->in_coeff = shifted_coeff; - round = 2.*(A[2]-A[0]) / sum_a; - //shift = coeff_shift(round); - data->out_coeff[0] = round;// * (1<<shift); - //data->out_coeff_shift[0] = shift; + round = 2.*(A[2]-A[0]); + data->out_coeff[0] = round * (1LL<<(LOWPASS_OUT_COEFF_SHIFT)) / sum_a; - round = (A[0]-A[1]+A[2]) / sum_a; - //shift = coeff_shift(round); - data->out_coeff[1] = round;// * (1<<shift); - //data->out_coeff_shift[1] = shift; + round = (A[0]-A[1]+A[2]); + data->out_coeff[1] = round * (1LL<<(LOWPASS_OUT_COEFF_SHIFT)) / sum_a; } + void lowpass_run(radspa_t * lowpass, uint16_t num_samples, uint32_t render_pass_id){ radspa_signal_t * output_sig = radspa_signal_get_by_index(lowpass, LOWPASS_OUTPUT); if(output_sig->buffer == NULL) return; @@ -102,16 +96,41 @@ void lowpass_run(radspa_t * lowpass, uint16_t num_samples, uint32_t render_pass_ int16_t input = radspa_signal_get_value(input_sig, i, render_pass_id); int32_t freq = radspa_signal_get_value(freq_sig, i, render_pass_id); int16_t q = radspa_signal_get_value(q_sig, i, render_pass_id); - int16_t gain = radspa_signal_get_value(gain_sig, i, render_pass_id); + int32_t gain = radspa_signal_get_value(gain_sig, i, render_pass_id); if((freq != data->prev_freq) | (q != data->prev_q)){ - set_lowpass_coeffs(data, freq, ((float) q + 1.)/1000.); + set_lowpass_coeffs(data, freq, q); +#ifdef LOWPASS_DEBUG_PRINT + printf("\nfreq: %ld, q: %d\n", freq, q); + printf("in_coeff: %ld >> %ld, ", data->in_coeff, data->in_coeff_shift); + printf("out_coeffs: %ld, %ld\n", data->out_coeff[0], data->out_coeff[1]); +#endif data->prev_freq = freq; data->prev_q = q; } - float out = apply_lowpass(data, input) + 0.5; - int16_t ret = radspa_clip(radspa_gain((int32_t) out, gain)); + data->pos++; + if(data->pos >= 3) data->pos = 0; + + data->in_history[data->pos] = input; + + int32_t in_acc = input; + int32_t ret = 0; + + for(int8_t i = 0; i<2; i++){ + int8_t pos = data->pos - i - 1; + if(pos < 0) pos += 3; + in_acc += (2-i)*data->in_history[pos]; + + ret -= (((int64_t) data->out_history[pos]) * data->out_coeff[i]) >> (LOWPASS_OUT_COEFF_SHIFT); + } + + ret += (in_acc * data->in_coeff) >> data->in_coeff_shift; + + data->out_history[data->pos] = ret; + + ret = ret >> (LOWPASS_INTERNAL_SHIFT); + ret = radspa_clip(radspa_gain(ret, gain)); radspa_signal_set_value(output_sig, i, ret); } } diff --git a/components/bl00mbox/plugins/lowpass.h b/components/bl00mbox/plugins/lowpass.h index d0b138eebabc6edc3efbf9e89d54013a184b1667..076a4cbfb28f32cf24f4e6b5952e42740bac259e 100644 --- a/components/bl00mbox/plugins/lowpass.h +++ b/components/bl00mbox/plugins/lowpass.h @@ -3,12 +3,11 @@ #include <radspa_helpers.h> typedef struct { - float in_history[3]; - float in_coeff; - //int16_t in_coeff_shift; - float out_history[3]; - float out_coeff[2]; - //int16_t out_coeff_shift[2]; + int32_t in_history[3]; + int32_t in_coeff; + int32_t in_coeff_shift; + int32_t out_history[3]; + int32_t out_coeff[2]; int32_t prev_freq; int16_t prev_q;