using System; using Microsoft.SPOT; using System.Text; using System.Threading; using System.IO.Ports; using Microsoft.SPOT.Hardware; using Microsoft.SPOT.IO; //using GHI.Hardware.EMX; using GHI.Hardware.G400; namespace CPF { class SBE41 { private static SerialPort SBE41Port = new SerialPort("COM4", 9600, Parity.None, 8, StopBits.One); private static Timer SBE41Timer = null; //Next 2 lines are for the spider // private static OutputPort DR = new OutputPort(Pin.IO15, false); // private static OutputPort Mode = new OutputPort(Pin.IO46, true); //These 2 lines are for the Raptor private static OutputPort DR = new OutputPort(Pin.PB0, false); private static OutputPort Mode = new OutputPort(Pin.PB5, true); private static OutputPort serialTX; private static byte[] pressureBytes = new byte[16]; private static byte[] portReadBytes = new byte[1024]; private static byte[] messageBytes = new byte[240]; private static byte[] header = UTF8Encoding.UTF8.GetBytes("CTD "); private static byte[] message = new byte[256]; private static byte space = 32; private static byte CR = 13; private static byte LF = 10; private static byte comma = 44; private static byte STX = LF; private static byte token = space; private static int pressureBytesIndex = 0; private static int portBytesToRead; //private static int messageBytesIndex = -1; private static int messageNum; private static int timerState = 0; private static int parserI = 0; private static int inArrayLength = 0; private static Object SBE41Lock = new Object(); private static StringBuilder sbTemp = new StringBuilder(256); //private static TimeSpan startTime; //private static TimeSpan endTime; //private static TimeSpan diffTime; private static StructQueue.qStruct qS = new StructQueue.qStruct(); public static void Init() { SBE41Port.DataReceived += new SerialDataReceivedEventHandler(portDataReceivedHandler); dt = configFile.SBE41SamplePeriod / 1000; //dt needs to be in seconds SBE41SamplePeriod is in milliSeconds //Initialize(); SBE41Port.ReadTimeout = 5000; SBE41Port.WriteTimeout = 20; SBE41Port.Open(); qS.byteArray = new byte[StructQueue.qRecordWidth]; //ReleaseWakeUp(); DR.Write(false); Mode.Write(true); SBE41Timer = new Timer(new TimerCallback(SBE41TimerCallback), null, 500, configFile.SBE41SamplePeriod); //Stop the timer by setting SBE41Timer = null } private static void SBE41TimerCallback(object state) { switch (timerState) { case 0: SBE41Port.Close(); serialTX = new OutputPort(Pin.PA7, true); DR.Write(true); Mode.Write(false); // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State 0 DR = true SM = false TX = true"); timerState = 1; SBE41Timer.Change(0, 50); break; case 1: DR.Write(false); // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State 1 DR = false SM = false TX = true"); timerState = 2; SBE41Timer.Change(0, 150); break; case 2: serialTX.Write(false); // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State 2 DR = false SM = false TX = false"); timerState = 3; SBE41Timer.Change(0, 150); break; case 3: Mode.Write(true); serialTX.Write(true); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State 3 DR = false SM = true TX = true"); timerState = 4; SBE41Timer.Change(0, 50); break; case 4: serialTX.Dispose(); SBE41Port.Open(); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State 3 DR = false SM = true TX = true"); timerState = 0; SBE41Timer.Change(0, (configFile.SBE41SamplePeriod - 400)); break; } } public static double pressureTimestamp; private static int readCounter = 0; private static int messageBytesLength = 0; private static DateTime timeStamp; private static Program.CPFStates state; private static byte ETX = LF; private static void portDataReceivedHandler(object sender, SerialDataReceivedEventArgs e) { // startTime = Utility.GetMachineTime(); //TODO might be able to do this in a way that if we start in the middle of a message, // we can resynch so subsequent reads get the whole message. portBytesToRead = SBE41Port.BytesToRead; try { SBE41Port.Read(portReadBytes, 0, portBytesToRead); Array.Copy(portReadBytes, 0, messageBytes, messageBytesLength, portBytesToRead); messageBytesLength = messageBytesLength + portBytesToRead; readCounter = readCounter + 1; //sbInArray.Clear(); //sbInArray.Append(UTF8Encoding.UTF8.GetChars(portReadBytes)); // Debug.Print(sbInArray.ToString()); } catch { Debug.Print("SBE41 Read Exception"); //TODO do something smarter here } if (messageBytes[messageBytesLength - 1] == ETX) //process the message when we get the ETX character { if (messageBytesLength >= 8) { pressureTimestamp = (double)Utility.GetMachineTime().Ticks / (double)TimeSpan.TicksPerSecond; sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(messageBytes)); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + // " SBE41 Message = " + sbTemp.ToString()); timeStamp = DateTime.Now; state = Program.CPFState; messageNum = messageNum + 1; //Array.Clear(message, 0, message.Length); //Array.Copy(header, message, header.Length); //Array.Copy(messageBytes, 0, message, header.Length, messageBytesLength); //lock (SBE41Lock) { Program.MessageQueue.Enqueue(message); } Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(messageBytes, qS.byteArray, messageBytesLength); qS.timeStamp = timeStamp; qS.state = state; qS.qRecordType = StructQueue.QRecordType.SBE41; lock (SBE41Lock) { Program.sQueue.Enqueue(qS); } Array.Clear(portReadBytes, 0, portReadBytes.Length); Array.Clear(messageBytes, 0, messageBytes.Length); messageBytesLength = 0; } else { messageBytesLength = 0; Array.Clear(messageBytes, 0, messageBytes.Length); } } //endTime = Utility.GetMachineTime(); //diffTime = endTime - startTime; //Debug.Print("SBE41 Event Handler time diff = " + diffTime.ToString()); } private static double parsedPressure = double.NaN; public static double temperature = double.NaN; public static double salinity = double.NaN; private static StringBuilder sbInArray = new StringBuilder(64); public static double parse(byte[] inArray) { inArrayLength = Array.IndexOf(inArray, 0); parserI = Array.IndexOf(inArray, token); while ((inArray[parserI] != CR) && (inArray[parserI] != comma) && (parserI < inArrayLength)) { pressureBytes[pressureBytesIndex] = inArray[parserI]; pressureBytesIndex = pressureBytesIndex + 1; parserI = parserI + 1; } sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(pressureBytes)); parsedPressure = double.Parse(sbTemp.ToString()); //Debug.Print(parsedPressure.ToString("f2")); Array.Clear(pressureBytes, 0, pressureBytes.Length); pressureBytesIndex = 0; return (parsedPressure); } 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 dt; 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 SBE41LPFResults; 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 SBE41LPFResults.LPFPressure = pressureSample; accum = 0.0; velFilterDelayLine[0] = pressureSample; for (filterI = 0; filterI < numCoef; filterI++) { accum = accum + (filterCoef[filterI] * velFilterDelayLine[filterI]); } SBE41LPFResults.LPFVel = -1.0 * accum / dt; //-1.0 because pressure increases positively down but velocity in the down direction needs to be negative SBE41LPFResults.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] = SBE41LPFResults.LPFVel; initializeFilter = false; } } accum = 0.0; accFilterDelayLine[0] = SBE41LPFResults.LPFVel; for (filterI = 0; filterI < numCoef; filterI++) { accum = accum + (filterCoef[filterI] * accFilterDelayLine[filterI]); } SBE41LPFResults.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)); SBE41LPFResults.pressureAverage = pressureAverage; SBE41LPFResults.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 (SBE41LPFResults); } } }