150 lines
4.7 KiB
C#
150 lines
4.7 KiB
C#
///
|
|
/// Copyright (c) 2018-2019 Sensus Slovensko a.s.
|
|
///
|
|
using System;
|
|
using log4net;
|
|
using Common;
|
|
|
|
namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
|
|
{
|
|
public class FlowDirectionDetection
|
|
{
|
|
private static readonly ILog log = LogManager.GetLogger(typeof(IperlHead));
|
|
|
|
const int FIFO_SIZE = 64; /// 8 sec. @ 8Hz
|
|
const double MAX_OPTO_DROPOUT = 4.5; /// sec.
|
|
|
|
|
|
Int64[] volumeRawFifo; /// Volume FIFO buffer
|
|
Int64[] timestampFifo; /// Timestamp FIFO buffer
|
|
|
|
int fifoCount; /// Number of valid FIFO items
|
|
int fifoIx; /// Index of the next FIFO item
|
|
DateTime lastFifoWriteTime; /// Time of the last write to FIFO
|
|
|
|
///
|
|
/// Sums for linear regression calculation
|
|
///
|
|
decimal sumXX;
|
|
decimal sumX;
|
|
decimal sumXY;
|
|
decimal sumY;
|
|
decimal N;
|
|
|
|
double minSlope; /// max. slope of the regressed line, always positive or 0
|
|
double maxSlope; /// min. slope of the regressed line, always negative or 0
|
|
|
|
public FlowDirectionDetection()
|
|
{
|
|
volumeRawFifo = new Int64[FIFO_SIZE];
|
|
timestampFifo = new Int64[FIFO_SIZE];
|
|
ClearFifo();
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Clear FIFO data
|
|
/// </summary>
|
|
public void ClearFifo()
|
|
{
|
|
fifoCount = 0;
|
|
fifoIx = 0;
|
|
lastFifoWriteTime = DateTime.MinValue;
|
|
|
|
sumXX = 0;
|
|
sumX = 0;
|
|
sumXY = 0;
|
|
sumY = 0;
|
|
N = 0;
|
|
|
|
minSlope = 0;
|
|
maxSlope = 0;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Write data to FIFO
|
|
/// </summary>
|
|
/// <param name="volumeRaw">Volume</param>
|
|
/// <param name="timestamp">Time stamp</param>
|
|
public void WriteToFifo(Int64 volumeRaw, Int64 timestamp)
|
|
{
|
|
///
|
|
/// Update sums for linear regression calculation
|
|
///
|
|
if (fifoCount == FIFO_SIZE)
|
|
{
|
|
/// Buffer is already full, the oldest item will be re-written
|
|
sumXX -= timestampFifo[fifoIx] * timestampFifo[fifoIx];
|
|
sumX -= timestampFifo[fifoIx];
|
|
sumXY -= timestampFifo[fifoIx] * volumeRawFifo[fifoIx];
|
|
sumY -= volumeRawFifo[fifoIx];
|
|
N--;
|
|
}
|
|
sumXX += timestamp * timestamp;
|
|
sumX += timestamp;
|
|
sumXY += timestamp * volumeRaw;
|
|
sumY += volumeRaw;
|
|
N++;
|
|
|
|
///
|
|
/// Save new values to FIFO
|
|
///
|
|
volumeRawFifo[fifoIx] = volumeRaw;
|
|
timestampFifo[fifoIx] = timestamp;
|
|
fifoIx = (fifoIx + 1) % FIFO_SIZE;
|
|
fifoCount = Math.Min(fifoCount + 1, FIFO_SIZE);
|
|
lastFifoWriteTime = DateTime.Now;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Determine whether there are enough recent FIFO data
|
|
/// </summary>
|
|
/// <returns>true when data valid</returns>
|
|
public bool AreFifoDataValid()
|
|
{
|
|
return (DateTime.Now.Subtract(lastFifoWriteTime).TotalSeconds <= MAX_OPTO_DROPOUT) && (fifoCount == FIFO_SIZE);
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// Verify whether the flow direction is correct
|
|
/// </summary>
|
|
/// <returns>OptoHeadState.OptoAndDirOK, OptoHeadState.OptoNok or OptoHeadState.DirNok</returns>
|
|
public OptoHeadState CheckFlowDirection(Counting counting, string iPerlHeadName)
|
|
{
|
|
if (!AreFifoDataValid()) return OptoHeadState.OptoNok;
|
|
|
|
try
|
|
{
|
|
decimal numer = N * sumXY - sumX * sumY;
|
|
decimal denom = N * sumXX - sumX * sumX;
|
|
|
|
if (denom == 0) return OptoHeadState.DirNok;
|
|
|
|
/// Calculate the slope of the regressed line, determine min. and max.
|
|
double slope = (double)(numer / denom);
|
|
if (slope > maxSlope) maxSlope = slope;
|
|
if (slope < minSlope) minSlope = slope;
|
|
|
|
if ( counting == Counting.Arbitrary ||
|
|
(counting == Counting.Positive && maxSlope > Math.Abs(2 * minSlope)) ||
|
|
(counting == Counting.Negative && minSlope < -Math.Abs(2 * maxSlope)))
|
|
{
|
|
return OptoHeadState.OptoAndDirOK;
|
|
}
|
|
else
|
|
{
|
|
return OptoHeadState.DirNok;
|
|
}
|
|
}
|
|
catch (Exception)
|
|
{
|
|
log.ErrorFormat("{0} : CheckFlowDirection() failed", iPerlHeadName);
|
|
return OptoHeadState.DirNok; /// ???
|
|
}
|
|
}
|
|
}
|
|
}
|