add bpf to rds2 carrier and lpf

lib/constants.h

@@ -4,4 +4,8 @@
 #endif
 #ifndef M_2PI
 #define M_2PI (M_PI * 2.0)
-#endif
+#endif
+
+inline float sincf(float x) {
+    return (x == 0.0f) ? 1.0f : sinf(M_PI * x) / (M_PI * x);
+}

lib/filters.c

@@ -14,36 +14,43 @@ float hard_clip(float sample, float threshold) {
 	return fmaxf(-threshold, fminf(threshold, sample));
 }
 
-void init_pll(PLL *pll, float freq, float loop_filter_bandwidth, float damping, int sample_rate) {
+void init_bpf(FIRFilter *bpf, float start, float end) {
-	pll->phase = 0.0f;
+	int m = FILTER_LEN - 1;
-	pll->freq = freq;
+	float sum = 0.0f;
-	pll->loop_filter_state = 0.0f;
+
-	pll->kp = M_2PI * loop_filter_bandwidth;
+	for(int n = 0; n < FILTER_LEN; n++) {
-	pll->ki = (4.0f*damping*damping) * pll->kp * pll->kp;
+		float x = n-m/2.0f;
-	pll->sample_rate = sample_rate;
+		float h1 = sincf(2.0f*start*x) * (0.54 - 0.46 * cosf(M_2PI * n / m));
+		float h2 = sincf(2.0f*end*x) * (0.54 - 0.46 * cosf(M_2PI * n / m));
+		bpf->filter[n] = h1-h2;
+		sum += bpf->filter[n];
+	}
+
+	for(int n = 0; n < FILTER_LEN; n++) {
+		bpf->filter[n] /= sum;
+	}
+}
+void init_lpf(FIRFilter *lpf, float freq) {
+	int m = FILTER_LEN - 1;
+	float sum = 0.0f;
+
+	for(int n = 0; n < FILTER_LEN; n++) {
+		float x = n-m/2.0f;
+		lpf->filter[n] = sincf(2.0f*freq*x) * (0.54 - 0.46 * cosf(M_2PI * n / m));
+		sum += lpf->filter[n];
+	}
+
+	for(int n = 0; n < FILTER_LEN; n++) {
+		lpf->filter[n] /= sum;
+	}
 }
 
-float apply_pll(PLL *pll, float ref_sample) {
+float fir_filter(FIRFilter *fir, float sample) {
-	float phase_error;
+    float out = 0.0f;
-
+    for(int i = 0; i < FILTER_LEN; i++) {
-	float vco_output = sinf(pll->phase);
+        out += fir->filter[i] * sample;
-
+    }
-	phase_error = atan2f(ref_sample, vco_output);;
+    fir->filter_idx++;
-
+    if(fir->filter_idx == FILTER_LEN) fir->filter_idx = 0;
-	pll->loop_filter_state += pll->ki * phase_error / pll->sample_rate;
+    return out;
-	float loop_output = pll->loop_filter_state + pll->kp * phase_error;
-
-	float freq_adjustment = loop_output / M_2PI;
-	float instantaneous_freq = pll->freq + freq_adjustment;
-
-	pll->phase += M_2PI * instantaneous_freq / pll->sample_rate;
-
-	while (pll->phase >= M_2PI) {
-		pll->phase -= M_2PI;
-	}
-	while (pll->phase < 0.0f) {
-		pll->phase += M_2PI;
-	}
-
-	return vco_output;
 }

lib/filters.h

@@ -5,6 +5,8 @@
 #include "constants.h"
 #include "oscillator.h"
 
+#define FILTER_LEN 51
+
 typedef struct
 {
 	float alpha;
@@ -16,13 +18,11 @@ float apply_preemphasis(ResistorCapacitor *filter, float sample);
 
 float hard_clip(float sample, float threshold);
 
-typedef struct {
+typedef struct filters
-	float phase;
+{
-	float freq;
+	float filter[FILTER_LEN];
-	float loop_filter_state;
+	int filter_idx;
-	float kp;
+} FIRFilter;
-	float ki;
+void init_bpf(FIRFilter *bpf, float start, float end);
-	int sample_rate;
+void init_lpf(FIRFilter *lpf, float freq);
-} PLL;
+float fir_filter(FIRFilter *fir, float sample);
-void init_pll(PLL *pll, float freq, float loop_filter_bandwidth, float damping, int sample_rate);
-float apply_pll(PLL *pll, float ref_sample);

src/fm95.c

@@ -415,8 +415,10 @@ int main(int argc, char **argv) {
 	init_preemphasis(&preemp_l, preemphasis_tau, sample_rate);
 	init_preemphasis(&preemp_r, preemphasis_tau, sample_rate);
 
-	PLL rds2_pll;
+	FIRFilter rds2_bpf, lpf_l, lpf_r;
-	init_pll(&rds2_pll, 66500, 5, 0.5f, sample_rate);
+	init_bpf(&rds2_bpf, 66000, 67000);
+	init_lpf(&lpf_l, 15000);
+	init_lpf(&lpf_r, 15000);
 	// #endregion
 
 	signal(SIGINT, stop);
@@ -474,6 +476,8 @@ int main(int argc, char **argv) {
 
 			float ready_l = apply_preemphasis(&preemp_l, l_in)*2;
 			float ready_r = apply_preemphasis(&preemp_r, r_in)*2;
+			ready_l = fir_filter(&lpf_l, ready_l);
+			ready_r = fir_filter(&lpf_l, ready_r);
 			ready_l = hard_clip(ready_l*audio_volume, clipper_threshold);
 			ready_r = hard_clip(ready_r*audio_volume, clipper_threshold);
 
@@ -495,7 +499,7 @@ int main(int argc, char **argv) {
 				float rds_carrier = get_oscillator_sin_multiplier_ni(&osc, 3);
 				output[i] += (current_rds_in*rds_carrier)*RDS_VOLUME;
 				if(!sca_on) {
-					float rds2_carrier_66 = apply_pll(&rds2_pll, rds_carrier);
+					float rds2_carrier_66 = fir_filter(&rds2_bpf, get_oscillator_sin_multiplier_ni(&osc, 3.5f));
 					output[i] += (current_rds2_in*rds2_carrier_66)*RDS2_VOLUME;
 				}
 			}