1
0
Fork 0
mirror of https://git.rwth-aachen.de/acs/public/villas/node/ synced 2025-03-09 00:00:00 +01:00
VILLASnode/lib/hooks/dp.cpp
Steffen Vogel 28d354cb84 Fix formatting with clang-format
Signed-off-by: Steffen Vogel <steffen.vogel@opal-rt.com>
2024-10-15 19:31:49 +02:00

311 lines
8 KiB
C++

/* Dynamic Phasor Interface Algorithm hook.
*
* Author: Steffen Vogel <post@steffenvogel.de>
* SPDX-FileCopyrightText: 2014-2023 Institute for Automation of Complex Power Systems, RWTH Aachen University
* SPDX-License-Identifier: Apache-2.0
*/
#include <cmath>
#include <cstring>
#include <complex>
#include <villas/dsp/window.hpp>
#include <villas/hook.hpp>
#include <villas/sample.hpp>
#include <villas/utils.hpp>
using namespace std::complex_literals;
using namespace villas::utils;
namespace villas {
namespace node {
class DPHook : public Hook {
protected:
char *signal_name;
unsigned signal_index;
int inverse;
double f0;
double timestep;
double time;
double steps;
std::complex<double> *coeffs;
int *fharmonics;
int fharmonics_len;
dsp::Window<double> window;
void step(double *in, std::complex<float> *out) {
int N = window.size();
std::complex<double> om_k, corr;
double newest = *in;
__attribute__((unused)) double oldest = window.update(newest);
for (int k = 0; k < fharmonics_len; k++) {
om_k = 2.0i * M_PI * (double)fharmonics[k] / (double)N;
// Correction for stationary phasor
corr = std::exp(-om_k * (steps - (N + 1)));
//corr = 1;
#if 0
// Recursive update
coeffs[k] = std::exp(om) * (coeffs[k] + (newest - oldest));
out[k] = (2.0 / N) * (coeffs[i] * corr);
// DC component
if (fharmonics[k] == 0)
out[k] /= 2.0;
#else
// Full DFT
std::complex<double> X_k = 0;
for (int n = 0; n < N; n++) {
double x_n = window[n];
X_k += x_n * std::exp(om_k * (double)n);
}
out[k] = X_k / (corr * (double)N);
#endif
}
}
void istep(std::complex<float> *in, double *out) {
std::complex<double> value = 0;
// Reconstruct the original signal
for (int k = 0; k < fharmonics_len; k++) {
double freq = fharmonics[k];
// cppcheck-suppress objectIndex
std::complex<double> coeff = in[k];
std::complex<double> om = 2.0i * M_PI * freq * time;
value += coeff * std::exp(om);
}
*out = std::real(value);
}
public:
DPHook(Path *p, Node *n, int fl, int prio, bool en = true)
: Hook(p, n, fl, prio, en), signal_name(nullptr), signal_index(0),
inverse(0), f0(50.0), timestep(50e-6), time(), steps(0), coeffs(),
fharmonics(), fharmonics_len(0) {}
virtual ~DPHook() {
// Release memory
if (fharmonics)
delete fharmonics;
if (coeffs)
delete coeffs;
if (signal_name)
free(signal_name);
}
virtual void start() {
assert(state == State::PREPARED);
time = 0;
steps = 0;
for (int i = 0; i < fharmonics_len; i++)
coeffs[i] = 0;
window = dsp::Window<double>((1.0 / f0) / timestep, 0.0);
if (window.size() == 0) {
throw RuntimeError(
"Windows size is 0: f0 * timestep < 1.0 not satisfied");
}
state = State::STARTED;
}
virtual void parse(json_t *json) {
int ret;
json_error_t err;
json_t *json_harmonics, *json_harmonic, *json_signal;
size_t i;
Hook::parse(json);
double rate = -1, dt = -1;
ret = json_unpack_ex(json, &err, 0,
"{ s: o, s: F, s?: F, s?: F, s: o, s?: b }", "signal",
&json_signal, "f0", &f0, "dt", &dt, "rate", &rate,
"harmonics", &json_harmonics, "inverse", &inverse);
if (ret)
throw ConfigError(json, err, "node-config-hook-dp");
if (rate > 0)
timestep = 1. / rate;
else if (dt > 0)
timestep = dt;
else
throw ConfigError(
json, "node-config-hook-dp",
"Either on of the settings 'dt' or 'rate' must be given");
if (!json_is_array(json_harmonics))
throw ConfigError(json_harmonics, "node-config-hook-dp-harmonics",
"Setting 'harmonics' must be a list of integers");
switch (json_typeof(json_signal)) {
case JSON_STRING:
signal_name = strdup(json_string_value(json_signal));
break;
case JSON_INTEGER:
signal_name = nullptr;
signal_index = json_integer_value(json_signal);
break;
default:
throw ConfigError(json_signal, "node-config-hook-dp-signal",
"Invalid value for setting 'signal'");
}
fharmonics_len = json_array_size(json_harmonics);
fharmonics = new int[fharmonics_len];
coeffs = new std::complex<double>[fharmonics_len];
if (!fharmonics || !coeffs)
throw MemoryAllocationError();
json_array_foreach(json_harmonics, i, json_harmonic) {
if (!json_is_integer(json_harmonic))
throw ConfigError(json_harmonic, "node-config-hook-dp-harmonics",
"Setting 'harmonics' must be a list of integers");
fharmonics[i] = json_integer_value(json_harmonic);
}
state = State::PARSED;
}
virtual void prepare() {
assert(state == State::CHECKED);
std::string new_sig_name;
assert(state != State::STARTED);
if (signal_name) {
int si = signals->getIndexByName(signal_name);
if (si < 0) {
throw RuntimeError("Failed to find signal: {}", signal_name);
}
signal_index = si;
}
if (inverse) {
// Remove complex-valued coefficient signals
for (int i = 0; i < fharmonics_len; i++) {
auto orig_sig = signals->getByIndex(signal_index + i);
if (!orig_sig)
throw RuntimeError("Failed to find signal");
if (orig_sig->type != SignalType::COMPLEX)
throw RuntimeError("Signal is not complex");
signals->erase(signals->begin() + signal_index + i);
}
// Add new real-valued reconstructed signals
auto new_sig = std::make_shared<Signal>("dp", "idp", SignalType::FLOAT);
if (!new_sig)
throw RuntimeError("Failed to create signal");
signals->insert(signals->begin() + signal_index, new_sig);
} else {
auto orig_sig = signals->getByIndex(signal_index);
if (!orig_sig)
throw RuntimeError("Failed to find signal");
if (orig_sig->type != SignalType::FLOAT)
throw RuntimeError("Signal is not float");
signals->erase(signals->begin() + signal_index);
for (int i = 0; i < fharmonics_len; i++) {
new_sig_name = fmt::format("{}_harm{}", orig_sig->name, i);
auto new_sig = std::make_shared<Signal>(new_sig_name, orig_sig->unit,
SignalType::COMPLEX);
if (!new_sig)
throw RuntimeError("Failed to create new signal");
signals->insert(signals->begin() + signal_index, new_sig);
}
}
state = State::PREPARED;
}
virtual void check() {
assert(state == State::PARSED);
if (signal_index < 0)
throw RuntimeError("Signal index not set");
if (fharmonics_len <= 0)
throw RuntimeError("No harmonics given");
if (timestep <= 0)
throw RuntimeError("Invalid timestep");
if (f0 <= 0)
throw RuntimeError("Invalid fundamental frequency");
state = State::CHECKED;
}
virtual Hook::Reason process(struct Sample *smp) {
if (signal_index >= smp->length)
return Hook::Reason::ERROR;
if (inverse) {
double signal;
std::complex<float> *coeffs =
reinterpret_cast<std::complex<float> *>(&smp->data[signal_index].z);
istep(coeffs, &signal);
sample_data_remove(smp, signal_index, fharmonics_len);
sample_data_insert(smp, (union SignalData *)&signal, signal_index, 1);
} else {
double signal = smp->data[signal_index].f;
std::complex<float> coeffs[fharmonics_len];
step(&signal, coeffs);
sample_data_remove(smp, signal_index, 1);
sample_data_insert(smp, (union SignalData *)coeffs, signal_index,
fharmonics_len);
}
time += timestep;
steps++;
return Reason::OK;
}
};
// Register hook
static char n[] = "dp";
static char d[] = "Transform to/from dynamic phasor domain";
static HookPlugin<DPHook, n, d,
(int)Hook::Flags::PATH | (int)Hook::Flags::NODE_READ |
(int)Hook::Flags::NODE_WRITE>
p;
} // namespace node
} // namespace villas