132 lines
3.9 KiB
C#
132 lines
3.9 KiB
C#
///
|
|
/// Copyright (c) 2018 Sensus Slovensko a.s.
|
|
///
|
|
using System;
|
|
using Config.Entities;
|
|
|
|
namespace TBF.BenchControl.TestMethods.iPerlCommunication.iPerlHead
|
|
{
|
|
public class FlowDirectionDetection
|
|
{
|
|
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;
|
|
|
|
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;
|
|
}
|
|
|
|
|
|
/// <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(FlowDir flowDir)
|
|
{
|
|
if (!AreFifoDataValid()) return OptoHeadState.OptoNok;
|
|
|
|
decimal numer = N * sumXY - sumX * sumY;
|
|
decimal denom = N * sumXX - sumX * sumX;
|
|
|
|
if (denom == 0) return OptoHeadState.DirNok;
|
|
|
|
decimal a = numer / denom;
|
|
|
|
if ((flowDir == FlowDir.R_L) && (a > 0))
|
|
{
|
|
return OptoHeadState.OptoAndDirOK;
|
|
}
|
|
else if ((flowDir == FlowDir.L_R) && (a < 0))
|
|
{
|
|
return OptoHeadState.OptoAndDirOK;
|
|
}
|
|
else
|
|
{
|
|
return OptoHeadState.DirNok;
|
|
}
|
|
}
|
|
}
|
|
}
|