Fixed PLL and subsampler.

examples/sdr_psk31.cc

@@ -18,20 +18,148 @@ static void __sigint_handler(int signo) {
   Queue::get().stop();
 }
 
+/** Implements a fractional sub-sampler. */
 template <class Scalar>
-class BPSK31: public Sink< std::complex<Scalar> >, public Source
+class FracSubSampleBase
+{
+public:
+  /** The input & output type super-scalar. */
+  typedef typename Traits<Scalar>::SScalar SScalar;
+
+public:
+  /** Constructor.
+   * @param frac Specifies the output sample rate relative to the input sample rate. I.e. 2 means
+   *        half the input sample rate. */
+  FracSubSampleBase(double frac)
+    : _avg(0), _sample_count(0), _period(0) {
+    if (frac < 1) {
+      ConfigError err;
+      err << "FracSubSampleBase: Can not sub-sample with fraction smaller one: " << frac;
+      throw err;
+    }
+    _period = (frac*(1<<10));
+  }
+
+  /** Destructor. */
+  virtual ~FracSubSampleBase() {
+    // pass...
+  }
+
+  /** Resets the sample rate fraction. */
+  inline void setFrac(double frac) {
+    if (frac < 1) {
+      ConfigError err;
+      err << "FracSubSampleBase: Can not sub-sample with fraction smaller one: " << frac;
+      throw err;
+    }
+    _period = (frac*(1<<10)); _sample_count=0; _avg = 0;
+  }
+
+  /** Returns the effective sub-sample fraction. */
+  inline double frac() const {
+    return double(_period)/(1<<10);
+  }
+
+  /** Reset sample counter. */
+  inline void reset() {
+    _avg=0; _sample_count=0;
+  }
+
+  /** Performs the sub-sampling. @c in and @c out may refer to the same buffer allowing for an
+   * in-place operation. Returns a view on the output buffer containing the sub-samples. */
+  inline Buffer<Scalar> subsample(const Buffer<Scalar> &in, const Buffer<Scalar> &out) {
+    size_t oidx = 0;
+    for (size_t i=0; i<in.size(); i++) {
+      _avg += in[i]; _sample_count += (1<<10);
+      if (_sample_count >= _period) {
+        out[oidx++] = _avg/SScalar(_sample_count/(1<<10)); _sample_count=0;
+      }
+    }
+    return out.head(oidx);
+  }
+
+protected:
+  /** The average. */
+  SScalar _avg;
+  /** The number of samples collected times (1<<10). */
+  size_t _sample_count;
+  /** The sub-sample period. */
+  size_t _period;
+};
+
+
+
+template <class Scalar>
+class PLL
+{
+public:
+  PLL(double F0, double dF, double bw)
+    : _F(F0), _dF(dF), _P(0)
+  {
+    double damp = sqrt(2)/2;
+    double tmp = (1.+2*damp*bw + bw*bw);
+    _alpha = 4*damp*bw/tmp;
+    _beta  = 4*bw*bw/tmp;
+    _Fmin = _F-_dF; _Fmax = _F+_dF;
+  }
+
+  virtual ~PLL() {
+    // pass...
+  }
+
+  inline void updatePLL(const Buffer<std::complex<Scalar> > &in, const Buffer<uint8_t> &out)
+  {
+    for (size_t i=0; i<in.size(); i++) {
+      out[i] = updatePLL(in[i]);
+    }
+  }
+
+  inline uint8_t updatePLL(const std::complex<Scalar> &in)
+  {
+    float phi = std::atan2(float(in.imag()), float(in.real())) - _P + M_PI;
+    _F += _beta*phi;
+    _P += _F + _alpha*phi;
+    // Limit phase and frequency
+    _mod2PI(_P);
+    _F = std::min(_Fmax, std::max(_Fmin, _F));
+    return ((_P/(2*M_PI))*(1<<8));
+  }
+
+  inline float phase() const { return _P; }
+  inline float frequency() const { return _F; }
+  void setFrequency(double F) {
+    _F = F; _Fmin = _F-_dF; _Fmax = _F+_dF;
+  }
+  void setFreqWidth(double dF) {
+    _dF = dF;
+    _Fmin = _F-_dF; _Fmax = _F+_dF;
+  }
+
+protected:
+  static inline void _mod2PI(float &arg) {
+    while (arg > (2*M_PI)) { arg -= 2*M_PI; }
+    while (arg < (-2*M_PI)) { arg += 2*M_PI; }
+  }
+
+protected:
+  float _F, _dF, _P;
+  float _Fmin, _Fmax;
+  float _alpha, _beta;
+};
+
+
+
+template <class Scalar>
+class BPSK31: public Sink< std::complex<Scalar> >, public Source, public PLL<Scalar>
 {
   typedef typename Traits<Scalar>::SScalar SScalar;
 
 public:
-  BPSK31()
+  BPSK31(double F0, double dF=200.0)
-    : Sink< std::complex<Scalar> >(), Source(), _lut(256)
+    : Sink< std::complex<Scalar> >(), Source(), PLL<Scalar>(0, 0, 2e-1),
+      _subsampler(1), _subsamplebuffer(), _F0(F0), _dF(dF)
   {
-    _shift = 8*(sizeof(int16_t)-sizeof(Scalar));
+    // pass...
-    // Initialize LUT
-    for (size_t i=0; i<256; i++) {
-      _lut[i] = std::exp(std::complex<double>(0., (-2*M_PI*i)/256))*(1<<8);
-    }
   }
 
   /** Destructor. */
@@ -41,8 +169,8 @@ public:
 
   /** Configures the FM demodulator. */
   virtual void config(const Config &src_cfg) {
-    // Requires type & buffer size
+    // Requires type, buffer size & sample rate
-    if (!src_cfg.hasType() || !src_cfg.hasBufferSize()) { return; }
+    if (!src_cfg.hasType() || !src_cfg.hasSampleRate() || !src_cfg.hasBufferSize()) { return; }
     // Check if buffer type matches template
     if (Config::typeId< std::complex<Scalar> >() != src_cfg.type()) {
       ConfigError err;
@@ -50,94 +178,86 @@ public:
           << ", expected " << Config::typeId< std::complex<Scalar> >();
       throw err;
     }
+
+    // Update frequencies of PLL
+    this->setFrequency(2*M_PI*_F0/src_cfg.sampleRate());
+    this->setFreqWidth(2*M_PI*_dF/src_cfg.sampleRate());
+
+    // Configure sub-sampler to 8kHz sample rate:
+    _subsampler.setFrac(src_cfg.sampleRate()/8000);
+    _subsamplebuffer = Buffer< std::complex<Scalar> >(src_cfg.bufferSize());
+
     // Unreference buffer if non-empty
     if (! _buffer.isEmpty()) { _buffer.unref(); }
     // Allocate buffer
-    _buffer =  Buffer<int16_t>(16);
+    _buffer = Buffer<uint8_t>(256);
+    _hist   = Buffer<uint8_t>(256);
+    // Clear history
+    for (size_t i=0; i<_hist.size(); i++) { _hist[i] = 0; }
 
-    _lut_idx = 0;
+    // Check input sample-rate
-    _lut_incr = ((256.*256.*2144.)/src_cfg.sampleRate());
+    if (src_cfg.sampleRate() < 8000) {
-    _pll_incr = 0;
+      ConfigError err;
-    double tau = 1./20;
+      err << "Can not configure BPSK31 node: Input sample rate must be at least 8000Hz! "
-    _alpha = 2*M_PI/(2*M_PI+src_cfg.sampleRate()*tau);
+          << "Sample rate = " << src_cfg.sampleRate();
-    _lp_phase = 0;
+      throw err;
-    // Results into a 4*31.25 Hz sample
+    }
-    _subsample = (256.*src_cfg.sampleRate()/(4*32.25));
+    // bit counter...
-    _sample_count = 0;
     _dec_count = 0;
 
     LogMessage msg(LOG_DEBUG);
     msg << "Configured BPSK31 node:" << std::endl
         << " sample-rate: " << src_cfg.sampleRate() << std::endl
+        << " sub-sample by: " << _subsampler.frac()
+        << " to: " << src_cfg.sampleRate()/_subsampler.frac() << "Hz" << std::endl
         << " in-type / out-type: " << src_cfg.type()
-        << " / " << Config::typeId<int16_t>() << std::endl
+        << " / " << Config::typeId<uint8_t>() << std::endl;
-        << " in-place: " << (_can_overwrite ? "true" : "false") << std::endl
-        << " output scale: 2^" << _shift << std::endl
-        << " PLL tau/alpha: " << tau << "/" << _alpha;
     Logger::get().log(msg);
 
     // Propergate config
-    this->setConfig(Config(Config::typeId<int16_t>(), 4*31.25, 16, 1));
+    this->setConfig(Config(Config::typeId<uint8_t>(), 8000.0, 256, 1));
   }
 
+
   /** Performs the FM demodulation. */
   virtual void process(const Buffer<std::complex<Scalar> > &buffer, bool allow_overwrite)
   {
-    if (0 == buffer.size()) { return; }
+    // First, sub-sample to 8000Hz
-    _process(buffer, _buffer);
+    Buffer< std::complex<Scalar> > samples;
-  }
+    if (allow_overwrite) { samples = _subsampler.subsample(buffer, buffer); }
+    else { samples = _subsampler.subsample(buffer, _subsamplebuffer); }
 
-protected:
+    uint8_t phase = 0;
-  /** The actual demodulation. */
+    for (size_t i=1; i<samples.size(); i++) {
-  void _process(const Buffer< std::complex<Scalar> > &in, const Buffer<int16_t> &out)
+      // Update PLL
-  {
+      phase = this->updatePLL(buffer[i]);
-    // Calc abs phase values
+      // Obtain phase-difference with respect to last bit
-    for (size_t i=1; i<in.size(); i++) {
+      // and store current phase
-      std::complex<double> val =
+      _buffer[_dec_count] = std::abs(int16_t(phase) - int16_t(_hist[_dec_count]));
-          std::complex<double>(in[i].real(), in[i].imag()) * _lut[_lut_idx>>8];
+      _hist[_dec_count] = phase; _dec_count++;
-      _lut_idx += (_lut_incr+_pll_incr);
+      if (256 == _dec_count) {
-      _sample_count += 256;
+        // propergate resulting 256 bits
-      // Get phase difference
+        this->send(_buffer.head(256)); _dec_count = 0;
-      double phase = std::atan2(double(val.imag()), double(val.real()))/M_PI;
-      // low-pass
-      _lp_phase = (1.-_alpha)*_lp_phase + _alpha*phase;
-      // Correct PLL frequency by LP-ed phase
-      _pll_incr += _lp_phase*10;
-      std::cout << "Fcorr=" << 22050. * double(_pll_incr)/(1<<16)
-                << "; Phi=" << 180*phase << " (" << 180*_lp_phase << ")" << std::endl;
-      if (_sample_count >= _subsample) {
-        out[_dec_count] = (_lp_phase*(1<<14));
-        _sample_count -= _subsample; _dec_count++;
-        // If 8 bits has been collected:
-        if (8 == _dec_count) {
-          // propergate resulting 8 bits
-          this->send(out.head(8));
-          _dec_count = 0;
-        }
       }
     }
   }
 
+
 protected:
-  int _shift;
+  /** Precise sub-sampler to 8000Hz sample rate. */
-  /** If true, in-place demodulation is poissible. */
+  FracSubSampleBase< std::complex<Scalar> > _subsampler;
-  bool _can_overwrite;
+  Buffer< std::complex<Scalar> > _subsamplebuffer;
-  Buffer< std::complex<SScalar> > _lut;
+
-  /** The current LUT index. It is defined in multiples of 256 for heiher precision. */
+  double _F0, _dF;
-  uint16_t _lut_idx;
-  /** The LUT increment is defined in multiples of 256 for higher precision. */
-  int32_t _lut_incr;
-  /** The PLL increment correction is defined in multiples of 256 for higher precision. */
-  double _pll_incr;
-  double _alpha;
-  double _lp_phase;
-  size_t _subsample;
-  size_t _sample_count;
   size_t _dec_count;
+
   /** The output buffer, unused if demodulation is performed in-place. */
-  Buffer<int16_t> _buffer;
+  Buffer<uint8_t> _buffer;
+  Buffer<uint8_t> _hist;
 };
 
+
+
 int main(int argc, char *argv[]) {
   if (2 != argc) {
     std::cerr << "Usage: sdr_psk31 FILENAME" << std::endl;
@@ -163,22 +283,24 @@ int main(int argc, char *argv[]) {
   PortAudio::init();
   Queue &queue = Queue::get();
 
-  //WavSource src(argv[1]);
+  WavSource src(argv[1]);
-  SigGen<int16_t> src(22050., 1024);
+  //SigGen<int16_t> src(22050., 1024);
-  src.addSine(2144);
+  //src.addSine(2144);
   AutoCast< std::complex<int16_t> > cast;
-  IQBaseBand<int16_t> baseband(0.0, 2144., 400.0, 15, 1, 8000.0);
+  IQBaseBand<int16_t> baseband(0, 2144., 400.0, 15, 1);
-  BPSK31<int16_t> demod;
+  BPSK31<int16_t> demod(2144.0);
   PortSink sink;
+  DebugDump<uint8_t> dump;
 
   src.connect(&cast, true);
   cast.connect(&baseband, true);
   baseband.connect(spec);
   baseband.connect(&sink);
   baseband.connect(&demod);
+  demod.connect(&dump, true);
 
-  //queue.addIdle(&src, &WavSource::next);
+  queue.addIdle(&src, &WavSource::next);
-  queue.addIdle(&src, &SigGen<int16_t>::next);
+  //queue.addIdle(&src, &SigGen<int16_t>::next);
 
   queue.start();
   app.exec();