Branch data     Line data    Source code

       1                 :             : #include "UpSampler.h"
       2                 :             : #include <algorithm>
       3                 :             : 
       4                 :             : namespace ffb_math {
       5                 :             : 
       6                 :          14 : PolyphaseResampler::PolyphaseResampler() {
       7                 :          14 :     Reset();
       8                 :          14 : }
       9                 :             : 
      10                 :          17 : void PolyphaseResampler::Reset() {
      11                 :          17 :     m_phase = 0;
      12                 :          17 :     m_needs_shift = false;
      13                 :          17 :     m_pending_sample = 0.0;
      14         [ +  - ]:          17 :     m_history.fill(0.0);
      15                 :          17 : }
      16                 :             : 
      17                 :        7995 : double PolyphaseResampler::Process(double latest_physics_sample, bool is_new_physics_tick) {
      18         [ +  + ]:        7995 :     if (is_new_physics_tick) {
      19                 :        3196 :         m_pending_sample = latest_physics_sample;
      20                 :             :     }
      21                 :             : 
      22         [ +  + ]:        7995 :     if (m_needs_shift) {
      23                 :             :         // Shift history and add pending sample
      24                 :        3189 :         m_history[0] = m_history[1];
      25                 :        3189 :         m_history[1] = m_history[2];
      26                 :        3189 :         m_history[2] = m_pending_sample;
      27                 :        3189 :         m_needs_shift = false;
      28                 :             :     }
      29                 :             : 
      30                 :             :     // Apply the 3-tap FIR filter for the current phase
      31                 :        7995 :     const double* c = COEFFS[m_phase];
      32                 :             : 
      33                 :             :     // FIX 1: Correct convolution order.
      34                 :             :     // c[0] is the earliest tap, so it must multiply the newest sample (m_history[2]).
      35                 :             :     // y[n] = c[0]*x[n] + c[1]*x[n-1] + c[2]*x[n-2]
      36                 :        7995 :     double output = c[0] * m_history[2] + c[1] * m_history[1] + c[2] * m_history[0];
      37                 :             : 
      38                 :             :     // Advance phase (1000Hz ticks)
      39                 :             :     // The phase accumulator in main.cpp handles the 2/5 relationship,
      40                 :             :     // but the resampler itself needs to know which branch of the polyphase filter to use.
      41                 :             :     // Each call to Process is one 1000Hz tick.
      42                 :             : 
      43                 :             :     // FIX 2: For a 5/2 resampling ratio (400Hz to 1000Hz), the phase must advance by 2
      44                 :             :     // modulo 5 for every output sample.
      45                 :        7995 :     m_phase += 2;
      46         [ +  + ]:        7995 :     if (m_phase >= 5) {
      47                 :        3196 :         m_phase -= 5;
      48                 :        3196 :         m_needs_shift = true;
      49                 :             :     }
      50                 :             : 
      51                 :        7995 :     return output;
      52                 :             : }
      53                 :             : 
      54                 :             : } // namespace ffb_math