Files
tbf/TBF/Rig/ControlBoard/Uni/UniCB.cs
T

597 lines
23 KiB
C#

///
/// Copyright (c) 2021 Sensus Slovensko a.s.
///
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO.Ports;
using System.Text;
using log4net;
using Common;
using Config.Entities;
using Dirichlet.Numerics;
using TBF.Rig.GenericDevices;
using TBF.Rig.Sequences;
namespace TBF.Rig.ControlBoard.Uni
{
public class UniCB : ComponentBase, IControlBoard, SchematicDrawing.IDrawingItCmpntWithMeasuredVal
{
private static readonly ILog log = LogManager.GetLogger(typeof(UniCB));
public override string ToString() { return string.Format("CBoard({0})", cbCfg != null ? cbCfg.ToString(1) : string.Empty); }
/// TBF component configuration and useful wrappers
readonly UniCBCfg cbCfg;
string comPort { get { return string.Format("COM{0}", cbCfg.ComPortNr); } }
short config { get { return (short)((cbCfg.DrainValvesCount << 8) + ((cbCfg.DivertersCount - 1) << 3) + cbCfg.RegValvesCount - 4); } }
/// IDrawingItCmpnt interface implementation
public SchematicDrawing.IDrawingItem DrawingItem { get { return cbCfg as SchematicDrawing.IDrawingItem; } }
public bool MsrmntAvailable { get { return true; } }
public double MeasuredVal { get { return 0; } }
StringBuilder altString = new StringBuilder();
public string AltString
{
get {
altString.Clear();
altString.AppendFormat("DI: {0}\n", ToBin40(DigitalInputs));
altString.AppendFormat("DO: {0}\n", ToBin40((UInt64)Route));
altString.AppendFormat("AD 0:{0:D4} 1:{1:D4} 2:{2:D4} 3:{3:D4} 4:{4:D4} 5:{5:D4}\n",
AnalogInput(0), AnalogInput(1), AnalogInput(2), AnalogInput(3), AnalogInput(4), AnalogInput(5));
altString.AppendFormat("AD 8:{0:D4} 9:{1:D4} 10:{2:D4} 11:{3:D4}",
AnalogInput(8), AnalogInput(9), AnalogInput(10), AnalogInput(11));
return altString.ToString();
}
}
string ToBin40(UInt64 val)
{
return string.Format("{0}-{1}-{2}-{3}-{4}", ToBin8((val >> 32) & 0xFF), ToBin8((val >> 24) & 0xFF),
ToBin8((val >> 16) & 0xFF), ToBin8((val >> 8) & 0xFF), ToBin8(val & 0xFF));
}
string ToBin8(UInt64 val)
{
string str = Convert.ToString((byte)val, 2);
switch (str.Length)
{
case 1: return "0000000" + str;
case 2: return "000000" + str;
case 3: return "00000" + str;
case 4: return "0000" + str;
case 5: return "000" + str;
case 6: return "00" + str;
case 7: return "0" + str;
default: return str;
}
}
/// Uni CB state
UInt32 currentOutputs;
bool outputsInitialized;
DateTime lastOutputsChange;
int lastStateMachineTime = 0;
/// Propagated from configurations of other components
UInt128 benchModelRoute;
UInt128 valvesToInvert; /// Route = valesToInvert ^ RequiredRouteWithoutInvertedVlaves
readonly UInt128 routeMask; /// This mask masks out virtual valves, routeMask is set in the constructor
/// Serial communication
SerialPort serialPort; /// Seral port or null: Check serialPort.IsOpen before using the serial port
const int BUFFER_SIZE = 4000; /// Size of input buffers
byte[] buffer; /// Buffer for data received in one attempt in RunDeviceBefore()
byte[] rcvdData; /// Buffer for all recevied but nut processed data. May contain data from several read attempts
int[] cumulativeSums;
int rcvdBytesCount; /// Count of valid bytes in rcvdData buffer
/// Message queue
Queue<Action> scheduledActionQueue;
/// Received data
public GeneralData Data;
DivTransitionData divTransitionData;
ScopeAnalyzerData scopeAnalyzerData;
SwitchCountersData switchCounterData;
public UInt64 State { get { return Data.State; } } /// Control board state
public UInt64 DigitalInputs { get { return Data.Vstupy; } } /// Digital inputs
public Int16 AnalogInput(int channel)
{
if (Data.AnalogInput != null && channel >= 0 && channel < Data.AnalogInput.Length)
{
return Data.AnalogInput[channel];
}
return 0;
}
/// <summary>
/// Route: 128-bit word representing the current state of all valves and pumps
/// </summary>
public UInt128 Route { get { return benchModelRoute; } }
/// Values valid during a test
public double RefFrequency { get { return Data.ReferenceFreq[0]; } }
public double RefFlow
{
get { return (Devices != null && Devices.FlowMeter != null) ? Devices.FlowMeter.ReadFlow() : 0.0; }
}
/// Last test results
public double TestTime { set; get; }
public int RefPulses { set; get; }
public float ValveOpenCloseTime { set; get; } /// Start valve open or close time in sec
/// <summary>
/// Select devices used for control and measurement
/// </summary>
/// <param name="outPath"></param>
public OutputPath Devices
{
set { devices = value; }
get { return devices; }
}
OutputPath devices;
IFlowMeter flowMeter { get { return devices.FlowMeter; } }
IRegValve regValve { get { return devices.RegValve; } }
IDiverter diverter { get { return devices.Diverter; } }
/// <summary>
/// Constructor for user interfaces (change settings only).
/// Initialize(), RunDeviceXY() and StopDevice() methods of the created object will not be called.
/// </summary>
public UniCB()
{
devices = new OutputPath(string.Empty, 0);
}
/// <summary>
/// Constructor invoked when starting the test bench and creating components.
/// </summary>
public UniCB(Generic.IComponentCfg cfg)
: base(cfg)
{
#if DEBUG
Debug.WriteLine("");
Debug.WriteLine("Uni CB enums");
Debug.WriteLine("");
foreach (var e in Enum.GetValues(typeof(Poz)))
{
Debug.WriteLine(string.Format("{0} ... {1}", e, ((int)e)));
}
#endif
cbCfg = cfg as UniCBCfg;
devices = new OutputPath(string.Empty, 0);
scheduledActionQueue = new Queue<Action>();
Data = new GeneralData();
divTransitionData = null;
scopeAnalyzerData = null;
switchCounterData = null;
serialPort = null;
buffer = new byte[BUFFER_SIZE];
rcvdData = new byte[BUFFER_SIZE];
cumulativeSums = new int[BUFFER_SIZE];
rcvdBytesCount = 0;
routeMask = 0;
for (int i = 0; i < 128; i++)
{
if (i < cbCfg.VirtValvesRangeLo || i > cbCfg.VirtValvesRangeHi)
{
routeMask = routeMask | ((UInt128)1 << i);
}
}
TBF.UiBridge.Bridge.RouteChangeHandler += delegate(object sndr, TBF.UiBridge.RouteChangeArgs args)
{
UInt128 mask = ((UInt128)1) << args.BitNr;
if ((benchModelRoute & mask) == 0)
{
benchModelRoute |= mask;
}
else
{
benchModelRoute &= (~mask);
}
};
log.Warn(this.ToString());
}
/// <summary>
/// Specify inverted valves
/// </summary>
/// <param name="valvesToInvert"></param>
public void SpecifyInvertedValves(UInt128 valvesToInvert)
{
this.valvesToInvert = valvesToInvert;
this.benchModelRoute = valvesToInvert;
}
#region IDevice interface
/// <summary>
/// Initialize control board device hardware
/// </summary>
public void Initialize()
{
if (cbCfg.DebugLevel != DebugMode.Normal)
{
log.FatalFormat("{0} - Device simulated", Name);
return;
}
serialPort = new SerialPort(comPort, 9600, Parity.None, 8, StopBits.One);
serialPort.DtrEnable = true;
serialPort.WriteTimeout = 4000;
serialPort.ParityReplace = 0xFF;
serialPort.Open();
log.FatalFormat("{0} - Device successfully initialized", Name);
}
/// <summary>
/// Reecive and process data from the control board serial port
/// </summary>
public void RunDeviceBefore()
{
if (cbCfg.DebugLevel != DebugMode.Normal || serialPort == null || !serialPort.IsOpen) return;
/// Read new received bytes and append them to a buffer
int bytesCount = serialPort.BytesToRead;
if (bytesCount > 0)
{
bytesCount = serialPort.Read(buffer, 0, Math.Min(bytesCount, BUFFER_SIZE));
}
for (int i = 0; i < bytesCount && rcvdBytesCount < BUFFER_SIZE; i++, rcvdBytesCount++)
{
rcvdData[rcvdBytesCount] = buffer[i];
}
/// Calculate cumulative sums (to verify checksums easily)
int cumulativeSum = 0;
for (int i = 0; i < rcvdBytesCount; i++)
{
cumulativeSum += rcvdData[i];
cumulativeSums[i] = cumulativeSum;
}
/// Try to detect various kinds of data in rcvdData buffer in a loop, start at offset 0
int offset = 0;
int lastValidOffset = 0; /// Offset of the first byte after the last successfully parsed frame
do
{
bool anyProcessed = false;
if (ScopeAnalyzerData.FrameFitsData(rcvdData, cumulativeSums, rcvdBytesCount, offset))
{
/// Scope analyzer data received
scopeAnalyzerData = ScopeAnalyzerData.GetData(rcvdData, offset);
string s = Telegram.LogTelegram("Scope analyzer data ", rcvdData, offset, ScopeAnalyzerData.MessageLen);
Debug.WriteLine(s);
log.Debug(s);
offset += ScopeAnalyzerData.MessageLen;
lastValidOffset = offset;
anyProcessed = true;
}
else if (GeneralData.FrameFitsData(rcvdData, cumulativeSums, rcvdBytesCount, offset))
{
/// Regular data received
Data.UpdateData(rcvdData, offset);
string s = Telegram.LogTelegram("General data ", rcvdData, offset, GeneralData.MessageLen);
Debug.WriteLine(s);
log.Debug(s);
offset += GeneralData.MessageLen;
lastValidOffset = offset;
anyProcessed = true;
}
else if (SwitchCountersData.FrameFitsData(rcvdData, cumulativeSums, rcvdBytesCount, offset))
{
/// Switch counter data received
switchCounterData = SwitchCountersData.GetData(rcvdData, offset);
string s = Telegram.LogTelegram("Switch counter data ", rcvdData, offset, SwitchCountersData.MessageLen);
Debug.WriteLine(s);
log.Debug(s);
offset += SwitchCountersData.MessageLen;
lastValidOffset = offset;
anyProcessed = true;
}
else if (DivTransitionData.FrameFitsData(rcvdData, cumulativeSums, rcvdBytesCount, offset))
{
/// Diverter transition data received
divTransitionData = DivTransitionData.GetData(rcvdData, offset);
string s = Telegram.LogTelegram("Diverter transition data ", rcvdData, offset, DivTransitionData.MessageLen);
Debug.WriteLine(s);
log.Debug(s);
offset += DivTransitionData.MessageLen;
lastValidOffset = offset;
anyProcessed = true;
}
if (!anyProcessed) offset++;
}
while (offset < rcvdBytesCount);
/// Remove processed data from the receive buffer
for (int i = lastValidOffset; i < rcvdBytesCount; i++)
{
rcvdData[i - lastValidOffset] = rcvdData[i];
}
rcvdBytesCount -= lastValidOffset;
}
/// <summary>
/// Send data to the control board serial port
/// </summary>
public void RunDeviceAfter()
{
if (cbCfg.DebugLevel != DebugMode.Normal) return;
/// Make sure the serial port is open
if (serialPort == null || !serialPort.IsOpen)
{
/// Serial port is closed or does not exist => re-open the serial port
try
{
if (serialPort == null)
{
serialPort = new SerialPort(comPort, 9600, Parity.None, 8, StopBits.One);
}
serialPort.DtrEnable = true;
serialPort.WriteTimeout = 4000;
serialPort.ParityReplace = 0xFF;
serialPort.Open();
}
catch (Exception)
{
/// Re-opening serial port failed
return;
}
}
if (!outputsInitialized || Data.Vystupy != (ulong)benchModelRoute || (StateMachine.Time - lastStateMachineTime) >= 15)
{
scheduledActionQueue.Enqueue(Action.ChangeRoute((ulong)benchModelRoute));
log.InfoFormat("Enqueue( ChangeRoute(route={0:X}) )", benchModelRoute);
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
var combinedAction = Action.FetchNonConflictingActions(scheduledActionQueue);
byte[] outData = OutputMessage.GetMessage(combinedAction, config);
if (outData != null && outData.Length > 0)
{
serialPort.Write(outData, 0, outData.Length);
string s = Telegram.LogTelegram("Sent: ", outData);
Debug.WriteLine(s);
log.Debug(s);
}
ProcessData.UpdateMeasuredValuesAndSetpoints();
TBF.UiBridge.Bridge.OnStateMachineTick(this, new TBF.UiBridge.StateMachineTickEventArgs(Route, ProcessData.MsrmntAvailableFlags,
ProcessData.MeasuredValues,
ProcessData.AltStrings,
ProcessData.Setpoints));
}
public void StopDevice()
{
if (serialPort != null) serialPort.Close();
serialPort = null;
}
public void StopDevice2()
{
}
#endregion
#region IControlBoard interface
///
/// Interface functions
///
public void ClearProcessValues() { }
public double TestTimeWM(int wmNr1)
{
if (wmNr1 >= 1 && wmNr1 < Data.ImpulseTime.Length)
{
return Data.ImpulseTime[wmNr1];
}
return 0;
}
public int RefPulsesWM(int wmNr1)
{
if (wmNr1 >= 1 && wmNr1 < Data.EtPulses.Length)
{
return Data.EtPulses[wmNr1];
}
return 0;
}
public int PulsesWM(int wmNr1)
{
if (wmNr1 >= 1 && wmNr1 < Data.WMeterPuls.Length)
{
return Data.WMeterPuls[wmNr1];
}
return 0;
}
public UInt128 SetValves(UInt128 valvesToOpen, UInt128 valvesToClose, IList<BuiltIn.ValveEx.Valve> extendedValves)
{
UInt128 oldRoute = benchModelRoute;
benchModelRoute = (((benchModelRoute ^ valvesToInvert) | valvesToOpen) & (~valvesToClose)) ^ valvesToInvert;
foreach (var extV in extendedValves) extV.UpdateRoute(ref benchModelRoute);
log.DebugFormat("SetValves(x,x), new route = {0}", Utils.RouteToStr(benchModelRoute));
UInt128 retVal = oldRoute ^ benchModelRoute;
log.DebugFormat("{0}.SetValves({1}, {2}) returns {3}", Name, valvesToOpen.ToString("X"), valvesToClose.ToString("X"), retVal.ToString("X"));
return retVal;
}
///
/// Operations
///
public IOperation SetFlowOp(double flowLimLo, double flowLimHi, TBF.Boxes.DoubleBox measuredFlow, int timeout)
{
return Devices.RegValve.SetFlowOp(Devices.FlowMeter,flowLimLo, flowLimHi, measuredFlow, timeout);
}
public IOperation StopFlowRegulationOp()
{
//scheduledActionQueue.Enqueue(Action.Stop());
return null;
}
/// <summary>
/// Start a test, Events: MeasurementStarted
/// </summary>
/// <param name="iParam">Int32 parameter 1</param>
/// <param name="dParam1">Double parameter 1 (alternative lower flow limit)</param>
/// <param name="dParam2">Double parameter 2 (alternative upper flow limit)</param>
/// <returns>Operation</returns>
public IOperation StartTestOp(Test test, int iParam = 0, double dParam1 = 0, double dParam2 = 0)
{
return null;
}
public IOperation ReadDiverterTransitionOp(bool isStart, Sequences.Statistics plotter, int batchNr, string testName, int repetition)
{
return null;
}
/// <summary>
/// Returns operation with events: Event.MeasurementCompleted or Event.None
/// </summary>
/// <returns>Operation</returns>
public IOperation QueryMeasurementEndOp()
{
return null;
}
public IOperation StopPreviousOp()
{
return null;
}
#endregion
#region IValveControl interface
/// <summary>
/// Open/close one valve.
/// Events: ValvesSet, ValvesBusy, Error
/// </summary>
/// <param name="valveToOpen">Valve to be opened</param>
/// <param name="valveToClose">Valve to be closed</param>
/// <returns>Newly created operation instance reference casted to IOperaton</returns>
public IOperation SetValvesOp(IValve valveOpen, IValve valveClose)
{
return new BuiltIn.SetValvesOp(this, valveOpen, valveClose);
}
/// <summary>
/// Open/close multiple valves.
/// Events: ValvesSet, ValvesBusy, Error
/// </summary>
/// <param name="valvesToOpen">A list of valves to be opened</param>
/// <param name="valvesToClose">A list of valves to be closed</param>
/// <returns>Newly created operation instance reference casted to IOperaton</returns>
public IOperation SetValvesOp(IList<IValve> valvesOpen, IList<IValve> valvesClose)
{
return new BuiltIn.SetValvesOp(this, valvesOpen, valvesClose);
}
/// <summary>
/// Open start/stop valve.
/// Events: ValvesSet, ValvesBusy, Error
/// </summary>
/// <returns>Created operation</returns>
public IOperation OpenStartValveOp()
{
return new BuiltIn.SetValvesOp(this, true, devices);
}
/// <summary>
/// Close start/stop valve.
/// Events: ValvesSet, ValvesBusy, Error
/// </summary>
/// <returns>Created operation</returns>
public IOperation CloseStartValveOp()
{
return new BuiltIn.SetValvesOp(this, false, devices);
}
#endregion
public void MeasureFlow(int flowMeterId)
{
scheduledActionQueue.Enqueue(Action.MeasureFlow(flowMeterId));
log.InfoFormat("Enqueue( MeasureFlow(fm={0}) )", flowMeterId);
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
public void SetFlow(int flowMeterId, int regVId, double freqLo, double freqHi)
{
scheduledActionQueue.Enqueue(Action.SetFlow(flowMeterId, regVId, freqLo, freqHi, Convert.ToInt32(Math.Round(Devices.PidCoef)), 0, 200));
log.InfoFormat("Enqueue( SetFlow(fm={0}, rv={1}, fLo={2}, fHi={3}, pid={4}) )", flowMeterId, regVId, freqLo, freqHi, Convert.ToInt32(Math.Round(Devices.PidCoef)));
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
/// <summary>
/// Perform incremental movement of a regulation valve
/// </summary>
/// <param name="rvId">Reg. valve ID</param>
/// <param name="time">Time in seconds, positive value opens the reg. valve</param>
public void RegVlvIncrMove(int regVId, double time)
{
scheduledActionQueue.Enqueue(Action.RegVlvIncrMove(regVId, time));
log.InfoFormat("Enqueue( RegVlvIncrMove(rv={0}, time={1}) )", regVId, time);
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
/// <summary>
/// Move a regulation valve to specfied position
/// </summary>
/// <param name="rvId">Reg. valve ID</param>
/// <param name="position">Reg. valve position (0 .. 1.0)</param>
public void RegVlvMoveToPos(int regVId, double positionLo, double positionHi = -1)
{
scheduledActionQueue.Enqueue(Action.RegVlvMoveToPos(regVId, positionLo, positionHi));
log.InfoFormat("Enqueue( RegVlvMoveToPos(rv={0}, positionLo={1}, positionHi={2}) )", regVId, positionLo, positionHi);
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
public void DiverterToTank(int divNr1)
{
scheduledActionQueue.Enqueue(Action.SwitchDiverter(divNr1, true));
log.InfoFormat("Enqueue( SwitchDiverter(div={0}, true) )", divNr1);
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
public void DiverterToSink(int divNr1)
{
scheduledActionQueue.Enqueue(Action.SwitchDiverter(divNr1, false));
log.InfoFormat("Enqueue( SwitchDiverter(div={0}, false) )", divNr1);
foreach (var a in scheduledActionQueue) log.DebugFormat(" {0}", a);
}
public void ReadDiverterTransition(int divNr1)
{
/// TODO
}
}
}