[feat](trx-backend-soapysdr): decode WFM stereo in SDR pipeline

Co-authored-by: OpenAI Codex <codex@openai.com>
Signed-off-by: Stan Grams <sjg@haxx.space>

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"),
                             };

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<f32>]) -> Vec<f32> {
+        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 {

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<f32>,
     /// 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<WfmStereoDecoder>,
 }
 
 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<Vec<f32>>,
     ) -> 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<dyn IqSource>,
         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::<Vec<f32>>(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::<Vec<f32>>(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);
     }

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,