mirror of
https://git.rwth-aachen.de/acs/public/villas/node/
synced 2025-03-09 00:00:00 +01:00
158 lines
4.5 KiB
C++
158 lines
4.5 KiB
C++
/** ipDFT PMU hook.
|
|
*
|
|
* @author Manuel Pitz <manuel.pitz@eonerc.rwth-aachen.de>
|
|
* @copyright 2014-2022, Institute for Automation of Complex Power Systems, EONERC
|
|
* @license Apache 2.0
|
|
*********************************************************************************/
|
|
|
|
#include <villas/hooks/pmu.hpp>
|
|
|
|
namespace villas {
|
|
namespace node {
|
|
|
|
class IpDftPmuHook : public PmuHook {
|
|
|
|
protected:
|
|
std::complex<double> omega;
|
|
std::vector<std::vector<std::complex<double>>> dftMatrix;
|
|
std::vector<std::complex<double>> dftResult;
|
|
|
|
unsigned frequencyCount; /* Number of requency bins that are calculated */
|
|
double estimationRange; /* The range around nominalFreq used for estimation */
|
|
|
|
public:
|
|
IpDftPmuHook(Path *p, Node *n, int fl, int prio, bool en = true):
|
|
PmuHook(p, n, fl, prio, en),
|
|
frequencyCount(0),
|
|
estimationRange(0)
|
|
|
|
{ }
|
|
|
|
void prepare()
|
|
{
|
|
PmuHook::prepare();
|
|
|
|
const double startFrequency = nominalFreq - estimationRange;
|
|
const double endFrequency = nominalFreq + estimationRange;
|
|
const double frequencyResolution = (double)sampleRate / windowSize;
|
|
|
|
frequencyCount = ceil((endFrequency - startFrequency) / frequencyResolution);
|
|
|
|
/* Initialize matrix of dft coeffients */
|
|
dftMatrix.clear();
|
|
for (unsigned i = 0; i < frequencyCount; i++)
|
|
dftMatrix.emplace_back(windowSize, 0.0);
|
|
|
|
using namespace std::complex_literals;
|
|
|
|
omega = exp((-2i * M_PI) / (double)(windowSize));
|
|
unsigned startBin = floor(startFrequency / frequencyResolution);
|
|
|
|
for (unsigned i = 0; i < frequencyCount ; i++) {
|
|
for (unsigned j = 0 ; j < windowSize; j++)
|
|
dftMatrix[i][j] = pow(omega, (i + startBin) * j);
|
|
dftResult.push_back(0.0);
|
|
}
|
|
}
|
|
|
|
void parse(json_t *json)
|
|
{
|
|
PmuHook::parse(json);
|
|
int ret;
|
|
|
|
json_error_t err;
|
|
|
|
assert(state != State::STARTED);
|
|
|
|
Hook::parse(json);
|
|
|
|
ret = json_unpack_ex(json, &err, 0, "{ s?: F}",
|
|
"estimation_range", &estimationRange
|
|
);
|
|
|
|
if (ret)
|
|
throw ConfigError(json, err, "node-config-hook-ip-dft-pmu");
|
|
|
|
if (estimationRange <= 0)
|
|
throw ConfigError(json, "node-config-hook-ip-dft-pmu-estimation_range", "Estimation range cannot be less or equal than 0 tried to set {}", estimationRange);
|
|
|
|
}
|
|
|
|
PmuHook::Phasor estimatePhasor(dsp::CosineWindow<double> *window, PmuHook::Phasor lastPhasor)
|
|
{
|
|
PmuHook::Phasor phasor = {0};
|
|
|
|
/* Calculate DFT */
|
|
for (unsigned i = 0; i < frequencyCount; i++) {
|
|
dftResult[i] = 0;
|
|
|
|
const unsigned size = (*window).size();
|
|
for (unsigned j = 0; j < size; j++)
|
|
dftResult[i] += (*window)[j] * dftMatrix[i][j];
|
|
}
|
|
/* End calculate DFT */
|
|
|
|
/* Find max bin */
|
|
unsigned maxBin = 0;
|
|
double absAmplitude = 0;
|
|
|
|
for(unsigned j = 0; j < frequencyCount; j++) {
|
|
if (absAmplitude < abs(dftResult[j])) {
|
|
absAmplitude = abs(dftResult[j]);
|
|
maxBin = j;
|
|
}
|
|
}
|
|
/* End find max bin */
|
|
|
|
if (maxBin == 0 || maxBin == (frequencyCount - 1)) {
|
|
logger->warn("Maximum frequency bin lies on window boundary. Using non-estimated results!");
|
|
//@todo add handling to not forward this phasor!!
|
|
} else {
|
|
const double startFrequency = nominalFreq - estimationRange;
|
|
const double frequencyResolution = (double)sampleRate / windowSize;
|
|
|
|
double a = abs(dftResult[ maxBin - 1 ]);
|
|
double b = abs(dftResult[ maxBin + 0 ]);
|
|
double c = abs(dftResult[ maxBin + 1 ]);
|
|
double bPhase = atan2(dftResult[maxBin].imag(), dftResult[maxBin].real());
|
|
|
|
/* Estimate phasor */
|
|
/* Based on https://ieeexplore.ieee.org/document/7980868 */
|
|
double delta = 0;
|
|
|
|
/* Paper eq 8 */
|
|
if (c > a) {
|
|
delta = 1. * (2. * c - b) / (b + c);
|
|
} else {
|
|
delta = -1. * (2. * a - b) / (b + a);
|
|
}
|
|
|
|
/* Frequency estimation (eq 4) */
|
|
phasor.frequency = startFrequency + ( (double) maxBin + delta) * frequencyResolution;
|
|
|
|
/* Amplitude estimation (eq 9) */
|
|
phasor.amplitude = b * abs( (M_PI * delta) / sin( M_PI * delta) ) * abs( pow(delta, 2) - 1);
|
|
phasor.amplitude *= 2 / (windowSize * window->getCorrectionFactor());
|
|
|
|
/* Phase estimation (eq 10) */
|
|
phasor.phase = bPhase - M_PI * delta;
|
|
|
|
/* ROCOF estimation */
|
|
phasor.rocof = ((phasor.frequency - lastPhasor.frequency) * (double)phasorRate);
|
|
/* End estimate phasor */
|
|
}
|
|
|
|
if (lastPhasor.frequency != 0) /* Check if we already calculated a phasor before */
|
|
phasor.valid = Status::VALID;
|
|
|
|
return phasor;
|
|
}
|
|
};
|
|
|
|
/* Register hook */
|
|
static char n[] = "ip-dft-pmu";
|
|
static char d[] = "This hook calculates a phasor based on ipDFT";
|
|
static HookPlugin<IpDftPmuHook, n, d, (int) Hook::Flags::NODE_READ | (int) Hook::Flags::NODE_WRITE | (int) Hook::Flags::PATH> p;
|
|
|
|
} /* namespace node */
|
|
} /* namespace villas */
|