michal/tbf
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

iPerl ASIC - implementation

Browse Source
This commit is contained in:
michal
2026-03-12 14:50:47 +01:00
parent 3c964f2aab
commit a997c3d7be
378 changed files with 5684 additions and 295925 deletions
Download Patch File Download Diff File Expand all files Collapse all files
TBF/Rig/TestMethods/iPerlCommunication/iPerlHead/FlowDirectionDetection.cs
+69 -75
View File
@@ -11,40 +11,35 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
{
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.
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
private readonly double[] volumeRawFifo;
private readonly double[] timestampFifo; // centered timestamps
int fifoCount; /// Number of valid FIFO items
int fifoIx; /// Index of the next FIFO item
DateTime lastFifoWriteTime; /// Time of the last write to FIFO
private int fifoCount;
private int fifoIx;
private DateTime lastFifoWriteTime;
///
/// Sums for linear regression calculation
///
decimal sumXX;
decimal sumX;
decimal sumXY;
decimal sumY;
decimal N;
// regression sums (double is ideal here)
private double sumXX;
private double sumX;
private double sumXY;
private double sumY;
double minSlope; /// max. slope of the regressed line, always positive or 0
double maxSlope; /// min. slope of the regressed line, always negative or 0
private double minSlope;
private double maxSlope;
// timestamp centering for numerical stability
private double firstTimestamp = double.NaN;
public FlowDirectionDetection()
{
volumeRawFifo = new Int64[FIFO_SIZE];
timestampFifo = new Int64[FIFO_SIZE];
volumeRawFifo = new double[FIFO_SIZE];
timestampFifo = new double[FIFO_SIZE];
ClearFifo();
}
/// <summary>
/// Clear FIFO data
/// </summary>
public void ClearFifo()
{
fifoCount = 0;
@@ -55,94 +50,93 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
sumX = 0;
sumXY = 0;
sumY = 0;
N = 0;
minSlope = 0;
maxSlope = 0;
firstTimestamp = double.NaN;
}
/// <summary>
/// Write data to FIFO
/// Add sample to rolling FIFO and update regression sums
/// </summary>
/// <param name="volumeRaw">Volume</param>
/// <param name="timestamp">Time stamp</param>
public void WriteToFifo(Int64 volumeRaw, Int64 timestamp)
public void WriteToFifo(double volumeRaw, double timestamp)
{
///
/// Update sums for linear regression calculation
///
// establish time origin (CRITICAL for double precision)
if (double.IsNaN(firstTimestamp))
firstTimestamp = timestamp;
double x = timestamp - firstTimestamp; // centered time
double y = volumeRaw;
// remove oldest sample if buffer full
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--;
double oldX = timestampFifo[fifoIx];
double oldY = volumeRawFifo[fifoIx];
sumXX -= oldX * oldX;
sumX -= oldX;
sumXY -= oldX * oldY;
sumY -= oldY;
}
sumXX += timestamp * timestamp;
sumX += timestamp;
sumXY += timestamp * volumeRaw;
sumY += volumeRaw;
N++;
else
{
fifoCount++;
}
// add new sample
sumXX += x * x;
sumX += x;
sumXY += x * y;
sumY += y;
// store sample
timestampFifo[fifoIx] = x;
volumeRawFifo[fifoIx] = y;
///
/// 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);
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;
if (!AreFifoDataValid())
return OptoHeadState.OptoNok;
try
{
decimal numer = N * sumXY - sumX * sumY;
decimal denom = N * sumXX - sumX * sumX;
double N = fifoCount;
if (denom == 0) return OptoHeadState.DirNok;
double numer = N * sumXY - sumX * sumY;
double denom = N * sumXX - sumX * sumX;
if (Math.Abs(denom) < 1e-12)
return OptoHeadState.DirNok;
double slope = numer / denom;
/// 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 ||
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;
}
return OptoHeadState.DirNok;
}
catch (Exception)
catch (Exception ex)
{
log.ErrorFormat("{0} : CheckFlowDirection() failed", iPerlHeadName);
return OptoHeadState.DirNok; /// ???
log.ErrorFormat("{0} : CheckFlowDirection() failed: {1}", iPerlHeadName, ex);
return OptoHeadState.DirNok;
}
}
}