266 lines
7.3 KiB
C++
266 lines
7.3 KiB
C++
// lasp_siggen_impl.cpp
|
|
//
|
|
// Author: J.A. de Jong -ASCEE
|
|
//
|
|
// Description:
|
|
// Signal generators implementation
|
|
//////////////////////////////////////////////////////////////////////
|
|
/* #define DEBUGTRACE_ENABLED */
|
|
#include "lasp_siggen_impl.h"
|
|
|
|
#include <cassert>
|
|
|
|
#include "debugtrace.hpp"
|
|
#include "lasp_mathtypes.h"
|
|
|
|
using rte = std::runtime_error;
|
|
using slock = std::scoped_lock<std::recursive_mutex>;
|
|
|
|
DEBUGTRACE_VARIABLES;
|
|
|
|
/** The fixed number of Newton iterations t.b.d. for tuning the sweep start and
|
|
* stop frequency in logarithmic sweeps */
|
|
#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; */
|
|
slock lck(_mtx);
|
|
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);
|
|
slock lck(_mtx);
|
|
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;
|
|
slock lck(_mtx);
|
|
|
|
_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);
|
|
}
|
|
}
|