mirror of
https://github.com/lwvmobile/dsd-fme.git
synced 2026-11-16 10:47:21 -05:00
New Analog Audio Filters WIP;
This commit is contained in:
211
src/3.c
211
src/3.c
@@ -321,4 +321,213 @@ uint16_t q_abs_diff(const uint16_t v1, const uint16_t v2)
|
||||
//
|
||||
// Wojciech Kaczmarski, SP5WWP
|
||||
// M17 Project, 29 December 2023
|
||||
//--------------------------------------------------------------------
|
||||
//--------------------------------------------------------------------
|
||||
|
||||
|
||||
//audio filter stuff sourced from: https://github.com/NedSimao/FilteringLibrary
|
||||
//no license / information provided in source code
|
||||
#define PI 3.141592653
|
||||
|
||||
//do we need this for test input sthit?
|
||||
// #define PLOT_POINTS 100
|
||||
// #define SAMPLE_TIME_S 0.001
|
||||
// #define CUTOFFFREQ_HZ 50
|
||||
// #define sine_freq 40
|
||||
// #define sine_freq_1 500
|
||||
// #define HFC 50
|
||||
// #define LFC 100
|
||||
|
||||
|
||||
void LPFilter_Init(LPFilter *filter, float cutoffFreqHz, float sampleTimeS){
|
||||
float RC=0.0;
|
||||
RC=1.0/(2*PI*cutoffFreqHz);
|
||||
filter->coef[0]=sampleTimeS/(sampleTimeS+RC);
|
||||
filter->coef[1]=RC/(sampleTimeS+RC);
|
||||
|
||||
filter->v_out[0]=0.0;
|
||||
filter->v_out[1]=0.0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
float LPFilter_Update(LPFilter *filter, float v_in){
|
||||
|
||||
filter->v_out[1]=filter->v_out[0];
|
||||
filter->v_out[0]=(filter->coef[0]*v_in) + (filter->coef[1]*filter->v_out[1]);
|
||||
|
||||
return (filter->v_out[0]);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/********************************************************************************************************
|
||||
* HIGH PASS FILTER
|
||||
********************************************************************************************************/
|
||||
void HPFilter_Init(HPFilter *filter, float cutoffFreqHz, float sampleTimeS){
|
||||
float RC=0.0;
|
||||
RC=1.0/(2*PI*cutoffFreqHz);
|
||||
|
||||
filter->coef=RC/(sampleTimeS+RC);
|
||||
|
||||
filter->v_in[0]=0.0;
|
||||
filter->v_in[1]=0.0;
|
||||
|
||||
filter->v_out[0]=0.0;
|
||||
filter->v_out[1]=0.0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
float HPFilter_Update(HPFilter *filter, float v_in){
|
||||
|
||||
filter->v_in[1]=filter->v_in[0];
|
||||
filter->v_in[0]=v_in;
|
||||
|
||||
|
||||
filter->v_out[1]=filter->v_out[0];
|
||||
|
||||
filter->v_out[0]=filter->coef * (filter->v_in[0] - filter->v_in[1]+filter->v_out[1]);
|
||||
|
||||
|
||||
|
||||
return (filter->v_out[0]);
|
||||
}
|
||||
|
||||
/********************************************************************************************************
|
||||
* BAND PASS FILTER
|
||||
********************************************************************************************************/
|
||||
|
||||
|
||||
void PBFilter_Init(PBFilter *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS){
|
||||
LPFilter_Init(&filter->lpf, LPF_cutoffFreqHz, sampleTimeS);
|
||||
HPFilter_Init(&filter->hpf, HPF_cutoffFreqHz, sampleTimeS);
|
||||
filter->out_in=0.0;
|
||||
|
||||
}
|
||||
|
||||
float PBFilter_Update(PBFilter *filter, float v_in){
|
||||
filter->out_in=HPFilter_Update(&filter->hpf, v_in);
|
||||
|
||||
filter->out_in=LPFilter_Update(&filter->lpf, filter->out_in);
|
||||
|
||||
return (filter->out_in);
|
||||
}
|
||||
|
||||
/********************************************************************************************************
|
||||
* NOTCH FILTER
|
||||
********************************************************************************************************/
|
||||
|
||||
|
||||
void NOTCHFilter_Init(NOTCHFilter *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS){
|
||||
//filter frequency to angular (rad/s)
|
||||
float w0_rps=2.0 * PI *centerFreqHz;
|
||||
float ww_rps=2.0 * PI *notchWidthHz;
|
||||
|
||||
//pre warp center frequency
|
||||
float w0_pw_rps=(2.0/sampleTimeS) * tanf(0.5 * w0_rps * sampleTimeS);
|
||||
|
||||
//computing filter coefficients
|
||||
|
||||
filter->alpha=4.0 + w0_rps*w0_pw_rps*sampleTimeS*sampleTimeS;
|
||||
filter->beta=2.0*ww_rps*sampleTimeS;
|
||||
|
||||
//clearing input and output buffers
|
||||
|
||||
for (uint8_t n=0; n<3; n++){
|
||||
filter->vin[n]=0;
|
||||
filter->vout[n]=0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float NOTCHFilter_Update(NOTCHFilter *filter, float vin){
|
||||
//shifting samples
|
||||
filter->vin[2]=filter->vin[1];
|
||||
filter->vin[1]=filter->vin[0];
|
||||
|
||||
filter->vout[2]=filter->vout[1];
|
||||
filter->vout[1]=filter->vout[0];
|
||||
|
||||
filter->vin[0]=vin;
|
||||
|
||||
//compute new output
|
||||
filter->vout[0]=(filter->alpha*filter->vin[0] + 2.0 *(filter->alpha -8.0)*filter->vin[1] + filter->alpha*filter->vin[2]
|
||||
-(2.0f*(filter->alpha-8.0)*filter->vout[1]+(filter->alpha-filter->beta)*filter->vout[2]))/(filter->alpha+filter->beta);
|
||||
|
||||
|
||||
return (filter->vout[0]);
|
||||
}
|
||||
|
||||
void init_audio_filters (dsd_state * state)
|
||||
{
|
||||
//still not sure if this is even correct or not, but 48k sounds good now
|
||||
LPFilter_Init(&state->RCFilter, 960, (float)1/(float)48000);
|
||||
HPFilter_Init(&state->HRCFilter, 960, (float)1/(float)48000);
|
||||
|
||||
//still unsure if 8k sounds good or not (may be the walkie-talkie is too close to SDR, even outside)
|
||||
LPFilter_Init(&state->RCFilter8, 160, (float)1/(float)8000);
|
||||
HPFilter_Init(&state->HRCFilter8, 160, (float)1/(float)8000);
|
||||
|
||||
//PBFilter_Init(PBFilter *filter, float HPF_cutoffFreqHz, float LPF_cutoffFreqHz, float sampleTimeS);
|
||||
// void NOTCHFilter_Init(NOTCHFilter *filter, float centerFreqHz, float notchWidthHz, float sampleTimeS);
|
||||
|
||||
|
||||
}
|
||||
|
||||
//FUNCTIONS for handing use of above filters
|
||||
|
||||
//lpf
|
||||
void lpf(dsd_state * state, short * input, int len)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < len; i++)
|
||||
input[i] = LPFilter_Update(&state->RCFilter, input[i]);
|
||||
}
|
||||
|
||||
//hpf
|
||||
void hpf(dsd_state * state, short * input, int len)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < len; i++)
|
||||
input[i] = HPFilter_Update(&state->HRCFilter, input[i]);
|
||||
}
|
||||
|
||||
//lpf
|
||||
void lpf8(dsd_state * state, short * input, int len)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < len; i++)
|
||||
input[i] = LPFilter_Update(&state->RCFilter8, input[i]);
|
||||
}
|
||||
|
||||
//hpf
|
||||
void hpf8(dsd_state * state, short * input, int len)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < len; i++)
|
||||
input[i] = HPFilter_Update(&state->HRCFilter8, input[i]);
|
||||
}
|
||||
|
||||
//Generic RMS function derived from RTL_FM (RTL_SDR) RMS code (doesnt' really work correctly outside of RTL)
|
||||
long int raw_rms(int16_t *samples, int len, int step) //use samplespersymbol as len
|
||||
{
|
||||
|
||||
int i;
|
||||
long int rms;
|
||||
long p, t, s;
|
||||
double dc, err;
|
||||
|
||||
p = t = 0L;
|
||||
for (i=0; i<len; i+=step) {
|
||||
s = (long)samples[i];
|
||||
t += s;
|
||||
p += s * s;
|
||||
}
|
||||
/* correct for dc offset in squares */
|
||||
dc = (double)(t*step) / (double)len;
|
||||
err = t * 2 * dc - dc * dc * len;
|
||||
|
||||
rms = (long int)sqrt((p-err) / len);
|
||||
if (rms < 0) rms = 150;
|
||||
return rms;
|
||||
}
|
||||
Reference in New Issue
Block a user