using System; using System.Threading; using System.IO.Ports; using System.Text; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; namespace miniCPF { public static class DGH { private static SerialPort DGHPort = new SerialPort("COM3", 38400, Parity.None, 8, StopBits.One); private static Object DGHlock = new Object(); private static Timer DGHTimer = null; private static StructQueue.qStruct qS = new StructQueue.qStruct(); private static StringBuilder sbTemp = new StringBuilder(4096); private static StringBuilder sbCharmsg = new StringBuilder(16); private static AutoResetEvent DGHReadEvent = new AutoResetEvent(false); private static DateTime timeStamp; private static Program.CPFStates cpfState; private static char DGHTerminatingChar = '\r'; private static string DGHRxString = ""; private static string DGHReadCmd = "$1RD\r"; //$1RD terminated with CR is the command to read data from the DGH private static byte[] txBytes = new byte[64]; private static byte[] portBytes = new byte[512]; private static byte[] msgBytes = new byte[512]; private static int msgIndex = 0; private static int numPortBytes = 0; private static int startIndex = 0; private static int endIndex = 0; private static int measurementIndex = 0; private static StringBuilder sbLogMsg = new StringBuilder(StructQueue.qRecordWidth); private static StringBuilder sbLogPrefix = new StringBuilder(StructQueue.qRecordWidth); public static double pressure = 0; private const double offset = 3.9; private const double ma2bar = (15.0 / 1.45038) / 16.0; //FILTER variables private static double dt; public static void init() { dt = configFile.DGHSamplePeriod/1000; DGHPort.ReadTimeout = 1000; DGHPort.Open(); DGHPort.DiscardInBuffer(); DGHPort.DiscardOutBuffer(); DGHPort.DataReceived += new SerialDataReceivedEventHandler(DGHPort_DataReceived); //DGHPort.ErrorReceived += new SerialErrorReceivedEventHandler(DGHPort_ErrorReceived); DGHTimer = new Timer(new TimerCallback(samplePressure), null, 500, configFile.DGHSamplePeriod); qS.byteArray = new byte[StructQueue.qRecordWidth]; } public static void samplePressure(object sender) { Array.Clear(txBytes, 0, txBytes.Length); txBytes = Encoding.UTF8.GetBytes(DGHReadCmd); DGHPort.Write(txBytes, 0, txBytes.Length); } static void DGHPort_DataReceived(object sender, SerialDataReceivedEventArgs e) { numPortBytes = DGHPort.BytesToRead; try { DGHPort.Read(portBytes, 0, numPortBytes); } catch { Debug.Print("Optode Port Read Exception"); //Probably should do something smarter here } for (int i = 0; i < numPortBytes; i++) { if (portBytes[i] != DGHTerminatingChar) { msgBytes[msgIndex] = portBytes[i]; msgIndex = msgIndex + 1; } else { Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(msgBytes, qS.byteArray, (msgIndex - 1)); qS.timeStamp = DateTime.Now; qS.state = Program.CPFState; qS.qRecordType = StructQueue.QRecordType.DGH; lock (DGHlock) { Program.sQueue.Enqueue(qS); } msgIndex = 0; } } } public static double parsePressureSample(byte[] inByteArray) { sbCharmsg.Clear(); sbCharmsg.Append(UTF8Encoding.UTF8.GetChars(inByteArray, 3, 8)); pressure = double.Parse(sbCharmsg.ToString()); //Debug.Print("output of DGH without first + char: " + pressure.ToString()); //DGHRxString = DGHRxString.Substring(2, 8); pressure = (pressure - offset) * ma2bar + 0.2; //0.2 addition is quick and dirty correction for error found in first TT ops return (pressure); } //////// //////// LPF CODE DUPLICATED FROM SBE41.cs //////// private static bool initializeFilter = true; //Designed with Matlab fdesign.differentiator('N,Fp,Fst,Ast', 6, .4, .45, 20) private static double[] filterCoef = { 0.090171043318574, 0.151330004225777, 0.093823854938812, 0.0, -0.093823854938812, -0.151330004225777, -0.090171043318574}; //Designed with Matlab fdesign.differentiator('N,Fp,Fst',20,.25,.45) //private static double[] filterCoef = { -0.010316320303577, 0.002862341441897, 0.026430189271868, 0.007331989949475, -0.025727381182904, // -0.054710187198367, -0.007726535812123, 0.094047649266387, 0.174529230732090, 0.139815454232672, // 0.000000000000000, -0.139815454232672, -0.174529230732090, -0.094047649266387, 0.007726535812123, // 0.054710187198367, 0.025727381182904, -0.007331989949475, -0.026430189271868, -0.002862341441897, // 0.010316320303577}; private static int numCoef = filterCoef.Length; private static double accum = 0.0; private static double x1 = 0, x2 = 0, y1 = 0, y2 = 0, sx1 = 0, sx2 = 0, pressureAverage = 0, pressureVariance = 0; private static double[] velFilterDelayLine = new double[numCoef]; private static double[] accFilterDelayLine = new double[numCoef]; private static double[] x1DelayLine = new double[numCoef]; private static double[] x2DelayLine = new double[numCoef]; private static int filterI; public struct LPFResults { public double LPFPressure; public double LPFVel; public double LPFAcc; public double diffVel; public double pressureAverage; public double pressureVariance; } private static LPFResults DGHLPFResults; public static LPFResults LPFPressure(double pressureSample) { if (initializeFilter) { sx1 = 0.0; sx2 = 0.0; for (filterI = 0; filterI < numCoef; filterI++) { velFilterDelayLine[filterI] = pressureSample; x1DelayLine[filterI] = pressureSample; x2DelayLine[filterI] = pressureSample * pressureSample; sx1 = sx1 + x1DelayLine[filterI]; sx2 = sx2 + x2DelayLine[filterI]; } y1 = x1DelayLine[numCoef - 1]; y2 = x2DelayLine[numCoef - 1]; } //TODO Eventually need a LPF for pressure DGHLPFResults.LPFPressure = pressureSample; accum = 0.0; velFilterDelayLine[0] = pressureSample; for (filterI = 0; filterI < numCoef; filterI++) { accum = accum + (filterCoef[filterI] * velFilterDelayLine[filterI]); } DGHLPFResults.LPFVel = -1.0 * accum / dt; //-1.0 because pressure increases positively down but velocity in the down direction needs to be negative DGHLPFResults.diffVel = (velFilterDelayLine[1] - velFilterDelayLine[0]) / dt; //Don't need the -1.0 because we're subtracting ( (t-1) - t0 ) if (initializeFilter) { for (filterI = 0; filterI < numCoef; filterI++) { accFilterDelayLine[filterI] = DGHLPFResults.LPFVel; initializeFilter = false; } } accum = 0.0; accFilterDelayLine[0] = DGHLPFResults.LPFVel; for (filterI = 0; filterI < numCoef; filterI++) { accum = accum + (filterCoef[filterI] * accFilterDelayLine[filterI]); } DGHLPFResults.LPFAcc = accum / dt; //Debug.Print("Pressure = " + pressureSample.ToString("F6") + // " Difference Velocity = " + platformDiffVel.ToString("f4") + // " Filtered Velocity = " + platformLPFVel.ToString("f4") + // " Filtered Accel = " + platformLPFAcc.ToString("f4")); //Calculate sliding window recursive depth variance for bottom detector x1 = pressureSample; x2 = x1 * x1; sx1 = sx1 + x1 - y1; sx2 = sx2 + x2 - y2; pressureAverage = sx1 / numCoef; pressureVariance = (numCoef * sx2 - (sx1 * sx1)) / (numCoef * (numCoef - 1)); DGHLPFResults.pressureAverage = pressureAverage; DGHLPFResults.pressureVariance = pressureVariance; x1DelayLine[0] = x1; x2DelayLine[0] = x2; y1 = x1DelayLine[numCoef - 1]; y2 = x2DelayLine[numCoef - 1]; //Debug.Print("x1 Delay Line " + x1DelayLine[0].ToString("F5") + " " + x1DelayLine[1].ToString("F5") + " " + x1DelayLine[2].ToString("F5") + " " + // x1DelayLine[3].ToString("F5") + " " + x1DelayLine[4].ToString("F3") + " " + x1DelayLine[5].ToString("F5") + " " + x1DelayLine[6].ToString("F5")); //Debug.Print("x2 Delay Line " + x2DelayLine[0].ToString("F5") + " " + x2DelayLine[1].ToString("F5") + " " + x2DelayLine[2].ToString("F5") + " " + // x2DelayLine[3].ToString("F5") + " " + x2DelayLine[4].ToString("F5") + " " + x2DelayLine[5].ToString("F5") + " " + x2DelayLine[6].ToString("F5")); //Debug.Print("Avg = " + pressureAverage.ToString("F3") + " Var = " + pressureVariance.ToString("f5")); for (filterI = numCoef - 2; filterI >= 0; filterI--) { velFilterDelayLine[filterI + 1] = velFilterDelayLine[filterI]; accFilterDelayLine[filterI + 1] = accFilterDelayLine[filterI]; x1DelayLine[filterI + 1] = x1DelayLine[filterI]; x2DelayLine[filterI + 1] = x2DelayLine[filterI]; } return (DGHLPFResults); } } }