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

OptoTelegramRaw and CalculateFeatureVector() moved to Common project.

Browse Source
This commit is contained in:
Milan Hanajik
2021-07-20 09:46:23 +02:00
parent 3b81f1a143
commit 16de6c5d8b
7 changed files with 180 additions and 173 deletions
Download Patch File Download Diff File Expand all files Collapse all files
TBF/Rig/TestMethods/iPerlCommunication/iPerlHead/Enums.cs
-9
View File
@@ -99,15 +99,6 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
Count /// Number of flow states
}
public enum OptoTelegramFlags : byte
{
OK = 0,
OK_TestStart,
OK_TestEnd,
InvalidTelegram, /// Wrong telegram format of checksum error
SyncError,
}
public enum OptoState
{
Read,
TBF/Rig/TestMethods/iPerlCommunication/iPerlHead/IperlHead.cs
+99 -60
View File
@@ -1,11 +1,12 @@
///
/// Copyright (c) 2015-2020 Sensus Slovensko a.s.
/// Copyright (c) 2015-2021 Sensus Slovensko a.s.
///
using System;
using System.IO;
using System.IO.Ports;
using log4net;
using Common;
using Common.Iperl;
using Config.Entities;
using TBF.Rig.Generic;
using TBF.Rig.GenericDevices;
@@ -613,8 +614,8 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
/// <summary>Stop this operation</summary>
public void Stop()
{
int startIx = OptoTelegramRaw.BufferIdx(TestStartTelegramIx);
int endIx = OptoTelegramRaw.BufferIdx(TestEndTelegramIx);
int startIx = BufferIdx(TestStartTelegramIx);
int endIx = BufferIdx(TestEndTelegramIx);
log.WarnFormat("IperlHeadd.Stop() startIx={0} endIx={1} optoData.Len={2} filename={3}", startIx, endIx, optoData.Length, !string.IsNullOrEmpty(optoDataLogFileName) ? optoDataLogFileName : "<null>");
@@ -634,17 +635,17 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
StopParsingOptoSerialPort();
if (optoDataCount <= MaxOptoDataCount)
{
OptoTelegramRaw.FIRFilterFlow(optoData, 0, optoDataCount - 1);
FIRFilterFlow(optoData, 0, optoDataCount - 1);
}
else
{
OptoTelegramRaw.FIRFilterFlow(optoData, 0, StartOptoDataCount - 1);
OptoTelegramRaw.FIRFilterFlow(optoData, (optoDataCount - EndOptoDataCount), optoDataCount - 1);
FIRFilterFlow(optoData, 0, StartOptoDataCount - 1);
FIRFilterFlow(optoData, (optoDataCount - EndOptoDataCount), optoDataCount - 1);
}
log.DebugFormat("Flow filtering end, feature vector calculation start: {0:HH:mm:ss.fff}", DateTime.Now);
CalculateFeatureVector(optoData, optoDataCount, startIx, endIx, ref x);
Common.Utils.CalculateFeatureVector(optoData, optoDataCount, startIx, endIx, ref x);
log.DebugFormat("Feature vector calculation end, save opto-file start: {0:HH:mm:ss.fff}", DateTime.Now);
@@ -662,51 +663,10 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
}
}
void CalculateFeatureVector(OptoTelegramRaw[] optoData, int optoDataCount, int startIx, int endIx, ref float[] x)
{
if (startIx < 0 || endIx >= optoDataCount || startIx >= endIx) return;
int n = 0;
Int64 sumFlow = 0;
Int64 sumFlow2 = 0;
Int64 sumEmf = 0;
Int64 sumEmf2 = 0;
Int64 sumMagF = 0;
Int64 sumMagF2 = 0;
Int64 sumImp = 0;
Int64 sumImp2 = 0;
for (int ix = startIx; ix <= endIx; ix++)
{
if (optoData[ix].Flags == OptoTelegramFlags.OK ||
optoData[ix].Flags == OptoTelegramFlags.OK_TestStart ||
optoData[ix].Flags == OptoTelegramFlags.OK_TestEnd)
{
n++;
sumFlow += optoData[ix].FlowRaw;
sumFlow2 += ((int)optoData[ix].FlowRaw * (int)optoData[ix].FlowRaw);
sumEmf += (int)optoData[ix].EmfRaw;
sumEmf2 += ((Int64)optoData[ix].EmfRaw * (Int64)optoData[ix].EmfRaw);
sumMagF += optoData[ix].MagneticFieldRaw;
sumMagF2 += ((int)optoData[ix].MagneticFieldRaw * (int)optoData[ix].MagneticFieldRaw);
sumImp += optoData[ix].Impedance;
sumImp2 += ((int)optoData[ix].Impedance * (int)optoData[ix].Impedance);
}
}
if (n >= 2)
{
if (x.Length > 0) x[0] = ((float)sumFlow2 - (float)sumFlow * (float)sumFlow / n) / (n - 1);
if (x.Length > 1) x[1] = ((float)sumEmf2 - (float)sumEmf * (float)sumEmf / n) / (n - 1);
if (x.Length > 2) x[2] = ((float)sumMagF2 - (float)sumMagF * (float)sumMagF / n) / (n - 1);
if (x.Length > 3) x[3] = ((float)sumImp2 - (float)sumImp * (float)sumImp / n) / (n - 1);
}
}
/// <summary>
/// Save opto data to a file.
/// </summary>
string SaveOptoData()
string SaveOptoData()
{
if (!string.IsNullOrEmpty(optoDataLogFileName))
{
@@ -747,11 +707,10 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
optoLogFile.WriteLine(" ...");
/// Second part of the buffer is an overflowed circular buffer
optoLogFile.WriteLine(optoData[OptoTelegramRaw.BufferIdx(optoDataCount)].ToString(scalFact, null));
optoLogFile.WriteLine(optoData[BufferIdx(optoDataCount)].ToString(scalFact, null));
for (int i = optoDataCount - EndOptoDataCount + 1; i < optoDataCount; i++)
{
optoLogFile.WriteLine(optoData[OptoTelegramRaw.BufferIdx(i)]
.ToString(scalFact, optoData[OptoTelegramRaw.BufferIdx(i - 1)]));
optoLogFile.WriteLine(optoData[BufferIdx(i)].ToString(scalFact, optoData[BufferIdx(i - 1)]));
}
}
@@ -844,7 +803,7 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
/// CR+LF found
if (optoState == OptoState.Read)
{
int bufferIx = OptoTelegramRaw.BufferIdx(optoDataCount);
int bufferIx = BufferIdx(optoDataCount);
if (pos < OptoTelegramRaw.Length - 2)
{
@@ -857,7 +816,10 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
}
synchronized = true;
}
else if (optoData[bufferIx].UpdateFromString(allRcvd.Substring(pos - OptoTelegramRaw.Length + 2), optoDataCount, ref volumeRawExtLast, ref timestampExtLast))
else if (optoData[bufferIx].UpdateFromString(allRcvd.Substring(pos - OptoTelegramRaw.Length + 2),
optoDataCount,
Convert.ToSingle(Sequences.ProcessData.RefFlow.Val),
ref volumeRawExtLast, ref timestampExtLast))
{
/// CR+LF was found && (pos >= OptoTelegramRaw.Length - 2) && the telegram is OK
flowDirectionDetection.WriteToFifo(volumeRawExtLast, timestampExtLast);
@@ -884,7 +846,10 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
synchronized = true;
}
// CR+LF found and (pos >= OptoTelegram.Length - 2)
else if (toBeFlushed.UpdateFromString(allRcvd.Substring(pos - OptoTelegramRaw.Length + 2), 0, ref volumeRawExtLast, ref timestampExtLast))
else if (toBeFlushed.UpdateFromString(allRcvd.Substring(pos - OptoTelegramRaw.Length + 2),
0,
Convert.ToSingle(Sequences.ProcessData.RefFlow.Val),
ref volumeRawExtLast, ref timestampExtLast))
{
flowDirectionDetection.WriteToFifo(volumeRawExtLast, timestampExtLast);
synchronized2 = synchronized;
@@ -971,9 +936,9 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
switch (units)
{
default:
case VolumeUnits.m3: return Common.Units.ConvertFrom(Common.Unit.m3, 1.0); /// liter
case VolumeUnits.UK_gallon: return Common.Units.ConvertFrom(Common.Unit.UKgal, 1.0); /// liter
case VolumeUnits.US_gallon: return Common.Units.ConvertFrom(Common.Unit.USgal, 1.0); /// liter
case VolumeUnits.m3: return Common.Units.ConvertFrom(Common.Unit.m3, 1.0); /// 1 liter
case VolumeUnits.UK_gallon: return Common.Units.ConvertFrom(Common.Unit.UKgal, 1.0); /// 1 imperial gallon
case VolumeUnits.US_gallon: return Common.Units.ConvertFrom(Common.Unit.USgal, 1.0); /// 1 US gallon
}
}
@@ -1032,7 +997,7 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
Int64 sum = 0;
for (int i = unwrappedIx - samplesCount2; i <= unwrappedIx + samplesCount2; i++)
{
int wrappedIx = OptoTelegramRaw.BufferIdx(i);
int wrappedIx = BufferIdx(i);
if (optoData[wrappedIx].Flags != OptoTelegramFlags.OK &&
optoData[wrappedIx].Flags != OptoTelegramFlags.OK_TestStart &&
@@ -1061,7 +1026,7 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
Int64 sum = 0;
for (int i = unwrappedIx - samplesCount2; i <= unwrappedIx + samplesCount2; i++)
{
int wrappedIx = OptoTelegramRaw.BufferIdx(i);
int wrappedIx = BufferIdx(i);
if (optoData[wrappedIx].Flags != OptoTelegramFlags.OK &&
optoData[wrappedIx].Flags != OptoTelegramFlags.OK_TestStart &&
@@ -1076,6 +1041,80 @@ namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
return sum / (double)(8192 * (2 * samplesCount2 + 1));
}
/// <summary>
/// Filter RefFlow data in an array of OptoTelegramRaw objects by a FIR filter:
///
/// kSize = 5, kSize2 = 2
///
/// i k
/// ---------------------------------------------------------------------------
/// 0 -5 filtered[0] = data[0]
/// 1 -4 filtered[1] = data[1]
/// 2 -3 filtered[2] = data[0]*k[0] + ... + data[4]*k[4]
/// 3 -2 filtered[3] = data[1]*k[0] + ... + data[5]*k[4]
/// 4 -1 filtered[4] = data[2]*k[0] + ... + data[6]*k[4]
/// 5 0 data[0] = filtered[0], filtered[0] = data[3]*k[0] + ... + data[7]*k[4]
/// 6 1 data[1] = filtered[1], filtered[1] = data[4]*k[0] + ... + data[8]*k[4]
/// 7 ...
/// </summary>
/// <param name="optoData">array of OptoTelegramRaw objects</param>
/// <param name="from">Index of the first optoData item to process</param>
/// <param name="to">Index of the last optoData item to process</param>
public static void FIRFilterFlow(OptoTelegramRaw[] optoData, int from, int to)
{
float[] kernel = new float[] { 0.1f, 0.2f, 0.4f, 0.2f, 0.1f };
int kSize = kernel.Length;
int kSize2 = kernel.Length / 2;
float[] filtered = new float[kSize];
for (int i = from; i <= to; i++)
{
if ((i < from + kSize2) || (i > to - kSize2))
{
/// Beginning or end of optoData buffer => Just copy data (=do not filter)
filtered[i % kSize] = optoData[BufferIdx(i)].RefFlow;
}
else
{
/// Make a convolution of optoData and the kernel
float weightedSum = 0;
for (int j = -kSize2; j <= kSize2; j++)
weightedSum += optoData[BufferIdx(i + j)].RefFlow * kernel[j + kSize2];
filtered[i % kSize] = weightedSum;
}
if (i >= from + kSize)
{
/// filtered[] buffer full => copy filtered data to optoData
optoData[BufferIdx(i - kSize)].RefFlow = filtered[i % kSize];
}
}
for (int i = to - kSize + 1; i <= to; i++)
{
if (i >= 0) optoData[BufferIdx(i)].RefFlow = filtered[i % kSize];
}
}
/// <summary>
/// Get index to optoData buffer
/// </summary>
/// <param name="index">Original unwrapped index</param>
/// <returns>Index to the buffer</returns>
public static int BufferIdx(int index)
{
if (index < IperlHead.MaxOptoDataCount)
{
return index;
}
else
{
return IperlHead.StartOptoDataCount + (index - IperlHead.MaxOptoDataCount) % IperlHead.EndOptoDataCount;
}
}
public void WriteBinary(BinaryWriter writer)
{
writer.Write(Disabled);
TBF/Rig/TestMethods/iPerlCommunication/iPerlHead/OptoTelegramRaw.cs
-332
View File
@@ -1,332 +0,0 @@
///
/// Copyright (c) 2015-2021 Sensus Metering Systems
///
using System;
using System.Globalization;
namespace TBF.Rig.TestMethods.iPerlCommunication.iPerlHead
{
public class OptoTelegramRaw
{
public static readonly int Length = 42;
private static CultureInfo culture;
///
/// Strobed value
///
public static decimal TestStartTimestampDec;
///
/// Stored values
///
public OptoTelegramFlags Flags;
public DateTime DateTime; /// From PC
public float RefFlow; /// [m3/h]
public int Counter;
public UInt32 EmfRaw; /// From iPerl opto data
public Int16 MagneticFieldRaw;
public Int16 FlowRaw;
public UInt32 VolumeRaw;
public Int64 VolumeRawExt;
public Int16 Impedance;
public UInt32 Timestamp;
public Int64 TimestampExt;
public byte CheckSum;
///
/// Calculated values
///
public double EMF()
{
Int32 signedEmf = (EmfRaw > 0x7FFFFF) ? ((int)EmfRaw - 0x1000000) : (int)EmfRaw;
return 0.000000333 * (double)signedEmf;
}
public double MagneticField() { return (double)MagneticFieldRaw; }
public double Flow(double scalingFactor) { return 0.225 * scalingFactor * (double)FlowRaw; }
public double Volume(double scalingFactor) { return 0.0000625 * scalingFactor * (double)VolumeRawExt; }
public Int32 FlipTime() { return Impedance; }
public decimal TimestampDec() { return (decimal)TimestampExt / (decimal)8192; }
public double VolumeDelta(double scalingFactor, OptoTelegramRaw previous) { return (previous == null) ? 0 : Volume(scalingFactor) - previous.Volume(scalingFactor); }
public decimal TimeDelta() { return TimestampDec() - TestStartTimestampDec; }
public string Label()
{
if (Flags == OptoTelegramFlags.OK_TestStart) return "#### start test ####";
else if (Flags == OptoTelegramFlags.OK_TestEnd) return "#### end of test ####";
else return string.Empty;
}
static OptoTelegramRaw()
{
culture = CultureInfo.CreateSpecificCulture("DE"); /// This is to use comma as decimal number separator
}
public OptoTelegramRaw()
{
}
/// <summary>
/// Parses optical telegram and returns OptoTelegramRaw object
/// </summary>
/// <description>
/// Create a configuration structure from a complete byte array
///
/// Telegram description:
///
/// AAAAAA[tab]BBBB[tab]CCCC[tab]DDDDDD[tab]EEEE[tab]FFFFFFFF[tab]GG[cr][lf] (42 bytes)
///
/// Data Comment Type Calculate to decimal
/// ----------------------------------------------------------------
/// AAAAAA EMF Int24 Value * 0.000000333
/// BBBB Magnetic field Int16 Value
/// CCCC Flow Int16 Value * 0.225 * Scalig factor
/// DDDDDD Volume Int24 Value / 16000 * Scaling factor
/// EEEE Impedance Int16 Value
/// FFFFFFFF Timestamp Uint32 Value / 8192
/// GG Checksum Byte
/// ----------------------------------------------------------------
///
/// Example:
/// FFFFFE 51EA 0000 65324E 0087 F6319DFF 86
/// FFDD3A 51F9 0000 65324E 0088 F631A60B 45
/// ...
/// </description>
/// <param name="data">A complete byte array data</param>
/// <returns>true = telegram OK, false = telegram NOK</returns>
public bool UpdateFromString(string telegram, int counter, ref Int64 volumeRawExtLast, ref Int64 timestampExtLast)
{
DateTime = DateTime.Now;
Counter = counter;
RefFlow = (float)Sequences.ProcessData.RefFlow.Val;
if ((telegram == null) || (telegram.Length < Length) ||
(telegram[6] != '\t') || (telegram[11] != '\t') || (telegram[16] != '\t') ||
(telegram[23] != '\t') || (telegram[28] != '\t') || (telegram[37] != '\t') ||
(telegram[40] != '\r') || (telegram[41] != '\n'))
{
Flags = OptoTelegramFlags.InvalidTelegram;
return false;
}
bool f1 = UInt32.TryParse(telegram.Substring(0, 6), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out EmfRaw);
bool f2 = Int16.TryParse(telegram.Substring(7, 4), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out MagneticFieldRaw);
bool f3 = Int16.TryParse(telegram.Substring(12, 4), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out FlowRaw);
bool f4 = UInt32.TryParse(telegram.Substring(17, 6), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out VolumeRaw);
bool f5 = Int16.TryParse(telegram.Substring(24, 4), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out Impedance);
bool f6 = UInt32.TryParse(telegram.Substring(29, 8), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out Timestamp);
bool f7 = byte.TryParse(telegram.Substring(38, 2), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out CheckSum);
byte calculatedCheckSum = 0;
for (int i = 0; i < Length - 4; i++)
{
calculatedCheckSum += (byte)telegram[i];
}
bool allOk = f1 && f2 && f3 && f4 && f5 && f6 && f7 && (calculatedCheckSum == CheckSum);
if (allOk)
{
///
/// Cope with 'VolumeRaw' overflow
///
Int64 uncorrected = (Int64)(((UInt64)volumeRawExtLast & 0xFFFFFFFFFF000000UL) | VolumeRaw);
if (Math.Abs(uncorrected - volumeRawExtLast) <= 0x800000L)
{
VolumeRawExt = volumeRawExtLast = uncorrected;
}
else if (Math.Abs(uncorrected + 0x1000000L - volumeRawExtLast) <= 0x800000L)
{
VolumeRawExt = volumeRawExtLast = uncorrected + 0x1000000L;
}
else if (Math.Abs(uncorrected - 0x1000000L - volumeRawExtLast) <= 0x800000L)
{
VolumeRawExt = volumeRawExtLast = uncorrected - 0x1000000L;
}
else
{
VolumeRawExt = volumeRawExtLast = uncorrected;
}
///
/// Cope with 'Timestamp' overflow
///
uncorrected = (Int64)(((UInt64)timestampExtLast & 0xFFFFFFFF00000000UL) | Timestamp);
if (Math.Abs(uncorrected - timestampExtLast) <= 0x80000000L)
{
TimestampExt = timestampExtLast = uncorrected;
}
else if (Math.Abs(uncorrected + 0x100000000L - timestampExtLast) <= 0x80000000L)
{
TimestampExt = timestampExtLast = uncorrected + 0x100000000L;
}
else if (Math.Abs(uncorrected - 0x100000000L - timestampExtLast) <= 0x80000000L)
{
TimestampExt = timestampExtLast = uncorrected - 0x100000000L;
}
else
{
TimestampExt = timestampExtLast = uncorrected;
}
}
Flags = allOk ? OptoTelegramFlags.OK : OptoTelegramFlags.InvalidTelegram;
return allOk;
}
/// <summary>
/// Alternative to UpdateFromString(...) when data are flushed
/// </summary>
public bool UpdateFromStringDummy(string telegram)
{
DateTime = DateTime.Now;
RefFlow = (float)Sequences.ProcessData.RefFlow.Val;
if ((telegram == null) || (telegram.Length < Length) ||
(telegram[6] != '\t') || (telegram[11] != '\t') || (telegram[16] != '\t') ||
(telegram[23] != '\t') || (telegram[28] != '\t') || (telegram[37] != '\t') ||
(telegram[40] != '\r') || (telegram[41] != '\n'))
{
Flags = OptoTelegramFlags.InvalidTelegram;
return false;
}
bool f7 = byte.TryParse(telegram.Substring(38, 2), NumberStyles.HexNumber, CultureInfo.CurrentCulture, out CheckSum);
byte calculatedCheckSum = 0;
for (int i = 0; i < Length - 4; i++)
{
calculatedCheckSum += (byte)telegram[i];
}
bool allOk = f7 && (calculatedCheckSum == CheckSum);
Flags = allOk ? OptoTelegramFlags.OK : OptoTelegramFlags.InvalidTelegram;
return allOk;
}
public void SetFlags(OptoTelegramFlags flags)
{
this.Flags = flags;
}
public string ToString(double scalingFactor, OptoTelegramRaw previous)
{
if (Flags == OptoTelegramFlags.SyncError)
{
return "Sychronization error";
}
else if (Flags == OptoTelegramFlags.InvalidTelegram)
{
return "Invalid telegram";
}
else /// if (flags == OptoTelegramFlags.OK / OptoTelegramFlags.OK_TestStart / OptoTelegramFlags.OK_TestEnd)
{
return string.Format("{0}:{1}:{2}.{3}\t{4} :\t{5}\t{6}\t{7}\t{8}\t{9}\t{10}\t{11}\t{12}\t{13}\t{14}\t{15}\t{16}\t{17}\t{18}\t{19}\t{20}\t{21}\t{22}",
/* 0 */ DateTime.Hour.ToString("D2"),
/* 1 */ DateTime.Minute.ToString("D2"),
/* 2 */ DateTime.Second.ToString("D2"),
/* 3 */ DateTime.Millisecond.ToString("D4"),
/* 4 */ Counter,
/* 5 */ EmfRaw.ToString("X6"),
/* 6 */ MagneticFieldRaw.ToString("X4"),
/* 7 */ FlowRaw.ToString("X4"),
/* 8 */ VolumeRaw.ToString("X6"),
/* 9 */ Impedance.ToString("X4"),
/* 10 */ Timestamp.ToString("X8"),
/* 11 */ CheckSum.ToString("X2"),
/* 12 */ EMF().ToString("F4", culture),
/* 13 */ MagneticField().ToString("F0", culture),
/* 14 */ Flow(scalingFactor).ToString("F2", culture),
/* 15 */ Volume(scalingFactor).ToString("F4", culture),
/* 16 */ FlipTime().ToString("F0", culture),
/* 17 */ TimestampDec().ToString("F4", culture),
/* 18 */ (RefFlow * 1000).ToString("F2", culture),
/* 19 */ VolumeDelta(scalingFactor, previous).ToString("F4", culture),
/* 20 */ TimeDelta().ToString("F3", culture),
/* 21 */ scalingFactor.ToString("F1", culture),
/* 22 */ Label());
}
}
/// <summary>
/// Filter RefFlow data in an array of OptoTelegramRaw objects by a FIR filter:
///
/// kSize = 5, kSize2 = 2
///
/// i k
/// ---------------------------------------------------------------------------
/// 0 -5 filtered[0] = data[0]
/// 1 -4 filtered[1] = data[1]
/// 2 -3 filtered[2] = data[0]*k[0] + ... + data[4]*k[4]
/// 3 -2 filtered[3] = data[1]*k[0] + ... + data[5]*k[4]
/// 4 -1 filtered[4] = data[2]*k[0] + ... + data[6]*k[4]
/// 5 0 data[0] = filtered[0], filtered[0] = data[3]*k[0] + ... + data[7]*k[4]
/// 6 1 data[1] = filtered[1], filtered[1] = data[4]*k[0] + ... + data[8]*k[4]
/// 7 ...
/// </summary>
/// <param name="optoData">array of OptoTelegramRaw objects</param>
/// <param name="from">Index of the first optoData item to process</param>
/// <param name="to">Index of the last optoData item to process</param>
public static void FIRFilterFlow(OptoTelegramRaw[] optoData, int from, int to)
{
float[] kernel = new float[] { 0.1f, 0.2f, 0.4f, 0.2f, 0.1f };
int kSize = kernel.Length;
int kSize2 = kernel.Length / 2;
float[] filtered = new float[kSize];
for (int i = from; i <= to; i++)
{
if ((i < from + kSize2) || (i > to - kSize2))
{
/// Beginning or end of optoData buffer => Just copy data (=do not filter)
filtered[i % kSize] = optoData[BufferIdx(i)].RefFlow;
}
else
{
/// Make a convolution of optoData and the kernel
float weightedSum = 0;
for (int j = -kSize2; j <= kSize2; j++)
weightedSum += optoData[BufferIdx(i + j)].RefFlow * kernel[j + kSize2];
filtered[i % kSize] = weightedSum;
}
if (i >= from + kSize)
{
/// filtered[] buffer full => copy filtered data to optoData
optoData[BufferIdx(i - kSize)].RefFlow = filtered[i % kSize];
}
}
for (int i = to - kSize + 1; i <= to; i++)
{
if (i >= 0) optoData[BufferIdx(i)].RefFlow = filtered[i % kSize];
}
}
/// <summary>
/// Get index to optoData buffer
/// </summary>
/// <param name="index">Original unwrapped index</param>
/// <returns>Index to the buffer</returns>
public static int BufferIdx(int index)
{
if (index < IperlHead.MaxOptoDataCount)
{
return index;
}
else
{
return IperlHead.StartOptoDataCount + (index - IperlHead.MaxOptoDataCount) % IperlHead.EndOptoDataCount;
}
}
}
}