using System; using Microsoft.SPOT; using Microsoft.SPOT.Hardware; using Microsoft.SPOT.IO; using System.Threading; using System.Text; using System.IO; using System.IO.Ports; using GHI.Premium.IO; using GHI.Premium.System; using GHI.Premium.Hardware; using GHI.Hardware.EMX; //TODO List //1) Add bellows position limit checking //2) Add emergency ascend mode //3) Add try/catch/finally resulting in recovery mode //4) Add recovery mode //5) Add bluetooth console //6) Investigate whether we reallly need a lock for methods in the same thread //7) Run CTD in CP mode //8) Add serial port error received error handlers //9) Convert ADuC voltages to engineering units //10) Figure out a solid way to start and stop CTD, Buoyancy control, and everything else that needs stopping/starting //11) Change state timedout functionality to force state transition, maybe use execution constraints //12) ctrl-shift-F find what: "^:b*[^:b#/]+.*$" (no quotes), Find options "use regular expressions", Look at these file types *.cs namespace miniCPF { public class Program { //Need to make sure string descriptions of states are properly assigned to correct value every time states are changed see setStateNames() in CPFUtilities public enum CPFStates : int { NotRunning = 0, PresetBellows, PreMissionDelay, Initialize, AutoballastSetBellows, SurfaceOps, AutoballastRetract, Descend, Anchor, DeAnchor, Ascend, Surface, EmergencyAscend, ProcessErrors, Exit, lastState, } public static CPFStates CPFState; private static Timer CPFStateTimer = new Timer(new TimerCallback(CPFStateTimerCallback), null, 1000000, 1000000); public static bool timeSynched = false; private static bool telemetryDone = false; private static bool missionRun = true; private static bool stateTimedout = false; private static bool stateEntry = true; public static bool hitBottom = false; private static bool surfaceOpsGo = false; private static bool parked = false; private static bool EAOnSurface = false; private static bool runPID = false; private static bool descendToFirstAscend = false; private static bool waitingForSurface = false; public static ByteQueue MessageQueue = new ByteQueue(); private static TimeSpan mtParkStartTime; private static TimeSpan lastParkSampleTime; private static TimeSpan mtTimeNow; private static TimeSpan timeDiff; private static TimeSpan EALastSendTime; private static double PVSetPoint = 0.0; public static int depthTableNum = 0; private static int profileNum = -1; private static Object programLock = new Object(); private static StringBuilder sbTemp = new StringBuilder(256); public static StringBuilder[] sbStateNames = new StringBuilder[(int)CPFStates.lastState + 1]; private static StringBuilder sbStateEntry = new StringBuilder("State Entry"); private static StringBuilder sbStateExit = new StringBuilder("Normal state exit"); private static StringBuilder sbStateTimedout = new StringBuilder("State timed out, exiting state"); private static StringBuilder emergencyAscendComment = new StringBuilder("Max Pressure Detected, Emergency Ascend"); private static StringBuilder surfaceOpsWaitLine = new StringBuilder("Waiting for Console Command\r\n"); private static StringBuilder lastFileName = new StringBuilder(128); public static StringBuilder stateSuffix = new StringBuilder(64); public static void Main() { Utility.SetLocalTime(RealTimeClock.GetTime()); //TODO Add synch to GPS CPFUtilities.setStateNames(); BTConsole.OpenConsolePort(); CPFState = CPFStates.NotRunning; sbTemp.Clear(); sbTemp.Append("Program Started"); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbTemp).ToString())); } MissionConfigFile.init(); Logger.init(); SBQue.init(); SBE41.Init(); ADuC.Init(); Elmo.init(); CPFState = CPFStates.PresetBellows; CPFStateTimer.Change(configFile.presetBellowsTimeout, 5000); stateSuffix.Clear(); bool runPresetBellows = true; runPresetBellows = false; while (missionRun) { switch (CPFState) { case (CPFStates.PresetBellows): if (runPresetBellows) { if (stateEntry) { Elmo.stopMotor(); runPID = false; stateSuffix.Clear(); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; runPresetBellows = false; int speed = 10000; //Elmo.extendBellows(speed); Elmo.retractBellows(speed); } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.preMissionDelayTimeout, 5000); CPFState = CPFStates.PreMissionDelay; stateSuffix.Clear(); } } else { Elmo.stopMotor(); CPFStateTimer.Change(configFile.preMissionDelayTimeout, 5000); CPFState = CPFStates.PreMissionDelay; } break; case (CPFStates.PreMissionDelay): if (stateEntry) { Elmo.stopMotor(); runPID = false; stateSuffix.Clear(); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; stateSuffix.Append((configFile.preMissionDelayTimeout / 1000).ToString()); stateSuffix.Append(" seconds"); } if (stateTimedout) { stateEntry = true; stateTimedout = false; stateSuffix.Clear(); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } stateSuffix.Clear(); CPFStateTimer.Change(configFile.initializeTimeout, 5000); CPFState = CPFStates.Initialize; } break; case (CPFStates.Initialize): if (stateEntry) { Elmo.stopMotor(); runPID = false; stateEntry = false; stateTimedout = false; stateSuffix.Clear(); profileNum = profileNum + 1; depthTableNum = 0; sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } } if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } stateSuffix.Clear(); CPFStateTimer.Change(configFile.ABSetBellowsTimeout, 5000); CPFState = CPFStates.AutoballastSetBellows; } break; case (CPFStates.AutoballastSetBellows): if (stateEntry) { runPID = false; stateSuffix.Clear(); stateSuffix.Append(" bellows set position = "); stateSuffix.Append(configFile.ABSetBellowsPosition.ToString()); stateSuffix.Append(" mm at "); stateSuffix.Append(configFile.ABSetBellowsJV.ToString()); stateSuffix.Append(" CPS"); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; } //Decide what direction to go and keep checking if (bellowsPosition > (configFile.ABSetBellowsPosition + 0.5)) { if (!Elmo.retracting) { Elmo.retractBellows(configFile.ABSetBellowsJV); } } else if (bellowsPosition < (configFile.ABSetBellowsPosition - 0.5)) { if (!Elmo.extending) { Elmo.extendBellows(configFile.ABSetBellowsJV); } } else //Normal state exit criteria { Elmo.stopMotor(); CPFStateTimer.Change(configFile.surfaceOpsTimeout, 5000); CPFState = CPFStates.SurfaceOps; stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } stateSuffix.Clear(); stateTimedout = false; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } stateSuffix.Clear(); CPFStateTimer.Change(configFile.surfaceOpsTimeout, 5000); CPFState = CPFStates.SurfaceOps; } break; case (CPFStates.SurfaceOps): if (stateEntry) { Elmo.stopMotor(); runPID = false; stateSuffix.Clear(); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; //Send up the data file (if not the first profile) if (profileNum > 0) { lastFileName.Clear(); lastFileName.Append(Logger.fileName); Logger.closeFile(); Logger.openFile(); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix("Uploading Data File").ToString())); } // BTConsole.uploadDataFileWhole(lastFileName); //TODO The BTConsole might need to go in a separate thread } //Synch time with GPS //TODO Eventually lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix("Waiting for Console Command").ToString())); } } //timeSynched = true; //Uncomment this to skip time synch // telemetryDone = true; //Uncomment this to skip time synch if ((timeSynched && telemetryDone) || (surfaceOpsGo)) { timeSynched = false; telemetryDone = false; surfaceOpsGo = false; stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } stateSuffix.Clear(); CPFStateTimer.Change(configFile.ABRetractTimeout, 5000); CPFState = CPFStates.AutoballastRetract; } if (stateTimedout) { timeSynched = false; telemetryDone = false; surfaceOpsGo = false; stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } stateSuffix.Clear(); CPFStateTimer.Change(configFile.ABRetractTimeout, 5000); CPFState = CPFStates.AutoballastRetract; } break; case (CPFStates.AutoballastRetract): if (stateEntry) { runPID = false; stateSuffix.Clear(); stateSuffix.Append(" Retracting to "); stateSuffix.Append(configFile.ABVelocityThreshold.ToString()); stateSuffix.Append(" dBar/sec at "); stateSuffix.Append(configFile.ABRetractJV.ToString()); stateSuffix.Append(" CPS"); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } Elmo.retractBellows(configFile.ABRetractJV); stateEntry = false; } //Normal state exit criteria if (lpfResults.diffVel < configFile.ABVelocityThreshold) //This should probably use LPFVel with faster LPF { Elmo.stopMotor(); CPFStateTimer.Change(configFile.descendTimeout, 5000); CPFState = CPFStates.Descend; stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } stateTimedout = false; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.descendTimeout, 5000); CPFState = CPFStates.Descend; } break; case (CPFStates.Descend): if (stateEntry) { parked = false; depthTableNum = 0; stateSuffix.Clear(); stateSuffix.Append("Descend Depth Num = "); stateSuffix.Append(depthTableNum.ToString()); //Make sure the PV set point is negative regardless of typos etc. PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); runPID = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; } //A positive park time means a transition to park if (!parked) { //check to see if this is a park station if (pressure >= MissionConfigFile.descendTable[depthTableNum].depth) { //if it is, take a sample and set park = true if (MissionConfigFile.descendTable[depthTableNum].parkTime > TimeSpan.Zero) { PVSetPoint = 0.0; //used as reference for exiting park mode mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); //used as reference for when to sample during park mode lastParkSampleTime = mtParkStartTime; sbTemp.Clear(); sbTemp.Append("Starting park period = "); sbTemp.Append(MissionConfigFile.descendTable[depthTableNum].parkTime.ToString()); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbTemp).ToString())); } InstrumentHandler.sampleInstruments(depthTableNum); //TODO Probably needs to be done in it's own thread or with a event parked = true; stateSuffix.Append(" ...Parked"); } //else just take a sample else { InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; stateSuffix.Clear(); stateSuffix.Append("Descend Depth Num = "); stateSuffix.Append(depthTableNum.ToString()); PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); } } } else { mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); timeDiff = mtTimeNow - mtParkStartTime; //check if it is time to take a sample based on time while parked if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod) { lastParkSampleTime = mtTimeNow; InstrumentHandler.sampleInstruments(depthTableNum); } //check to see if park time has expired if (timeDiff >= MissionConfigFile.descendTable[depthTableNum].parkTime) { depthTableNum = depthTableNum + 1; stateSuffix.Clear(); stateSuffix.Append("Descend Depth Num = "); stateSuffix.Append(depthTableNum.ToString()); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix("Exiting Park Period").ToString())); } } } //check to see if this is that last descend station if (depthTableNum >= MissionConfigFile.descendTable.Length) { Elmo.stopMotor(); runPID = false; stateSuffix.Clear(); CPFStateTimer.Change(configFile.ascendTimeout, 5000); CPFState = CPFStates.Ascend; stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } stateTimedout = false; } if (stateTimedout) { PVSetPoint = 0.0; stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } stateSuffix.Clear(); CPFStateTimer.Change(configFile.ascendTimeout, 5000); CPFState = CPFStates.Ascend; } break; case (CPFStates.Ascend): if (stateEntry) { lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } parked = false; waitingForSurface = false; depthTableNum = 0; if (MissionConfigFile.ascendTable[depthTableNum].depth > pressure) { //Make sure the PV set point is positive regardless of typos etc. PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPID = true; stateEntry = false; descendToFirstAscend = false; } else { //need to descend to first ascend station PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPID = true; stateEntry = false; descendToFirstAscend = true; } } if (!waitingForSurface) { //First deal with descend to first ascend station if (descendToFirstAscend) { if (pressure >= MissionConfigFile.ascendTable[depthTableNum].depth) { PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); descendToFirstAscend = false; } } else { if (!parked) { //check to see if this is a park station if (pressure <= MissionConfigFile.ascendTable[depthTableNum].depth) { //if it is, take a sample and set park = true if (MissionConfigFile.ascendTable[depthTableNum].parkTime > TimeSpan.Zero) { PVSetPoint = 0.0; //used as reference for exiting park mode mtParkStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); //used as reference for when to sample during park mode lastParkSampleTime = mtParkStartTime; sbTemp.Clear(); sbTemp.Append("Starting park period = "); sbTemp.Append(MissionConfigFile.ascendTable[depthTableNum].parkTime.ToString()); lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbTemp).ToString())); } InstrumentHandler.sampleInstruments(depthTableNum); //TODO Probably needs to be done in it's own thread or with a event parked = true; } //else just take a sample else { InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); } } } else { mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); timeDiff = mtTimeNow - mtParkStartTime; //check if it is time to take a sample based on time while parked if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod) { lastParkSampleTime = mtTimeNow; InstrumentHandler.sampleInstruments(depthTableNum); } //check to see if park time has expired if (timeDiff >= MissionConfigFile.ascendTable[depthTableNum].parkTime) { depthTableNum = depthTableNum + 1; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix("Exiting Park Period").ToString())); } } } if (depthTableNum >= MissionConfigFile.ascendTable.Length) { waitingForSurface = true; } } } //check to see if we've hit the surface if (pressure < configFile.surfacePressure) { Elmo.stopMotor(); runPID = false; CPFStateTimer.Change(configFile.initializeTimeout, 5000); CPFState = CPFStates.Initialize; stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } stateTimedout = false; } if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.initializeTimeout, 5000); CPFState = CPFStates.Initialize; } break; case (CPFStates.Anchor): if (stateEntry) { runPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; } //normal state exit criteria goes here //if (true) //{ // stateEntry = true; // stateTimedout = false; // lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } // CPFStateTimer.Change(configFile.deAnchorTimeout, 5000); // CPFState = CPFStates.DeAnchor; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.deAnchorTimeout, 5000); CPFState = CPFStates.DeAnchor; } break; case (CPFStates.DeAnchor): if (stateEntry) { runPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; } //normal state exit criteria goes here //if (true) //{ // stateEntry = true; // stateTimedout = false; // lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } // CPFStateTimer.Change(configFile.ascendTimeout, 5000); // CPFState = CPFStates.Ascend; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ascendTimeout, 5000); CPFState = CPFStates.Ascend; } break; case (CPFStates.Surface): if (stateEntry) { runPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; } //normal state exit criteria goes here //if (true) //{ // stateEntry = true; // stateTimedout = false; // lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } // CPFStateTimer.Change(configFile.initializeTimeout, 5000); // CPFState = CPFStates.Initialize; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.initializeTimeout, 5000); CPFState = CPFStates.Initialize; } break; case (CPFStates.EmergencyAscend): if (stateEntry) { CPFStateTimer = null; Elmo.stopMotor(); runPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; EAOnSurface = false; //TODO stop everything else } if (bellowsPosition < (configFile.bellowsEAPosition - 1.0)) { if (!Elmo.extending) Elmo.extendBellows(configFile.EAJV); } else if (bellowsPosition >= configFile.bellowsEAPosition) { if (!Elmo.motorStopped) Elmo.stopMotor(); } if (!EAOnSurface) { if (pressure >= configFile.surfacePressure) { Iridium.sendPositionSBD(); EALastSendTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); EAOnSurface = true; } } else { if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - EALastSendTime) > configFile.EASendPeriod) { Iridium.sendPositionSBD(); } } break; case (CPFStates.ProcessErrors): runPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } break; case (CPFStates.Exit): runPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateEntry).ToString())); } missionRun = false; break; } if (MessageQueue.Count > 0) { processMessageQueue(); } Thread.Sleep(configFile.SMSleepPeriod); } Logger.closeFile(); Logger.unMount(); } private static BuoyancyControl.PIDLogValues pidLogValues; private static byte[] dequeueMessage = new byte[256]; private static double pressure = double.NaN; private static double[] ADuCVolts = new double[12]; public static double bellowsPosition = double.NaN; public static double batteryBusVolts = double.NaN; private static SBE41.LPFResults lpfResults; private static StringBuilder SBE41Header = new StringBuilder("SBE41 Message"); private static StringBuilder ADuCHeader = new StringBuilder("ADuC Message"); private static StringBuilder velPIDHeader = new StringBuilder("Vel PID Values"); private static StringBuilder sbPMQ = new StringBuilder(256); private static StringBuilder sbConsoleCommand = new StringBuilder(32); private static StringBuilder consoleCommandNONE = new StringBuilder("NONE"); private static void processMessageQueue() { do { Array.Clear(dequeueMessage, 0, dequeueMessage.Length); lock (programLock) { dequeueMessage = Program.MessageQueue.Dequeue(); } if ((dequeueMessage[0] == (byte)'C') && (dequeueMessage[1] == (byte)'T') && (dequeueMessage[2] == (byte)'D')) //TODO Probably need a more robust way to do this { pressure = SBE41.parse(dequeueMessage); lpfResults = SBE41.LPFPressure(pressure); if (runPID) { pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel); Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(velPIDHeader), Logger.ColumnNums.velPID_setPoint, pidLogValues.setPoint, Logger.ColumnNums.velPID_PV, pidLogValues.PV, Logger.ColumnNums.velPID_y1, pidLogValues.y1, Logger.ColumnNums.velPID_y2, pidLogValues.y2, Logger.ColumnNums.velPID_I, pidLogValues.I, Logger.ColumnNums.velPID_v, pidLogValues.v, Logger.ColumnNums.velPID_u, pidLogValues.u, Logger.ColumnNums.velPID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source } Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(SBE41Header), Logger.ColumnNums.pressure, pressure, Logger.ColumnNums.platformLPFVel, lpfResults.LPFVel, Logger.ColumnNums.platformDiffVel, lpfResults.diffVel, Logger.ColumnNums.platformLPFAcc, lpfResults.LPFAcc); //TODO Should do this in a way that the message is time stamped at the source if (lpfResults.LPFPressure > configFile.maxPressure) { Elmo.stopMotor(); runPID = false; Logger.write(Logger.ColumnNums.comment, Logger.getPrefix("Current depth exceeds max pressure, beginning Emergency Ascend")); CPFStateTimer = null; CPFState = CPFStates.EmergencyAscend; } //TODO Check for anchor condition, if true transition to anchor } else if ((dequeueMessage[0] == (byte)'$') && (dequeueMessage[1] == (byte)'A') && (dequeueMessage[2] == (byte)'D') && (dequeueMessage[3] == (byte)'U')) { ADuCVolts = ADuC.parseADuC(dequeueMessage); bellowsPosition = ADuCVolts[0]; //Check bellows position, unless we're emergency ascending if (CPFState != CPFStates.EmergencyAscend) { if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition)) { Elmo.stopMotor(); CPFStateTimer = null; runPID = false; Logger.write(Logger.ColumnNums.comment, Logger.getPrefix("Bellows position exceeds limits, beginning Emergency Ascend")); CPFState = CPFStates.EmergencyAscend; } } batteryBusVolts = (ADuCVolts[1] * configFile.aducCH0Scale) - configFile.aducCH0Offset; if (batteryBusVolts < configFile.minBatteryBusVolts) { Elmo.stopMotor(); CPFStateTimer = null; runPID = false; Logger.write(Logger.ColumnNums.comment, Logger.getPrefix("Battery Bus Voltage below threshold, beginning Emergency Ascend")); CPFState = CPFStates.EmergencyAscend; } Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(ADuCHeader), //TODO Should do this in a way that the message is time stamped at the source Logger.ColumnNums.bellowsPosition, bellowsPosition, Logger.ColumnNums.batteryBusVolts, batteryBusVolts, Logger.ColumnNums.batteryBusAmps, ADuCVolts[2], Logger.ColumnNums.Amps12V, ADuCVolts[3], Logger.ColumnNums.Amps33V, ADuCVolts[4]); } else if ((dequeueMessage[0] == (byte)'C') && (dequeueMessage[1] == (byte)'O') && (dequeueMessage[2] == (byte)'N')) { processConsoleCommand(dequeueMessage); } else { sbPMQ.Clear(); sbPMQ.Append(UTF8Encoding.UTF8.GetChars(dequeueMessage)); Logger.write(Logger.ColumnNums.comment, sbPMQ); } } while (Program.MessageQueue.Count > 0); } private static void CPFStateTimerCallback(object o) { stateTimedout = true; } private static StringBuilder consoleCommandComment = new StringBuilder(); private static void processConsoleCommand(byte[] consoleCommandMessage) { sbConsoleCommand.Clear(); sbConsoleCommand.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 3, (Array.IndexOf(consoleCommandMessage, (byte)13) - 3))); consoleCommandComment.Clear(); consoleCommandComment.Append("Processing Console Command: "); consoleCommandComment.Append(sbConsoleCommand); Logger.write(Logger.ColumnNums.comment, Logger.getPrefix(consoleCommandComment)); switch (sbConsoleCommand.ToString().ToUpper()) { case "DIR": { break; } case "DOWNENGR": { break; } case "DOWNMIS": { break; } case "UPDATA": { break; } case "UPDATAFILE": { break; } case "UPENGR": { break; } case "GO": { surfaceOpsGo = true; break; } default: { break; } } sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); } } }