diff --git a/src/trx-server/src/main.rs b/src/trx-server/src/main.rs index 221ef9b..8fa149b 100644 --- a/src/trx-server/src/main.rs +++ b/src/trx-server/src/main.rs @@ -242,7 +242,7 @@ fn default_audio_bandwidth_for_mode(mode: &trx_core::rig::state::RigMode) -> u32 RigMode::CW | RigMode::CWR => 500, RigMode::AM => 6_000, RigMode::FM => 12_500, - RigMode::WFM => 75_000, + RigMode::WFM => 180_000, RigMode::Other(_) => 3_000, } } @@ -314,6 +314,7 @@ fn build_sdr_rig_from_instance( &rig_cfg.sdr.gain.mode, rig_cfg.sdr.gain.value, rig_cfg.audio.sample_rate, + rig_cfg.audio.channels as usize, rig_cfg.audio.frame_duration_ms, Freq { hz: rig_cfg.rig.initial_freq_hz, @@ -492,6 +493,9 @@ fn spawn_rig_audio_stack( let rx_audio_tx_sdr = rx_audio_tx.clone(); let sdr_sample_rate = rig_cfg.audio.sample_rate; let sdr_channels = rig_cfg.audio.channels; + let sdr_frame_samples = (rig_cfg.audio.sample_rate as usize + * rig_cfg.audio.frame_duration_ms as usize) + / 1000; let sdr_bitrate_bps = rig_cfg.audio.bitrate_bps; handles.push(tokio::spawn(async move { let opus_ch = match sdr_channels { @@ -520,12 +524,16 @@ fn spawn_rig_audio_stack( let pcm_frame = match sdr_channels { 1 => frame, 2 => { - let mut stereo = Vec::with_capacity(frame.len() * 2); - for sample in frame { - stereo.push(sample); - stereo.push(sample); + if frame.len() >= sdr_frame_samples * 2 { + frame + } else { + let mut stereo = Vec::with_capacity(frame.len() * 2); + for sample in frame { + stereo.push(sample); + stereo.push(sample); + } + stereo } - stereo } _ => unreachable!("validated above"), }; 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 04e1f1f..61e480c 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 @@ -5,6 +5,132 @@ use num_complex::Complex; use trx_core::rig::state::RigMode; +#[derive(Debug, Clone)] +struct OnePoleLowPass { + alpha: f32, + y: f32, +} + +impl OnePoleLowPass { + fn new(sample_rate: f32, cutoff_hz: f32) -> Self { + let sr = sample_rate.max(1.0); + let cutoff = cutoff_hz.clamp(1.0, sr * 0.49); + let dt = 1.0 / sr; + let rc = 1.0 / (2.0 * std::f32::consts::PI * cutoff); + let alpha = dt / (rc + dt); + Self { alpha, y: 0.0 } + } + + fn process(&mut self, x: f32) -> f32 { + self.y += self.alpha * (x - self.y); + self.y + } +} + +#[derive(Debug, Clone)] +struct Deemphasis { + alpha: f32, + y: f32, +} + +impl Deemphasis { + fn new(sample_rate: f32, tau_us: f32) -> Self { + let sr = sample_rate.max(1.0); + let tau = (tau_us.max(1.0)) * 1e-6; + let alpha = 1.0 - (-1.0 / (sr * tau)).exp(); + Self { alpha, y: 0.0 } + } + + fn process(&mut self, x: f32) -> f32 { + self.y += self.alpha * (x - self.y); + self.y + } +} + +#[derive(Debug, Clone)] +pub struct WfmStereoDecoder { + output_channels: usize, + pilot_phase: f32, + pilot_freq: f32, + pilot_freq_err: f32, + pilot_i_lp: OnePoleLowPass, + pilot_q_lp: OnePoleLowPass, + sum_lp: OnePoleLowPass, + diff_lp: OnePoleLowPass, + deemph_m: Deemphasis, + deemph_l: Deemphasis, + deemph_r: Deemphasis, + output_decim: usize, + output_counter: usize, +} + +impl WfmStereoDecoder { + pub fn new(composite_rate: u32, audio_rate: u32, output_channels: usize) -> Self { + let composite_rate_f = composite_rate.max(1) as f32; + let output_decim = (composite_rate / audio_rate.max(1)).max(1) as usize; + Self { + output_channels: output_channels.max(1), + pilot_phase: 0.0, + pilot_freq: 2.0 * std::f32::consts::PI * 19_000.0 / composite_rate_f, + pilot_freq_err: 0.0, + pilot_i_lp: OnePoleLowPass::new(composite_rate_f, 400.0), + pilot_q_lp: OnePoleLowPass::new(composite_rate_f, 400.0), + sum_lp: OnePoleLowPass::new(composite_rate_f, 15_000.0), + diff_lp: OnePoleLowPass::new(composite_rate_f, 15_000.0), + deemph_m: Deemphasis::new(audio_rate.max(1) as f32, 75.0), + deemph_l: Deemphasis::new(audio_rate.max(1) as f32, 75.0), + deemph_r: Deemphasis::new(audio_rate.max(1) as f32, 75.0), + output_decim, + output_counter: 0, + } + } + + pub fn process_iq(&mut self, samples: &[Complex]) -> Vec { + let composite = demod_fm(samples); + if composite.is_empty() { + return Vec::new(); + } + + let mut output = Vec::with_capacity( + (composite.len() / self.output_decim.max(1)) * self.output_channels.max(1), + ); + + for x in composite { + 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 phase_err = q.atan2(i); + self.pilot_freq_err = (self.pilot_freq_err + phase_err * 0.00002).clamp(-0.02, 0.02); + self.pilot_phase += self.pilot_freq + self.pilot_freq_err + phase_err * 0.0015; + self.pilot_phase = self.pilot_phase.rem_euclid(std::f32::consts::TAU); + + let pilot_mag = (i * i + q * q).sqrt(); + let stereo_blend = (pilot_mag * 40.0).clamp(0.0, 1.0); + + let sum = self.sum_lp.process(x); + let stereo_carrier = (2.0 * self.pilot_phase).cos() * 2.0; + let diff = self.diff_lp.process(x * stereo_carrier) * stereo_blend; + + self.output_counter += 1; + if self.output_counter < self.output_decim { + continue; + } + self.output_counter = 0; + + if self.output_channels >= 2 { + let left = self.deemph_l.process((sum + diff) * 0.5).clamp(-1.0, 1.0); + let right = self.deemph_r.process((sum - diff) * 0.5).clamp(-1.0, 1.0); + output.push(left); + output.push(right); + } else { + output.push(self.deemph_m.process(sum).clamp(-1.0, 1.0)); + } + } + + output + } +} + /// Selects the demodulation algorithm for a channel. #[derive(Debug, Clone, PartialEq)] pub enum Demodulator { diff --git a/src/trx-server/trx-backend/trx-backend-soapysdr/src/dsp.rs b/src/trx-server/trx-backend/trx-backend-soapysdr/src/dsp.rs index 85417ca..3f6fc4c 100644 --- a/src/trx-server/trx-backend/trx-backend-soapysdr/src/dsp.rs +++ b/src/trx-server/trx-backend/trx-backend-soapysdr/src/dsp.rs @@ -20,7 +20,7 @@ use rustfft::{Fft, FftPlanner}; use tokio::sync::broadcast; use trx_core::rig::state::RigMode; -use crate::demod::Demodulator; +use crate::demod::{Demodulator, WfmStereoDecoder}; // --------------------------------------------------------------------------- // IQ source abstraction @@ -252,14 +252,24 @@ pub struct ChannelDsp { pub channel_if_hz: f64, /// Current demodulator (can be swapped via `set_mode`). pub demodulator: Demodulator, + /// Current rig mode so the decimation pipeline can be rebuilt. + mode: RigMode, /// FFT-based FIR low-pass filter applied to I component before decimation. lpf_i: BlockFirFilter, /// FFT-based FIR low-pass filter applied to Q component before decimation. lpf_q: BlockFirFilter, /// SDR capture sample rate — kept for filter rebuilds. sdr_sample_rate: u32, + /// Output audio sample rate. + audio_sample_rate: u32, + /// Requested audio bandwidth. + audio_bandwidth_hz: u32, + /// FIR tap count used when rebuilding filters. + fir_taps: usize, /// Decimation factor: `sdr_sample_rate / audio_sample_rate`. pub decim_factor: usize, + /// Number of PCM channels emitted in each frame. + output_channels: usize, /// Accumulator for output PCM frames. pub frame_buf: Vec, /// Target frame size in samples. @@ -272,40 +282,97 @@ pub struct ChannelDsp { pub mixer_phase_inc: f64, /// Decimation counter. decim_counter: usize, + /// Dedicated WFM decoder that preserves the FM composite baseband. + wfm_decoder: Option, } impl ChannelDsp { + fn pipeline_rates( + mode: &RigMode, + sdr_sample_rate: u32, + audio_sample_rate: u32, + audio_bandwidth_hz: u32, + ) -> (usize, u32) { + if sdr_sample_rate == 0 { + return (1, audio_sample_rate.max(1)); + } + + let target_rate = if *mode == RigMode::WFM { + audio_bandwidth_hz.max(audio_sample_rate.saturating_mul(4)).max(228_000) + } else { + audio_sample_rate.max(1) + }; + let decim_factor = (sdr_sample_rate / target_rate.max(1)).max(1) as usize; + let channel_sample_rate = (sdr_sample_rate / decim_factor as u32).max(1); + (decim_factor, channel_sample_rate) + } + + fn rebuild_filters(&mut self) { + let (_, channel_sample_rate) = Self::pipeline_rates( + &self.mode, + self.sdr_sample_rate, + self.audio_sample_rate, + self.audio_bandwidth_hz, + ); + let cutoff_hz = self + .audio_bandwidth_hz + .min(channel_sample_rate.saturating_sub(1)) as f32 + / 2.0; + let cutoff_norm = if self.sdr_sample_rate == 0 { + 0.1 + } else { + (cutoff_hz / self.sdr_sample_rate as f32).min(0.499) + }; + self.lpf_i = BlockFirFilter::new(cutoff_norm, self.fir_taps, IQ_BLOCK_SIZE); + self.lpf_q = BlockFirFilter::new(cutoff_norm, self.fir_taps, IQ_BLOCK_SIZE); + self.decim_factor = Self::pipeline_rates( + &self.mode, + self.sdr_sample_rate, + self.audio_sample_rate, + self.audio_bandwidth_hz, + ) + .0; + self.decim_counter = 0; + self.wfm_decoder = if self.mode == RigMode::WFM { + Some(WfmStereoDecoder::new( + channel_sample_rate, + self.audio_sample_rate, + self.output_channels, + )) + } else { + None + }; + self.frame_buf.clear(); + } + #[allow(clippy::too_many_arguments)] pub fn new( channel_if_hz: f64, mode: &RigMode, sdr_sample_rate: u32, audio_sample_rate: u32, + output_channels: usize, frame_duration_ms: u16, audio_bandwidth_hz: u32, fir_taps: usize, pcm_tx: broadcast::Sender>, ) -> Self { - let decim_factor = if audio_sample_rate == 0 || sdr_sample_rate == 0 { - 1 - } else { - (sdr_sample_rate / audio_sample_rate).max(1) as usize - }; - + let output_channels = output_channels.max(1); let frame_size = if audio_sample_rate == 0 || frame_duration_ms == 0 { - 960 + 960 * output_channels } else { - (audio_sample_rate as usize * frame_duration_ms as usize) / 1000 + (audio_sample_rate as usize * frame_duration_ms as usize * output_channels) / 1000 }; + let taps = fir_taps.max(1); + let (decim_factor, channel_sample_rate) = + Self::pipeline_rates(mode, sdr_sample_rate, audio_sample_rate, audio_bandwidth_hz); + let cutoff_hz = audio_bandwidth_hz.min(channel_sample_rate.saturating_sub(1)) as f32 / 2.0; let cutoff_norm = if sdr_sample_rate == 0 { 0.1 } else { - (audio_bandwidth_hz as f32 / 2.0) / sdr_sample_rate as f32 - } - .min(0.499); - - let taps = fir_taps.max(1); + (cutoff_hz / sdr_sample_rate as f32).min(0.499) + }; let mixer_phase_inc = if sdr_sample_rate == 0 { 0.0 @@ -316,36 +383,46 @@ impl ChannelDsp { Self { channel_if_hz, demodulator: Demodulator::for_mode(mode), + mode: mode.clone(), lpf_i: BlockFirFilter::new(cutoff_norm, taps, IQ_BLOCK_SIZE), lpf_q: BlockFirFilter::new(cutoff_norm, taps, IQ_BLOCK_SIZE), sdr_sample_rate, + audio_sample_rate, + audio_bandwidth_hz, + fir_taps: taps, decim_factor, - frame_buf: Vec::with_capacity(frame_size * 2), + output_channels, + frame_buf: Vec::with_capacity(frame_size + output_channels), frame_size, pcm_tx, mixer_phase: 0.0, mixer_phase_inc, decim_counter: 0, + wfm_decoder: if *mode == RigMode::WFM { + Some(WfmStereoDecoder::new( + channel_sample_rate, + audio_sample_rate, + output_channels, + )) + } else { + None + }, } } pub fn set_mode(&mut self, mode: &RigMode) { + self.mode = mode.clone(); self.demodulator = Demodulator::for_mode(mode); + self.rebuild_filters(); } /// Rebuild the FIR low-pass filters with new bandwidth and tap count. /// /// Changes take effect on the next call to `process_block`. pub fn set_filter(&mut self, bandwidth_hz: u32, taps: usize) { - let cutoff_norm = if self.sdr_sample_rate == 0 { - 0.1 - } else { - (bandwidth_hz as f32 / 2.0) / self.sdr_sample_rate as f32 - } - .min(0.499); - let taps = taps.max(1); - self.lpf_i = BlockFirFilter::new(cutoff_norm, taps, IQ_BLOCK_SIZE); - self.lpf_q = BlockFirFilter::new(cutoff_norm, taps, IQ_BLOCK_SIZE); + self.audio_bandwidth_hz = bandwidth_hz; + self.fir_taps = taps.max(1); + self.rebuild_filters(); } /// Process a block of raw IQ samples through the full DSP chain. @@ -406,7 +483,11 @@ impl ChannelDsp { } // --- 4. Demodulate -------------------------------------------------- - let audio = self.demodulator.demodulate(&decimated); + let audio = if let Some(decoder) = self.wfm_decoder.as_mut() { + decoder.process_iq(&decimated) + } else { + self.demodulator.demodulate(&decimated) + }; // --- 5. Emit complete PCM frames ------------------------------------ self.frame_buf.extend_from_slice(&audio); @@ -438,6 +519,7 @@ impl SdrPipeline { source: Box, sdr_sample_rate: u32, audio_sample_rate: u32, + output_channels: usize, frame_duration_ms: u16, channels: &[(f64, RigMode, u32, usize)], ) -> Self { @@ -456,6 +538,7 @@ impl SdrPipeline { mode, sdr_sample_rate, audio_sample_rate, + output_channels, frame_duration_ms, audio_bandwidth_hz, fir_taps, @@ -677,7 +760,7 @@ mod tests { #[test] fn channel_dsp_processes_silence() { let (pcm_tx, _pcm_rx) = broadcast::channel::>(8); - let mut dsp = ChannelDsp::new(0.0, &RigMode::USB, 48_000, 8_000, 20, 3000, 31, pcm_tx); + let mut dsp = ChannelDsp::new(0.0, &RigMode::USB, 48_000, 8_000, 1, 20, 3000, 31, pcm_tx); let block = vec![Complex::new(0.0_f32, 0.0_f32); 4096]; dsp.process_block(&block); } @@ -685,7 +768,7 @@ mod tests { #[test] fn channel_dsp_set_mode() { let (pcm_tx, _) = broadcast::channel::>(8); - let mut dsp = ChannelDsp::new(0.0, &RigMode::USB, 48_000, 8_000, 20, 3000, 31, pcm_tx); + let mut dsp = ChannelDsp::new(0.0, &RigMode::USB, 48_000, 8_000, 1, 20, 3000, 31, pcm_tx); assert_eq!(dsp.demodulator, Demodulator::Usb); dsp.set_mode(&RigMode::FM); assert_eq!(dsp.demodulator, Demodulator::Fm); @@ -697,6 +780,7 @@ mod tests { Box::new(MockIqSource), 1_920_000, 48_000, + 1, 20, &[(200_000.0, RigMode::USB, 3000, 64)], ); @@ -706,7 +790,7 @@ mod tests { #[test] fn pipeline_empty_channels() { - let pipeline = SdrPipeline::start(Box::new(MockIqSource), 1_920_000, 48_000, 20, &[]); + let pipeline = SdrPipeline::start(Box::new(MockIqSource), 1_920_000, 48_000, 1, 20, &[]); assert_eq!(pipeline.pcm_senders.len(), 0); assert_eq!(pipeline.channel_dsps.len(), 0); } diff --git a/src/trx-server/trx-backend/trx-backend-soapysdr/src/lib.rs b/src/trx-server/trx-backend/trx-backend-soapysdr/src/lib.rs index 41025b7..ec6c0df 100644 --- a/src/trx-server/trx-backend/trx-backend-soapysdr/src/lib.rs +++ b/src/trx-server/trx-backend/trx-backend-soapysdr/src/lib.rs @@ -69,6 +69,7 @@ impl SoapySdrRig { gain_mode: &str, gain_db: f64, audio_sample_rate: u32, + audio_channels: usize, frame_duration_ms: u16, initial_freq: Freq, initial_mode: RigMode, @@ -108,6 +109,7 @@ impl SoapySdrRig { iq_source, sdr_sample_rate, audio_sample_rate, + audio_channels, frame_duration_ms, channels, ); @@ -188,6 +190,7 @@ impl SoapySdrRig { "auto", 30.0, 48_000, + 1, 20, Freq { hz: 144_300_000 }, RigMode::USB,