lasp_siggen_impl.cpp

Go to the documentation of this file.

// lasp_siggen_impl.cpp
//
// Author: J.A. de Jong -ASCEE
//
// Description:
// Signal generators implementation
/* #define DEBUGTRACE_ENABLED */
#include "lasp_siggen_impl.h"
#include "debugtrace.hpp"
#include "lasp_mathtypes.h"
#include <cassert>
 
using rte = std::runtime_error;
 
DEBUGTRACE_VARIABLES;
 
#define NITER_NEWTON 20
 
Noise::Noise(){DEBUGTRACE_ENTER}
 
vd Noise::genSignalUnscaled(us nframes) {
  return arma::randn<vd>(nframes);
}
void Noise::resetImpl() {}
 
Sine::Sine(const d freq) : omg(2 * arma::datum::pi * freq) { DEBUGTRACE_ENTER; }
 
vd Sine::genSignalUnscaled(const us nframes) {
  /* DEBUGTRACE_ENTER; */
  const d pi = arma::datum::pi;
  vd phase_vec =
      arma::linspace(phase, phase + omg * (nframes - 1) / _fs, nframes);
  phase += omg * nframes / _fs;
  while (phase > 2 * arma::datum::pi) {
    phase -= 2 * pi;
  }
  return arma::sin(phase_vec);
}
 
vd Periodic::genSignalUnscaled(const us nframes) {
 
  vd res(nframes);
  if (_signal.size() == 0) {
    throw rte("No signal defined while calling");
  }
  for (us i = 0; i < nframes; i++) {
    res(i) = _signal[_cur_pos];
    _cur_pos++;
    _cur_pos %= _signal.size();
  }
  return res;
}
 
Sweep::Sweep(const d fl, const d fu, const d Ts, const d Tq, const us flags)
    : fl_(fl), fu_(fu), Ts(Ts), Tq(Tq), flags(flags) {
  if (fl <= 0 || fu < fl || Ts <= 0) {
    throw rte("Invalid sweep parameters");
  }
  if ((flags & ForwardSweep) && (flags & BackwardSweep)) {
    throw rte(
        "Both forward and backward sweep flag set. Please only set either one "
        "or none for a continuous sweep");
  }
  if ((flags & LinearSweep) && (flags & LogSweep)) {
    throw rte(
        "Both logsweep and linear sweep flag set. Please only set either one.");
  }
  if (!((flags & LinearSweep) || (flags & LogSweep))) {
    throw rte("Either LinearSweep or LogSweep should be given as flag");
  }
}
 
void Sweep::resetImpl() {
 
  DEBUGTRACE_ENTER;
 
  _cur_pos = 0;
 
  bool forward_sweep = flags & ForwardSweep;
  bool backward_sweep = flags & BackwardSweep;
 
  const d Dt = 1 / _fs; // Deltat
 
  // Estimate N, the number of samples in the sweep part (non-quiescent part):
  const us Ns = (us)(Ts * _fs);
  const us Nq = (us)(Tq * _fs);
  const us N = Ns + Nq;
 
  _signal = vd(N, arma::fill::zeros);
  index = 0;
 
  d fl, fu;
  /* Swap fl and fu for a backward sweep */
  if (backward_sweep) {
    fu = fl_;
    fl = fu_;
  } else {
    /* Case of continuous sweep, or forward sweep */
    fl = fl_;
    fu = fu_;
  }
 
  d phase = 0;
 
  /* Linear sweep */
  if (flags & LinearSweep) {
    if (forward_sweep || backward_sweep) {
      /* Forward or backward sweep */
      /* TRACE(15, "Forward or backward sweep"); */
      us K = (us)(Dt * (fl * Ns + 0.5 * (Ns - 1) * (fu - fl)));
      d eps_num = ((d)K) / Dt - fl * Ns - 0.5 * (Ns - 1) * (fu - fl);
      d eps = eps_num / (0.5 * (Ns - 1));
      /* iVARTRACE(15, K); */
      /* dVARTRACE(15, eps); */
 
      for (us n = 0; n < Ns; n++) {
        _signal(n) = d_sin(phase);
        d fn = fl + ((d)n) / Ns * (fu + eps - fl);
        phase += 2 * arma::datum::pi * Dt * fn;
      }
    } else {
      /* Continous sweep */
      /* TRACE(15, "continuous sweep"); */
 
      /* iVARTRACE(17, N); */
      /* dVARTRACE(17, fl); */
      /* dVARTRACE(17, fu); */
 
      const us Nf = Ns / 2;
      const us Nb = Ns - Nf;
 
      /* Phi halfway */
      d phih = 2 * number_pi * Dt * (fl * Nf + 0.5 * (Nf - 1) * (fu - fl));
 
      us K =
          (us)(phih / (2 * number_pi) + Dt * (fu * Nb - (Nb - 1) * (fu - fl)));
 
      d eps_num1 = (K - phih / (2 * number_pi)) / Dt;
      d eps_num2 = -fu * Nb + (Nb - 1) * (fu - fl);
      d eps = (eps_num1 + eps_num2) / (0.5 * (Nb + 1));
 
      /* iVARTRACE(15, K); */
      /* dVARTRACE(15, eps); */
      d phase = 0;
 
      for (us n = 0; n <= Ns; n++) {
        /* iVARTRACE(17, n); */
        if (n < N) {
          _signal[n] = d_sin(phase);
        }
 
        d fn;
        if (n <= Nf) {
          fn = fl + ((d)n) / Nf * (fu - fl);
        } else {
          fn = fu - ((d)n - Nf) / Nb * (fu + eps - fl);
        }
        /* dbgassert(fn >= 0, "BUG"); */
 
        phase += 2 * number_pi * Dt * fn;
      }
      /* This should be a very small number!! */
      /* dVARTRACE(15, phase); */
    }
  } else if (flags & LogSweep) {
 
    DEBUGTRACE_PRINT("Log sweep");
    if (forward_sweep || backward_sweep) {
      /* Forward or backward sweep */
      DEBUGTRACE_PRINT("Forward or backward sweep");
      d k1 = (fu / fl);
      us K = (us)(Dt * fl * (k1 - 1) / (d_pow(k1, 1.0 / Ns) - 1));
      d k = k1;
 
      /* Iterate k to the right solution */
      d E;
      for (us iter = 0; iter < 10; iter++) {
        E = 1 + K / (Dt * fl) * (d_pow(k, 1.0 / Ns) - 1) - k;
        d dEdk = K / (Dt * fl) * d_pow(k, 1.0 / Ns) / (Ns * k) - 1;
        k -= E / dEdk;
      }
 
      DEBUGTRACE_PRINT(K);
      DEBUGTRACE_PRINT(k1);
      DEBUGTRACE_PRINT(k);
      DEBUGTRACE_PRINT(E);
 
      for (us n = 0; n < Ns; n++) {
        _signal[n] = d_sin(phase);
        d fn = fl * d_pow(k, ((d)n) / Ns);
        phase += 2 * number_pi * Dt * fn;
      }
    } else {
 
      DEBUGTRACE_PRINT("Continuous sweep");
 
      const us Nf = Ns / 2;
      const us Nb = Ns - Nf;
      const d k1 = (fu / fl);
      const d phif1 =
          2 * number_pi * Dt * fl * (k1 - 1) / (d_pow(k1, 1.0 / Nf) - 1);
      const us K = (us)(phif1 / (2 * number_pi) +
                        Dt * fu * (1 / k1 - 1) / (d_pow(1 / k1, 1.0 / Nb) - 1));
 
      d E;
      d k = k1;
 
      /* Newton iterations to converge k to the value such that the sweep is
       * continuous */
      for (us iter = 0; iter < NITER_NEWTON; iter++) {
        E = (k - 1) / (d_pow(k, 1.0 / Nf) - 1) +
            (k - 1) / (1 - d_pow(k, -1.0 / Nb)) - K / Dt / fl;
        DEBUGTRACE_PRINT(E);
 
        /* All parts of the derivative of above error E to k */
        d dEdk1 = 1 / (d_pow(k, 1.0 / Nf) - 1);
        d dEdk2 = (1 / k - 1) / (d_pow(k, -1.0 / Nb) - 1);
        d dEdk3 = -1 / (k * (d_pow(k, -1.0 / Nb) - 1));
        d dEdk4 = d_pow(k, -1.0 / Nb) * (1 / k - 1) /
                  (Nb * d_pow(d_pow(k, -1.0 / Nb) - 1, 2));
        d dEdk5 = -d_pow(k, 1.0 / Nf) * (k - 1) /
                  (Nf * k * d_pow(d_pow(k, 1.0 / Nf) - 1, 2));
        d dEdk = dEdk1 + dEdk2 + dEdk3 + dEdk4 + dEdk5;
 
        /* Iterate! */
        k -= E / dEdk;
      }
 
      DEBUGTRACE_PRINT(K);
      DEBUGTRACE_PRINT(k1);
      DEBUGTRACE_PRINT(k);
      DEBUGTRACE_PRINT(E);
 
      for (us n = 0; n <= Ns; n++) {
        /* iVARTRACE(17, n); */
        if (n < Ns) {
          _signal[n] = d_sin(phase);
        }
 
        d fn;
        if (n <= Nf) {
          fn = fl * d_pow(k, ((d)n) / Nf);
        } else {
          fn = fl * k * d_pow(1 / k, ((d)n - Nf) / Nb);
        }
        /* dbgassert(fn >= 0, "BUG"); */
 
        phase += 2 * number_pi * Dt * fn;
        while (phase > 2 * number_pi)
          phase -= 2 * number_pi;
        /* dVARTRACE(17, phase); */
      }
      /* This should be a very small number!! */
      DEBUGTRACE_PRINT(phase);
    }
  } // End of log sweep
  else {
    // Either log or linear sweep had to be given as flags.
    assert(false);
 
  }
}