Hook pmu timing

common/include/villas/timing.hpp

@@ -20,8 +20,11 @@ struct timespec time_diff(const struct timespec *start,
 
 // Get sum of two timespec structs.
 struct timespec time_add(const struct timespec *start,
-                         const struct timespec *end);
+                         const struct timespec *duration);
 
+// Get sub of two timespec structs.
+struct timespec time_sub(const struct timespec *start,
+                         const struct timespec *duration);
 // Return current time as a struct timespec.
 struct timespec time_now();
 

common/lib/timing.cpp

@@ -18,9 +18,9 @@ struct timespec time_now() {
 }
 
 struct timespec time_add(const struct timespec *start,
-                         const struct timespec *end) {
-  struct timespec sum = {.tv_sec = end->tv_sec + start->tv_sec,
-                         .tv_nsec = end->tv_nsec + start->tv_nsec};
+                         const struct timespec *duration) {
+  struct timespec sum = {.tv_sec = duration->tv_sec + start->tv_sec,
+                         .tv_nsec = duration->tv_nsec + start->tv_nsec};
 
   if (sum.tv_nsec >= 1000000000) {
     sum.tv_sec += 1;
@@ -30,6 +30,19 @@ struct timespec time_add(const struct timespec *start,
   return sum;
 }
 
+struct timespec time_sub(const struct timespec *start,
+                         const struct timespec *duration) {
+  struct timespec sum = {.tv_sec = start->tv_sec - duration->tv_sec,
+                         .tv_nsec = start->tv_nsec - duration->tv_nsec};
+
+  if (sum.tv_nsec < 0) {
+    sum.tv_sec -= 1;
+    sum.tv_nsec += 1000000000;
+  }
+
+  return sum;
+}
+
 struct timespec time_diff(const struct timespec *start,
                           const struct timespec *end) {
   struct timespec diff = {.tv_sec = end->tv_sec - start->tv_sec,

include/villas/hooks/pmu.hpp

@@ -33,22 +33,30 @@ class PmuHook : public MultiSignalHook {
     RIGHT,
   };
 
+  enum class OutputMode {
+    FLOAT,
+    COMPLEX,
+  };
+
   std::vector<dsp::CosineWindow<double> *> windows;
   dsp::Window<timespec> *windowsTs;
-  std::vector<Phasor> lastPhasors;
+  std::vector<Phasor> currentPhasors;
 
   enum TimeAlign timeAlignType;
   enum WindowType windowType;
+  enum OutputMode outputMode;
 
   unsigned sampleRate;
-  double phasorRate;
+  int dataRate;
   double nominalFreq;
   double numberPlc;
   unsigned windowSize;
   bool channelNameEnable;
   double angleUnitFactor;
   uint64_t lastSequence;
   timespec nextRun;
+
+  bool run;
   bool init;
   unsigned initSampleCount;
 
@@ -59,7 +67,7 @@ class PmuHook : public MultiSignalHook {
   double rocofOffset;
 
   virtual Phasor estimatePhasor(dsp::CosineWindow<double> *window,
-                                const Phasor &lastPhasor);
+                                dsp::Window<timespec> *windowTs);
 
 public:
   PmuHook(Path *p, Node *n, int fl, int prio, bool en = true);
@@ -69,6 +77,9 @@ class PmuHook : public MultiSignalHook {
   void parse(json_t *json) override;
 
   Hook::Reason process(struct Sample *smp) override;
+
+private:
+  timespec calcNextRun(timespec currentTimetag);
 };
 
 } // namespace node

lib/hooks/pmu.cpp

@@ -14,11 +14,11 @@ namespace node {
 PmuHook::PmuHook(Path *p, Node *n, int fl, int prio, bool en)
     : MultiSignalHook(p, n, fl, prio, en), windows(), windowsTs(),
       timeAlignType(TimeAlign::CENTER), windowType(WindowType::NONE),
-      sampleRate(1), phasorRate(1.0), nominalFreq(1.0), numberPlc(1.),
+      sampleRate(1), dataRate(1), nominalFreq(1.0), numberPlc(1.),
       windowSize(1), channelNameEnable(true), angleUnitFactor(1.0),
-      lastSequence(0), nextRun({0}), init(false), initSampleCount(0),
-      phaseOffset(0.0), amplitudeOffset(0.0), frequencyOffset(0.0),
-      rocofOffset(0.0) {}
+      lastSequence(0), nextRun({0}), run(false), init(false),
+      initSampleCount(0), phaseOffset(0.0), amplitudeOffset(0.0),
+      frequencyOffset(0.0), rocofOffset(0.0) {}
 
 void PmuHook::prepare() {
   MultiSignalHook::prepare();
@@ -30,30 +30,35 @@ void PmuHook::prepare() {
         std::make_shared<Signal>("frequency", "Hz", SignalType::FLOAT);
     auto amplSig =
         std::make_shared<Signal>("amplitude", "V", SignalType::FLOAT);
+    auto phasorSig =
+        std::make_shared<Signal>("phasor", "V", SignalType::COMPLEX);
     auto phaseSig = std::make_shared<Signal>(
         "phase", (angleUnitFactor) ? "rad" : "deg",
         SignalType::FLOAT); //angleUnitFactor==1 means rad
     auto rocofSig =
         std::make_shared<Signal>("rocof", "Hz/s", SignalType::FLOAT);
 
-    if (!freqSig || !amplSig || !phaseSig || !rocofSig)
-      throw RuntimeError("Failed to create new signals");
-
     if (channelNameEnable) {
       auto suffix = fmt::format("_{}", signalNames[i]);
 
       freqSig->name += suffix;
       amplSig->name += suffix;
       phaseSig->name += suffix;
       rocofSig->name += suffix;
+      phasorSig->name += suffix;
     }
 
     signals->push_back(freqSig);
-    signals->push_back(amplSig);
-    signals->push_back(phaseSig);
+    if (outputMode == OutputMode::COMPLEX) {
+      signals->push_back(phasorSig);
+    } else {
+      signals->push_back(amplSig);
+      signals->push_back(phaseSig);
+    }
+
     signals->push_back(rocofSig);
 
-    lastPhasors.push_back({0., 0., 0., 0.});
+    currentPhasors.push_back({0., 0., 0., 0.});
   }
 
   windowSize = ceil(sampleRate * numberPlc / nominalFreq);
@@ -83,23 +88,23 @@ void PmuHook::parse(json_t *json) {
   const char *windowTypeC = nullptr;
   const char *angleUnitC = nullptr;
   const char *timeAlignC = nullptr;
+  const char *outputModeC = nullptr;
 
   json_error_t err;
 
   assert(state != State::STARTED);
 
-  Hook::parse(json);
-
   ret = json_unpack_ex(
       json, &err, 0,
-      "{ s?: i, s?: F, s?: F, s?: F, s?: s, s?: s, s?: b, s?: s, s?: F, s?: F, "
-      "s?: F, s?: F}",
-      "sample_rate", &sampleRate, "dft_rate", &phasorRate, "nominal_freq",
+      "{ s?: i, s?: i, s?: F, s?: F, s?: s, s?: s, s?: b, s?: s, s?: F, s?: F, "
+      "s?: F, s?: F, s?: s}",
+      "sample_rate", &sampleRate, "data_rate", &dataRate, "nominal_freq",
       &nominalFreq, "number_plc", &numberPlc, "window_type", &windowTypeC,
       "angle_unit", &angleUnitC, "add_channel_name", &channelNameEnable,
       "timestamp_align", &timeAlignC, "phase_offset", &phaseOffset,
       "amplitude_offset", &amplitudeOffset, "frequency_offset",
-      &frequencyOffset, "rocof_offset", &rocofOffset);
+      &frequencyOffset, "rocof_offset", &rocofOffset, "output_mode",
+      &outputModeC);
 
   if (ret)
     throw ConfigError(json, err, "node-config-hook-pmu");
@@ -109,10 +114,9 @@ void PmuHook::parse(json_t *json) {
                       "Sample rate cannot be less than 0 tried to set {}",
                       sampleRate);
 
-  if (phasorRate <= 0)
-    throw ConfigError(json, "node-config-hook-pmu-phasor_rate",
-                      "Phasor rate cannot be less than 0 tried to set {}",
-                      phasorRate);
+  if (dataRate == 0)
+    throw ConfigError(json, "node-config-hook-pmu-data_rate",
+                      "Data rate cannot be set to 0");
 
   if (nominalFreq <= 0)
     throw ConfigError(json, "node-config-hook-pmu-nominal_freq",
@@ -125,6 +129,13 @@ void PmuHook::parse(json_t *json) {
         "Number of power line cycles cannot be less than 0 tried to set {}",
         numberPlc);
 
+  if (!outputModeC)
+    outputMode = OutputMode::FLOAT;
+  else if (strcmp(outputModeC, "complex") == 0)
+    outputMode = OutputMode::COMPLEX;
+  else
+    outputMode = OutputMode::FLOAT;
+
   if (!windowTypeC)
     logger->info("No Window type given, assume no windowing");
   else if (strcmp(windowTypeC, "flattop") == 0)
@@ -164,6 +175,55 @@ void PmuHook::parse(json_t *json) {
                       "Timestamp alignment {} not recognized", timeAlignC);
 }
 
+timespec PmuHook::calcNextRun(timespec currentTimetag) {
+
+  timespec nextPhasor = currentTimetag;
+
+  timespec offset_ts = {.tv_sec = 0, .tv_nsec = 1};
+  if (timeAlignType == TimeAlign::CENTER) {
+    auto offset_ns = 1'000'000'000ll * windowSize / (sampleRate * 2);
+    offset_ts = {
+        .tv_sec = offset_ns / 1'000'000'000ll,
+        .tv_nsec = offset_ns % 1'000'000'000ll,
+    };
+  } else if (timeAlignType == TimeAlign::LEFT) {
+    auto offset_ns = 1'000'000'000ll * windowSize / (sampleRate);
+    offset_ts = {
+        .tv_sec = offset_ns / 1'000'000'000ll,
+        .tv_nsec = offset_ns % 1'000'000'000ll,
+    };
+  }
+  currentTimetag = time_sub(&currentTimetag, &offset_ts);
+  if (dataRate > 0) {
+    //handle rates of multiple phasors per second. dataRate is the phasors per second
+
+    int n = (currentTimetag.tv_nsec * dataRate) / 1'000'000'000l;
+    int next = (n + 1) % dataRate;
+    nextPhasor = {
+        .tv_sec =
+            (next <= n) ? currentTimetag.tv_sec + 1 : currentTimetag.tv_sec,
+        .tv_nsec = (next * 1'000'000'000l) / dataRate,
+    };
+  } else {
+    //handle rates of less then one phasor per second. dataRate is the seconds between phasors
+    int period = -dataRate;
+    int hour = currentTimetag.tv_sec / 3600;
+    int n = (currentTimetag.tv_sec - hour * 3600) / period;
+    int next = n + 1;
+    if (next * period > 3600) {
+      hour += 1;
+      next = 0;
+    }
+
+    nextPhasor = {
+        .tv_sec = hour * 3600 + period * next,
+        .tv_nsec = 0,
+    };
+  }
+
+  return time_add(&nextPhasor, &offset_ts);
+}
+
 Hook::Reason PmuHook::process(struct Sample *smp) {
   assert(state == State::STARTED);
 
@@ -174,60 +234,74 @@ Hook::Reason PmuHook::process(struct Sample *smp) {
                  smp->sequence - lastSequence);
   lastSequence = smp->sequence;
 
-  if (!init && initSampleCount > windowSize)
+  // Update sample memory before run
+  unsigned i = 0;
+  for (auto index : signalIndices)
+    windows[i++]->update(smp->data[index].f);
+  windowsTs->update(smp->ts.origin);
+
+  if (!init && initSampleCount >= windowSize) {
     init = true;
+    nextRun = calcNextRun(smp->ts.origin);
+  }
 
   timespec timeDiff = time_diff(&nextRun, &smp->ts.origin);
-  double tmpTimeDiff = time_to_double(&timeDiff);
-  bool run = false;
-  if (tmpTimeDiff > 0. && init)
+  int64_t timeDiffNs =
+      static_cast<int64_t>(timeDiff.tv_sec) * 1'000'000'000l + timeDiff.tv_nsec;
+
+  if (!run && timeDiffNs >= (-1'000'000'000l /
+                             sampleRate)) { //timeDiffNs >= -1e9/sampleRate
     run = true;
+  } else if (run && timeDiffNs > 0) {
+    nextRun = calcNextRun(smp->ts.origin);
+    run = false;
+  } else if (run)
+    return Reason::SKIP_SAMPLE;
 
   Status phasorStatus = Status::VALID;
   timespec phasorTimestamp = {0};
   if (run) {
     for (unsigned i = 0; i < signalIndices.size(); i++) {
-      lastPhasors[i] = estimatePhasor(windows[i], lastPhasors[i]);
-      if (lastPhasors[i].valid != Status::VALID)
+      currentPhasors[i] = estimatePhasor(windows[i], windowsTs);
+      if (currentPhasors[i].valid != Status::VALID)
         phasorStatus = Status::INVALID;
     }
 
-    // Align time tag
-    double currentTimeTag = time_to_double(&smp->ts.origin);
-    double alignedTime = currentTimeTag - fmod(currentTimeTag, 1 / phasorRate);
-    nextRun = time_from_double(alignedTime + 1 / phasorRate);
-
     size_t tsPos = 0;
     if (timeAlignType == TimeAlign::RIGHT)
-      tsPos = windowSize;
+      tsPos = windowSize - 1;
     else if (timeAlignType == TimeAlign::LEFT)
       tsPos = 0;
     else if (timeAlignType == TimeAlign::CENTER)
       tsPos = windowSize / 2;
     phasorTimestamp = (*windowsTs)[tsPos];
   }
 
-  // Update sample memory
-  unsigned i = 0;
-  for (auto index : signalIndices)
-    windows[i++]->update(smp->data[index].f);
-  windowsTs->update(smp->ts.origin);
-
   // Make sure to update phasors after window update but estimate them before
   if (run) {
     for (unsigned i = 0; i < signalIndices.size(); i++) {
-      smp->data[i * 4 + 0].f =
-          lastPhasors[i].frequency + frequencyOffset; // Frequency
-      smp->data[i * 4 + 1].f = (lastPhasors[i].amplitude / pow(2, 0.5)) +
-                               amplitudeOffset; // Amplitude
-      smp->data[i * 4 + 2].f =
-          (lastPhasors[i].phase * 180 / M_PI) + phaseOffset;       // Phase
-      smp->data[i * 4 + 3].f = lastPhasors[i].rocof + rocofOffset; /* ROCOF */
-      ;
+      if (outputMode == OutputMode::COMPLEX) {
+        smp->data[i * 3 + 0].f =
+            currentPhasors[i].frequency + frequencyOffset; // Frequency
+        smp->data[i * 3 + 1].z =
+            std::polar(currentPhasors[i].amplitude / std::numbers::sqrt2,
+                       currentPhasors[i].phase); // Phasor
+        smp->data[i * 3 + 2].f = currentPhasors[i].rocof + rocofOffset; // ROCOF
+        smp->length = signalIndices.size() * 3;
+        smp->ts.origin = phasorTimestamp;
+      } else {
+        smp->data[i * 4 + 0].f =
+            currentPhasors[i].frequency + frequencyOffset; // Frequency
+        smp->data[i * 4 + 1].f = (currentPhasors[i].amplitude / pow(2, 0.5)) +
+                                 amplitudeOffset; // Amplitude
+        smp->data[i * 4 + 2].f =
+            (currentPhasors[i].phase * angleUnitFactor) + phaseOffset; // Phase
+        smp->data[i * 4 + 3].f =
+            currentPhasors[i].rocof + rocofOffset; /* ROCOF */
+        smp->length = signalIndices.size() * 4;
+        smp->ts.origin = phasorTimestamp;
+      }
     }
-    smp->ts.origin = phasorTimestamp;
-
-    smp->length = signalIndices.size() * 4;
   }
 
   if (!run || phasorStatus != Status::VALID)
@@ -237,13 +311,13 @@ Hook::Reason PmuHook::process(struct Sample *smp) {
 }
 
 PmuHook::Phasor PmuHook::estimatePhasor(dsp::CosineWindow<double> *window,
-                                        const Phasor &lastPhasor) {
+                                        dsp::Window<timespec> *windowTs) {
   return {0., 0., 0., 0., Status::INVALID};
 }
 
 // Register hook
 static char n[] = "pmu";
-static char d[] = "This hook estimates a phsor";
+static char d[] = "This hook estimates a phasor";
 static HookPlugin<PmuHook, n, d,
                   (int)Hook::Flags::NODE_READ | (int)Hook::Flags::NODE_WRITE |
                       (int)Hook::Flags::PATH>