diff --git a/src/trx-server/trx-backend/trx-backend-soapysdr/src/demod.rs b/src/trx-server/trx-backend/trx-backend-soapysdr/src/demod.rs index dee8e0b..94202bf 100644 --- a/src/trx-server/trx-backend/trx-backend-soapysdr/src/demod.rs +++ b/src/trx-server/trx-backend/trx-backend-soapysdr/src/demod.rs @@ -23,8 +23,10 @@ const BW4_Q1: f32 = 0.5412; const BW4_Q2: f32 = 1.3066; /// Q for the 19 kHz pilot notch (~3.8 kHz 3 dB bandwidth). const PILOT_NOTCH_Q: f32 = 5.0; +/// Tighter post-matrix stereo notch for suppressing residual pilot leakage. +const STEREO_PILOT_NOTCH_Q: f32 = 12.0; /// Narrow 19 kHz band-pass used to derive zero-crossings for switching stereo demod. -const PILOT_BPF_Q: f32 = 10.0; +const PILOT_BPF_Q: f32 = 20.0; /// Fixed phase trim on the recovered L-R channel to compensate pilot-path delay. const STEREO_SEPARATION_PHASE_TRIM: f32 = 0.0; /// Fixed gain trim on the recovered L-R channel. @@ -393,6 +395,9 @@ pub struct WfmStereoDecoder { dc_m: DcBlocker, dc_l: DcBlocker, dc_r: DcBlocker, + /// Post-matrix pilot notches for stereo outputs. + pilot_notch_l: BiquadNotch, + pilot_notch_r: BiquadNotch, deemph_m: Deemphasis, deemph_l: Deemphasis, deemph_r: Deemphasis, @@ -459,6 +464,8 @@ impl WfmStereoDecoder { dc_m: DcBlocker::new(0.9999), dc_l: DcBlocker::new(0.9999), dc_r: DcBlocker::new(0.9999), + pilot_notch_l: BiquadNotch::new(audio_rate.max(1) as f32, PILOT_HZ, STEREO_PILOT_NOTCH_Q), + pilot_notch_r: BiquadNotch::new(audio_rate.max(1) as f32, PILOT_HZ, STEREO_PILOT_NOTCH_Q), deemph_m: Deemphasis::new(audio_rate.max(1) as f32, deemphasis_us), deemph_l: Deemphasis::new(audio_rate.max(1) as f32, deemphasis_us), deemph_r: Deemphasis::new(audio_rate.max(1) as f32, deemphasis_us), @@ -493,8 +500,8 @@ impl WfmStereoDecoder { // --- Pilot phase estimator --- let (sin_p, cos_p) = self.pilot_phase.sin_cos(); - let i = self.pilot_i_lp.process(x * cos_p); - let q = self.pilot_q_lp.process(x * -sin_p); + let i = self.pilot_i_lp.process(pilot_tone * cos_p); + let q = self.pilot_q_lp.process(pilot_tone * -sin_p); let phase_err = q.atan2(i); let pilot_phase_est = self.pilot_phase + phase_err; self.pilot_phase += self.pilot_freq; @@ -567,11 +574,13 @@ impl WfmStereoDecoder { // Single-band: uniform blend across all frequencies. diff_i * blend_i }; + let left_corr = (sum_i + diff_denoised) * 0.5; + let right_corr = (sum_i - diff_denoised) * 0.5; let left = self.dc_l - .process(self.deemph_l.process((sum_i + diff_denoised) * 0.5)) + .process(self.pilot_notch_l.process(self.deemph_l.process(left_corr))) .clamp(-1.0, 1.0); let right = self.dc_r - .process(self.deemph_r.process((sum_i - diff_denoised) * 0.5)) + .process(self.pilot_notch_r.process(self.deemph_r.process(right_corr))) .clamp(-1.0, 1.0); output.push(left); output.push(right);