diff --git a/lib/hooks/CMakeLists.txt b/lib/hooks/CMakeLists.txt index 2872a0354..afc12824d 100644 --- a/lib/hooks/CMakeLists.txt +++ b/lib/hooks/CMakeLists.txt @@ -36,6 +36,7 @@ set(HOOK_SRC cast.c average.c dump.c + dp.c ) if(WITH_IO) diff --git a/lib/hooks/dp.c b/lib/hooks/dp.c index 2aab9edcd..b0aafc5fc 100644 --- a/lib/hooks/dp.c +++ b/lib/hooks/dp.c @@ -1,7 +1,7 @@ /** Dynamic Phasor Interface Algorithm hook. * * @author Steffen Vogel - * @copyright 2017, Institute for Automation of Complex Power Systems, EONERC + * @copyright 2014-2019, Institute for Automation of Complex Power Systems, EONERC * @license GNU General Public License (version 3) * * VILLASnode @@ -24,131 +24,235 @@ * @{ */ +#include +#include #include -#include "villas/hook.h" -#include "villas/plugin.h" -#include "villas/sample.h" -#include "villas/bitset.h" +#include +#include +#include -struct dp_series { - int idx; - int *harmonics; - int nharmonics; - double f0; - double rate; - enum { - DP_INTERPOLATION_NONE, - DP_INTERPOLATION_STEP, - DP_INTERPOLATION_LINEAR - } interpolation; +#define J _Complex_I - double *history; +struct delay { + double *data; + size_t steps; + size_t mask; + size_t pos; +} - unsigned counter; - unsigned last_sequence; -}; +int delay_init(struct delay *w, double dt, double period) +{ + size_t len = LOG2_CEIL(period / dt); + + /* Allocate memory for ciruclar history buffer */ + w->data = alloc(len * sizeof(double)); + if (!w->data) + return -1; + + w->pos = 0; + w->mask = len - 1; + + return 0; +} + +int delay_destroy(struct delay *w) +{ + free(w->data); +} + +double delay_update(struct delay *w, double in) +{ + double out = w->data[pos & w->mask]; + + w->data[w->pos++] + + return out; +} struct dp { - struct list series; -}; + int index; + int inverse; -static double dp_series_step(struct dp_series *s, double val) -{ + double f0; + double dt; + double t; + double complex *coeffs; + int *fharmonics; + int fharmonics_len; + + struct window history; } -static double dp_series_istep(struct dp_series *s, double val) +static void dp_step(struct dp *d, double *in, float complex *out) { + double newest = *in; + double oldest = delay_update(&d->hist, newest); + for (int i = 0; i < d->fharmonics_len; i++) { + double pi_fharm = 2.0 * M_PI * d->fharmonics[i]; + + /* Recursive update */ + d->coeffs[i] = (d->coeffs[i] + (newest - oldest)) * cexp(pi_fharm); + + /* Correction for stationary phasor */ + double complex correction = cexp(pi_fharm * (d->t - (d->history_len + 1))); + double complex result = 2.0 / d->history_len * d->coeffs[i] / correction; + + /* DC component */ + if (i == 0) + result /= 2.0; + + out[i] = result; + } } -static int dp_series_init(struct dp_series *s, int idx, double f0, double rate) +static void dp_istep(struct dp *d, complex float *in, double *out) { - d->idx = idx; - d->f0 = f0; + double complex value = 0; + + /* Reconstruct the original signal */ + for (int i = 0; i < d->fharmonics_len; i++) { + double freq = d->fharmonics[i]; + double complex coeff = in[i]; + + value += coeff * cexp(2.0 * M_PI * freq * d->t); + } + + *out = creal(value); } -static int dp_series_destroy(struct dp_series *s) +static int dp_start(struct hook *h) { - free(s->history); + struct dp *d = (struct dp *) h->_vd; + + d->t = 0; + + double cycle = 1.0 / d->f0; + + /* Delay for one cycle */ + ret = delay_init(&d->history, d->dt, cycle); + if (ret) + return ret; return 0; } static int dp_init(struct hook *h) { - struct dp *p = (struct dp *) h->_vd; + struct dp *d = (struct dp *) h->_vd; - list_init(&p->series); + /* Default values */ + d->inverse = 0; return 0; } -static int dp_deinit(struct hook *h) +static int dp_destroy(struct hook *h) { - struct dp *p = (struct dp *) h->_vd; + struct dp *d = (struct dp *) h->_vd; - bitset_destroy(p->mask, (dtor_cb_t) dp_series_destroy, true); + /* Release memory */ + free(d->history); + free(d->fharmonics); + free(d->coeffs); return 0; } static int dp_parse(struct hook *h, json_t *cfg) { - struct dp *p = (struct dp *) h->_vd; + struct dp *d = (struct dp *) h->_vd; int ret; json_error_t err; - const char *mode = NULL; + json_t *json_harmonics, *json_harmonic; + size_t i; - ret = json_unpack_ex(cfg, &err, 0, "{ s: F, s?: i }", - "f0", &p->f0, - "mask", &p->mask + double rate = -1, dt = -1; + + ret = json_unpack_ex(cfg, &err, 0, "{ s: i, s: F, s: F, s: o, s?: b }", + "index", &d->index, + "f0", &d->f0, + "dt", &dt, + "rate", &rate, + "harmonics", &json_harmonics, + "inverse", &d->inverse ); if (ret) jerror(&err, "Failed to parse configuration of hook '%s'", plugin_name(h->_vt)); - return 0; -} + if (rate > 0) + d->dt = 1. / rate; + else if (dt > 0) + d->dt = dt; + else + error("Either on of the settings 'dt' or 'rate' must be gived for hook '%s'", plugin_name(h->_vt)); -static int dp_read(struct hook *h, struct sample *smps[], unsigned *cnt) -{ - struct dp *p = (struct dp *) h->_vd; + if (!json_is_array(json_harmonics)) + error("Setting 'harmonics' of hook '%s' must be a list of integers", plugin_name(h->_vt)); - for (unsigned j = 0; j < cnt; j++) { - struct sample *t = smps[j]; + d->fharmonics_len = json_array_size(json_harmonics); + d->fharmonics = alloc(d->fharmonics_len * sizeof(double)); + d->coeffs = alloc(d->fharmonics_len * sizeof(double complex)); + if (!d->fharmonics || !d->coeffs) + return -1; - for (size_t i = 0; i < list_length(&p->series); i++) { - struct dp_series *s = (struct dp_series *) list_at(&p->series, i); + json_array_foreach(json_harmonics, i, json_harmonic) { + if (!json_is_integer(json_harmonic)) + error("Setting 'harmonics' of hook '%s' must be a list of integers", plugin_name(h->_vt)); - if (s->idx > t->length) - continue; - } + d->fharmonics[i] = d->f0 * json_integer_value(json_harmonic); } return 0; } -static int dp_write(struct hook *h, struct sample *smps[], unsigned *cnt) +static int dp_process(struct hook *h, struct sample *smps[], unsigned *cnt) { - struct dp *p = (struct dp *) h->_vd; + struct dp *d = (struct dp *) h->_vd; + + for (unsigned j = 0; j < *cnt; j++) { + struct sample *smp = smps[j]; + + if (d->index > smp->length) + continue; + + struct signal *s = (struct signal *) vlist_at(smp->signals, d->index); + + if (d->inverse) { + if (s->type != SIGNAL_TYPE_FLOAT) + return -1; + + dp_istep(d, &smp->data[d->index].z, &smp->data[d->index].f); + } + else { + if (s->type != SIGNAL_TYPE_COMPLEX) + return -1; + + dp_step(d, &smp->data[d->index].f, &smp->data[d->index].z); + } + } + + d->t += d->dt; return 0; } static struct plugin p = { .name = "dp", - .description = "Transform to dynamic phasor domain", + .description = "Transform to/from dynamic phasor domain", .type = PLUGIN_TYPE_HOOK, .hook = { - .flags = HOOK_NODE, - .priority = 99, - .init = dp_init, - .parse = dp_parse, - .read = dp_read, - .write = dp_write, - .size = sizeof(struct dp) + .flags = HOOK_PATH, + .priority = 99, + .init = dp_init, + .destroy = dp_destroy, + .start = dp_start, + .parse = dp_parse, + .process = dp_process, + .size = sizeof(struct dp) } };