//#define simPressure using System; using System.Text; using System.Threading; using System.IO.Ports; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; using Microsoft.SPOT.IO; using GHI.Pins; namespace CPF { public static class SBE41 { //RAPTOR private static SerialPort SBE41Port = new SerialPort("COM4", 9600, Parity.None, 8, StopBits.One); private static Timer SBE41Timer = null; public static bool stopSBE41Timer = false; public static bool SBE41TimerStopped = true; //RAPTOR private static OutputPort DR = new OutputPort(G400.PB0, false); private static OutputPort Mode = new OutputPort(G400.PB5, true); private static OutputPort serialTX; private static byte[] pressureBytes = new byte[16]; private static byte[] header = UTF8Encoding.UTF8.GetBytes("CTD "); private static byte[] message = new byte[256]; private static byte space = 32; private static byte LF = 10; private static byte STX = LF; private static byte token = space; //private static int messageBytesIndex = -1; private static int messageNum; private static int timerState = 0; private static StringBuilder sbTemp = new StringBuilder(512); //private static TimeSpan startTime; //private static TimeSpan endTime; //private static TimeSpan diffTime; private static StructQueue.qStruct qS = new StructQueue.qStruct(); private static readonly StringBuilder startProfileCommand = new StringBuilder("startprofile5\r"); private static byte[] startProfileCommandBytes = new byte[startProfileCommand.Length]; public static void init() { Array.Clear(messageBytes, 0, messageBytes.Length); Array.Clear(bytesRead, 0, bytesRead.Length); Array.Clear(startProfileCommandBytes, 0, startProfileCommandBytes.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(startProfileCommand.ToString()), startProfileCommandBytes, startProfileCommand.Length); qS.byteArray = new byte[StructQueue.byteArrayWidth]; setCommandMode(); SBE41Port.DataReceived += new SerialDataReceivedEventHandler(portDataReceivedHandler); ctdSamplePeriod = configFile.SBE41SamplePeriod / 1000; // needs to be in seconds SBE41SamplePeriod is in milliSeconds //Initialize(); SBE41Port.ReadTimeout = 5000; SBE41Port.WriteTimeout = 20; SBE41Port.Open(); SBE41Port.DiscardInBuffer(); SBE41Port.DiscardOutBuffer(); //Setup SBE41 Options //First make sure it is awake sendCRLF(); //Opening the port may force a PTS sample so wait for it here Thread.Sleep(5000); sendStopProfile(); Thread.Sleep(1000); setPressureCutoff(); Thread.Sleep(1000); sendOutputPTSYesCommand(); Thread.Sleep(1000); setTSWait(); Thread.Sleep(1000); setBinParameters(); Thread.Sleep(1000); #if simPressure getSimPressure(); #endif } private static byte[] dontAutoBinAvg = UTF8Encoding.UTF8.GetBytes("autobinavg=n\r"); private static byte[] doAutoBinAvg = UTF8Encoding.UTF8.GetBytes("autobinavg=y\r"); private static byte[] topBinInterval = UTF8Encoding.UTF8.GetBytes("top_bin_interval=2\r"); private static byte[] topBinSize = UTF8Encoding.UTF8.GetBytes("top_bin_size=2\r"); private static byte[] topBinMax = UTF8Encoding.UTF8.GetBytes("top_bin_max=10\r"); private static byte[] middleBinInterval = UTF8Encoding.UTF8.GetBytes("middle_bin_interval=2\r"); private static byte[] middleBinSize = UTF8Encoding.UTF8.GetBytes("middle_bin_size=2\r"); private static byte[] middleBinMax = UTF8Encoding.UTF8.GetBytes("middle_bin_max=20\r"); private static byte[] bottomBinInterval = UTF8Encoding.UTF8.GetBytes("bottom_bin_interval=2\r"); private static byte[] bottomBinSize = UTF8Encoding.UTF8.GetBytes("bottom_bin_size=2\r"); private static byte[] dontIncludeTransitionBin = UTF8Encoding.UTF8.GetBytes("includetransitionbin=n\r"); private static byte[] doIncludeNumScansPerBin = UTF8Encoding.UTF8.GetBytes("includenbin=y\r"); private static void setBinParameters() { EngrLogger.writeToColumns("Sending 11 bin setup commands"); SBE41Port.Write(dontAutoBinAvg, 0, dontAutoBinAvg.Length); Thread.Sleep(500); SBE41Port.Write(topBinInterval, 0, topBinInterval.Length); Thread.Sleep(500); SBE41Port.Write(topBinSize, 0, topBinSize.Length); Thread.Sleep(500); SBE41Port.Write(topBinMax, 0, topBinMax.Length); Thread.Sleep(500); SBE41Port.Write(middleBinInterval, 0, middleBinInterval.Length); Thread.Sleep(500); SBE41Port.Write(middleBinSize, 0, middleBinSize.Length); Thread.Sleep(500); SBE41Port.Write(middleBinMax, 0, middleBinMax.Length); Thread.Sleep(500); SBE41Port.Write(bottomBinInterval, 0, bottomBinInterval.Length); Thread.Sleep(500); SBE41Port.Write(bottomBinSize, 0, bottomBinSize.Length); Thread.Sleep(500); SBE41Port.Write(dontIncludeTransitionBin, 0, dontIncludeTransitionBin.Length); Thread.Sleep(500); SBE41Port.Write(doIncludeNumScansPerBin, 0, doIncludeNumScansPerBin.Length); Thread.Sleep(500); } public static void startSBE41CommandModeTimer() { //Start CTD command mode sample timer SBE41Timer = new Timer(new TimerCallback(SBE41CommandModeTimerCallback), null, 500, configFile.SBE41SamplePeriod); SBE41TimerStopped = false; } public static void stopTimer() { stopSBE41Timer = true; } public static void restartTimer(int dueTime, int period) { stopSBE41Timer = false; SBE41Timer.Change(dueTime, period); SBE41TimerStopped = false; } private static byte[] fpCommand = new byte[3] { 102, 112, 13 }; //102 = f, 112 = p, 13 = CR public static void sendFastPressure() { SBE41Port.Write(fpCommand, 0, fpCommand.Length); } private static byte[] dsCommand = new byte[3] { 100, 115, 13 }; //100 = d, 115 = s, 13 = CR public static void sendDS() { EngrLogger.writeToColumns("Sending ds command"); SBE41Port.Write(dsCommand, 0, dsCommand.Length); } private static byte[] dcCommand = new byte[3] { 100, 99, 13 }; //100 = d, 99 = c, 13 = CR public static void sendDC() { EngrLogger.writeToColumns("Sending dc command"); SBE41Port.Write(dcCommand, 0, dcCommand.Length); } private static byte[] qsCommand = new byte[3] { 113, 115, 13 }; //113 = q, 115 = s, 13 = CR public static void sendQS() { SBE41Port.Write(qsCommand, 0, qsCommand.Length); } public static void resetSBE41() { sendQSR(); Thread.Sleep(2000); sendCRLF(); } private static byte[] qsrCommand = new byte[4] { 113, 115, 114, 13 }; //113 = q, 115 = s, 114 = r, 13 = CR public static void sendQSR() { EngrLogger.writeToColumns("Sending qsr command"); SBE41Port.Write(qsrCommand, 0, qsrCommand.Length); } //Note: this is just the header for the pcutoff command. private static StringBuilder pcutoff = new StringBuilder("pcutoff="); private static void setPressureCutoff() { pcutoff.Append(configFile.cpPressureCutoff.ToString()); pcutoff.Append("\r"); EngrLogger.writeToColumns(pcutoff.ToString()); SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(pcutoff.ToString()), 0, pcutoff.Length); } private static StringBuilder qs = new StringBuilder("qs"); private static void sendQSCommand() { SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(qs.ToString()), 0, qs.Length); } private static byte[] outputptsYes = UTF8Encoding.UTF8.GetBytes("outputpts=y\r"); private static void sendOutputPTSYesCommand() { EngrLogger.writeToColumns("Sending outputPTSyes"); SBE41Port.Write(outputptsYes, 0, outputptsYes.Length); } private static byte[] tsWait = UTF8Encoding.UTF8.GetBytes("tswait=2\r"); private static void setTSWait() { EngrLogger.writeToColumns("Sending tsWait"); SBE41Port.Write(tsWait, 0, tsWait.Length); } private static StringBuilder outputptsNo = new StringBuilder("outputpts=n\r"); private static void sendOutputPTSNoCommand() { EngrLogger.writeToColumns("Sending outputPTSNo"); SBE41Port.Write(UTF8Encoding.UTF8.GetBytes(outputptsNo.ToString()), 0, outputptsNo.Length); } public static void startProfile() { SBE41Port.Write(startProfileCommandBytes, 0, startProfileCommandBytes.Length); } private static byte[] stopProfileCommand = UTF8Encoding.UTF8.GetBytes("stopprofile\r"); public static void sendStopProfile() { EngrLogger.writeToColumns("Sending SBE41 stopProfile"); SBE41Port.Write(stopProfileCommand, 0, stopProfileCommand.Length); } private static byte[] ddCommand = new byte[3] { 100, 100, 13 }; //100 = d, 13 = CR public static void dumpData() { EngrLogger.writeToColumns("Sending dd command"); SBE41Port.Write(ddCommand, 0, ddCommand.Length); } private static byte[] daCommand = UTF8Encoding.UTF8.GetBytes("da\r"); public static void dumpBinnedData() { EngrLogger.writeToColumns("Sending da command"); SBE41Port.Write(daCommand, 0, daCommand.Length); } private static byte[] dahCommand = UTF8Encoding.UTF8.GetBytes("dah\r"); public static void dumpBinnedDataHex() { EngrLogger.writeToColumns("Sending dah command"); SBE41Port.Write(dahCommand, 0, dahCommand.Length); } private static byte[] binaverageCommand = UTF8Encoding.UTF8.GetBytes("binaverage\r"); public static void sendBinAverage() { EngrLogger.writeToColumns("Sending binaverage"); SBE41Port.Write(binaverageCommand, 0, binaverageCommand.Length); } private static byte[] ptsCommand = new byte[4] { 112, 116, 115, 13 }; //112 = p, 116 = t, 115 = s, 13 = CR public static void sendPTS() { SBE41Port.Write(ptsCommand, 0, ptsCommand.Length); } private static byte[] crlfCommand = new byte[2] { 10, 13 }; public static void sendCRLF() { //Note: this send command doesn't log to the engrLogger since it is called every time a pressure sample is requested //which clogs the engr log and the bluetooth console. //If you need to see this command in the engr log, issue an EngrLogger.WriteToColumns before you call this method. SBE41Port.Write(crlfCommand, 0, crlfCommand.Length); } public static void exitCommandMode() { sendQSR(); Thread.Sleep(1000); DR.Write(false); Mode.Write(true); Thread.Sleep(1000); } public static void setCommandMode() { //Debug.Print("Setting Command Mode"); Mode.Write(false); Thread.Sleep(1); DR.Write(true); } //This version from SBE41Gen2 test program based on Dana's code public static bool sbe41SamplePTS = false; private static bool onePTSSample = false; //sendFastPressure(); // Thread.Sleep(900); // Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending CR/LF to wake CTD after fp command"); // sendCRLF(); // Thread.Sleep(500); private static void SBE41CommandModeTimerCallback(object state) { if (onePTSSample) { sendPTS(); onePTSSample = false; if (stopSBE41Timer) { SBE41Timer.Change(-1, -1); SBE41TimerStopped = true; } else { SBE41Timer.Change(0, 10000); SBE41TimerStopped = false; } } else { switch (timerState) { case 0: // Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending fp command number " + i.ToString()); sendFastPressure(); timerState = 1; SBE41Timer.Change(0, 1000); break; case 1: // Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending CR/LF to wake CTD after fp command"); sendCRLF(); timerState = 2; SBE41Timer.Change(0, 500); break; case 2: timerState = 0; if (sbe41SamplePTS) { onePTSSample = true; sbe41SamplePTS = false; } if (stopSBE41Timer) { SBE41Timer.Change(-1, -1); SBE41TimerStopped = true; } else { SBE41Timer.Change(0, (configFile.SBE41SamplePeriod - 1500)); SBE41TimerStopped = false; } break; } } } private static void SBE41TimerCallback(object state) { //Add stop timer logic from command mode timer if we use this again if (onePTSSample) { switch (timerState) { case 0: //Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " PTS only case 0"); SBE41Port.Close(); //Debug.Print("Port Closed"); //RAPTOR Config serialTX = new OutputPort(G400.PA7, false); //SPIDER Config //serialTX = new OutputPort(EMX.IO29, false); Mode.Write(true); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 1; SBE41Timer.Change(0, 100); break; case 1: // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 2; DR.Write(true); SBE41Timer.Change(0, 50); break; case 2: DR.Write(false); // Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 3; SBE41Timer.Change(0, 500); break; case 3: Mode.Write(false); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 4; SBE41Timer.Change(0, 200); break; case 4: serialTX.Dispose(); SBE41Port.Open(); //Debug.Print("Port Open"); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 0; onePTSSample = false; SBE41Timer.Change(0, 10000); break; } } else { switch (timerState) { case 0: SBE41Port.Close(); //Debug.Print("Port Closed"); serialTX = new OutputPort(G400.PA7, false); //SPIDER Config //serialTX = new OutputPort(EMX.IO29, false); Mode.Write(true); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 1; SBE41Timer.Change(0, 100); break; case 1: DR.Write(true); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 2; SBE41Timer.Change(0, 50); break; case 2: DR.Write(false); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 3; SBE41Timer.Change(0, 50); break; case 3: Mode.Write(false); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 4; SBE41Timer.Change(0, 200); break; case 4: serialTX.Dispose(); SBE41Port.Open(); //Debug.Print("Port Open"); //Debug.Print(DateTime.Now + "." + DateTime.Now.TimeOfDay.Milliseconds.ToString() + " SBE41 State " + timerState.ToString()); timerState = 0; if (sbe41SamplePTS) { onePTSSample = true; sbe41SamplePTS = false; } SBE41Timer.Change(0, (configFile.SBE41SamplePeriod - 400)); break; } } } private static int bufferLength = 512; private static byte ETX = LF; private static byte[] bytesRead = new byte[bufferLength]; private static byte[] messageBytes = new byte[bufferLength]; private static int bytesToRead; private static int messageIndex = 0; private static void portDataReceivedHandler(object sender, SerialDataReceivedEventArgs e) { bytesToRead = SBE41Port.BytesToRead; try { SBE41Port.Read(bytesRead, 0, bytesToRead); } catch { Debug.Print("SBE41 Read Exception"); //TODO do something smarter here } for (int i = 0; i < bytesToRead; i++) { messageBytes[messageIndex] = bytesRead[i]; if (messageBytes[messageIndex] == LF) { qS.timeStamp = DateTime.Now; qS.state = Program.CurrentState; qS.errorCode = 1; //sbTemp.Clear(); //sbTemp.Append(UTF8Encoding.UTF8.GetChars(messageBytes, 0, messageIndex - 1)); //Debug.Print("SBE41 Message = " + sbTemp.ToString()); Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(messageBytes, qS.byteArray, messageIndex + 1); qS.qRecordType = StructQueue.QRecordType.SBE41; Program.sQueue.Enqueue(qS); messageNum = messageNum + 1; messageIndex = 0; Array.Clear(messageBytes, 0, messageBytes.Length); } else { //TODO need to check messageIndex against bufferLength in all drivers //Make sure messageIndex doesn't exceed bufferLength messageIndex = messageIndex + 1; if (messageIndex >= bufferLength - 2) { qS.timeStamp = DateTime.Now; qS.state = Program.CurrentState; qS.errorCode = -1; Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(messageBytes, qS.byteArray, messageIndex); qS.qRecordType = StructQueue.QRecordType.SBE41; Program.sQueue.Enqueue(qS); messageIndex = 0; Array.Clear(messageBytes, 0, messageBytes.Length); } } } } private static StringBuilder sbInArray = new StringBuilder(64); private static StringBuilder inSB = new StringBuilder(4096); private static StringBuilder sbParsePOnlyErrorMsg = new StringBuilder("Error in SBE41 message parse"); private static StringBuilder sbTemp2 = new StringBuilder(512); public static double parse(byte[] inBytes) { double parsedPressure = double.NaN; double parsedTemperature = double.NaN; double parsedSalinity = double.NaN; int CRIndex = -1; int LFIndex = -1; int byteArrayLength = -1; int startIndex = 0; int stopIndex = 0; //sbTemp.Clear(); //sbTemp.Append("SBE 41 in string: "); //sbTemp.Append(UTF8Encoding.UTF8.GetChars(inBytes)); //EngrLogger.writeToColumns(sbTemp); if (Program.CurrentState == Program.CPFStates.dumpCPData) return (Double.NaN); #if simPressure return (getSimPressure()); #endif //sbTemp.Clear(); //sbTemp.Append("SBE41 starting parse"); //EngrLogger.writeToColumns(sbTemp); byteArrayLength = Array.IndexOf(inBytes, 0); if ((inBytes[0] == 32) && (byteArrayLength <= 10) && (byteArrayLength >= 8)) { inSB.Clear(); inSB.Append(UTF8Encoding.UTF8.GetChars(inBytes)); //Debug.Print("Parse inBytes = " + inSB.ToString()); try { CRIndex = Array.IndexOf(inBytes, 13); LFIndex = Array.IndexOf(inBytes, 10); if ((CRIndex >= 8) && (LFIndex >= 9)) { try { inSB.Replace((char)13, '\0'); inSB.Replace((char)10, '\0'); } catch { sbTemp.Clear(); sbTemp.Append("SBE41 /r/n .Replace error location 1 string: "); sbTemp.Append(inSB); EngrLogger.writeToColumns(sbTemp); } parsedPressure = double.Parse(inSB.ToString()); } else parsedPressure = double.NaN; } catch { ErrorHandler.incrParsePOnlyErrors(); qS.timeStamp = DateTime.Now; qS.state = Program.CurrentState; Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length); qS.qRecordType = StructQueue.QRecordType.SBE41; Program.sQueue.Enqueue(qS); parsedPressure = double.NaN; } } else if ((byteArrayLength <= 29) && (byteArrayLength >= 27) && (numberOfCommas(inBytes) == 2)) { inSB.Clear(); inSB.Append(UTF8Encoding.UTF8.GetChars(inBytes)); sbTemp.Clear(); sbTemp.Append("SBE41 parse PTS in chars: "); sbTemp.Append(inSB); // Debug.Print(sbTemp.ToString()); try { //Get pressure from the first field by removing eveything after and including the first comma stopIndex = Array.IndexOf(inBytes, 44); sbTemp.Clear(); sbTemp.Append(inSB); sbTemp.Remove(stopIndex, (sbTemp.Length - stopIndex)); parsedPressure = double.Parse(sbTemp.ToString()); //Get temperature from the second field startIndex = Array.IndexOf(inBytes, 44); stopIndex = Array.IndexOf(inBytes, 44, startIndex + 1); sbTemp.Clear(); sbTemp.Append(inSB); //remove eveything after and including the second comma sbTemp.Remove(stopIndex, (sbTemp.Length - stopIndex)); //remove everything upto and including the first comma sbTemp.Remove(0, startIndex + 1); parsedTemperature = double.Parse(sbTemp.ToString()); //Get salinity from the third field sbTemp.Clear(); sbTemp.Append(inSB); sbTemp.Remove(0, stopIndex + 1); try { sbTemp.Replace((char)13, '\0'); sbTemp.Replace((char)10, '\0'); } catch { sbTemp2.Clear(); sbTemp2.Append("SBE41 /r/n .Replace error location 2 string:"); sbTemp2.Append(sbTemp); EngrLogger.writeToColumns(sbTemp2); } parsedSalinity = double.Parse(sbTemp.ToString()); } catch { ErrorHandler.incrParsePOnlyErrors(); qS.timeStamp = DateTime.Now; qS.state = Program.CurrentState; Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length); qS.qRecordType = StructQueue.QRecordType.SBE41; Program.sQueue.Enqueue(qS); parsedPressure = double.NaN; } } else { sbTemp.Clear(); sbTemp.Append("SBE41 parse in bytes didn't look like fp or pts response. "); sbTemp.Append("in bytes length: "); sbTemp.Append(byteArrayLength.ToString()); sbTemp.Append(" Num commas: "); sbTemp.Append(numberOfCommas(inBytes).ToString()); EngrLogger.writeToColumns(sbTemp); } //Debug.Print("Parsed pressure = " + parsedPressure.ToString("f2")); return (parsedPressure); } private static int numberOfCommas(byte[] inBytes) { int numCommas = 0; for(int i = 0; i < inBytes.Length; i++) { if (inBytes[i] == 44) numCommas = numCommas + 1; } return(numCommas); } public static double parsePOnly(byte[] inBytes) { double pressure = double.NaN; int CRIndex = -1; int LFIndex = -1; inSB.Clear(); inSB.Append(UTF8Encoding.UTF8.GetChars(inBytes)); //Debug.Print("Parse inBytes = " + inSB.ToString()); try { CRIndex = Array.IndexOf(inBytes, 13); LFIndex = Array.IndexOf(inBytes, 10); if ((CRIndex >= 8) && (LFIndex >= 9)) { try { inSB.Replace((char)13, '\0'); inSB.Replace((char)10, '\0'); } catch { sbTemp.Clear(); sbTemp.Append("SBE41 /r/n .Replace error location 3 string: "); sbTemp.Append(inSB); EngrLogger.writeToColumns(sbTemp); } pressure = double.Parse(inSB.ToString()); } else pressure = double.NaN; } catch { ErrorHandler.incrParsePOnlyErrors(); qS.timeStamp = DateTime.Now; qS.state = Program.CurrentState; Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(sbParsePOnlyErrorMsg.ToString()), qS.byteArray, sbParsePOnlyErrorMsg.Length); qS.qRecordType = StructQueue.QRecordType.SBE41; Program.sQueue.Enqueue(qS); pressure = double.NaN; } //Debug.Print("Parsed pressure = " + pressure.ToString("f2")); return (pressure); } //See .m files in directory for non-CP Mode PFilter design parameters //for CP Mode using filter design and automation tool from SPTool with //Response type = lowpass, Design Method = FIR Window, Order = 10, Window = Blackman-Harris, Units = Hz, Fs = 1, Fc = .02 private static double[][] PFilterCoef = new double[2][] { //These are for non CP mode new double[] {8.81131298055604e-06, 0.00211708177469335, 0.0240149216743935, 0.102119533086314, 0.225973798612221, 0.291531707078795, 0.225973798612221, 0.102119533086314, 0.0240149216743935, 0.00211708177469335, 8.81131298055604e-06}, //These are for CP mode new double[] {1.57246287686607e-05, 0.00294876425891433, 0.0281799141639923, 0.106973234459911, 0.221807456935806, 0.280149811105215, 0.221807456935806, 0.106973234459911, 0.0281799141639923, 0.00294876425891433, 1.57246287686607e-05} }; //Designed with for nonCP mode //d = fdesign.differentiator('N,Fp,Fst', 8, .1, .4); //Hd = design(d, 'equiripple'); //and for CP mode //d = fdesign.differentiator('N,Fp,Fst', 8, .1/3, .4/3); //Hd = design(d, 'equiripple') private static double[][] VFilterCoef = new double[2][] { //These are for non CP mode new double[] {-0.035538709937166, 0.121018262495538, 0.110204653307495, 0.075228720456011, 0.0, -0.075228720456011, -0.110204653307495, -0.121018262495538, 0.035538709937166}, //These are for CP mode new double[] {0.123601865482124, 0.000071541838701, 0.000051713468660, 0.000027758225109, 0.0, -0.000027758225109, -0.000051713468660, -0.000071541838701, -0.123601865482124} }; private static int[] numPCoef = new int[] { PFilterCoef[0].Length, PFilterCoef[1].Length }; private static int[] numVCoef = new int[] { VFilterCoef[0].Length, VFilterCoef[1].Length }; private static double accum = 0.0; public static double ctdSamplePeriod = double.NaN; private static int maxDelayLineLength = 64; private static double[] PFilterDelayLine = new double[maxDelayLineLength]; private static double[] VFilterDelayLine = new double[maxDelayLineLength]; private static double[] lpfPressureArray = new double[128]; private static double[] lpfPressureArrayTimeStamps = new double[128]; private static int filterIndex; public struct LPFResults { public double Pressure; public double LPFPressure; public double LPFVelocity; public double diffVelocity; public double diff2Velocity; public double slope; } private static LPFResults SBE41LPFResults; private static int filterType = 0; //This is the number of points to use in the slope calculation from Numerical Recipes in C, Chapter 15; private static int ss = 40; //Variables for slope calculation private static double sx = 0.0; private static double sy = 0.0; private static double sxoss = 0.0; private static double st2 = 0.0; private static double b = 0.0; private static double t = 0.0; public static LPFResults LPFPressure(double pressureSample, double timeStamp, bool initialize) { double dt; if (SBE41.ctdSamplePeriod == 1.0) { filterType = 1; dt = 1.0; } else { filterType = 0; dt = 3.0; } if (initialize) { EngrLogger.writeToColumns("Initializing SBE 41 LPF"); for (filterIndex = 0; filterIndex < maxDelayLineLength; filterIndex++) { PFilterDelayLine[filterIndex] = pressureSample; } } accum = 0.0; PFilterDelayLine[0] = pressureSample; for (filterIndex = 0; filterIndex < numPCoef[filterType]; filterIndex++) { accum = accum + (PFilterCoef[filterType][filterIndex] * PFilterDelayLine[filterIndex]); } SBE41LPFResults.Pressure = pressureSample; SBE41LPFResults.LPFPressure = accum; SBE41LPFResults.diffVelocity = (PFilterDelayLine[1] - PFilterDelayLine[0]) / dt; //Don't need the -1.0 because we're subtracting ( (t-1) - t0 ) SBE41LPFResults.diff2Velocity = (PFilterDelayLine[2] - PFilterDelayLine[0]) / (2 * dt); //Don't need the -1.0 because we're subtracting ( (t-1) - t0 ) if (initialize) { for (int i = 0; i < 128; i++) { lpfPressureArray[i] = SBE41LPFResults.LPFPressure; lpfPressureArrayTimeStamps[i] = timeStamp - (i * dt); } } lpfPressureArray[0] = SBE41LPFResults.LPFPressure; lpfPressureArrayTimeStamps[0] = timeStamp; sx = 0.0; sy = 0.0; for (int i = 0; i < ss; i++) { sx = sx + lpfPressureArrayTimeStamps[i]; sy = sy + lpfPressureArray[i]; //Debug.Print(lpfPressureArrayTimeStamps[i].ToString("f2") + " " + lpfPressureArray[i].ToString("f2")); } sxoss = sx / ss; st2 = 0; b = 0; for (int i = 0; i < ss; i++) { t = lpfPressureArrayTimeStamps[i] - sxoss; st2 = st2 + t * t; b = b + t * lpfPressureArray[i]; } b = -b / st2; SBE41LPFResults.slope = b; if (initialize) { for (filterIndex = 0; filterIndex < maxDelayLineLength; filterIndex++) { VFilterDelayLine[filterIndex] = SBE41LPFResults.LPFPressure; } } accum = 0.0; VFilterDelayLine[0] = SBE41LPFResults.LPFPressure; for (filterIndex = 0; filterIndex < numVCoef[filterType]; filterIndex++) { accum = accum + (VFilterCoef[filterType][filterIndex] * VFilterDelayLine[filterIndex]); } SBE41LPFResults.LPFVelocity = -1.0 * accum / dt; //-1.0 because pressure increases positively down but velocity in the down direction needs to be negative //Console.WriteLine("Pressure = " + SBE41LPFResults.Pressure.ToString("F6") + //" LPF Pressure = " + SBE41LPFResults.LPFPressure.ToString("f4") + //" LPF Velocity = " + SBE41LPFResults.LPFVelocity.ToString("f4") + //" Difference Velocity = " + SBE41LPFResults.diffVelocity.ToString("f4") + //" Slope = " + SBE41LPFResults.slope.ToString("f4") + //" CTD Sample Period = " + dt.ToString()); //for (int i = 0; i < PFilterDelayLine.Length; i++) // Console.WriteLilne(PFilterDelayLine[i].ToString("f4") + " "); //Shift delay lines 1 sample for (filterIndex = maxDelayLineLength - 2; filterIndex >= 0; filterIndex--) { PFilterDelayLine[filterIndex + 1] = PFilterDelayLine[filterIndex]; } for (filterIndex = maxDelayLineLength - 2; filterIndex >= 0; filterIndex--) { VFilterDelayLine[filterIndex + 1] = VFilterDelayLine[filterIndex]; } for (int i = 126; i >= 0; i--) { lpfPressureArray[i + 1] = lpfPressureArray[i]; lpfPressureArrayTimeStamps[i + 1] = lpfPressureArrayTimeStamps[i]; } return (SBE41LPFResults); } public enum commands : int { none = 0, crlf, ds, dc, startProfile, stopProfile, binaverage, dd, da, dah } public static bool waitingForResponse = false; private static bool gotResponse = false; private static readonly StringBuilder dsResponse = new StringBuilder("real-time output is P"); private static readonly StringBuilder dcResponse = new StringBuilder("POFFSET"); private static readonly StringBuilder crlfResponse = new StringBuilder("S>"); private static readonly StringBuilder startProfileResponse = new StringBuilder("profile started"); private static readonly StringBuilder stopProfileResponse = new StringBuilder("profile stopped"); private static readonly StringBuilder ddResponse = new StringBuilder("upload complete"); private static readonly StringBuilder daResponse = new StringBuilder("upload complete"); private static readonly StringBuilder dahResponse = new StringBuilder("upload complete"); private static readonly StringBuilder binaverageResponse = new StringBuilder("done"); private static byte[] expectedResponse = new byte[64]; private static int expectedResponseLength = 0; private static int numRetries = 0; private static readonly int maxNumRetries = 3; private static TimeSpan wfrStartTime = new TimeSpan(); private static readonly TimeSpan wfrTimeout = new TimeSpan(0, 0, 40); private static readonly string wfrFail = "SBE41 waitingForResponse expended all retries"; public static int waitForResponse(commands commandNum) { if (waitingForResponse) //Do this after the first call to this method { if (gotResponse) //success { waitingForResponse = false; numRetries = 0; //Debug.Print("Got SBE41 Response"); return (1); } else if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - wfrStartTime) > wfrTimeout) //if timeout has expired, try again up to the max number of retries { if (numRetries > maxNumRetries) { waitingForResponse = false; numRetries = 0; gotResponse = false; qS.timeStamp = DateTime.Now; qS.state = Program.CurrentState; qS.qRecordType = StructQueue.QRecordType.SBE41; Array.Clear(qS.byteArray, 0, qS.byteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(wfrFail), qS.byteArray, wfrFail.Length); Program.sQueue.Enqueue(qS); return (-1); } else { waitingForResponse = false; sbTemp.Clear(); sbTemp.Append("SBE41 waitingForResponse try number "); sbTemp.Append(numRetries); return (0); } } else return (0); } else //Do this the first time the method is called and after a timeout { Array.Clear(expectedResponse, 0, expectedResponse.Length); switch (commandNum) { case commands.ds: Array.Copy(UTF8Encoding.UTF8.GetBytes(dsResponse.ToString()), expectedResponse, dsResponse.Length); expectedResponseLength = dsResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending ds command"); sendDS(); break; case commands.dc: Array.Copy(UTF8Encoding.UTF8.GetBytes(dcResponse.ToString()), expectedResponse, dcResponse.Length); expectedResponseLength = dcResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending ds command"); sendDC(); break; case commands.crlf: Array.Copy(UTF8Encoding.UTF8.GetBytes(crlfResponse.ToString()), expectedResponse, crlfResponse.Length); expectedResponseLength = crlfResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 CR/LF"); sendCRLF(); break; case commands.startProfile: Array.Copy(UTF8Encoding.UTF8.GetBytes(startProfileResponse.ToString()), expectedResponse, startProfileResponse.Length); expectedResponseLength = startProfileResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 startProfile"); startProfile(); break; case commands.stopProfile: Array.Copy(UTF8Encoding.UTF8.GetBytes(stopProfileResponse.ToString()), expectedResponse, stopProfileResponse.Length); expectedResponseLength = stopProfileResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 stopProfile"); sendStopProfile(); break; case commands.binaverage: Array.Copy(UTF8Encoding.UTF8.GetBytes(binaverageResponse.ToString()), expectedResponse, binaverageResponse.Length); expectedResponseLength = binaverageResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 binaverage"); sendBinAverage(); break; case commands.dd: Array.Copy(UTF8Encoding.UTF8.GetBytes(ddResponse.ToString()), expectedResponse, ddResponse.Length); expectedResponseLength = ddResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 dd command"); dumpData(); break; case commands.da: Array.Copy(UTF8Encoding.UTF8.GetBytes(daResponse.ToString()), expectedResponse, daResponse.Length); expectedResponseLength = daResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 da command"); dumpBinnedData(); break; case commands.dah: Array.Copy(UTF8Encoding.UTF8.GetBytes(dahResponse.ToString()), expectedResponse, dahResponse.Length); expectedResponseLength = dahResponse.Length; //Debug.Print(DateTime.Now.TimeOfDay.ToString() + " Sending SBE41 dah command"); dumpBinnedDataHex(); break; } wfrStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); numRetries = numRetries + 1; gotResponse = false; waitingForResponse = true; return (0); } } public static void checkResponse(byte[] response) { int msgLength = response.Length; //check for expectedResponse anywhere in the SBE41 response message if (SBE41.waitingForResponse) { if (msgLength > expectedResponseLength) { //start at the beginning of the qStructure byte array and test for the right character sequence //then start at the 2nd character in the qStrucutre byte array and so on sbTemp.Clear(); sbTemp.Append("Looking for SBE 41: "); sbTemp.Append(UTF8Encoding.UTF8.GetChars(expectedResponse)); sbTemp.Append(" got: "); sbTemp.Append(UTF8Encoding.UTF8.GetChars(response, 0, Array.IndexOf(response, 0))); //EngrLogger.writeToColumns(sbTemp); if (GHI.Utilities.Arrays.Contains(response, 0, expectedResponse, 0, expectedResponseLength) >= 0) { gotResponse = true; //Debug.Print("Correct response received"); } //for (int j = 0; j < msgLength - expectedResponseLength; j++) //{ // for (int k = 0; k < expectedResponseLength; k++) // { // if (response[j + k] != expectedResponse[k]) // { // gotResponse = false; // break; // } // else // gotResponse = true; // } // if (gotResponse) // break; //} } } } #if simPressure private static double simPressure = 0.0; private static double simPressureCount = 0; private static double getSimPressure() { switch (Program.CurrentState) { case Program.CPFStates.SMNotRunning: // SMNotRunning = 1, initMission, initProfile, descend, simPressure = 0.0; break; case Program.CPFStates.initMission: simPressure = 0.0; break; case Program.CPFStates.initProfile: simPressure = 0.0; break; case Program.CPFStates.descend: if (simPressureCount < 10) { simPressure = simPressure - Profile.PVSetPoint * 3.0; simPressureCount++; } break; case Program.CPFStates.ascend: // ascend, surfaceOpsSetBellows, surfaceOps, preMissionDelay, simPressure = simPressure + Profile.PVSetPoint * 3.0; break; case Program.CPFStates.surfaceOpsSetBellows: simPressure = 0.0; break; case Program.CPFStates.surfaceOps: simPressure = 0.0; break; case Program.CPFStates.preMissionDelay: simPressure = 0.0; break; case Program.CPFStates.dumpCPData: // dumpCPData, ABRetractFast, ABRetractSlow, park, simPressure = 0.0; break; case Program.CPFStates.ABRetractFast: simPressure = simPressure + 0.025; break; case Program.CPFStates.ABRetractSlow: break; case Program.CPFStates.park: break; case Program.CPFStates.anchor: // anchor, deAnchor, startCPMode, recoveryMode, exit, break; case Program.CPFStates.startCPMode: simPressure = 0.0; break; case Program.CPFStates.recoveryMode: simPressure = simPressure + .3; break; case Program.CPFStates.exit: simPressure = 0.0; break; default: simPressure = 0.0; break; } return (simPressure); } #endif } }