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; namespace miniCPF { class SBE41 { private static SerialPort SBE41Port = new SerialPort("COM4", 9600, Parity.None, 8, StopBits.One); private static Timer SBE41Timer = null; private static OutputPort DR = new OutputPort(Pin.IO15, false); private static OutputPort SPM = new OutputPort(Pin.IO46, true); private static OutputPort serialTX; private static byte[] pressureBytes = new byte[16]; private static byte[] portReadBytes = new byte[1024]; private static byte[] portInBytes = 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 ETX = CR; private static byte token = space; private static int pressureBytesIndex = 0; private static int portBytesToRead; private static int portInBytesIndex = -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; 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(); //ReleaseWakeUp(); DR.Write(false); SPM.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.IO6, true); DR.Write(true); SPM.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: SPM.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; } } 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); } catch { Debug.Print("SBE41 Read Exception"); //TODO do something smarter here } for (int i = 0; i < portBytesToRead; i++) { if (portInBytesIndex == -1) //Check to make sure first character is = LF { if (portReadBytes[i] == STX) //only move on to byte index 1 after we get the STX character { portInBytesIndex = portInBytesIndex + 1; } } else { portInBytes[portInBytesIndex] = portReadBytes[i]; if (portInBytes[portInBytesIndex] == ETX) //process the message when we get the ETX character { messageNum = messageNum + 1; Array.Clear(message, 0, message.Length); Array.Copy(header, message, header.Length); Array.Copy(portInBytes, 1, message, header.Length, portInBytesIndex); lock (SBE41Lock) { Program.MessageQueue.Enqueue(message); } Array.Clear(portReadBytes, 0, portReadBytes.Length); Array.Clear(portInBytes, 0, portInBytes.Length); portInBytesIndex = -1; } else { portInBytesIndex = portInBytesIndex + 1; } } } //endTime = Utility.GetMachineTime(); //diffTime = endTime - startTime; //Debug.Print("SBE41 Event Handler time diff = " + diffTime.ToString()); } 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)); Array.Clear(pressureBytes, 0, pressureBytes.Length); pressureBytesIndex = 0; return (double.Parse(sbTemp.ToString())); } private static bool initializeFilter = true; 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[] velFilterDelayLine = new double[numCoef]; private static double[] accFilterDelayLine = new double[numCoef]; private static int filterI; public struct LPFResults { public double LPFPressure; public double LPFVel; public double LPFAcc; public double diffVel; } private static LPFResults SBE41LPFResults; public static LPFResults LPFPressure(double pressureSample) { if (initializeFilter) { for (filterI = 0; filterI < numCoef; filterI++) { velFilterDelayLine[filterI] = pressureSample; } } //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 down but velocity down needs to be negative SBE41LPFResults.diffVel = (velFilterDelayLine[1] - velFilterDelayLine[0]) / dt; 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")); for (filterI = numCoef - 2; filterI >= 0; filterI--) { velFilterDelayLine[filterI + 1] = velFilterDelayLine[filterI]; accFilterDelayLine[filterI + 1] = accFilterDelayLine[filterI]; } //TODO Check for bottom contact if (SBE41LPFResults.LPFAcc > configFile.hitBottomAccThreshold) { Elmo.stopMotor(); Program.hitBottom = true; } else { Program.hitBottom = false; } return (SBE41LPFResults); } } }