diff --git a/src/Gemstone.PhasorProtocols.sln.DotSettings b/src/Gemstone.PhasorProtocols.sln.DotSettings index 3f3b82e9b2..a8df163179 100644 --- a/src/Gemstone.PhasorProtocols.sln.DotSettings +++ b/src/Gemstone.PhasorProtocols.sln.DotSettings @@ -6,4 +6,7 @@ True True True + True + True + True True \ No newline at end of file diff --git a/src/Gemstone.PhasorProtocols/SelCWS/Common.cs b/src/Gemstone.PhasorProtocols/SelCWS/Common.cs index 5f833ff0fc..033c0d2f7d 100644 --- a/src/Gemstone.PhasorProtocols/SelCWS/Common.cs +++ b/src/Gemstone.PhasorProtocols/SelCWS/Common.cs @@ -23,6 +23,7 @@ // ReSharper disable InconsistentNaming using System; +using System.ComponentModel; using Gemstone.Numeric.EE; namespace Gemstone.PhasorProtocols.SelCWS; @@ -67,15 +68,33 @@ public enum PhaseChannel /// IA = 3, /// - /// Phase B current (IB). + /// Phase B voltage (VB). /// IB = 4, /// - /// Phase C current (IC). + /// Phase C voltage (VC). /// IC = 5, } +/// +/// Phase estimation algorithm used to derive synchrophasor, frequency and ROCOF components from +/// SEL CWS point-on-wave data. +/// +public enum PhaseEstimationAlgorithm +{ + /// + /// Rolling sliding DFT estimator with optional EMA smoothing (see ). + /// + [Description("Rolling sliding DFT estimator with optional EMA smoothing")] + SlidingDft, + /// + /// IEEE C37.118-2018 Annex D filter-based estimator (see ). + /// + [Description("IEEE C37.118-2018 Annex D filter-based estimator")] + IEEEC37_118 +} + /// /// IEEE C37.118-2018 Annex D filter class for phasor estimation. /// diff --git a/src/Gemstone.PhasorProtocols/SelCWS/ConfigurationCell.cs b/src/Gemstone.PhasorProtocols/SelCWS/ConfigurationCell.cs index 805e8badd4..ee2d5f06c2 100644 --- a/src/Gemstone.PhasorProtocols/SelCWS/ConfigurationCell.cs +++ b/src/Gemstone.PhasorProtocols/SelCWS/ConfigurationCell.cs @@ -59,10 +59,10 @@ internal ConfigurationCell(ConfigurationFrame parent, LineFrequency nominalFrequ }); } + PhasorDefinitionCollection phasorDefinitions = PhasorDefinitions; + for (int i = 0; i < Common.MaximumPhasorValues; i++) - { - PhasorDefinitions.Add(new PhasorDefinition(this, analogNames[i], i < 3 ? PhasorType.Voltage : PhasorType.Current)); - } + phasorDefinitions.Add(new PhasorDefinition(this, analogNames[i], i < 3 ? PhasorType.Voltage : PhasorType.Current)); } /// diff --git a/src/Gemstone.PhasorProtocols/SelCWS/ConnectionParameters.cs b/src/Gemstone.PhasorProtocols/SelCWS/ConnectionParameters.cs index 628d5229c9..33ed4b6908 100644 --- a/src/Gemstone.PhasorProtocols/SelCWS/ConnectionParameters.cs +++ b/src/Gemstone.PhasorProtocols/SelCWS/ConnectionParameters.cs @@ -1,4 +1,4 @@ -//****************************************************************************************************** +//****************************************************************************************************** // ConnectionParameters.cs - Gbtc // // Copyright © 2025, Grid Protection Alliance. All Rights Reserved. @@ -29,7 +29,8 @@ using System.ComponentModel; using System.Runtime.Serialization; using Gemstone.Numeric.EE; -using static Gemstone.PhasorProtocols.SelCWS.RollingPhaseEstimator; +using static Gemstone.PhasorProtocols.SelCWS.SlidingDftPhaseEstimator; +using static Gemstone.PhasorProtocols.SelCWS.IEEEC37_118PhaseEstimator; namespace Gemstone.PhasorProtocols.SelCWS; @@ -58,6 +59,11 @@ public class ConnectionParameters : ConnectionParametersBase /// public const bool DefaultRepeatLastCalculatedValueWhenDownSampling = true; + /// + /// Default value for . + /// + public const PhaseEstimationAlgorithm DefaultAlgorithm = PhaseEstimationAlgorithm.SlidingDft; + #endregion #region [ Constructors ] @@ -71,6 +77,19 @@ public ConnectionParameters() NominalFrequency = Common.DefaultNominalFrequency; CalculationFrameRate = Common.DefaultFramePerSecond; RepeatLastCalculatedValueWhenDownSampling = DefaultRepeatLastCalculatedValueWhenDownSampling; + Algorithm = DefaultAlgorithm; + ReferenceChannel = DefaultReferenceChannel; + TargetCycles = DefaultTargetCycles; + EnableIntervalAveraging = DefaultEnableIntervalAveraging; + EnablePublishEMA = DefaultEnablePublishEMA; + PublishAnglesTauSeconds = DefaultPublishAnglesTauSeconds; + PublishMagnitudesTauSeconds = DefaultPublishMagnitudesTauSeconds; + PublishFrequencyTauSeconds = DefaultPublishFrequencyTauSeconds; + PublishRocofTauSeconds = DefaultPublishRocofTauSeconds; + SampleFrequencyTauSeconds = DefaultSampleFrequencyTauSeconds; + SampleRocofTauSeconds = DefaultSampleRocofTauSeconds; + RecalculationCycles = DefaultRecalculationCycles; + MaxGapFillSamples = DefaultMaxGapFillSamples; FilterClass = DefaultFilterClass; } @@ -86,6 +105,19 @@ protected ConnectionParameters(SerializationInfo info, StreamingContext context) NominalFrequency = info.GetOrDefault("nominalFrequency", Common.DefaultNominalFrequency); CalculationFrameRate = info.GetOrDefault("calculationFrameRate", Common.DefaultFramePerSecond); RepeatLastCalculatedValueWhenDownSampling = info.GetOrDefault("repeatLastCalculatedValueWhenDownSampling", DefaultRepeatLastCalculatedValueWhenDownSampling); + Algorithm = info.GetOrDefault("algorithm", DefaultAlgorithm); + ReferenceChannel = info.GetOrDefault("referenceChannel", DefaultReferenceChannel); + TargetCycles = info.GetOrDefault("targetCycles", DefaultTargetCycles); + EnableIntervalAveraging = info.GetOrDefault("enableIntervalAveraging", DefaultEnableIntervalAveraging); + EnablePublishEMA = info.GetOrDefault("enablePublishEMA", DefaultEnablePublishEMA); + PublishAnglesTauSeconds = info.GetOrDefault("publishAnglesTauSeconds", DefaultPublishAnglesTauSeconds); + PublishMagnitudesTauSeconds = info.GetOrDefault("publishMagnitudesTauSeconds", DefaultPublishMagnitudesTauSeconds); + PublishFrequencyTauSeconds = info.GetOrDefault("publishFrequencyTauSeconds", DefaultPublishFrequencyTauSeconds); + PublishRocofTauSeconds = info.GetOrDefault("publishRocofTauSeconds", DefaultPublishRocofTauSeconds); + SampleFrequencyTauSeconds = info.GetOrDefault("sampleFrequencyTauSeconds", DefaultSampleFrequencyTauSeconds); + SampleRocofTauSeconds = info.GetOrDefault("sampleRocofTauSeconds", DefaultSampleRocofTauSeconds); + RecalculationCycles = info.GetOrDefault("recalculationCycles", DefaultRecalculationCycles); + MaxGapFillSamples = info.GetOrDefault("maxGapFillSamples", DefaultMaxGapFillSamples); FilterClass = info.GetOrDefault("filterClass", DefaultFilterClass); } @@ -97,7 +129,7 @@ protected ConnectionParameters(SerializationInfo info, StreamingContext context) /// Gets or sets flag that determines if current and voltage phase estimates, frequency and dF/dt should be /// calculated for PoW data. /// - [Category("Phase Estimation Parameters")] + [Category("General Phase Estimation Settings")] [Description("Determines if current and voltage phase estimates, frequency and dF/dt should be calculated for PoW data.")] [DefaultValue(DefaultCalculatePhaseEstimates)] public bool CalculatePhaseEstimates { get; set; } @@ -105,7 +137,7 @@ protected ConnectionParameters(SerializationInfo info, StreamingContext context) /// /// Gets or sets the nominal of this SEL CWS device. /// - [Category("Phase Estimation Parameters")] + [Category("General Phase Estimation Settings")] [Description("Configured nominal frequency for SEL CWS device.")] [DefaultValue(typeof(LineFrequency), "Hz60")] public LineFrequency NominalFrequency { get; set; } @@ -113,7 +145,7 @@ protected ConnectionParameters(SerializationInfo info, StreamingContext context) /// /// Gets or sets the configured frame rate for phase estimate calculations. /// - [Category("Phase Estimation Parameters")] + [Category("General Phase Estimation Settings")] [Description("Configured frame rate for phase estimate calculations.")] [DefaultValue(Common.DefaultFramePerSecond)] public ushort CalculationFrameRate { get; set; } @@ -123,7 +155,7 @@ protected ConnectionParameters(SerializationInfo info, StreamingContext context) /// when is less than SEL CWS frame rate (commonly 3000Hz); /// otherwise will be used. /// - [Category("Phase Estimation Parameters")] + [Category("General Phase Estimation Settings")] [Description( "Gets or sets flag that determines if last value should be repeated when down-sampling, i.e.," + "when 'CalculationFrameRate' is less than SEL CWS frame rate (commonly 3000Hz);" + @@ -132,11 +164,158 @@ protected ConnectionParameters(SerializationInfo info, StreamingContext context) [DefaultValue(DefaultRepeatLastCalculatedValueWhenDownSampling)] public bool RepeatLastCalculatedValueWhenDownSampling { get; set; } + /// + /// Gets or sets the used to derive synchrophasor, frequency + /// and ROCOF components from SEL CWS point-on-wave data. + /// + /// + /// uses a rolling sliding DFT estimator with optional + /// EMA smoothing; uses the IEEE C37.118-2018 Annex D + /// filter-based estimator. Algorithm-specific options appear in the matching property category. + /// + [Category("General Phase Estimation Settings")] + [Description("Phase estimation algorithm: SlidingDft (rolling sliding DFT with optional EMA smoothing) or IEEEC37_118 (IEEE C37.118-2018 Annex D filter-based). Algorithm-specific options appear in the matching category.")] + [DefaultValue(DefaultAlgorithm)] + public PhaseEstimationAlgorithm Algorithm { get; set; } + + /// + /// Gets or sets the reference channel for frequency tracking. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("Reference channel for frequency tracking.")] + [DefaultValue(typeof(PhaseChannel), "VA")] + public PhaseChannel ReferenceChannel { get; set; } + + /// + /// Gets or sets the number of nominal cycles contained in the sliding DFT analysis window. + /// + /// + /// Larger values generally reduce noise/jitter (more averaging) but increase latency and reduce step response. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("Number of nominal cycles contained in the sliding DFT analysis window. Larger values generally reduce noise/jitter (more averaging) but increase latency and reduce step response.")] + [DefaultValue(DefaultTargetCycles)] + public int TargetCycles { get; set; } + + /// + /// Gets or sets a flag that determines if interval averaging (boxcar averaging) is enabled across each publish interval when down-sampling. + /// + /// + /// Down-sampling without an anti-alias / low-pass step will preserve high-rate jitter and can alias higher-frequency + /// content into the published stream. Interval averaging acts as a simple, cheap low-pass filter that reduces + /// jitter and improves published stability. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("Enables interval averaging (boxcar averaging) across each publish interval when down-sampling. Down-sampling without an anti-alias / low-pass step will preserve high-rate jitter and can alias higher-frequency content into the published stream. Interval averaging acts as a simple, cheap low-pass filter that reduces jitter and improves published stability.")] + [DefaultValue(DefaultEnableIntervalAveraging)] + public bool EnableIntervalAveraging { get; set; } + + /// + /// Gets or sets a flag that determines if an additional exponential moving average (EMA) is applied to the published stream (after interval averaging). + /// + /// + /// Interval averaging removes high-rate noise; publish-EMA further reduces remaining jitter and produces a "calm" + /// display or control signal. This is usually the most intuitive "knob" for operators/consumers because it acts on + /// the actual output cadence. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("Enables an additional exponential moving average (EMA) applied to the published stream (after interval averaging). Interval averaging removes high-rate noise; publish-EMA further reduces remaining jitter and produces a calm display or control signal.")] + [DefaultValue(DefaultEnablePublishEMA)] + public bool EnablePublishEMA { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published phase angles. + /// + /// + /// Angles are circular quantities; this implementation performs wrap-safe smoothing by operating on unit vectors + /// (cos/sin) rather than naïvely averaging radians. This avoids discontinuities at ±π. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("EMA time constant τ (seconds) for published phase angles. Angles are circular quantities; this implementation performs wrap-safe smoothing by operating on unit vectors (cos/sin) rather than naïvely averaging radians.")] + [DefaultValue(DefaultPublishAnglesTauSeconds)] + public double PublishAnglesTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published RMS magnitudes. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("EMA time constant τ (seconds) for published RMS magnitudes.")] + [DefaultValue(DefaultPublishMagnitudesTauSeconds)] + public double PublishMagnitudesTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published frequency. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("EMA time constant τ (seconds) for published frequency.")] + [DefaultValue(DefaultPublishFrequencyTauSeconds)] + public double PublishFrequencyTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published ROCOF (dF/dt). + /// + /// + /// ROCOF is effectively a derivative signal and is typically much noisier than frequency; it generally benefits from + /// heavier smoothing (larger τ) than frequency. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("EMA time constant τ (seconds) for published ROCOF (dF/dt). ROCOF is effectively a derivative signal and is typically much noisier than frequency; it generally benefits from heavier smoothing (larger τ) than frequency.")] + [DefaultValue(DefaultPublishRocofTauSeconds)] + public double PublishRocofTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for the internal per-sample frequency smoothing that occurs inside the estimator before any down-sampling/publish filtering. + /// + /// + /// When interval averaging + publish EMA are enabled, this can be relatively light. If you disable publish smoothing, + /// you may want to increase this τ. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("EMA time constant τ (seconds) for the internal per-sample frequency smoothing that occurs inside the estimator before any down-sampling/publish filtering. When interval averaging + publish EMA are enabled, this can be relatively light.")] + [DefaultValue(DefaultSampleFrequencyTauSeconds)] + public double SampleFrequencyTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for the internal per-sample ROCOF smoothing (computed from the internally smoothed frequency). + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("EMA time constant τ (seconds) for the internal per-sample ROCOF smoothing (computed from the internally smoothed frequency).")] + [DefaultValue(DefaultSampleRocofTauSeconds)] + public double SampleRocofTauSeconds { get; set; } + + /// + /// Gets or sets the number of nominal cycles between full DFT recalculations for numerical stability. + /// + /// + /// Sliding DFT updates are O(1) per sample but can accumulate numerical drift; periodic full recomputation + /// re-anchors the phasor sums. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("Number of nominal cycles between full DFT recalculations for numerical stability. Sliding DFT updates are O(1) per sample but can accumulate numerical drift; periodic full recomputation re-anchors the phasor sums.")] + [DefaultValue(DefaultRecalculationCycles)] + public int RecalculationCycles { get; set; } + + /// + /// Gets or sets the maximum gap (in input samples) filled by phase-continued synthesis before resynchronizing. + /// + /// + /// When dropped samples are inferred from the input timestamp cadence, gaps up to this size are coasted + /// (the last phasors are continued at the tracked frequency); larger gaps drop and refill the analysis window. + /// A negative value means "auto" (one full analysis window); 0 resynchronizes on any gap. + /// + [Category("Sliding DFT Estimation Algorithm Settings")] + [Description("Maximum gap (in input samples) filled by phase-continued synthesis before resynchronizing. Negative means auto (one full analysis window); 0 resynchronizes on any gap.")] + [DefaultValue(DefaultMaxGapFillSamples)] + public int MaxGapFillSamples { get; set; } + /// /// Gets or sets the IEEE C37.118 filter class: P (Protection, fast response) or M (Measurement, better out-of-band rejection). /// - [Category("Phase Estimation Parameters")] - [Description("IEEE C37.118 filter class: P (Protection, fast response) or M (Measurement, better out-of-band rejection).")] + /// + /// Only applies when is . + /// + [Category("IEEE C37.118-2018 Annex D Estimation Algorithm Settings")] + [Description("IEEE C37.118 filter class: P (Protection, fast response) or M (Measurement, better out-of-band rejection). Applies only when the IEEE C37.118 algorithm is selected.")] [DefaultValue(DefaultFilterClass)] public FilterClass FilterClass { get; set; } @@ -156,6 +335,19 @@ public override void GetObjectData(SerializationInfo info, StreamingContext cont info.AddValue("nominalFrequency", NominalFrequency, typeof(LineFrequency)); info.AddValue("calculationFrameRate", CalculationFrameRate); info.AddValue("repeatLastCalculatedValueWhenDownSampling", RepeatLastCalculatedValueWhenDownSampling); + info.AddValue("algorithm", Algorithm, typeof(PhaseEstimationAlgorithm)); + info.AddValue("referenceChannel", ReferenceChannel, typeof(PhaseChannel)); + info.AddValue("targetCycles", TargetCycles); + info.AddValue("enableIntervalAveraging", EnableIntervalAveraging); + info.AddValue("enablePublishEMA", EnablePublishEMA); + info.AddValue("publishAnglesTauSeconds", PublishAnglesTauSeconds); + info.AddValue("publishMagnitudesTauSeconds", PublishMagnitudesTauSeconds); + info.AddValue("publishFrequencyTauSeconds", PublishFrequencyTauSeconds); + info.AddValue("publishRocofTauSeconds", PublishRocofTauSeconds); + info.AddValue("sampleFrequencyTauSeconds", SampleFrequencyTauSeconds); + info.AddValue("sampleRocofTauSeconds", SampleRocofTauSeconds); + info.AddValue("recalculationCycles", RecalculationCycles); + info.AddValue("maxGapFillSamples", MaxGapFillSamples); info.AddValue("filterClass", FilterClass, typeof(FilterClass)); } diff --git a/src/Gemstone.PhasorProtocols/SelCWS/FrameParser.cs b/src/Gemstone.PhasorProtocols/SelCWS/FrameParser.cs index 3c542da85e..456fb87c98 100644 --- a/src/Gemstone.PhasorProtocols/SelCWS/FrameParser.cs +++ b/src/Gemstone.PhasorProtocols/SelCWS/FrameParser.cs @@ -31,7 +31,8 @@ using Gemstone.Numeric.EE; using static Gemstone.PhasorProtocols.SelCWS.Common; using static Gemstone.PhasorProtocols.SelCWS.ConnectionParameters; -using static Gemstone.PhasorProtocols.SelCWS.RollingPhaseEstimator; +using static Gemstone.PhasorProtocols.SelCWS.SlidingDftPhaseEstimator; +using static Gemstone.PhasorProtocols.SelCWS.IEEEC37_118PhaseEstimator; namespace Gemstone.PhasorProtocols.SelCWS; @@ -69,7 +70,7 @@ public class FrameParser : FrameParserBase // Fields private ConfigurationFrame? m_configurationFrame; private DataFrame? m_initialDataFrame; - private RollingPhaseEstimator? m_phaseEstimator; + private IPhaseEstimator? m_phaseEstimator; private long[]? m_nanosecondPacketFrameOffsets; private DataCell? m_lastCell; @@ -89,6 +90,19 @@ public FrameParser(CheckSumValidationFrameTypes checkSumValidationFrameTypes = C NominalFrequency = DefaultNominalFrequency; CalculationFrameRate = DefaultFramePerSecond; RepeatLastCalculatedValueWhenDownSampling = DefaultRepeatLastCalculatedValueWhenDownSampling; + Algorithm = DefaultAlgorithm; + ReferenceChannel = DefaultReferenceChannel; + TargetCycles = DefaultTargetCycles; + EnableIntervalAveraging = DefaultEnableIntervalAveraging; + EnablePublishEMA = DefaultEnablePublishEMA; + PublishAnglesTauSeconds = DefaultPublishAnglesTauSeconds; + PublishMagnitudesTauSeconds = DefaultPublishMagnitudesTauSeconds; + PublishFrequencyTauSeconds = DefaultPublishFrequencyTauSeconds; + PublishRocofTauSeconds = DefaultPublishRocofTauSeconds; + SampleFrequencyTauSeconds = DefaultSampleFrequencyTauSeconds; + SampleRocofTauSeconds = DefaultSampleRocofTauSeconds; + RecalculationCycles = DefaultRecalculationCycles; + MaxGapFillSamples = DefaultMaxGapFillSamples; FilterClass = DefaultFilterClass; } @@ -147,9 +161,115 @@ public override IConfigurationFrame ConfigurationFrame /// public bool RepeatLastCalculatedValueWhenDownSampling { get; set; } + /// + /// Gets or sets the used to derive synchrophasor, frequency + /// and ROCOF components from SEL CWS point-on-wave data. + /// + public PhaseEstimationAlgorithm Algorithm { get; set; } + + /// + /// Gets or sets the reference channel for frequency tracking. + /// + /// + /// Only applies when is . + /// + public PhaseChannel ReferenceChannel { get; set; } + + /// + /// Gets or sets the number of nominal cycles contained in the sliding DFT analysis window. + /// + /// + /// Larger values generally reduce noise/jitter (more averaging) but increase latency and reduce step response. + /// + public int TargetCycles { get; set; } + + /// + /// Gets or sets a flag that determines if interval averaging (boxcar averaging) is enabled across each publish interval when down-sampling. + /// + /// + /// Down-sampling without an anti-alias / low-pass step will preserve high-rate jitter and can alias higher-frequency + /// content into the published stream. Interval averaging acts as a simple, cheap low-pass filter that reduces + /// jitter and improves published stability. + /// + public bool EnableIntervalAveraging { get; set; } + + /// + /// Gets or sets a flag that determines if an additional exponential moving average (EMA) is applied to the published stream (after interval averaging). + /// + /// + /// Interval averaging removes high-rate noise; publish-EMA further reduces remaining jitter and produces a "calm" + /// display or control signal. This is usually the most intuitive "knob" for operators/consumers because it acts on + /// the actual output cadence. + /// + public bool EnablePublishEMA { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published phase angles. + /// + /// + /// Angles are circular quantities; this implementation performs wrap-safe smoothing by operating on unit vectors + /// (cos/sin) rather than naïvely averaging radians. This avoids discontinuities at ±π. + /// + public double PublishAnglesTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published RMS magnitudes. + /// + public double PublishMagnitudesTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published frequency. + /// + public double PublishFrequencyTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for published ROCOF (dF/dt). + /// + /// + /// ROCOF is effectively a derivative signal and is typically much noisier than frequency; it generally benefits from + /// heavier smoothing (larger τ) than frequency. + /// + public double PublishRocofTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for the internal per-sample frequency smoothing that occurs inside the estimator before any down-sampling/publish filtering. + /// + /// + /// When interval averaging + publish EMA are enabled, this can be relatively light. If you disable publish smoothing, + /// you may want to increase this τ. + /// + public double SampleFrequencyTauSeconds { get; set; } + + /// + /// Gets or sets the EMA time constant τ (seconds) for the internal per-sample ROCOF smoothing (computed from the internally smoothed frequency). + /// + public double SampleRocofTauSeconds { get; set; } + + /// + /// Gets or sets the number of nominal cycles between full DFT recalculations for numerical stability. + /// + /// + /// Sliding DFT updates are O(1) per sample but can accumulate numerical drift; periodic full recomputation + /// re-anchors the phasor sums. + /// + public int RecalculationCycles { get; set; } + + /// + /// Gets or sets the maximum gap (in input samples) filled by phase-continued synthesis before resynchronizing. + /// + /// + /// Dropped samples (e.g., lost UDP packets) are inferred from the input timestamp cadence. Gaps up to this size + /// are coasted by continuing the last phasors at the tracked frequency; larger gaps drop and refill the analysis + /// window. A negative value means "auto" (one full analysis window); 0 resynchronizes on any gap. + /// + public int MaxGapFillSamples { get; set; } + /// /// Gets or sets the IEEE C37.118 filter class: P (Protection, fast response) or M (Measurement, better out-of-band rejection). /// + /// + /// Only applies when is . + /// public FilterClass FilterClass { get; set; } #endregion @@ -282,9 +402,12 @@ protected override int ParseFrame(byte[] buffer, int offset, int length) // Ensure nanosecond frame distribution is initialized m_nanosecondPacketFrameOffsets ??= CalculateNanosecondPacketFrameOffsets(FramesPerPacket); - // Move offset past initial data frame which includes 64-bit nanosecond timestamp - offset += 32; - length -= 32; + // Move offset past the entire initial data frame to reach the second sample. The initial + // frame spans the 16-byte common header + 8-byte nanosecond timestamp + first 24-byte sample + // (6 analogs x 4 bytes) = 48 bytes. (Previously skipped only 32, omitting the common header, + // which misaligned samples 1-49 of every packet by 16 bytes / 4 analog channels.) + offset += 48; + length -= 48; // In the case of data frames in CWS, the source buffer has 49 more frames to parse after the first for (int i = 1; i < FramesPerPacket; i++) @@ -334,15 +457,11 @@ private void ApplyEstimatedPhases(DataFrame dataFrame) double ib = cell.AnalogValues[(int)PhaseChannel.IB].Value; double ic = cell.AnalogValues[(int)PhaseChannel.IC].Value; - // Ensure phase estimator is created - m_phaseEstimator ??= new RollingPhaseEstimator( - DefaultFramePerSecond, - CalculationFrameRate, - NominalFrequency, - FilterClass); + // Ensure phase estimator is created for the selected algorithm + m_phaseEstimator ??= CreatePhaseEstimator(); // Calculate next phase estimation - bool calculated = m_phaseEstimator.Step(ia, ib, ic, va, vb, vc, timestamp, processPhaseEstimate); + bool calculated = m_phaseEstimator.Step(va, vb, vc, ia, ib, ic, timestamp, processPhaseEstimate); if (!RepeatLastCalculatedValueWhenDownSampling) return; @@ -383,6 +502,36 @@ void processPhaseEstimate(in PhaseEstimate estimate) } } + // Creates the phase estimator for the currently selected algorithm. Both estimators consume + // sample-groups in VA, VB, VC, IA, IB, IC order and publish a common PhaseEstimate. + private IPhaseEstimator CreatePhaseEstimator() + { + return Algorithm switch + { + PhaseEstimationAlgorithm.IEEEC37_118 => new IEEEC37_118PhaseEstimator( + DefaultFramePerSecond, + CalculationFrameRate, + NominalFrequency, + FilterClass), + _ => new SlidingDftPhaseEstimator( + DefaultFramePerSecond, + CalculationFrameRate, + NominalFrequency, + ReferenceChannel, + TargetCycles, + EnableIntervalAveraging, + EnablePublishEMA, + PublishAnglesTauSeconds, + PublishMagnitudesTauSeconds, + PublishFrequencyTauSeconds, + PublishRocofTauSeconds, + SampleFrequencyTauSeconds, + SampleRocofTauSeconds, + RecalculationCycles, + MaxGapFillSamples) + }; + } + /// protected override void OnParsingException(Exception ex) { @@ -462,7 +611,23 @@ public override IConnectionParameters ConnectionParameters NominalFrequency = parameters.NominalFrequency; CalculationFrameRate = parameters.CalculationFrameRate; RepeatLastCalculatedValueWhenDownSampling = parameters.RepeatLastCalculatedValueWhenDownSampling; + Algorithm = parameters.Algorithm; + ReferenceChannel = parameters.ReferenceChannel; + TargetCycles = parameters.TargetCycles; + EnableIntervalAveraging = parameters.EnableIntervalAveraging; + EnablePublishEMA = parameters.EnablePublishEMA; + PublishAnglesTauSeconds = parameters.PublishAnglesTauSeconds; + PublishMagnitudesTauSeconds = parameters.PublishMagnitudesTauSeconds; + PublishFrequencyTauSeconds = parameters.PublishFrequencyTauSeconds; + PublishRocofTauSeconds = parameters.PublishRocofTauSeconds; + SampleFrequencyTauSeconds = parameters.SampleFrequencyTauSeconds; + SampleRocofTauSeconds = parameters.SampleRocofTauSeconds; + RecalculationCycles = parameters.RecalculationCycles; + MaxGapFillSamples = parameters.MaxGapFillSamples; FilterClass = parameters.FilterClass; + + // Force the estimator to be rebuilt so any change of algorithm or its options takes effect + m_phaseEstimator = null; } } diff --git a/src/Gemstone.PhasorProtocols/SelCWS/RollingPhaseEstimator.cs b/src/Gemstone.PhasorProtocols/SelCWS/IEEEC37_118PhaseEstimator.cs similarity index 90% rename from src/Gemstone.PhasorProtocols/SelCWS/RollingPhaseEstimator.cs rename to src/Gemstone.PhasorProtocols/SelCWS/IEEEC37_118PhaseEstimator.cs index 39beaf3ed8..92d1a7d0f5 100644 --- a/src/Gemstone.PhasorProtocols/SelCWS/RollingPhaseEstimator.cs +++ b/src/Gemstone.PhasorProtocols/SelCWS/IEEEC37_118PhaseEstimator.cs @@ -1,5 +1,5 @@ //****************************************************************************************************** -// RollingPhaseEstimator.cs - Gbtc +// IEEEC37_118PhaseEstimator.cs - Gbtc // // Copyright © 2025, Grid Protection Alliance. All Rights Reserved. // @@ -16,13 +16,13 @@ // // Code Modification History: // ---------------------------------------------------------------------------------------------------- -// 11/04/2025 - Ritchie Carroll -// Generated original version of source code in collaboration with ChatGPT. -// // 03/19/2026 - Ritchie Carroll -// Replaced sliding DFT algorithm with IEEE C37.118-2018 Annex D filter-based phasor estimation. -// Added P-class and M-class filter support, positive-sequence computation, and IEEE-standard -// frequency/ROCOF estimation. +// Generated original version of source code in collaboration with ChatGPT implementing the +// IEEE C37.118-2018 Annex D filter-based phasor estimation algorithm. +// 05/23/2026 - Ritchie Carroll +// Standardized on the VA, VB, VC, IA, IB, IC channel order used by the sliding DFT estimator +// (which also corrects the voltage positive-sequence channel indexing for frequency/ROCOF +// estimation) as part of the selectable SEL CWS phase-estimation algorithm work. // //****************************************************************************************************** // ReSharper disable IdentifierTypo @@ -38,52 +38,6 @@ namespace Gemstone.PhasorProtocols.SelCWS; -/// -/// Represents the output of the phase estimation algorithm. -/// -public readonly ref struct PhaseEstimate -{ - /// - /// Gets frequency estimate in hertz. - /// - public required double Frequency { get; init; } - - /// - /// Gets rate of change of frequency (ROCOF) in Hz/s. - /// - public required double dFdt { get; init; } - - /// - /// Gets angles in radians, length 6: IA, IB, IC, VA, VB, VC. - /// - /// - /// - /// The data this span references is owned by the instance, - /// it is only valid during the scope of the delegate call. - /// If you need to retain the data, make a copy. - /// - /// - public required ReadOnlySpan Angles { get; init; } - - /// - /// Gets RMS magnitudes, length 6: IA, IB, IC, VA, VB, VC. - /// - /// - /// - /// The data this span references is owned by the instance, - /// it is only valid during the scope of the delegate call. - /// If you need to retain the data, make a copy. - /// - /// - public required ReadOnlySpan Magnitudes { get; init; } -} - -/// -/// Delegate for handling a . -/// -/// Phase estimate. -public delegate void PhaseEstimateHandler(in PhaseEstimate estimate); - /// /// Represents a rolling six-phase estimator using the IEEE C37.118-2018 Annex D filter-based /// phasor estimation algorithm with P-class and M-class support. @@ -108,17 +62,17 @@ public readonly ref struct PhaseEstimate /// to compensate for spectral leakage when the signal frequency deviates from nominal. /// /// -/// Outputs: 6 phasors (IA, IB, IC, VA, VB, VC) plus frequency and ROCOF estimates. +/// Outputs: 6 phasors (VA, VB, VC, IA, IB, IC) plus frequency and ROCOF estimates. /// Internally computes voltage positive-sequence for IEEE Annex D.4 frequency/ROCOF estimation. /// /// -public sealed class RollingPhaseEstimator +public sealed class IEEEC37_118PhaseEstimator : IPhaseEstimator { #region [ Members ] // Constants - // Number of input channels (IA, IB, IC, VA, VB, VC). + // Number of input channels (VA, VB, VC, IA, IB, IC). private const int NumInputChannels = 6; private const double TwoPI = 2.0 * Math.PI; @@ -172,7 +126,7 @@ public sealed class RollingPhaseEstimator #region [ Constructors ] /// - /// Initializes a new instance of the class implementing + /// Initializes a new instance of the class implementing /// the IEEE C37.118-2018 Annex D phasor estimation algorithm. /// /// @@ -198,16 +152,21 @@ public sealed class RollingPhaseEstimator /// Thrown if the and combination /// is not supported for M-class filters per IEEE C37.118-2018 Table D.1. /// - public RollingPhaseEstimator( + public IEEEC37_118PhaseEstimator( double sampleRateHz, double outputRateHz, LineFrequency nominalFrequency, FilterClass filterClass = DefaultFilterClass) { // Validate rates - ArgumentOutOfRangeException.ThrowIfNegativeOrZero(sampleRateHz); - ArgumentOutOfRangeException.ThrowIfNegativeOrZero(outputRateHz); - ArgumentOutOfRangeException.ThrowIfGreaterThan(outputRateHz, sampleRateHz); + if (sampleRateHz <= 0) + throw new ArgumentOutOfRangeException(nameof(sampleRateHz), "Input sample rate must be positive."); + + if (outputRateHz <= 0) + throw new ArgumentOutOfRangeException(nameof(outputRateHz), "Output rate must be positive."); + + if (outputRateHz > sampleRateHz) + throw new ArgumentOutOfRangeException(nameof(outputRateHz), "Output rate must be <= input sample rate."); double f0 = (double)nominalFrequency; double fs = sampleRateHz; @@ -306,14 +265,14 @@ public RollingPhaseEstimator( #region [ Methods ] /// - /// Push one interleaved sample-group (IA, IB, IC, VA, VB, VC) with its epoch nanoseconds. + /// Push one interleaved sample-group (VA, VB, VC, IA, IB, IC) with its epoch nanoseconds. /// - /// Current sample for phase A current. - /// Current sample for phase B current. - /// Current sample for phase C current. /// Current sample for phase A voltage. /// Current sample for phase B voltage. /// Current sample for phase C voltage. + /// Current sample for phase A current. + /// Current sample for phase B current. + /// Current sample for phase C current. /// Timestamp in nanoseconds since epoch. /// Handler for phase estimate result. /// @@ -323,22 +282,22 @@ public RollingPhaseEstimator( /// /// The data referenced in the span-based properties of the /// parameter provided to the delegate is owned by the - /// instance, it is only valid during the scope of the + /// instance, it is only valid during the scope of the /// delegate call. If you need to retain the data, make a copy. /// public bool Step( - double ia, - double ib, - double ic, double va, double vb, double vc, + double ia, + double ib, + double ic, long epochNanoseconds, PhaseEstimateHandler phaseEstimateHandler) { ArgumentNullException.ThrowIfNull(phaseEstimateHandler); - Span samples = [ia, ib, ic, va, vb, vc]; // Implicit stackalloc + Span samples = [ va, vb, vc, ia, ib, ic ]; // Push new samples into circular buffers for (int ch = 0; ch < NumInputChannels; ch++) @@ -416,8 +375,8 @@ public void Reset() m_lastUnwrappedPhase = 0.0D; // Reset output buffers - Array.Clear(m_publishAngles); - Array.Clear(m_publishMagnitudes); + Array.Clear(m_publishAngles, 0, NumInputChannels); + Array.Clear(m_publishMagnitudes, 0, NumInputChannels); } /// @@ -512,15 +471,15 @@ private void ComputeVoltagePositiveSequence() // Reconstruct derotated complex phasors from published magnitudes and angles // (which have the nominal frequency rotation already removed in ExtractMagnitudesAndAngles). // This matches the Python __estimate output: XM*cos(XA) + j*XM*sin(XA) - double vaAngle = (double)m_publishAngles[VA]; + double vaAngle = m_publishAngles[VA]; double vaReal = m_publishMagnitudes[VA] * Math.Cos(vaAngle); double vaImag = m_publishMagnitudes[VA] * Math.Sin(vaAngle); - double vbAngle = (double)m_publishAngles[VB]; + double vbAngle = m_publishAngles[VB]; double vbReal = m_publishMagnitudes[VB] * Math.Cos(vbAngle); double vbImag = m_publishMagnitudes[VB] * Math.Sin(vbAngle); - double vcAngle = (double)m_publishAngles[VC]; + double vcAngle = m_publishAngles[VC]; double vcReal = m_publishMagnitudes[VC] * Math.Cos(vcAngle); double vcImag = m_publishMagnitudes[VC] * Math.Sin(vcAngle); diff --git a/src/Gemstone.PhasorProtocols/SelCWS/IPhaseEstimator.cs b/src/Gemstone.PhasorProtocols/SelCWS/IPhaseEstimator.cs new file mode 100644 index 0000000000..1e79ec9b1a --- /dev/null +++ b/src/Gemstone.PhasorProtocols/SelCWS/IPhaseEstimator.cs @@ -0,0 +1,67 @@ +//****************************************************************************************************** +// IPhaseEstimator.cs - Gbtc +// +// Copyright © 2026, Grid Protection Alliance. All Rights Reserved. +// +// Licensed to the Grid Protection Alliance (GPA) under one or more contributor license agreements. See +// the NOTICE file distributed with this work for additional information regarding copyright ownership. +// The GPA licenses this file to you under the MIT License (MIT), the "License"; you may not use this +// file except in compliance with the License. You may obtain a copy of the License at: +// +// http://opensource.org/licenses/MIT +// +// Unless agreed to in writing, the subject software distributed under the License is distributed on an +// "AS-IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. Refer to the +// License for the specific language governing permissions and limitations. +// +// Code Modification History: +// ---------------------------------------------------------------------------------------------------- +// 05/23/2026 - Ritchie Carroll +// Generated original version of source code. +// +//****************************************************************************************************** +// ReSharper disable IdentifierTypo +// ReSharper disable InconsistentNaming + +namespace Gemstone.PhasorProtocols.SelCWS; + +/// +/// Defines a SEL CWS six-phase estimator that consumes point-on-wave sample groups and produces +/// results. +/// +/// +/// Implementations represent interchangeable phasor estimation algorithms (see +/// ) selected via the SEL CWS connection parameters. +/// +public interface IPhaseEstimator +{ + /// + /// Pushes one interleaved sample-group (VA, VB, VC, IA, IB, IC) with its epoch nanoseconds. + /// + /// Current sample for phase A voltage. + /// Current sample for phase B voltage. + /// Current sample for phase C voltage. + /// Current sample for phase A current. + /// Current sample for phase B current. + /// Current sample for phase C current. + /// Timestamp in nanoseconds since epoch. + /// Handler for phase estimate result. + /// + /// true when an estimate is available, and it is time to publish at the target output rate; + /// otherwise, false. + /// + bool Step( + double va, + double vb, + double vc, + double ia, + double ib, + double ic, + long epochNanoseconds, + PhaseEstimateHandler phaseEstimateHandler); + + /// + /// Resets the estimator to its initial state. + /// + void Reset(); +} diff --git a/src/Gemstone.PhasorProtocols/SelCWS/PhaseEstimate.cs b/src/Gemstone.PhasorProtocols/SelCWS/PhaseEstimate.cs new file mode 100644 index 0000000000..8c40ecc07b --- /dev/null +++ b/src/Gemstone.PhasorProtocols/SelCWS/PhaseEstimate.cs @@ -0,0 +1,79 @@ +//****************************************************************************************************** +// PhaseEstimate.cs - Gbtc +// +// Copyright © 2025, Grid Protection Alliance. All Rights Reserved. +// +// Licensed to the Grid Protection Alliance (GPA) under one or more contributor license agreements. See +// the NOTICE file distributed with this work for additional information regarding copyright ownership. +// The GPA licenses this file to you under the MIT License (MIT), the "License"; you may not use this +// file except in compliance with the License. You may obtain a copy of the License at: +// +// http://opensource.org/licenses/MIT +// +// Unless agreed to in writing, the subject software distributed under the License is distributed on an +// "AS-IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. Refer to the +// License for the specific language governing permissions and limitations. +// +// Code Modification History: +// ---------------------------------------------------------------------------------------------------- +// 11/04/2025 - Ritchie Carroll +// Generated original version of source code in collaboration with ChatGPT. +// 05/23/2026 - Ritchie Carroll +// Extracted shared phase-estimate result type and handler so multiple phase-estimation +// algorithms (sliding DFT and IEEE C37.118-2018 Annex D) can produce a common output. +// +//****************************************************************************************************** +// ReSharper disable IdentifierTypo +// ReSharper disable CommentTypo +// ReSharper disable InconsistentNaming + +using System; +using Gemstone.Units; + +namespace Gemstone.PhasorProtocols.SelCWS; + +/// +/// Represents the output of a SEL CWS phase estimation algorithm. +/// +/// +/// This is the common result type produced by every implementation, +/// regardless of the underlying estimation algorithm. +/// +public readonly ref struct PhaseEstimate +{ + /// + /// Gets frequency estimate in hertz. + /// + public required double Frequency { get; init; } + + /// + /// Gets rate of change of frequency (ROCOF) in Hz/s. + /// + public required double dFdt { get; init; } + + /// + /// Gets angles in radians, length 6: VA, VB, VC, IA, IB, IC. + /// + /// + /// The data this span references is owned by the phase estimator instance, + /// it is only valid during the scope of the delegate call. + /// If you need to retain the data, make a copy. + /// + public required ReadOnlySpan Angles { get; init; } + + /// + /// Gets RMS magnitudes, length 6: VA, VB, VC, IA, IB, IC. + /// + /// + /// The data this span references is owned by the phase estimator instance, + /// it is only valid during the scope of the delegate call. + /// If you need to retain the data, make a copy. + /// + public required ReadOnlySpan Magnitudes { get; init; } +} + +/// +/// Delegate for handling a . +/// +/// Phase estimate. +public delegate void PhaseEstimateHandler(in PhaseEstimate estimate); diff --git a/src/Gemstone.PhasorProtocols/SelCWS/RollingPhaseEstimationTest.cs b/src/Gemstone.PhasorProtocols/SelCWS/RollingPhaseEstimationTest.cs deleted file mode 100644 index 7a4b5fb951..0000000000 --- a/src/Gemstone.PhasorProtocols/SelCWS/RollingPhaseEstimationTest.cs +++ /dev/null @@ -1,382 +0,0 @@ -// ReSharper disable UnusedParameter.Local -// ReSharper disable InconsistentNaming -// ReSharper disable PossiblyImpureMethodCallOnReadonlyVariable - -using System; -using Gemstone.Numeric.EE; - -namespace Gemstone.PhasorProtocols.SelCWS; - -/// -/// Test program demonstrating the RollingPhaseEstimator with IEEE C37.118-2018 Annex D algorithm. -/// -internal class Program -{ - private static void Main(string[] args) - { - System.Console.WriteLine("RollingPhaseEstimator Test Program (IEEE C37.118-2018 Annex D)"); - System.Console.WriteLine("==============================================================\n"); - - // Test 1: Basic operation at nominal frequency (P-class) - TestNominalFrequency(); - - // Test 2: Frequency deviation detection - TestFrequencyDeviation(); - - // Test 3: Magnitude measurement - TestMagnitudeMeasurement(); - - // Test 4: Phase angle relationships - TestPhaseAngles(); - - // Test 5: P-class vs M-class comparison - TestPClassVsMClass(); - - // Test 6: Decimation count verification - TestDecimationCount(); - - System.Console.WriteLine("\nAll tests completed."); - } - - private static void TestNominalFrequency() - { - System.Console.WriteLine("Test 1: Nominal Frequency Operation (60 Hz, P-class)"); - System.Console.WriteLine("-----------------------------------------------------"); - - const double sampleRate = 960.0; - const double outputRate = 60.0; - const double nominalFreq = 60.0; - const double amplitude = 100.0; - - RollingPhaseEstimator estimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.P); - - // Generate samples for 50 cycles worth of data - int totalSamples = (int)(sampleRate / nominalFreq * 50); - long samplePeriodNs = (long)(1e9 / sampleRate); - double omega = 2.0 * Math.PI * nominalFreq; - long epochNs = 0; - - double lastFrequency = 0; - double lastRocof = 0; - double lastVAMagnitude = 0; - bool gotEstimate = false; - int estimateCount = 0; - - for (int i = 0; i < totalSamples; i++) - { - double t = i / sampleRate; - - // Generate balanced three-phase signals (120° apart) - double ia = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double ib = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double ic = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - double va = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double vb = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double vc = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - - if (estimator.Step(ia, ib, ic, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - lastFrequency = estimate.Frequency; - lastRocof = estimate.dFdt; - lastVAMagnitude = estimate.Magnitudes[(int)PhaseChannel.VA]; - gotEstimate = true; - estimateCount++; - })) - { - // Step returned true, meaning an estimate was produced - } - - epochNs += samplePeriodNs; - } - - if (gotEstimate) - { - double expectedMag = amplitude / Math.Sqrt(2); - System.Console.WriteLine($" Filter order: {estimator.FilterOrder}"); - System.Console.WriteLine($" Total estimates: {estimateCount}"); - System.Console.WriteLine($" Measured Frequency: {lastFrequency:F4} Hz (expected: {nominalFreq} Hz)"); - System.Console.WriteLine($" ROCOF: {lastRocof:F4} Hz/s (expected: ~0)"); - System.Console.WriteLine($" VA Magnitude: {lastVAMagnitude:F4} (expected: ~{expectedMag:F4} RMS)"); - System.Console.WriteLine($" Frequency error: {Math.Abs(lastFrequency - nominalFreq):F6} Hz"); - } - System.Console.WriteLine(); - } - - private static void TestFrequencyDeviation() - { - System.Console.WriteLine("Test 2: Frequency Deviation Detection (61 Hz vs 60 Hz nominal)"); - System.Console.WriteLine("--------------------------------------------------------------"); - - const double sampleRate = 960.0; - const double outputRate = 60.0; - const double actualFreq = 61.0; - const double amplitude = 100.0; - - RollingPhaseEstimator estimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.P); - - int totalSamples = (int)(sampleRate / 60.0 * 30); // 30 nominal cycles for convergence - long samplePeriodNs = (long)(1e9 / sampleRate); - long epochNs = 0; - - double lastFrequency = 0; - bool gotEstimate = false; - - for (int i = 0; i < totalSamples; i++) - { - double t = i / sampleRate; - - double ia = amplitude * Math.Cos(2.0 * Math.PI * actualFreq * t); - double ib = amplitude * Math.Cos(2.0 * Math.PI * actualFreq * t - 2.0 * Math.PI / 3.0); - double ic = amplitude * Math.Cos(2.0 * Math.PI * actualFreq * t + 2.0 * Math.PI / 3.0); - double va = amplitude * Math.Cos(2.0 * Math.PI * actualFreq * t); - double vb = amplitude * Math.Cos(2.0 * Math.PI * actualFreq * t - 2.0 * Math.PI / 3.0); - double vc = amplitude * Math.Cos(2.0 * Math.PI * actualFreq * t + 2.0 * Math.PI / 3.0); - - estimator.Step(ia, ib, ic, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - lastFrequency = estimate.Frequency; - gotEstimate = true; - }); - - epochNs += samplePeriodNs; - } - - if (gotEstimate) - { - System.Console.WriteLine($" Measured Frequency: {lastFrequency:F4} Hz (actual: {actualFreq} Hz)"); - System.Console.WriteLine($" Frequency error: {Math.Abs(lastFrequency - actualFreq):F4} Hz"); - } - - System.Console.WriteLine(); - } - - private static void TestMagnitudeMeasurement() - { - System.Console.WriteLine("Test 3: Magnitude Measurement (various amplitudes)"); - System.Console.WriteLine("--------------------------------------------------"); - - const double sampleRate = 960.0; - const double outputRate = 60.0; - const double nominalFreq = 60.0; - - RollingPhaseEstimator estimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.P); - - // Different amplitudes for each channel - double[] peakAmplitudes = [100.0, 150.0, 200.0, 120.0, 180.0, 90.0]; - double[] expectedRms = new double[6]; - - for (int i = 0; i < 6; i++) - expectedRms[i] = peakAmplitudes[i] / Math.Sqrt(2); - - int totalSamples = (int)(sampleRate / nominalFreq * 20); // 20 cycles - long samplePeriodNs = (long)(1e9 / sampleRate); - - long epochNs = 0; - - double[] lastMagnitudes = new double[6]; - bool gotEstimate = false; - - for (int i = 0; i < totalSamples; i++) - { - double t = i / sampleRate; - - double ia = peakAmplitudes[0] * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double ib = peakAmplitudes[1] * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double ic = peakAmplitudes[2] * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - double va = peakAmplitudes[3] * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double vb = peakAmplitudes[4] * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double vc = peakAmplitudes[5] * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - - estimator.Step(ia, ib, ic, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - estimate.Magnitudes.CopyTo(lastMagnitudes); - gotEstimate = true; - }); - - epochNs += samplePeriodNs; - } - - if (gotEstimate) - { - string[] names = ["IA", "IB", "IC", "VA", "VB", "VC"]; - for (int i = 0; i < 6; i++) - { - double error = Math.Abs(lastMagnitudes[i] - expectedRms[i]); - double errorPct = error / expectedRms[i] * 100; - System.Console.WriteLine($" {names[i]}: Measured={lastMagnitudes[i]:F4}, Expected={expectedRms[i]:F4}, Error={errorPct:F2}%"); - } - } - System.Console.WriteLine(); - } - - private static void TestPhaseAngles() - { - System.Console.WriteLine("Test 4: Phase Angle Relationships"); - System.Console.WriteLine("----------------------------------"); - - const double sampleRate = 960.0; - const double outputRate = 60.0; - const double nominalFreq = 60.0; - const double amplitude = 100.0; - - RollingPhaseEstimator estimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.P); - - // Currents lead voltages by 30 degrees (capacitive load) - const double CurrentLeadRad = 30.0 * Math.PI / 180.0; - int totalSamples = (int)(sampleRate / nominalFreq * 20); - long samplePeriodNs = (long)(1e9 / sampleRate); - long epochNs = 0; - - double[] lastAngles = new double[6]; - bool gotEstimate = false; - - for (int i = 0; i < totalSamples; i++) - { - double t = i / sampleRate; - - // Currents with 30° lead - double ia = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + CurrentLeadRad); - double ib = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0 + CurrentLeadRad); - double ic = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0 + CurrentLeadRad); - - // Voltages (reference) - double va = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double vb = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double vc = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - - estimator.Step(ia, ib, ic, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - for (int j = 0; j < 6; j++) - lastAngles[j] = estimate.Angles[j].ToDegrees(); - gotEstimate = true; - }); - - epochNs += samplePeriodNs; - } - - if (gotEstimate) - { - string[] names = ["IA", "IB", "IC", "VA", "VB", "VC"]; - - System.Console.WriteLine(" Angles:"); - - for (int i = 0; i < 6; i++) - System.Console.WriteLine($" {names[i]}: {lastAngles[i]:F2}°"); - - System.Console.WriteLine($"\n IA-VA angle difference: {WrapAngleDeg(lastAngles[(int)PhaseChannel.IA] - lastAngles[(int)PhaseChannel.VA]):F2}° (expected: 30°)"); - } - - System.Console.WriteLine(); - } - - private static void TestPClassVsMClass() - { - System.Console.WriteLine("Test 5: P-class vs M-class Comparison"); - System.Console.WriteLine("--------------------------------------"); - - const double sampleRate = 960.0; - const double outputRate = 60.0; - const double nominalFreq = 60.0; - const double amplitude = 100.0; - - RollingPhaseEstimator pEstimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.P); - RollingPhaseEstimator mEstimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.M); - - System.Console.WriteLine($" P-class filter order: {pEstimator.FilterOrder}"); - System.Console.WriteLine($" M-class filter order: {mEstimator.FilterOrder}"); - - int totalSamples = (int)(sampleRate / nominalFreq * 50); - long samplePeriodNs = (long)(1e9 / sampleRate); - long epochNs = 0; - - double pFreq = 0, mFreq = 0; - double pMag = 0, mMag = 0; - - for (int i = 0; i < totalSamples; i++) - { - double t = i / sampleRate; - - double va = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double vb = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double vc = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - double ia = va, ib = vb, ic = vc; - - pEstimator.Step(ia, ib, ic, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - pFreq = estimate.Frequency; - pMag = estimate.Magnitudes[(int)PhaseChannel.VA]; - }); - - mEstimator.Step(ia, ib, ic, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - mFreq = estimate.Frequency; - mMag = estimate.Magnitudes[(int)PhaseChannel.VA]; - }); - - epochNs += samplePeriodNs; - } - - double expectedRms = amplitude / Math.Sqrt(2); - System.Console.WriteLine($" P-class: Freq={pFreq:F4} Hz, VA Mag={pMag:F4} (expected RMS: {expectedRms:F4})"); - System.Console.WriteLine($" M-class: Freq={mFreq:F4} Hz, VA Mag={mMag:F4} (expected RMS: {expectedRms:F4})"); - - System.Console.WriteLine(); - } - - private static void TestDecimationCount() - { - System.Console.WriteLine("Test 6: Decimation Count Verification"); - System.Console.WriteLine("--------------------------------------"); - - const double sampleRate = 960.0; - const double outputRate = 60.0; - const double nominalFreq = 60.0; - const double amplitude = 100.0; - const double testDurationSeconds = 1.0; // exactly 1 second - - RollingPhaseEstimator estimator = new(sampleRate, outputRate, LineFrequency.Hz60, FilterClass.P); - - int totalSamples = (int)(sampleRate * testDurationSeconds); - long samplePeriodNs = (long)(1e9 / sampleRate); - long epochNs = 0; - int estimateCount = 0; - - for (int i = 0; i < totalSamples; i++) - { - double t = i / sampleRate; - - double va = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t); - double vb = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t - 2.0 * Math.PI / 3.0); - double vc = amplitude * Math.Cos(2.0 * Math.PI * nominalFreq * t + 2.0 * Math.PI / 3.0); - - estimator.Step(va, vb, vc, va, vb, vc, epochNs, (in PhaseEstimate estimate) => - { - estimateCount++; - }); - - epochNs += samplePeriodNs; - } - - // Expected: total outputs = (totalSamples - WindowSamples) / decimationStep + some - // At 960 Hz with filter order ~28, about 960-29=931 valid samples, 931/16=58 ish - // Exact count depends on filter order - int samplesPerReport = (int)(sampleRate / outputRate); - System.Console.WriteLine($" Total samples: {totalSamples}"); - System.Console.WriteLine($" Samples per report: {samplesPerReport}"); - System.Console.WriteLine($" Filter startup samples: {estimator.WindowSamples}"); - System.Console.WriteLine($" Total estimates produced: {estimateCount}"); - System.Console.WriteLine($" Expected approx: {(totalSamples - estimator.WindowSamples) / samplesPerReport}"); - - System.Console.WriteLine(); - } - - private static double WrapAngleDeg(double degrees) - { - double result = (degrees + 180.0) % 360.0; - - if (result < 0) - result += 360.0; - - return result - 180.0; - } -} diff --git a/src/Gemstone.PhasorProtocols/SelCWS/SlidingDftPhaseEstimator.cs b/src/Gemstone.PhasorProtocols/SelCWS/SlidingDftPhaseEstimator.cs new file mode 100644 index 0000000000..07e85d9f3f --- /dev/null +++ b/src/Gemstone.PhasorProtocols/SelCWS/SlidingDftPhaseEstimator.cs @@ -0,0 +1,1195 @@ +//****************************************************************************************************** +// SlidingDftPhaseEstimator.cs - Gbtc +// +// Copyright © 2025, Grid Protection Alliance. All Rights Reserved. +// +// Licensed to the Grid Protection Alliance (GPA) under one or more contributor license agreements. See +// the NOTICE file distributed with this work for additional information regarding copyright ownership. +// The GPA licenses this file to you under the MIT License (MIT), the "License"; you may not use this +// file except in compliance with the License. You may obtain a copy of the License at: +// +// http://opensource.org/licenses/MIT +// +// Unless agreed to in writing, the subject software distributed under the License is distributed on an +// "AS-IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. Refer to the +// License for the specific language governing permissions and limitations. +// +// Code Modification History: +// ---------------------------------------------------------------------------------------------------- +// 11/04/2025 - Ritchie Carroll +// Generated original version of source code in collaboration with ChatGPT/Claude. +// +//****************************************************************************************************** +// ReSharper disable IdentifierTypo +// ReSharper disable CommentTypo +// ReSharper disable GrammarMistakeInComment +// ReSharper disable InconsistentNaming + +using System; +using System.Runtime.CompilerServices; +using Gemstone.Numeric.EE; +using Gemstone.Numeric.UnitExtensions; +using Gemstone.Units; + +namespace Gemstone.PhasorProtocols.SelCWS; + +/// +/// Represents a rolling six-phase estimator using a sliding DFT algorithm with optional smoothing. +/// +/// +/// +/// This class implements a real-time phasor measurement algorithm suitable for power system +/// applications. It uses a Sliding Discrete Fourier Transform (SDFT) approach to efficiently +/// compute phasors at each sample with O(1) complexity. +/// +/// +/// Frequency estimation is performed by tracking the rate of change of phase angle from the +/// reference channel (VA by default). The frequency and ROCOF outputs are smoothed using +/// exponential moving average filters. +/// +/// +/// The algorithm is designed for nominal 50Hz or 60Hz systems sampled at 3000Hz, but can be +/// configured for other sample rates. +/// +/// +public sealed class SlidingDftPhaseEstimator : IPhaseEstimator +{ + #region [ Members ] + + // Nested Types + private struct SamplePhaseEstimate + { + public double Frequency; + public double dFdt; + public Angle[] Angles; + public double[] Magnitudes; + } + + // Constants + private const int NumChannels = 6; + private const double TwoPI = 2.0D * Math.PI; + private const double Sqrt2 = 1.4142135623730951D; + private const long NanosecondsPerSecond = 1_000_000_000L; + + /// + /// Default value for reference channel. + /// + public const PhaseChannel DefaultReferenceChannel = PhaseChannel.VA; + + /// + /// Default value for number of target cycles. + /// + public const int DefaultTargetCycles = 2; + + /// + /// Default value for determining if interval averaging is enabled. + /// + public const bool DefaultEnableIntervalAveraging = true; + + /// + /// Default value for determining if EMA publishing is enabled. + /// + public const bool DefaultEnablePublishEMA = true; + + /// + /// Default value for EMA time constant τ (seconds) published angles. + /// + public const double DefaultPublishAnglesTauSeconds = 0.35D; + + /// + /// Default value for EMA time constant τ (seconds) published RMS magnitudes. + /// + public const double DefaultPublishMagnitudesTauSeconds = 0.50D; + + /// + /// Default value for EMA time constant τ (seconds) published frequency. + /// + public const double DefaultPublishFrequencyTauSeconds = 0.75D; + + /// + /// Default value for EMA time constant τ (seconds) published ROCOF. + /// + public const double DefaultPublishRocofTauSeconds = 1.50D; + + /// + /// Default value for EMA time constant τ (seconds) published for the internal per-sample frequency smoothing. + /// + public const double DefaultSampleFrequencyTauSeconds = 0.15D; + + /// + /// Default value for EMA time constant τ (seconds) published for the internal per-sample ROCOF smoothing. + /// + public const double DefaultSampleRocofTauSeconds = 0.30D; + + /// + /// Default value for number of recalculation cycles. + /// + public const int DefaultRecalculationCycles = 10; + + /// + /// Default maximum gap (in samples) filled by phase-continued synthesis before resynchronizing. + /// A negative value means "auto" (one full analysis window). + /// + public const int DefaultMaxGapFillSamples = -1; + + // Fields + private readonly double m_samplePeriodSeconds; + private readonly int m_recalculationInterval; + private readonly int m_initialSamplesUntilRecalc; // countdown to the first recalc after the window fills + private int m_samplesUntilRecalc; // samples remaining until the next full DFT recalc + + // Output decimation / publish gating + private readonly long m_publishPeriodNs; // 0 means "publish every sample" + private long m_nextPublishEpochNs; // first publish scheduled when window becomes ready + private bool m_publishScheduleInitialized; // guards first-time publish-schedule initialization + + // If enabled, we boxcar-average (anti-alias) across each publish interval. + private readonly bool m_enableIntervalAveraging; + private double m_lastFrequency; + private double m_lastRocof; + private bool m_hasLastSampleEstimate; + + // Optional EMA applied to the *published* stream (after interval averaging). + private readonly bool m_enablePublishEMA; + private readonly double m_publishAnglesAlpha; + private readonly double m_publishMagnitudesAlpha; + private readonly double m_publishFrequencyAlpha; + private readonly double m_publishRocofAlpha; + + // Precomputed (1 - α) complements for the published-stream EMA + private readonly double m_oneMinusPublishAnglesAlpha; + private readonly double m_oneMinusPublishMagnitudesAlpha; + private readonly double m_oneMinusPublishFrequencyAlpha; + private readonly double m_oneMinusPublishRocofAlpha; + + // Circular sample buffers for each channel + private readonly double[][] m_sampleBuffers; + private int m_bufferWriteIndex; + + // Reusable per-sample working buffers (avoids allocating Angle[] / double[] every sample) + private readonly Angle[] m_workingAngles = new Angle[NumChannels]; + private readonly double[] m_workingMagnitudes = new double[NumChannels]; + + // Dedicated storage for "sample-and-hold" mode (only used when interval averaging is disabled) + private readonly Angle[] m_lastAngles = new Angle[NumChannels]; + private readonly double[] m_lastMagnitudes = new double[NumChannels]; + + // Publish output buffers (single set for all modes suffices) + private readonly Angle[] m_publishAngles = new Angle[NumChannels]; + private readonly double[] m_publishMagnitudes = new double[NumChannels]; + + // DFT twiddle factor for sliding update at bin k: e^(j*2π*k/N), where k = targetCycles + private readonly double m_twiddleReal; // cos(...) + private readonly double m_twiddleImag; // sin(...) + + // Accumulated DFT phasors (real and imaginary parts) for each channel + private readonly double[] m_phasorReal; + private readonly double[] m_phasorImag; + + // Precomputed cosine/sine tables for full DFT recalculation + private readonly double[] m_cosTable; + private readonly double[] m_sinTable; + + // Precomputed RMS magnitude scale factor: sqrt(2) / N + private readonly double m_magnitudeScale; + + // Precomputed frequency-tracking constants (derived from nominal frequency) + private readonly double m_minFrequency; // NominalFrequencyHz * 0.9 (clamp lower bound) + private readonly double m_maxFrequency; // NominalFrequencyHz * 1.1 (clamp upper bound) + + // Previous phase angle for frequency calculation (from reference channel) + private double m_prevPhaseAngle; + private bool m_hasPrevPhase; + + // Frequency smoothing (exponential moving average) on per-sample instantaneous frequency + private readonly double m_frequencyAlpha; + private readonly double m_oneMinusFrequencyAlpha; // precomputed 1 - α + private double m_smoothedFrequency; + private bool m_frequencyInitialized; + + // ROCOF smoothing on per-sample instantaneous ROCOF + private readonly double m_rocofAlpha; + private readonly double m_oneMinusRocofAlpha; // precomputed 1 - α + private double m_prevSmoothedFrequency; + private double m_smoothedRocof; + private bool m_rocofInitialized; + + // Timing + private long m_prevEpochNs; + + // Gap detection / fill (missing-data handling) + private readonly double m_samplePeriodNs; // expected nanoseconds between consecutive input samples + private readonly int m_maxGapFillSamples; // max gap (samples) coasted before a resynchronization + private long m_prevInputEpochNs; // epoch of the previous Step call (tracked every sample) + + // Interval accumulators (for down-sampling / anti-aliasing) + private int m_intervalCount; + private readonly double[] m_intervalAngleCosSum = new double[NumChannels]; + private readonly double[] m_intervalAngleSinSum = new double[NumChannels]; + private readonly double[] m_intervalMagnitudeSum = new double[NumChannels]; + private double m_intervalFreqSum; + private double m_intervalRocofSum; + + // Published EMA state (wrap-safe for angles) + private bool m_publishEMAInitialized; + private readonly double[] m_pubAngleCos = new double[NumChannels]; + private readonly double[] m_pubAngleSin = new double[NumChannels]; + private readonly double[] m_pubMagnitude = new double[NumChannels]; + private double m_pubFrequency; + private double m_pubRocof; + + #endregion + + #region [ Constructors ] + + /// + /// Initializes a new instance of the class with independent + /// input sample rate and output publish rate, plus smoothing expressed as time constants (τ). + /// + /// + /// Input sample rate in Hz (PoW samples per second). Must be > 0. + /// This is the rate at which is called with new samples. + /// + /// + /// Output publish rate in Hz (estimates per second). Must be > 0 and <= . + /// Common synchrophasor-friendly values: 30, 60 or 120Hz. Defaults chosen for "calm" 60Hz publish rates; + /// τ-to-α mapping adapts automatically to other publish rates. + ///
+ /// Behavioral note: + /// may return false even after the estimator is ready, if the current sample does not + /// land on a publish boundary (i.e., it is not yet time to produce the next output). + /// + /// + /// Nominal line frequency (typically 50Hz or 60Hz). This determines the nominal-cycle length used + /// for the DFT window and the expected reference-phase progression. + /// + /// + /// Reference channel for frequency tracking. + /// Default: . + /// + /// + /// Number of nominal cycles contained in the sliding DFT analysis window (default: 2). + /// Larger values generally reduce noise/jitter (more averaging) but increase latency and reduce step response. + /// + /// + /// Enables interval averaging (boxcar averaging) across each publish interval when down-sampling + /// ( < ). + ///
+ /// Why this exists: + /// Down-sampling without an anti-alias / low-pass step will preserve high-rate jitter and can alias higher-frequency + /// content into the published stream. Interval averaging acts as a simple, cheap low-pass filter that reduces + /// jitter and improves published stability. + ///
+ /// Consequences:
+ /// - Improves calmness and reduces aliasing artifacts.
+ /// - Adds a small amount of additional latency (effectively ~½ publish interval group delay).
+ /// Recommended: true (default). + /// + /// + /// Enables an additional exponential moving average (EMA) applied to the published stream (after interval averaging). + ///
+ /// Why this exists:
+ /// Interval averaging removes high-rate noise; publish-EMA further reduces remaining jitter and produces a "calm" + /// display or control signal. This is usually the most intuitive "knob" for operators/consumers because it acts on + /// the actual output cadence. + ///
+ /// Consequences:
+ /// - Larger time constants (τ) produce smoother outputs but slower response to real changes.
+ /// - Smaller τ tracks faster but appears noisier.
+ /// Default: true. + /// + /// + /// EMA time constant τ (seconds) for published phase angles. Default: 0.35s. + ///
+ /// Important:
+ /// Angles are circular quantities; this implementation performs wrap-safe smoothing by operating on unit vectors + /// (cos/sin) rather than naïvely averaging radians. This avoids discontinuities at ±π. + /// + /// + /// EMA time constant τ (seconds) for published RMS magnitudes. Default: 0.50s. + /// + /// + /// EMA time constant τ (seconds) for published frequency. Default: 0.75s. + /// + /// + /// EMA time constant τ (seconds) for published ROCOF (dF/dt). Default: 1.50s. + ///
+ /// Note:
+ /// ROCOF is effectively a derivative signal and is typically much noisier than frequency; it generally benefits from + /// heavier smoothing (larger τ) than frequency. + /// + /// + /// EMA time constant τ (seconds) for the internal per-sample frequency smoothing that occurs inside the estimator + /// before any down-sampling/publish filtering. Default: 0.15s. + ///
+ /// Guidance:
+ /// When interval averaging + publish EMA are enabled, this can be relatively light. If you disable publish smoothing, + /// you may want to increase this τ. + /// + /// + /// EMA time constant τ (seconds) for the internal per-sample ROCOF smoothing (computed from the internally smoothed + /// frequency). Default: 0.30s. + /// + /// + /// Number of nominal cycles between full DFT recalculations for numerical stability (default: 10). + /// Sliding DFT updates are O(1) per sample but can accumulate numerical drift; periodic full recomputation + /// re-anchors the phasor sums. + /// + /// + /// Maximum gap (in input samples) that will be filled by phase-continued synthesis (coasting the last + /// phasors at the tracked frequency) when the input timestamp cadence indicates dropped samples. Gaps + /// larger than this trigger a resynchronization (the analysis window is dropped and refilled). + /// A negative value (default) means "auto" = one full analysis window; 0 disables coasting + /// (any gap resynchronizes). Must not exceed twice . + /// + /// + /// Thrown if any rate is non-positive, output rate exceeds input rate, targetCycles < 1, or any τ is negative. + /// + /// + /// + /// Time constant (τ) vs "alpha" (α):
+ /// Internally, EMAs use α constrained from 0 to 1. This API exposes τ because it is easier to reason about. + /// For update period Δt (seconds), α is derived as: + /// + /// α = 1 - exp(-Δt / τ) + /// + /// Interpretation: after a step change, an EMA will move about 63% toward the new value after ~τ seconds. + /// + /// + /// Two-stage smoothing (when outputRate < inputRate): + /// + /// + /// Interval averaging (boxcar) across the publish interval (anti-alias / jitter reduction). + /// + /// + /// Publish EMA applied to the down-sampled stream (additional "calmness"). + /// + /// + /// This combination yields stable outputs at 60Hz without producing thousands of nearly-identical jittery estimates per second. + /// + /// + /// Knob-tweaking guidance: + /// + /// + /// Increase to reduce noise/jitter, at the cost of more latency. + /// + /// + /// Increase publish τ values to make outputs calmer, at the cost of slower response. + /// + /// + /// If you disable interval averaging, consider increasing publish τ values to compensate. + /// + /// + /// Keep ROCOF τ larger than frequency τ; ROCOF is inherently noisier. + /// + /// + /// + /// + public SlidingDftPhaseEstimator( + double sampleRateHz, + double outputRateHz, + LineFrequency nominalFrequency, + PhaseChannel referenceChannel = DefaultReferenceChannel, + int targetCycles = DefaultTargetCycles, + bool enableIntervalAveraging = DefaultEnableIntervalAveraging, + bool enablePublishEMA = DefaultEnablePublishEMA, + double publishAnglesTauSeconds = DefaultPublishAnglesTauSeconds, + double publishMagnitudesTauSeconds = DefaultPublishMagnitudesTauSeconds, + double publishFrequencyTauSeconds = DefaultPublishFrequencyTauSeconds, + double publishRocofTauSeconds = DefaultPublishRocofTauSeconds, + double sampleFrequencyTauSeconds = DefaultSampleFrequencyTauSeconds, + double sampleRocofTauSeconds = DefaultSampleRocofTauSeconds, + int recalculationCycles = DefaultRecalculationCycles, + int maxGapFillSamples = DefaultMaxGapFillSamples) + { + // Validate rates / window + if (sampleRateHz <= 0) + throw new ArgumentOutOfRangeException(nameof(sampleRateHz), "Input sample rate must be positive."); + + if (outputRateHz <= 0) + throw new ArgumentOutOfRangeException(nameof(outputRateHz), "Output rate must be positive."); + + if (outputRateHz > sampleRateHz) + throw new ArgumentOutOfRangeException(nameof(outputRateHz), "Output rate must be <= input sample rate."); + + if (targetCycles < 1) + throw new ArgumentOutOfRangeException(nameof(targetCycles), "Target cycles must be at least 1."); + + if (recalculationCycles < 1) + throw new ArgumentOutOfRangeException(nameof(recalculationCycles), "Recalculation cycles must be at least 1."); + + // Validate taus (τ >= 0). τ == 0 means "no smoothing" (alpha = 1). + if (publishAnglesTauSeconds < 0.0D) + throw new ArgumentOutOfRangeException(nameof(publishAnglesTauSeconds), "Tau must be >= 0."); + + if (publishMagnitudesTauSeconds < 0.0D) + throw new ArgumentOutOfRangeException(nameof(publishMagnitudesTauSeconds), "Tau must be >= 0."); + + if (publishFrequencyTauSeconds < 0.0D) + throw new ArgumentOutOfRangeException(nameof(publishFrequencyTauSeconds), "Tau must be >= 0."); + + if (publishRocofTauSeconds < 0.0D) + throw new ArgumentOutOfRangeException(nameof(publishRocofTauSeconds), "Tau must be >= 0."); + + if (sampleFrequencyTauSeconds < 0.0D) + throw new ArgumentOutOfRangeException(nameof(sampleFrequencyTauSeconds), "Tau must be >= 0."); + + if (sampleRocofTauSeconds < 0.0D) + throw new ArgumentOutOfRangeException(nameof(sampleRocofTauSeconds), "Tau must be >= 0."); + + // Store configuration + SampleRateHz = sampleRateHz; + OutputRateHz = outputRateHz; + NominalFrequencyHz = (double)nominalFrequency; + ReferenceChannel = referenceChannel; + + // Precompute nominal-derived frequency-tracking constants + m_minFrequency = NominalFrequencyHz * 0.9D; + m_maxFrequency = NominalFrequencyHz * 1.1D; + + m_samplePeriodSeconds = 1.0D / sampleRateHz; + + m_publishPeriodNs = outputRateHz >= sampleRateHz ? + 0L : (long)Math.Round(NanosecondsPerSecond / outputRateHz); + + m_enableIntervalAveraging = enableIntervalAveraging && m_publishPeriodNs != 0L; + m_enablePublishEMA = enablePublishEMA; + + // Convert τ -> α using the appropriate Δt for each EMA + double publishDt = 1.0D / outputRateHz; // published stream cadence + double sampleDt = 1.0D / sampleRateHz; // per-sample cadence + + m_publishAnglesAlpha = AlphaFromTau(publishDt, publishAnglesTauSeconds); + m_publishMagnitudesAlpha = AlphaFromTau(publishDt, publishMagnitudesTauSeconds); + m_publishFrequencyAlpha = AlphaFromTau(publishDt, publishFrequencyTauSeconds); + m_publishRocofAlpha = AlphaFromTau(publishDt, publishRocofTauSeconds); + + m_frequencyAlpha = AlphaFromTau(sampleDt, sampleFrequencyTauSeconds); + m_rocofAlpha = AlphaFromTau(sampleDt, sampleRocofTauSeconds); + + // Precompute (1 - α) complements used in the EMA hot paths + m_oneMinusPublishAnglesAlpha = 1.0D - m_publishAnglesAlpha; + m_oneMinusPublishMagnitudesAlpha = 1.0D - m_publishMagnitudesAlpha; + m_oneMinusPublishFrequencyAlpha = 1.0D - m_publishFrequencyAlpha; + m_oneMinusPublishRocofAlpha = 1.0D - m_publishRocofAlpha; + m_oneMinusFrequencyAlpha = 1.0D - m_frequencyAlpha; + m_oneMinusRocofAlpha = 1.0D - m_rocofAlpha; + + int samplesPerNominalCycle = (int)Math.Round(sampleRateHz / NominalFrequencyHz); + WindowSamples = samplesPerNominalCycle * targetCycles; + m_recalculationInterval = samplesPerNominalCycle * recalculationCycles; + + // Align the first recalc countdown so recalcs land on the same absolute cadence a modulo would + m_initialSamplesUntilRecalc = m_recalculationInterval - WindowSamples % m_recalculationInterval; + + // Gap-fill bound: validate for reasonability against the window, then resolve "auto" (negative) to one window + if (maxGapFillSamples > 2 * WindowSamples) + throw new ArgumentOutOfRangeException(nameof(maxGapFillSamples), "Max gap fill samples should not exceed twice the window size."); + + m_maxGapFillSamples = maxGapFillSamples < 0 ? WindowSamples : maxGapFillSamples; + m_samplePeriodNs = NanosecondsPerSecond / sampleRateHz; + + // Precompute RMS magnitude scale factor from DFT magnitude. + // For a pure sinusoid: |X| = N * A / 2 (A = peak amplitude); RMS = A / sqrt(2), so RMS = sqrt(2) * |X| / N. + m_magnitudeScale = Sqrt2 / WindowSamples; + + // Initialize sample buffers + m_sampleBuffers = new double[NumChannels][]; + + for (int ch = 0; ch < NumChannels; ch++) + m_sampleBuffers[ch] = new double[WindowSamples]; + + // DFT twiddle factor for sliding update at bin k: e^(j*2π*k/N), where k = targetCycles + double twiddleAngle = TwoPI * targetCycles / WindowSamples; + m_twiddleReal = Math.Cos(twiddleAngle); + m_twiddleImag = Math.Sin(twiddleAngle); + + // Precompute cos/sin tables for full DFT calculation at bin k = targetCycles + m_cosTable = new double[WindowSamples]; + m_sinTable = new double[WindowSamples]; + + for (int i = 0; i < WindowSamples; i++) + { + double angle = TwoPI * targetCycles * i / WindowSamples; + m_cosTable[i] = Math.Cos(angle); + m_sinTable[i] = Math.Sin(angle); + } + + // Initialize phasor accumulators + m_phasorReal = new double[NumChannels]; + m_phasorImag = new double[NumChannels]; + + // Initialize frequency tracking (internal) + m_smoothedFrequency = NominalFrequencyHz; + m_prevSmoothedFrequency = NominalFrequencyHz; + } + + #endregion + + #region [ Properties ] + + /// + /// Gets the configured sample rate in Hz. + /// + public double SampleRateHz { get; } + + /// + /// Gets the configured output publish rate in Hz. + /// + public double OutputRateHz { get; } + + /// + /// Gets the nominal frequency in Hz. + /// + public double NominalFrequencyHz { get; } + + /// + /// Gets the reference channel index used for frequency tracking (typically VA). + /// + public PhaseChannel ReferenceChannel { get; } + + /// + /// Gets the number of samples in the analysis window. + /// + public int WindowSamples { get; } + + /// + /// Gets the total number of samples processed. + /// + public long TotalSamplesProcessed { get; private set; } + + /// + /// Gets whether the estimator has filled its window and is producing valid estimates. + /// + public bool IsReady => TotalSamplesProcessed >= WindowSamples; + + #endregion + + #region [ Methods ] + + /// + /// Push one interleaved sample-group (VA, VB, VC, IA, IB, IC) with its epoch nanoseconds. + /// + /// Current sample for phase A voltage. + /// Current sample for phase B voltage. + /// Current sample for phase C voltage. + /// Current sample for phase A current. + /// Current sample for phase B current. + /// Current sample for phase C current. + /// Timestamp in nanoseconds since epoch. + /// Handler for phase estimate result. + /// + /// true when an estimate is available (window has filled) and it is time to + /// publish at target ; otherwise, false. + /// + /// + /// The data referenced in the span-based properties of the + /// parameter provided to the delegate is owned by the + /// instance, it is only valid during the scope of the + /// delegate call. If you need to retain the data, make a copy. + /// + public bool Step( + double va, + double vb, + double vc, + double ia, + double ib, + double ic, + long epochNanoseconds, + PhaseEstimateHandler phaseEstimateHandler) + { + ArgumentNullException.ThrowIfNull(phaseEstimateHandler); + + // Missing-data handling: infer dropped samples from the input timestamp cadence, then either + // coast across a small gap (phase-continued synthesis) or resynchronize across a large one. + if (m_prevInputEpochNs > 0L) + { + long deltaNs = epochNanoseconds - m_prevInputEpochNs; + + if (deltaNs < 0L) + { + // Backward timestamp: the input timeline moved earlier than the last sample. This happens when + // the source restarts/loops or a new configuration frame re-bases timing. Treat it as a stream + // discontinuity and fully reset so the window, frequency state, AND publish schedule adopt the + // new timeline. (A plain Resync would keep the stale publish schedule, stalling the down-sampled + // output until the new timeline caught up; and simply returning false here -- the previous + // behavior -- permanently wedged reporting once any non-increasing timestamp arrived.) + Reset(); + } + else if (deltaNs == 0L) + { + // Duplicate timestamp: skip so the sample is not fed into the SDFT twice. The timeline reference + // is unchanged, so the next forward-advancing sample resumes normally. + return false; + } + else + { + // Samples missing between the previous input and this one (rounding absorbs minor timing jitter). + double missing = Math.Round(deltaNs / m_samplePeriodNs) - 1.0D; + + if (missing > 0.0D) + { + if (IsReady && missing <= m_maxGapFillSamples) + { + CoastFillGap((int)missing); + + // Re-anchor frequency tracking to the post-coast state so the upcoming real sample yields a + // clean single-sample phase delta (a multi-sample delta would wrap and corrupt the estimate). + int referenceChannel = (int)ReferenceChannel; + m_prevPhaseAngle = Math.Atan2(m_phasorImag[referenceChannel], m_phasorReal[referenceChannel]); + m_prevEpochNs = epochNanoseconds - (long)m_samplePeriodNs; + } + else + { + Resync(); + } + } + } + } + + // Apply the real sample to the buffers and SDFT + Span samples = [ va, vb, vc, ia, ib, ic ]; + AdvanceOneSample(samples); + + m_prevInputEpochNs = epochNanoseconds; + + // Check if window has filled + if (TotalSamplesProcessed < WindowSamples) + return false; + + // Compute per-sample estimate (this is the "high-rate" stream) + SamplePhaseEstimate sampleEstimate = ComputeEstimate(epochNanoseconds); + + if (m_publishPeriodNs == 0L) + { + // Full-rate publishing: publish cadence equals sample cadence (so publish EMA is well-defined here) + sampleEstimate.Angles.AsSpan().CopyTo(m_publishAngles); + sampleEstimate.Magnitudes.AsSpan().CopyTo(m_publishMagnitudes); + + double freq = sampleEstimate.Frequency; + double rocof = sampleEstimate.dFdt; + + if (m_enablePublishEMA) + ApplyPublishEMA(m_publishAngles, m_publishMagnitudes, ref freq, ref rocof); + + phaseEstimateHandler(new PhaseEstimate + { + Frequency = freq, + dFdt = rocof, + Angles = m_publishAngles, + Magnitudes = m_publishMagnitudes + }); + + return true; + } + + if (!m_enableIntervalAveraging) + StoreLastSample(sampleEstimate); + + // Initialize publish schedule the first time we become ready + if (!m_publishScheduleInitialized) + { + m_nextPublishEpochNs = AlignToNextBoundary(epochNanoseconds, m_publishPeriodNs); + m_publishScheduleInitialized = true; + } + + // Accumulate all per-sample estimates within a publish interval. + // This acts as an anti-alias low-pass when outputRate < sampleRate. + if (m_enableIntervalAveraging) + AccumulateInterval(sampleEstimate); + + // Not time to publish yet + if (epochNanoseconds < m_nextPublishEpochNs) + return false; + + // Time to publish: produce a down-sampled estimate + PhaseEstimate estimate = ProduceIntervalEstimate(); + + // Advance next publish time + if (m_publishPeriodNs > 0L) + { + // Handle possible gaps safely + long behind = epochNanoseconds - m_nextPublishEpochNs; + long steps = behind >= 0L ? behind / m_publishPeriodNs + 1L : 1L; + m_nextPublishEpochNs += steps * m_publishPeriodNs; + } + + phaseEstimateHandler(estimate); + return true; + } + + // Applies one sample-group to the circular buffers and the sliding DFT (no estimate / no publish). + // Shared by real input samples and synthesized gap-fill samples so the window stays contiguous. + private void AdvanceOneSample(ReadOnlySpan samples) + { + bool isFillingUp = TotalSamplesProcessed < WindowSamples; + int oldestIndex = m_bufferWriteIndex; + + for (int ch = 0; ch < NumChannels; ch++) + { + double oldSample = m_sampleBuffers[ch][oldestIndex]; + double newSample = samples[ch]; + + m_sampleBuffers[ch][oldestIndex] = newSample; + + if (isFillingUp) + { + // During fill-up, add contribution of new sample to DFT at correct phase + int dftIndex = (int)(TotalSamplesProcessed % WindowSamples); + m_phasorReal[ch] += newSample * m_cosTable[dftIndex]; + m_phasorImag[ch] -= newSample * m_sinTable[dftIndex]; + } + else + { + // Sliding DFT update: X_new = (X_old - x_oldest + x_new) * e^(j*2π/N) + double unrotatedReal = m_phasorReal[ch] + (newSample - oldSample); + double unrotatedImag = m_phasorImag[ch]; + + m_phasorReal[ch] = unrotatedReal * m_twiddleReal - unrotatedImag * m_twiddleImag; + m_phasorImag[ch] = unrotatedReal * m_twiddleImag + unrotatedImag * m_twiddleReal; + } + } + + // Advance buffer write index (compare/reset avoids a per-sample modulo) + if (++m_bufferWriteIndex >= WindowSamples) + m_bufferWriteIndex = 0; + + TotalSamplesProcessed++; + + // Periodic full recalculation for numerical stability: immediately after fill-up, then on a fixed + // sample countdown aligned to m_recalculationInterval (avoids a per-sample modulo). + bool justFilled = TotalSamplesProcessed == WindowSamples; + + if (justFilled) + { + m_samplesUntilRecalc = m_initialSamplesUntilRecalc; + RecalculateFullDft(); + } + else if (!isFillingUp && --m_samplesUntilRecalc == 0) + { + m_samplesUntilRecalc = m_recalculationInterval; + RecalculateFullDft(); + } + } + + // Fills a detected gap of "missing" samples by synthesizing a phase-continued sinusoid for each + // channel from the current phasor state, feeding them through the SDFT so the window stays + // temporally contiguous. Only valid when IsReady (a settled phasor exists to extrapolate from). + private void CoastFillGap(int missing) + { + // Per-sample phase advance at the tracked frequency: ω = 2π·f/fs. + double omega = TwoPI * m_smoothedFrequency * m_samplePeriodSeconds; + + // Reconstruct each channel's raw-waveform argument at the last real sample from its phasor. + // For a tone at bin k, the phasor angle ψ relates to the newest sample's cosine argument by + // rawArg = ψ + ω·(N-1), and peak amplitude A = 2·|X|/N. + double windowOffset = omega * (WindowSamples - 1); + + Span amplitude = stackalloc double[NumChannels]; + Span rawArg0 = stackalloc double[NumChannels]; + + for (int ch = 0; ch < NumChannels; ch++) + { + double re = m_phasorReal[ch]; + double im = m_phasorImag[ch]; + + amplitude[ch] = 2.0D * Math.Sqrt(re * re + im * im) / WindowSamples; + rawArg0[ch] = Math.Atan2(im, re) + windowOffset; + } + + Span synthesized = stackalloc double[NumChannels]; + + for (int k = 1; k <= missing; k++) + { + for (int ch = 0; ch < NumChannels; ch++) + synthesized[ch] = amplitude[ch] * Math.Cos(rawArg0[ch] + omega * k); + + AdvanceOneSample(synthesized); + } + } + + // Resynchronizes after a gap too large to coast: drops the in-flight SDFT/window and frequency + // state so the window refills from fresh contiguous samples. Published EMA state and the publish + // schedule are intentionally preserved so consumers see continuity while the window refills. + // NOTE: to instead snap outputs to the resynchronized values, also call ResetPublishEMA() here. + private void Resync() + { + for (int ch = 0; ch < NumChannels; ch++) + Array.Clear(m_sampleBuffers[ch], 0, WindowSamples); + + m_bufferWriteIndex = 0; + TotalSamplesProcessed = 0L; + + Array.Clear(m_phasorReal, 0, NumChannels); + Array.Clear(m_phasorImag, 0, NumChannels); + + m_prevPhaseAngle = 0.0D; + m_hasPrevPhase = false; + m_hasLastSampleEstimate = false; + m_smoothedFrequency = NominalFrequencyHz; + m_prevSmoothedFrequency = NominalFrequencyHz; + m_smoothedRocof = 0.0D; + m_frequencyInitialized = false; + m_rocofInitialized = false; + m_prevEpochNs = 0L; + + ResetInterval(); + } + + /// + /// Resets the estimator to its initial state. + /// + public void Reset() + { + // Clear sample buffers + for (int ch = 0; ch < NumChannels; ch++) + Array.Clear(m_sampleBuffers[ch], 0, WindowSamples); + + // Reset indices and counters + m_bufferWriteIndex = 0; + TotalSamplesProcessed = 0L; + + // Reset phasor accumulators + Array.Clear(m_phasorReal, 0, NumChannels); + Array.Clear(m_phasorImag, 0, NumChannels); + + // Reset frequency tracking state + m_prevPhaseAngle = 0.0D; + m_hasPrevPhase = false; + m_hasLastSampleEstimate = false; + m_smoothedFrequency = NominalFrequencyHz; + m_prevSmoothedFrequency = NominalFrequencyHz; + m_smoothedRocof = 0.0D; + m_frequencyInitialized = false; + m_rocofInitialized = false; + m_prevEpochNs = 0L; + m_prevInputEpochNs = 0L; + m_nextPublishEpochNs = 0L; + m_publishScheduleInitialized = false; + + ResetInterval(); + ResetPublishEMA(); + } + + private void ResetInterval() + { + m_intervalCount = 0; + + Array.Clear(m_intervalAngleCosSum, 0, NumChannels); + Array.Clear(m_intervalAngleSinSum, 0, NumChannels); + Array.Clear(m_intervalMagnitudeSum, 0, NumChannels); + + m_intervalFreqSum = 0.0D; + m_intervalRocofSum = 0.0D; + } + + private void ResetPublishEMA() + { + m_publishEMAInitialized = false; + + Array.Clear(m_pubAngleCos, 0, NumChannels); + Array.Clear(m_pubAngleSin, 0, NumChannels); + Array.Clear(m_pubMagnitude, 0, NumChannels); + + m_pubFrequency = 0.0D; + m_pubRocof = 0.0D; + } + + // Performs a full DFT recalculation from the sample buffers for numerical stability. + private void RecalculateFullDft() + { + // The buffer write index points to the oldest sample (next to be overwritten) + // So we read starting from that index + int startIndex = m_bufferWriteIndex; + + for (int ch = 0; ch < NumChannels; ch++) + { + double sumReal = 0.0D; + double sumImag = 0.0D; + + for (int i = 0; i < WindowSamples; i++) + { + int sampleIndex = (startIndex + i) % WindowSamples; + double sample = m_sampleBuffers[ch][sampleIndex]; + + // DFT at bin k = m_targetCycles: X_k = Σ x[n] * e^(-j*2π*k*n/N) + // Using pre-computed tables which already incorporate the bin number + sumReal += sample * m_cosTable[i]; + sumImag -= sample * m_sinTable[i]; + } + + m_phasorReal[ch] = sumReal; + m_phasorImag[ch] = sumImag; + } + } + + private void AccumulateInterval(in SamplePhaseEstimate estimate) + { + m_intervalCount++; + + // Angles: accumulate unit vectors to avoid wrap issues + for (int ch = 0; ch < NumChannels; ch++) + { + double angle = estimate.Angles[ch]; + m_intervalAngleCosSum[ch] += Math.Cos(angle); + m_intervalAngleSinSum[ch] += Math.Sin(angle); + m_intervalMagnitudeSum[ch] += estimate.Magnitudes[ch]; + } + + m_intervalFreqSum += estimate.Frequency; + m_intervalRocofSum += estimate.dFdt; + } + + // Do not call ProduceIntervalEstimate twice without another interval fill, + // otherwise you will be smoothing already-smoothed values + private PhaseEstimate ProduceIntervalEstimate() + { + Angle[] angles; + double[] magnitudes; + double freq; + double rocof; + + // If interval averaging is disabled, publish the most recent per-sample estimate ("sample-and-hold"). + // This avoids pretending to "average" while the knob says we shouldn’t. + if (!m_enableIntervalAveraging) + { + if (!m_hasLastSampleEstimate) + throw new InvalidOperationException("No sample estimate available"); // Unexpected + + // Copy scalars to locals to not mutate the stored last sample + freq = m_lastFrequency; + rocof = m_lastRocof; + + // Copy held sample into publish buffers so ApplyPublishEMA can safely write back. + m_lastAngles.AsSpan().CopyTo(m_publishAngles); + m_lastMagnitudes.AsSpan().CopyTo(m_publishMagnitudes); + + angles = m_publishAngles; + magnitudes = m_publishMagnitudes; + + if (m_enablePublishEMA) + ApplyPublishEMA(angles, magnitudes, ref freq, ref rocof); + + return new PhaseEstimate + { + Frequency = freq, + dFdt = rocof, + Angles = angles, + Magnitudes = magnitudes + }; + } + + // Interval-averaged publish path (anti-aliasing low-pass) + int intervalCount = m_intervalCount > 0 ? m_intervalCount : 1; + + angles = m_publishAngles; + magnitudes = m_publishMagnitudes; + + for (int ch = 0; ch < NumChannels; ch++) + { + double meanAngle = Math.Atan2(m_intervalAngleSinSum[ch], m_intervalAngleCosSum[ch]); + angles[ch] = NormalizeAngle(meanAngle); + magnitudes[ch] = m_intervalMagnitudeSum[ch] / intervalCount; + } + + freq = m_intervalFreqSum / intervalCount; + rocof = m_intervalRocofSum / intervalCount; + + ResetInterval(); + + if (m_enablePublishEMA) + ApplyPublishEMA(angles, magnitudes, ref freq, ref rocof); + + return new PhaseEstimate + { + Frequency = freq, + dFdt = rocof, + Angles = angles, + Magnitudes = magnitudes + }; + } + + private void ApplyPublishEMA(Angle[] angles, double[] magnitudes, ref double freq, ref double rocof) + { + if (m_publishEMAInitialized) + { + // Angles: EMA on unit vectors (wrap-safe) + for (int ch = 0; ch < NumChannels; ch++) + { + double angle = angles[ch]; + double cos = Math.Cos(angle); + double sin = Math.Sin(angle); + + m_pubAngleCos[ch] = m_publishAnglesAlpha * cos + m_oneMinusPublishAnglesAlpha * m_pubAngleCos[ch]; + m_pubAngleSin[ch] = m_publishAnglesAlpha * sin + m_oneMinusPublishAnglesAlpha * m_pubAngleSin[ch]; + + m_pubMagnitude[ch] = m_publishMagnitudesAlpha * magnitudes[ch] + m_oneMinusPublishMagnitudesAlpha * m_pubMagnitude[ch]; + } + + m_pubFrequency = m_publishFrequencyAlpha * freq + m_oneMinusPublishFrequencyAlpha * m_pubFrequency; + m_pubRocof = m_publishRocofAlpha * rocof + m_oneMinusPublishRocofAlpha * m_pubRocof; + } + else + { + for (int ch = 0; ch < NumChannels; ch++) + { + double angle = angles[ch]; + m_pubAngleCos[ch] = Math.Cos(angle); + m_pubAngleSin[ch] = Math.Sin(angle); + m_pubMagnitude[ch] = magnitudes[ch]; + } + + m_pubFrequency = freq; + m_pubRocof = rocof; + m_publishEMAInitialized = true; + } + + // Write back smoothed values + for (int ch = 0; ch < NumChannels; ch++) + { + double angle = Math.Atan2(m_pubAngleSin[ch], m_pubAngleCos[ch]); + angles[ch] = NormalizeAngle(angle); + magnitudes[ch] = m_pubMagnitude[ch]; + } + + freq = m_pubFrequency; + rocof = m_pubRocof; + } + + // Computes the per-sample phase estimate from current phasor values. + private SamplePhaseEstimate ComputeEstimate(long epochNanoseconds) + { + Angle[] angles = m_workingAngles; + double[] magnitudes = m_workingMagnitudes; + int referenceChannel = (int)ReferenceChannel; + + // Reference angle for phase normalization (typically VA); also reused below as the reference + // channel's current phase for frequency tracking, which avoids a duplicate Atan2 evaluation. + double referenceAngle = Math.Atan2(m_phasorImag[referenceChannel], m_phasorReal[referenceChannel]); + + for (int ch = 0; ch < NumChannels; ch++) + { + double real = m_phasorReal[ch]; + double imag = m_phasorImag[ch]; + + // Calculate magnitude (RMS) + magnitudes[ch] = Math.Sqrt(real * real + imag * imag) * m_magnitudeScale; + + // Calculate angle relative to reference (VA) + double rawAngle = Math.Atan2(imag, real); + angles[ch] = NormalizeAngle(rawAngle - referenceAngle); + } + + // Calculate time delta since last sample + double deltaTimeSeconds = m_samplePeriodSeconds; + + if (m_prevEpochNs > 0) + { + double measuredDelta = (epochNanoseconds - m_prevEpochNs) / (double)NanosecondsPerSecond; + + if (measuredDelta is > 0.0D and < 1.0D) + deltaTimeSeconds = measuredDelta; + } + + // Frequency from reference channel phase progression (reuses referenceAngle as current phase). + // FUTURE: Track frequency from the positive-sequence component instead of a single reference + // channel, so a single-channel fault (e.g., a VA PT failure) does not degrade the frequency / + // ROCOF estimate while the remaining phases are still healthy. + double instantaneousFrequency = NominalFrequencyHz; + + if (m_hasPrevPhase) + { + double deltaPhase = NormalizeAngle(referenceAngle - m_prevPhaseAngle); + + // Instantaneous frequency from phase progression. The full form is + // NominalFrequencyHz + (deltaPhase - 2π·fNom·dt) / (2π·dt), which cancels exactly to + // deltaPhase / (2π·dt) — fewer ops and avoids adding/subtracting the large nominal term. + instantaneousFrequency = deltaPhase / (TwoPI * deltaTimeSeconds); + + // Clamp instantaneous frequency to ±10% of nominal to reject transient / bad-data excursions. + // This saturates silently, so it effectively acts as a data-quality guard. Surfacing that as an + // output quality flag is protocol-dependent: SEL CWS currently carries no quality flags, so any + // quality indication would have to be expressed through the existing base-model quality fields. + // FUTURE: Revisit quality reporting if/when SEL CWS adds native quality flags. + instantaneousFrequency = Math.Max(m_minFrequency, Math.Min(m_maxFrequency, instantaneousFrequency)); + } + + m_prevPhaseAngle = referenceAngle; + m_hasPrevPhase = true; + + // Smooth frequency + if (m_frequencyInitialized) + { + m_smoothedFrequency = m_frequencyAlpha * instantaneousFrequency + m_oneMinusFrequencyAlpha * m_smoothedFrequency; + } + else + { + m_smoothedFrequency = instantaneousFrequency; + m_frequencyInitialized = true; + } + + // ROCOF + double rocof = 0.0D; + + if (m_rocofInitialized) + { + double instantaneousRocof = (m_smoothedFrequency - m_prevSmoothedFrequency) / deltaTimeSeconds; + m_smoothedRocof = m_rocofAlpha * instantaneousRocof + m_oneMinusRocofAlpha * m_smoothedRocof; + rocof = m_smoothedRocof; + } + else + { + m_rocofInitialized = true; + } + + m_prevSmoothedFrequency = m_smoothedFrequency; + m_prevEpochNs = epochNanoseconds; + + // Arrays are reused; callers must treat results as ephemeral per Step call + return new SamplePhaseEstimate + { + Frequency = m_smoothedFrequency, + dFdt = rocof, + Angles = angles, + Magnitudes = magnitudes + }; + } + + // Normalizes an angle to the range [-π, π] + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static Angle NormalizeAngle(double angle) + { + return new Angle(angle).ToRange(-Math.PI, false); + } + + // Converts a time constant (τ) to an EMA alpha (α) for a given update period (Δt) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static double AlphaFromTau(double dtSeconds, double tauSeconds) + { + // τ == 0 => α == 1 (no smoothing; output follows input immediately) + if (tauSeconds <= 0.0D) + return 1.0D; + + // Numerically stable for typical dt/tau ratios; dt and tau are positive here + return 1.0D - Math.Exp(-dtSeconds / tauSeconds); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static long AlignToNextBoundary(long timestampNs, long periodNs) + { + long remainder = timestampNs % periodNs; + return remainder == 0 ? timestampNs : timestampNs + (periodNs - remainder); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private void StoreLastSample(in SamplePhaseEstimate estimate) + { + estimate.Angles.AsSpan().CopyTo(m_lastAngles); + estimate.Magnitudes.AsSpan().CopyTo(m_lastMagnitudes); + m_lastFrequency = estimate.Frequency; + m_lastRocof = estimate.dFdt; + m_hasLastSampleEstimate = true; + } + + #endregion +} diff --git a/src/UnitTests/Gemstone.PhasorProtocols.UnitTests.csproj b/src/UnitTests/Gemstone.PhasorProtocols.UnitTests.csproj index aaedc705ed..09f767fa3b 100644 --- a/src/UnitTests/Gemstone.PhasorProtocols.UnitTests.csproj +++ b/src/UnitTests/Gemstone.PhasorProtocols.UnitTests.csproj @@ -20,4 +20,8 @@ + + + + diff --git a/src/UnitTests/IEEEC37_118PhaseEstimatorTest.cs b/src/UnitTests/IEEEC37_118PhaseEstimatorTest.cs new file mode 100644 index 0000000000..25dbae8c10 --- /dev/null +++ b/src/UnitTests/IEEEC37_118PhaseEstimatorTest.cs @@ -0,0 +1,267 @@ +//****************************************************************************************************** +// IEEEC37_118PhaseEstimatorTest.cs - Gbtc +// +// Copyright © 2026, Grid Protection Alliance. All Rights Reserved. +// +// Licensed to the Grid Protection Alliance (GPA) under one or more contributor license agreements. See +// the NOTICE file distributed with this work for additional information regarding copyright ownership. +// The GPA licenses this file to you under the MIT License (MIT), the "License"; you may +// not use this file except in compliance with the License. You may obtain a copy of the License at: +// +// http://www.opensource.org/licenses/MIT +// +// Unless agreed to in writing, the subject software distributed under the License is distributed on an +// "AS-IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. Refer to the +// License for the specific language governing permissions and limitations. +// +//****************************************************************************************************** +// ReSharper disable InconsistentNaming +// ReSharper disable PossiblyImpureMethodCallOnReadonlyVariable +// ReSharper disable RedundantArgumentDefaultValue + +using System; +using Gemstone.Numeric.EE; +using Gemstone.PhasorProtocols.SelCWS; +using Microsoft.VisualStudio.TestTools.UnitTesting; + +namespace Gemstone.PhasorProtocols.UnitTests; + +/// +/// Unit tests for , covering the IEEE C37.118-2018 Annex D +/// phasor, frequency and ROCOF estimation for both P-class and M-class filters. +/// +/// +/// Converted from the original Gemstone console demonstration program into deterministic assertions. +/// +[TestClass] +public class IEEEC37_118PhaseEstimatorTest +{ + // 960 Hz (= 16 samples/cycle at 60 Hz) reporting at 60 Hz matches the original Annex D test vectors. + private const double SampleRate = 960.0D; + private const double OutputRate = 60.0D; + private const double NominalFrequency = 60.0D; + private const double Sqrt2 = 1.4142135623730951D; + private const double DegreesToRadians = Math.PI / 180.0D; + + // Captures the most recent published estimate. Spans are only valid during the callback, + // so values are copied out into fields/arrays. + private sealed class Capture + { + public bool Got; + public double Frequency; + public double Rocof; + public long Count; + public readonly double[] MagnitudesRms = new double[6]; + public readonly double[] AnglesDegrees = new double[6]; + + public void OnEstimate(in PhaseEstimate estimate) + { + Got = true; + Frequency = estimate.Frequency; + Rocof = estimate.dFdt; + Count++; + + for (int i = 0; i < 6; i++) + { + MagnitudesRms[i] = estimate.Magnitudes[i]; + AnglesDegrees[i] = estimate.Angles[i].ToDegrees(); + } + } + } + + private static IEEEC37_118PhaseEstimator BuildEstimator(FilterClass filterClass = FilterClass.P) + { + return new IEEEC37_118PhaseEstimator( + sampleRateHz: SampleRate, + outputRateHz: OutputRate, + nominalFrequency: LineFrequency.Hz60, + filterClass: filterClass); + } + + // Balanced three-phase channel phase offsets (radians): VA/VB/VC at 0/-120/+120 deg, + // IA/IB/IC at the same with an optional current lead. + private static double[] BalancedPhases(double currentLeadDegrees = 0.0D) + { + return + [ + 0.0D * DegreesToRadians, + -120.0D * DegreesToRadians, + 120.0D * DegreesToRadians, + currentLeadDegrees * DegreesToRadians, + (-120.0D + currentLeadDegrees) * DegreesToRadians, + (120.0D + currentLeadDegrees) * DegreesToRadians + ]; + } + + private static double[] UniformPeaks(double amplitude) + { + return [amplitude, amplitude, amplitude, amplitude, amplitude, amplitude]; + } + + private static long EpochNs(long sampleIndex) + { + return (long)Math.Round(sampleIndex * 1_000_000_000.0D / SampleRate); + } + + // Steps the estimator with a cosine-referenced sinusoidal sample-group (VA, VB, VC, IA, IB, IC) + // for an absolute sample index. The Annex D angle convention assumes a cosine reference. + private static void StepIndex(IEEEC37_118PhaseEstimator estimator, Capture capture, long sampleIndex, double frequency, double[] peak, double[] phaseRadians) + { + double t = sampleIndex / SampleRate; + double w = 2.0D * Math.PI * frequency * t; + + double va = peak[0] * Math.Cos(w + phaseRadians[0]); + double vb = peak[1] * Math.Cos(w + phaseRadians[1]); + double vc = peak[2] * Math.Cos(w + phaseRadians[2]); + double ia = peak[3] * Math.Cos(w + phaseRadians[3]); + double ib = peak[4] * Math.Cos(w + phaseRadians[4]); + double ic = peak[5] * Math.Cos(w + phaseRadians[5]); + + estimator.Step(va, vb, vc, ia, ib, ic, EpochNs(sampleIndex), capture.OnEstimate); + } + + // Wraps an angle in degrees to (-180, 180]. + private static double WrapDegrees(double degrees) + { + double result = (degrees + 180.0D) % 360.0D; + + if (result < 0.0D) + result += 360.0D; + + return result - 180.0D; + } + + [TestMethod] + public void TracksNominalFrequency() + { + IEEEC37_118PhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + // 50 nominal cycles worth of samples + const long totalSamples = (long)(SampleRate / NominalFrequency * 50); + + for (long i = 0; i < totalSamples; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(capture.Got, "Expected an estimate once the filter window filled."); + Assert.AreEqual(NominalFrequency, capture.Frequency, 0.1D, "Frequency should track 60 Hz."); + Assert.AreEqual(0.0D, capture.Rocof, 0.1D, "ROCOF should be ~0 for a steady frequency."); + Assert.AreEqual(100.0D / Sqrt2, capture.MagnitudesRms[(int)PhaseChannel.VA], 0.5D, "VA RMS magnitude."); + } + + [TestMethod] + public void TracksOffNominalFrequency() + { + IEEEC37_118PhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + const double ActualFrequency = 61.0D; + + // 30 nominal cycles for convergence + const long totalSamples = (long)(SampleRate / NominalFrequency * 30); + + for (long i = 0; i < totalSamples; i++) + StepIndex(estimator, capture, i, ActualFrequency, peak, phases); + + Assert.IsTrue(capture.Got); + Assert.AreEqual(ActualFrequency, capture.Frequency, 0.2D, "Frequency should track 61 Hz."); + } + + [TestMethod] + public void MeasuresRmsMagnitudesPerChannel() + { + IEEEC37_118PhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = [100.0D, 150.0D, 200.0D, 120.0D, 180.0D, 90.0D]; + double[] phases = BalancedPhases(); + + const long totalSamples = (long)(SampleRate / NominalFrequency * 20); + + for (long i = 0; i < totalSamples; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(capture.Got); + + for (int ch = 0; ch < 6; ch++) + Assert.AreEqual(peak[ch] / Sqrt2, capture.MagnitudesRms[ch], peak[ch] * 0.02D, $"RMS magnitude for channel {ch}."); + } + + [TestMethod] + public void ReportsAnglesWithExpectedPhaseRelationships() + { + IEEEC37_118PhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(currentLeadDegrees: 30.0D); + + const long totalSamples = (long)(SampleRate / NominalFrequency * 20); + + for (long i = 0; i < totalSamples; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(capture.Got); + + double va = capture.AnglesDegrees[(int)PhaseChannel.VA]; + + // Annex D reports absolute synchrophasor angles, so validate the inter-channel relationships. + Assert.AreEqual(-120.0D, WrapDegrees(capture.AnglesDegrees[(int)PhaseChannel.VB] - va), 1.5D, "VB ~ -120 deg from VA."); + Assert.AreEqual(120.0D, WrapDegrees(capture.AnglesDegrees[(int)PhaseChannel.VC] - va), 1.5D, "VC ~ +120 deg from VA."); + Assert.AreEqual(30.0D, WrapDegrees(capture.AnglesDegrees[(int)PhaseChannel.IA] - va), 1.5D, "IA leads VA by ~30 deg."); + } + + [TestMethod] + public void MClassFilterOrderExceedsPClass() + { + IEEEC37_118PhaseEstimator pEstimator = BuildEstimator(FilterClass.P); + IEEEC37_118PhaseEstimator mEstimator = BuildEstimator(FilterClass.M); + + Assert.IsTrue(mEstimator.FilterOrder > pEstimator.FilterOrder, "M-class filter order should exceed P-class."); + + Capture pCapture = new(); + Capture mCapture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + const long totalSamples = (long)(SampleRate / NominalFrequency * 50); + + for (long i = 0; i < totalSamples; i++) + { + StepIndex(pEstimator, pCapture, i, NominalFrequency, peak, phases); + StepIndex(mEstimator, mCapture, i, NominalFrequency, peak, phases); + } + + Assert.IsTrue(pCapture.Got && mCapture.Got); + + const double expectedRms = 100.0D / Sqrt2; + Assert.AreEqual(NominalFrequency, pCapture.Frequency, 0.1D, "P-class frequency."); + Assert.AreEqual(NominalFrequency, mCapture.Frequency, 0.1D, "M-class frequency."); + Assert.AreEqual(expectedRms, pCapture.MagnitudesRms[(int)PhaseChannel.VA], 0.5D, "P-class VA RMS magnitude."); + Assert.AreEqual(expectedRms, mCapture.MagnitudesRms[(int)PhaseChannel.VA], 0.5D, "M-class VA RMS magnitude."); + } + + [TestMethod] + public void ProducesExpectedDecimationCount() + { + IEEEC37_118PhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + // Exactly one second of data. + const long totalSamples = (long)SampleRate; + + for (long i = 0; i < totalSamples; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + const int samplesPerReport = (int)(SampleRate / OutputRate); + long expected = (totalSamples - estimator.WindowSamples) / samplesPerReport; + + Assert.IsTrue(capture.Got); + Assert.IsTrue(Math.Abs(capture.Count - expected) <= 1, + $"Expected ~{expected} estimates at {OutputRate} Hz report rate, got {capture.Count}."); + } +} diff --git a/src/UnitTests/SlidingDftPhaseEstimatorTest.cs b/src/UnitTests/SlidingDftPhaseEstimatorTest.cs new file mode 100644 index 0000000000..93c8a65010 --- /dev/null +++ b/src/UnitTests/SlidingDftPhaseEstimatorTest.cs @@ -0,0 +1,356 @@ +//****************************************************************************************************** +// SlidingDftPhaseEstimatorTest.cs - Gbtc +// +// Copyright © 2026, Grid Protection Alliance. All Rights Reserved. +// +// Licensed to the Grid Protection Alliance (GPA) under one or more contributor license agreements. See +// the NOTICE file distributed with this work for additional information regarding copyright ownership. +// The GPA licenses this file to you under the MIT License (MIT), the "License"; you may +// not use this file except in compliance with the License. You may obtain a copy of the License at: +// +// http://www.opensource.org/licenses/MIT +// +// Unless agreed to in writing, the subject software distributed under the License is distributed on an +// "AS-IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. Refer to the +// License for the specific language governing permissions and limitations. +// +//****************************************************************************************************** +// ReSharper disable InconsistentNaming +// ReSharper disable PossiblyImpureMethodCallOnReadonlyVariable + +using System; +using Gemstone.Numeric.EE; +using Gemstone.PhasorProtocols.SelCWS; +using Microsoft.VisualStudio.TestTools.UnitTesting; + +namespace Gemstone.PhasorProtocols.UnitTests; + +/// +/// Unit tests for , covering the core phasor/frequency estimation +/// and the missing-data handling (phase-continued coast and resynchronization). +/// +[TestClass] +public class SlidingDftPhaseEstimatorTest +{ + private const double SampleRate = 3000.0D; + private const double NominalFrequency = 60.0D; + private const double Sqrt2 = 1.4142135623730951D; + private const double DegreesToRadians = Math.PI / 180.0D; + + // Captures the most recent published estimate. Spans are only valid during the callback, + // so scalar values are copied out into arrays. + private sealed class Capture + { + public bool Got; + public double Frequency; + public readonly double[] MagnitudesRms = new double[6]; + public readonly double[] AnglesDegrees = new double[6]; + + // Running sums over every published estimate. With internal smoothing off, the raw per-sample + // frequency/ROCOF carry deterministic ripple (spectral-leakage beat off-nominal; timestamp + // rounding jitter in dt), so a single sample is unrepresentative. The mean over the run recovers + // the true value (the ripple is zero-mean about it). + private double m_frequencySum; + private double m_rocofSum; + private long m_count; + + public double MeanFrequency => m_count > 0 ? m_frequencySum / m_count : 0.0D; + public double MeanRocof => m_count > 0 ? m_rocofSum / m_count : 0.0D; + + public void OnEstimate(in PhaseEstimate estimate) + { + Got = true; + Frequency = estimate.Frequency; + + m_frequencySum += estimate.Frequency; + m_rocofSum += estimate.dFdt; + m_count++; + + for (int i = 0; i < 6; i++) + { + MagnitudesRms[i] = estimate.Magnitudes[i]; + AnglesDegrees[i] = estimate.Angles[i].ToDegrees(); + } + } + } + + // Builds an estimator configured for deterministic, full-rate testing: publish rate equals sample + // rate (an estimate per sample) and all smoothing disabled so outputs reflect the raw per-sample DFT. + private static SlidingDftPhaseEstimator BuildEstimator(int maxGapFillSamples = SlidingDftPhaseEstimator.DefaultMaxGapFillSamples) + { + return new SlidingDftPhaseEstimator( + sampleRateHz: SampleRate, + outputRateHz: SampleRate, + nominalFrequency: LineFrequency.Hz60, + referenceChannel: PhaseChannel.VA, + targetCycles: 2, + enableIntervalAveraging: false, + enablePublishEMA: false, + publishAnglesTauSeconds: 0.0D, + publishMagnitudesTauSeconds: 0.0D, + publishFrequencyTauSeconds: 0.0D, + publishRocofTauSeconds: 0.0D, + sampleFrequencyTauSeconds: 0.0D, + sampleRocofTauSeconds: 0.0D, + recalculationCycles: 10, + maxGapFillSamples: maxGapFillSamples); + } + + // Balanced three-phase channel phase offsets (radians): VA/VB/VC at 0/-120/+120 deg, + // IA/IB/IC at the same with an optional current lead. + private static double[] BalancedPhases(double currentLeadDegrees = 0.0D) + { + return + [ + 0.0D * DegreesToRadians, + -120.0D * DegreesToRadians, + 120.0D * DegreesToRadians, + currentLeadDegrees * DegreesToRadians, + (-120.0D + currentLeadDegrees) * DegreesToRadians, + (120.0D + currentLeadDegrees) * DegreesToRadians + ]; + } + + // Contiguous per-sample epoch (nanoseconds) for an absolute sample index. + private static long EpochNs(long sampleIndex) + { + return (long)Math.Round(sampleIndex * 1_000_000_000.0D / SampleRate); + } + + // Steps the estimator with the sinusoidal sample-group for an absolute sample index, using an + // explicit epoch (so gaps and out-of-order timestamps can be simulated). + private static bool StepWithEpoch(SlidingDftPhaseEstimator estimator, Capture capture, long sampleIndex, double frequency, double[] peak, double[] phaseRadians, long epochNs) + { + double t = sampleIndex / SampleRate; + double w = 2.0D * Math.PI * frequency * t; + + double va = peak[0] * Math.Sin(w + phaseRadians[0]); + double vb = peak[1] * Math.Sin(w + phaseRadians[1]); + double vc = peak[2] * Math.Sin(w + phaseRadians[2]); + double ia = peak[3] * Math.Sin(w + phaseRadians[3]); + double ib = peak[4] * Math.Sin(w + phaseRadians[4]); + double ic = peak[5] * Math.Sin(w + phaseRadians[5]); + + return estimator.Step(va, vb, vc, ia, ib, ic, epochNs, capture.OnEstimate); + } + + // Steps with the natural contiguous epoch for the given sample index. + private static bool StepIndex(SlidingDftPhaseEstimator estimator, Capture capture, long sampleIndex, double frequency, double[] peak, double[] phaseRadians) + { + return StepWithEpoch(estimator, capture, sampleIndex, frequency, peak, phaseRadians, EpochNs(sampleIndex)); + } + + private static double[] UniformPeaks(double amplitude) + { + return [amplitude, amplitude, amplitude, amplitude, amplitude, amplitude]; + } + + [TestMethod] + public void TracksNominalFrequency() + { + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + for (long i = 0; i < 600; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(capture.Got, "Expected an estimate once the window filled."); + // Assert on the mean: per-sample frequency/ROCOF carry zero-mean ripple when smoothing is off. + Assert.AreEqual(NominalFrequency, capture.MeanFrequency, 0.1D, "Mean frequency should track 60 Hz."); + Assert.AreEqual(0.0D, capture.MeanRocof, 0.1D, "Mean ROCOF should be ~0 for a steady frequency."); + Assert.AreEqual(100.0D / Sqrt2, capture.MagnitudesRms[(int)PhaseChannel.VA], 0.5D, "VA RMS magnitude."); + } + + [TestMethod] + public void TracksOffNominalFrequency() + { + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + const double ActualFrequency = 61.0D; // within the +/-10% clamp + + for (long i = 0; i < 600; i++) + StepIndex(estimator, capture, i, ActualFrequency, peak, phases); + + Assert.IsTrue(capture.Got); + // Off-nominal per-sample frequency oscillates (spectral-leakage beat) about the true value; + // its mean recovers it. A single sample lands mid-beat (~60.5) and is not representative. + Assert.AreEqual(ActualFrequency, capture.MeanFrequency, 0.1D, "Mean frequency should track 61 Hz."); + } + + [TestMethod] + public void MeasuresRmsMagnitudesPerChannel() + { + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = [100.0D, 150.0D, 200.0D, 120.0D, 180.0D, 90.0D]; + double[] phases = BalancedPhases(); + + for (long i = 0; i < 600; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(capture.Got); + + for (int ch = 0; ch < 6; ch++) + Assert.AreEqual(peak[ch] / Sqrt2, capture.MagnitudesRms[ch], peak[ch] * 0.02D, $"RMS magnitude for channel {ch}."); + } + + [TestMethod] + public void ReportsAnglesRelativeToReference() + { + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(currentLeadDegrees: 30.0D); + + for (long i = 0; i < 600; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(capture.Got); + Assert.AreEqual(0.0D, capture.AnglesDegrees[(int)PhaseChannel.VA], 0.05D, "VA is the reference (~0 deg)."); + Assert.AreEqual(-120.0D, capture.AnglesDegrees[(int)PhaseChannel.VB], 1.5D, "VB ~ -120 deg."); + Assert.AreEqual(120.0D, capture.AnglesDegrees[(int)PhaseChannel.VC], 1.5D, "VC ~ +120 deg."); + Assert.AreEqual(30.0D, capture.AnglesDegrees[(int)PhaseChannel.IA], 1.5D, "IA leads VA by ~30 deg."); + } + + [TestMethod] + public void CoastsAcrossSmallGap() + { + // Auto max-gap-fill resolves to one window (100 samples), so a 50-sample drop coasts. + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + for (long i = 0; i < 300; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(estimator.IsReady, "Estimator should be ready before the gap."); + + // Drop samples 300..349 (50 missing), then resume at 350. + bool produced = StepIndex(estimator, capture, 350, NominalFrequency, peak, phases); + + Assert.IsTrue(produced, "Coasting a sub-threshold gap should still publish an estimate."); + Assert.IsTrue(estimator.IsReady, "Estimator should remain ready after coasting."); + Assert.AreEqual(NominalFrequency, capture.Frequency, 0.5D, "Frequency should stay stable through a coasted gap."); + Assert.AreEqual(100.0D / Sqrt2, capture.MagnitudesRms[(int)PhaseChannel.VA], 2.0D, "Magnitude should stay stable through a coasted gap."); + } + + [TestMethod] + public void ResynchronizesAcrossLargeGap() + { + // Small fill bound forces a 50-sample drop to resynchronize. + SlidingDftPhaseEstimator estimator = BuildEstimator(maxGapFillSamples: 10); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + for (long i = 0; i < 300; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(estimator.IsReady, "Estimator should be ready before the gap."); + + // Drop samples 300..349 (50 missing) > maxGapFill (10) -> resync. + bool produced = StepIndex(estimator, capture, 350, NominalFrequency, peak, phases); + + Assert.IsFalse(produced, "A gap beyond the fill bound should resync (no estimate this step)."); + Assert.IsFalse(estimator.IsReady, "Estimator should be refilling after a resync."); + } + + [TestMethod] + public void IgnoresDuplicateTimestamp() + { + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + for (long i = 0; i < 300; i++) + StepIndex(estimator, capture, i, NominalFrequency, peak, phases); + + long processedBefore = estimator.TotalSamplesProcessed; + + // Re-send the previous sample's exact epoch: a duplicate must be skipped, not fed into the SDFT twice. + bool produced = StepWithEpoch(estimator, capture, 300, NominalFrequency, peak, phases, EpochNs(299)); + + Assert.IsFalse(produced, "A duplicate timestamp should be ignored."); + Assert.AreEqual(processedBefore, estimator.TotalSamplesProcessed, "Duplicate samples must not advance processing."); + + // The next forward-advancing sample resumes normally (the duplicate did not wedge the timeline). + bool resumed = StepIndex(estimator, capture, 300, NominalFrequency, peak, phases); + Assert.IsTrue(resumed, "Reporting resumes on the next forward sample after a duplicate."); + } + + [TestMethod] + public void RecoversAfterBackwardTimestampRebase() + { + // Regression: a backward timestamp jump (source restart/loop, or a new config frame that re-bases + // timing) must NOT permanently stop reporting. The estimator should treat it as a discontinuity, + // reset onto the new timeline, and resume once the window refills. + SlidingDftPhaseEstimator estimator = BuildEstimator(); + Capture capture = new(); + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(); + + const long Base1 = 10_000_000_000L; // first stream's epoch base (10 s) + const long Base2 = 1_000_000_000L; // re-based stream starts ~9 s earlier (e.g., looped replay) + + for (long i = 0; i < 400; i++) + StepWithEpoch(estimator, capture, i, NominalFrequency, peak, phases, Base1 + EpochNs(i)); + + Assert.IsTrue(estimator.IsReady, "Estimator should be reporting before the backward re-base."); + + // Feed a fresh contiguous timeline from the earlier base; the first sample is a backward jump. + bool reportedAfterRebase = false; + + for (long j = 0; j < 400; j++) + { + if (StepWithEpoch(estimator, capture, j, NominalFrequency, peak, phases, Base2 + EpochNs(j))) + reportedAfterRebase = true; + } + + Assert.IsTrue(reportedAfterRebase, "Reporting must resume after a backward timestamp re-base (no permanent stall)."); + Assert.IsTrue(estimator.IsReady, "Estimator should be ready again on the new timeline."); + } + + [TestMethod] + public void CoastApproximatesContinuousStream() + { + // Compare a continuously-fed estimator against one that coasts a 50-sample drop. Once the gap + // has slid out of the window, both should agree closely, validating that coasting does not + // corrupt long-term state and that the synthesized phase continuation is faithful. + SlidingDftPhaseEstimator continuous = BuildEstimator(); + SlidingDftPhaseEstimator gapped = BuildEstimator(); + Capture continuousCapture = new(); + Capture gappedCapture = new(); + + double[] peak = UniformPeaks(100.0D); + double[] phases = BalancedPhases(currentLeadDegrees: 15.0D); + + for (long i = 0; i <= 200; i++) + { + StepIndex(continuous, continuousCapture, i, NominalFrequency, peak, phases); + StepIndex(gapped, gappedCapture, i, NominalFrequency, peak, phases); + } + + // Continuous gets every sample; gapped drops 201..250 (50 missing) then resumes contiguously. + for (long i = 201; i <= 400; i++) + StepIndex(continuous, continuousCapture, i, NominalFrequency, peak, phases); + + for (long i = 251; i <= 400; i++) + StepIndex(gapped, gappedCapture, i, NominalFrequency, peak, phases); + + Assert.IsTrue(continuousCapture.Got && gappedCapture.Got); + Assert.AreEqual(continuousCapture.Frequency, gappedCapture.Frequency, 0.05D, "Frequency should converge after the gap flushes."); + + for (int ch = 0; ch < 6; ch++) + { + Assert.AreEqual(continuousCapture.MagnitudesRms[ch], gappedCapture.MagnitudesRms[ch], 0.5D, $"Magnitude mismatch on channel {ch}."); + Assert.AreEqual(continuousCapture.AnglesDegrees[ch], gappedCapture.AnglesDegrees[ch], 0.5D, $"Angle mismatch on channel {ch}."); + } + } +}