lasp/cpp_src/dsp/lasp_siggen_impl.cpp
J.A. de Jong - Redu-Sone B.V., ASCEE V.O.F 26eef040a4
Some checks failed
Building, testing and releasing LASP if it has a tag / Build-Test-Ubuntu (push) Failing after 1m33s
Building, testing and releasing LASP if it has a tag / Release-Ubuntu (push) Has been skipped
More locks on signal generator.
2024-03-04 15:49:29 +01:00

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