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 //3) Add try/catch/finally resulting in recovery mode //4) Add recovery mode //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 //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 transitions even if SM hangs up, 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 //13) Maybe discard serial buffers at end of every serial port event handler 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, RunEngrTest, Initialize, SurfaceOpsSetBellows, PreMissionDelay, SurfaceOps, AutoballastSetBellows, AutoballastRetract, Descend, Anchor, DeAnchor, Ascend, Surface, EmergencyAscend, ProcessErrors, Exit, lastState } public static CPFStates CPFState; public enum VelStates : int { none = 0, down, //descend at MissionConfig specified speed downSlow, //descend at approach speed stop, //PVSetPoint = 0 upSlow, //ascend at approach speed up //ascend at MissionConfig specified speed } public static VelStates VelState; public enum MovingStates : int { none = 0, transit, //when in transit to next station enterPark, //triggered when first arrives within DepthSetPoint (+/- inner band) parked, //period when float works to stay in park zone leavePark, //triggered when park timer expires sample, //triggered when passing a non-park DepthSetPoint exitDescend, //triggered when leaving Descend state exitAscend //triggered when leaving Ascend state } public static MovingStates MovingState; private enum EngrTests : int { none = 0, noOps, runBellows, cycleBellows, stepBellows, holdDepth, readOptode, exitProgram } private static EngrTests engrTest = EngrTests.none; private static Timer CPFStateTimer = new Timer(new TimerCallback(CPFStateTimerCallback), null, 1000000, 1000000); private static Timer ADCTimer; 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 runPVPID = false; private static bool runDepthPID = false; private static bool descendToFirstAscend = false; private static bool uploadEngrFile = false; //private static bool readADC = false; public static ByteQueue MessageQueue = new ByteQueue(); public static StructQueue sQueue = new StructQueue(); private static StructQueue.qStruct qStructure = new StructQueue.qStruct(); private static TimeSpan mtParkStartTime; private static TimeSpan lastParkSampleTime; private static TimeSpan mtTimeNow; private static TimeSpan timeDiff; private static TimeSpan EALastSendTime; private static TimeSpan parkTime; private static double PVSetPoint = 0.0; private static double DepthSetPoint = 0.0; private static double IB = configFile.IB; private static double OB = configFile.OB; public static int depthTableNum = -1; private static int profileNum = 0; private static int verbosityLevel = 3; private static Object programLock = new Object(); private static StringBuilder sbTemp = new StringBuilder(256); public static StringBuilder[] sbStateNames = new StringBuilder[(int)CPFStates.lastState + 1]; public static StringBuilder[] VelStateNames = new StringBuilder[(int)VelStates.up + 1]; //up is last state public static StringBuilder[] MovingStateNames = new StringBuilder[(int)MovingStates.exitAscend + 1]; //exitAscend is last state private static readonly StringBuilder sbStateEntry = new StringBuilder("State Entry"); private static readonly StringBuilder sbStateExit = new StringBuilder("Normal state exit"); private static readonly StringBuilder sbStateTimedout = new StringBuilder("State timed out, exiting state"); private static readonly StringBuilder emergencyAscendComment = new StringBuilder("Max Pressure Detected, Emergency Ascend"); private static readonly StringBuilder surfaceOpsWaitLine = new StringBuilder("Waiting for Console Command\r\n"); private static readonly StringBuilder lastFileName = new StringBuilder(128); public static readonly StringBuilder UDF = new StringBuilder("Uploading Data File"); public static readonly StringBuilder WFCC = new StringBuilder("Waiting for Console Command"); public static readonly StringBuilder BBVError = new StringBuilder("Battery Bus Voltage below threshold, beginning Emergency Ascend"); public static readonly StringBuilder BPError = new StringBuilder("Bellows position exceeds limits, beginning Emergency Ascend"); public static readonly StringBuilder EPP = new StringBuilder("Exiting Park Period"); public static readonly StringBuilder DepthError = new StringBuilder("Current depth exceeds max pressure, beginning Emergency Ascend"); public static readonly StringBuilder BottomDetect = new StringBuilder("Bottom detected based on pressure variance value"); public static readonly StringBuilder TimerNull = new StringBuilder("Surface Ops set state timer null after first profile"); private static TimeSpan engrTestStartTime; private static TimeSpan engrTestCyclePeriod = new TimeSpan(0, 0, 30); private static bool firstEngrTest = true; private static bool engrTestExtend = true; private static int engrTestSpeed = 0; private static double initialAnchorBP = 0.0; public static void Main() { Utility.SetLocalTime(RealTimeClock.GetTime()); //TODO Add synch to GPS CPFUtilities.setStateNames(); CPFUtilities.setMovingStateNames(); CPFUtilities.setVelStateNames(); BTConsole.OpenConsolePort(); CPFState = CPFStates.NotRunning; sbTemp.Clear(); sbTemp.Append("Program Started"); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); } for (int i = 0; i < 10; i++) Debug.Print("Be sure to uncomment bellows position test"); MissionConfigFile.init(); EngrLogger.init(); SBQue.init(); //SBE41.Init(); //ADuC.Init(); Elmo.init(); //Optode.init(); DGH.init(); MTS.init(); //CN0254.scanAll(); //Seems like we need to do this once to flush bad data //ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 1000, 3000); //wait a little bit to get messages from ADuC and SBE41 //then process the messages Thread.Sleep(3000); // processMessageQueue(); //CPFState = CPFStates.RunEngrTest; CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Initialize; while (missionRun) { switch (CPFState) { case (CPFStates.RunEngrTest): if (stateEntry) { basicStateEntry(); firstEngrTest = true; } //Comment out all but one of these engrTest = EngrTests.stepBellows; engrTest = EngrTests.runBellows; //engrTest = EngrTests.cycleBellows; engrTest = EngrTests.readOptode; //engrTest = EngrTests.noOps; //engrTest = EngrTests.holdDepth; //engrTest = EngrTests.none; switch (engrTest) { case (EngrTests.none): Elmo.stopMotor(); stateEntry = true; stateTimedout = false; CPFState = CPFStates.Initialize; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); break; case (EngrTests.noOps): //don't do anything just monitor stuff Elmo.stopMotor(); runPVPID = false; runDepthPID = false; break; //case (EngrTests.readOptode): //Optode.doSample(); //break; case (EngrTests.runBellows): if (firstEngrTest) { engrTestSpeed = 10000; //Comment out extend or retract //Elmo.extendBellows(engrTestSpeed); Elmo.retractBellows(engrTestSpeed); firstEngrTest = false; } break; case (EngrTests.cycleBellows): if (firstEngrTest) { engrTestSpeed = 5000; engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); engrTestExtend = false; Elmo.retractBellows(engrTestSpeed); Debug.Print("Cycle Bellows: Retract"); firstEngrTest = false; } if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - engrTestStartTime) >= engrTestCyclePeriod) { engrTestExtend = !engrTestExtend; if (engrTestExtend) { engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); Elmo.extendBellows(engrTestSpeed); Debug.Print("Cycle Bellows: Extend"); } else { engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); Elmo.retractBellows(engrTestSpeed); Debug.Print("Cycle Bellows: Retract"); } } break; case (EngrTests.stepBellows): if (firstEngrTest) { engrTestSpeed = 20000; engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); engrTestExtend = true; Elmo.extendBellows(engrTestSpeed); firstEngrTest = false; } if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - engrTestStartTime) >= engrTestCyclePeriod) { if (Elmo.extending) { Elmo.stopMotor(); engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); } else { Elmo.extendBellows(engrTestSpeed); engrTestStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); } } break; case (EngrTests.holdDepth): DepthSetPoint = 2.0; runPVPID = false; runDepthPID = true; break; case (EngrTests.exitProgram): Elmo.stopMotor(); stateEntry = true; stateTimedout = false; CPFState = CPFStates.Exit; break; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Initialize; } break; case (CPFStates.Initialize): basicStateEntry(); profileNum = profileNum + 1; depthTableNum = 0; sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.SOSetBellowsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOpsSetBellows; break; case (CPFStates.SurfaceOpsSetBellows): if (stateEntry) { basicStateEntry(); } //Decide what direction to go and keep checking if (bellowsPosition > (configFile.SOSetBellowsPosition + 0.5)) { if (!Elmo.retracting) { Elmo.retractBellows(configFile.SOSetBellowsJV); } } else if (bellowsPosition < (configFile.SOSetBellowsPosition - 0.5)) { if (!Elmo.extending) { Elmo.extendBellows(configFile.SOSetBellowsJV); } } else //Normal state exit criteria { Elmo.stopMotor(); stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); if (profileNum == 1) { CPFStateTimer.Change(configFile.preMissionDelayTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.PreMissionDelay; } else { CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } stateTimedout = false; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } break; case (CPFStates.PreMissionDelay): if (stateEntry) { basicStateEntry(); } if (surfaceOpsGo) { stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } if (stateTimedout) { stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } break; case (CPFStates.SurfaceOps): if (stateEntry) { basicStateEntry(); //Send up the data file (if not the first profile) //if (profileNum > 1) //{ // lastFileName.Clear(); // lastFileName.Append(Logger.fileName); // Logger.closeFile(); // Logger.openFile(); // lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(UDF).ToString())); } // BTConsole.uploadDataFileWhole(lastFileName); //TODO The BTConsole might need to go in a separate thread // //TODO check for successful upload and set telemeteryDone T/F //} //Synch time with GPS //TODO Eventually //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(WFCC).ToString())); } } if (uploadEngrFile) { lastFileName.Clear(); lastFileName.Append(EngrLogger.fileName); EngrLogger.closeFile(); EngrLogger.openFile(); EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(UDF).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(UDF).ToString())); } BTConsole.uploadDataFileWhole(lastFileName); //TODO The BTConsole might need to go in a separate thread uploadEngrFile = false; } //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; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastSetBellows; } if (stateTimedout) { //This line is only for Carson test dive with 1 profile then stay on surface for recovery if (profileNum < 2) { timeSynched = false; telemetryDone = false; surfaceOpsGo = false; stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastSetBellows; } } break; case (CPFStates.AutoballastSetBellows): if (stateEntry) { basicStateEntry(); } //Decide what direction to go and keep checking if (bellowsPosition > (configFile.ABSetBellowsPosition + 0.25)) { if (!Elmo.retracting) { Elmo.retractBellows(configFile.ABSetBellowsJV); } } else if (bellowsPosition < (configFile.ABSetBellowsPosition - 0.25)) { if (!Elmo.extending) { Elmo.extendBellows(configFile.ABSetBellowsJV); } } else //Normal state exit criteria { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastRetract; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastRetract; } break; case (CPFStates.AutoballastRetract): if (stateEntry) { basicStateEntry(); } //slowly retract bellows untill ABDepthThreshold is reached; if (pressure < configFile.ABDepthThreshold) { if (!Elmo.retracting) { Elmo.retractBellows(configFile.ABRetractJV); } } //Normal state exit criteria //if (lpfResults.LPFVel < configFile.ABVelocityThreshold) //This should probably use LPFVel with faster LPF if (pressure >= configFile.ABDepthThreshold) { Elmo.stopMotor(); stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod); //CPFState = CPFStates.RunEngrTest; CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Descend; stateTimedout = false; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Descend; } break; case (CPFStates.Descend): if (stateEntry) { basicStateEntry(); parked = false; depthTableNum = 0; //Make sure the PV set point is negative regardless of typos etc. //PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); BuoyancyControl.resetPVPID(); BuoyancyControl.resetDepthPID(); runDepthPID = false; runPVPID = true; MovingState = MovingStates.transit; VelState = VelStates.none; } if (depthTableNum < MissionConfigFile.descendTable.Length) { DepthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth; parkTime = MissionConfigFile.descendTable[depthTableNum].parkTime; IB = configFile.IB; OB = configFile.OB; } else //when we've finished all stations in descend, keep the old values, they'll be reset in Ascend mode { DepthSetPoint = MissionConfigFile.descendTable[depthTableNum - 1].depth; parkTime = MissionConfigFile.descendTable[depthTableNum - 1].parkTime; } switch (VelState) { case (VelStates.none): break; case (VelStates.down): PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); runPVPID = true; break; case (VelStates.downSlow): //BuoyancyControl.resetPVPID(); PVSetPoint = -1 * System.Math.Abs(configFile.approachVel); runPVPID = true; break; case (VelStates.stop): //BuoyancyControl.resetPVPID(); PVSetPoint = 0.0; runPVPID = true; break; case (VelStates.upSlow): //BuoyancyControl.resetPVPID(); PVSetPoint = System.Math.Abs(configFile.approachVel); runPVPID = true; break; case (VelStates.up): //BuoyancyControl.resetPVPID(); PVSetPoint = 1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); runPVPID = true; break; } switch (MovingState) { case (MovingStates.transit): //do nothing much except wait break; case (MovingStates.enterPark): //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()); EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); } InstrumentHandler.sampleInstruments(depthTableNum); //TODO Probably needs to be done in it's own thread or with a event MovingState = MovingStates.parked; break; case (MovingStates.parked): mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); timeDiff = mtTimeNow - mtParkStartTime; //check to see if we need to sample if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod) { lastParkSampleTime = mtTimeNow; InstrumentHandler.sampleInstruments(depthTableNum); } //check to see if park time has expired if (timeDiff >= MissionConfigFile.descendTable[depthTableNum].parkTime) { MovingState = MovingStates.leavePark; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); } } break; case (MovingStates.leavePark): EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); MovingState = MovingStates.transit; depthTableNum = depthTableNum + 1; break; case (MovingStates.sample): InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; MovingState = MovingStates.transit; break; case (MovingStates.exitDescend): Elmo.stopMotor(); runPVPID = false; runDepthPID = false; stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; MovingState = MovingStates.transit; VelState = VelStates.none; stateTimedout = false; break; } //above DepthSetPoint & Outter Bound if (pressure < (DepthSetPoint - OB)) { VelState = VelStates.down; } //approach velocity from above depth target if (pressure > (DepthSetPoint - OB) && pressure < (DepthSetPoint - IB) && parkTime > TimeSpan.Zero) { VelState = VelStates.downSlow; } //deadband where PVSetPoint = 0; this may also trigger enterPark state, but only if coming from transit if (pressure > (DepthSetPoint - IB) && pressure < (DepthSetPoint + IB) && parkTime > TimeSpan.Zero) { VelState = VelStates.stop; //move into park routine only if coming from transit moe if (MovingState == MovingStates.transit) { MovingState = MovingStates.enterPark; } } //approach from below depth target if (pressure > (DepthSetPoint + IB) && pressure < (DepthSetPoint + OB) && parkTime > TimeSpan.Zero) { VelState = VelStates.upSlow; } //below DepthSetPoint + Outterbound NOTE: unlikely, but possible in descend mode if (pressure > (DepthSetPoint + OB) && parkTime > TimeSpan.Zero) { VelState = VelStates.up; } //sample state - triggers when passing target depth, but with no hold if (pressure > DepthSetPoint && parkTime == TimeSpan.Zero) { MovingState = MovingStates.sample; } //Exit CPFState.Descned if (depthTableNum >= MissionConfigFile.descendTable.Length) { MovingState = MovingStates.exitDescend; } if (stateTimedout) { PVSetPoint = 0.0; runPVPID = false; runDepthPID = false; stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; } break; case (CPFStates.Ascend): if (stateEntry) { basicStateEntry(); parked = false; depthTableNum = 0; BuoyancyControl.resetPVPID(); BuoyancyControl.resetDepthPID(); runDepthPID = false; runPVPID = true; MovingState = MovingStates.transit; VelState = VelStates.none; Debug.Print("ASCEND STATEENTRY"); } if (depthTableNum <= MissionConfigFile.ascendTable.Length -1) { DepthSetPoint = MissionConfigFile.ascendTable[depthTableNum].depth; parkTime = MissionConfigFile.ascendTable[depthTableNum].parkTime; IB = configFile.IB; OB = configFile.OB; } else //when we've finished all stations in ascend, exit, and go to initialize { //DepthSetPoint = 0; //parkTime = TimeSpan.Zero; } //eliminate IB & OB for last station; float needs to hit surface if (depthTableNum == MissionConfigFile.ascendTable.Length - 1) { IB = 0; OB = 0; } switch (VelState) { case (VelStates.none): break; case (VelStates.down): PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPVPID = true; break; case (VelStates.downSlow): //BuoyancyControl.resetPVPID(); PVSetPoint = -1 * System.Math.Abs(configFile.approachVel); runPVPID = true; break; case (VelStates.stop): //BuoyancyControl.resetPVPID(); PVSetPoint = 0.0; runPVPID = true; break; case (VelStates.upSlow): //BuoyancyControl.resetPVPID(); PVSetPoint = System.Math.Abs(configFile.approachVel); runPVPID = true; break; case (VelStates.up): //BuoyancyControl.resetPVPID(); PVSetPoint = 1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPVPID = true; break; } switch (MovingState) { case (MovingStates.transit): //do nothing except wait break; case (MovingStates.enterPark): //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()); EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbTemp).ToString())); } InstrumentHandler.sampleInstruments(depthTableNum); //TODO Probably needs to be done in it's own thread or with a event MovingState = MovingStates.parked; break; case (MovingStates.parked): mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); timeDiff = mtTimeNow - mtParkStartTime; //check to see if we need to sample if ((mtTimeNow - lastParkSampleTime) >= MissionConfigFile.parkSamplePeriod) { lastParkSampleTime = mtTimeNow; InstrumentHandler.sampleInstruments(depthTableNum); } //check to see if park time has expired if (timeDiff >= MissionConfigFile.ascendTable[depthTableNum].parkTime) { MovingState = MovingStates.leavePark; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); } depthTableNum = depthTableNum + 1; } break; case (MovingStates.leavePark): EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); MovingState = MovingStates.transit; break; case (MovingStates.sample): InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; MovingState = MovingStates.transit; break; case (MovingStates.exitAscend): Elmo.stopMotor(); runPVPID = true; runDepthPID = false; stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); //CPFState = CPFStates.Ascend; MovingState = MovingStates.transit; stateTimedout = false; break; } //above DepthSetPoint & Outter Bound if (pressure < (DepthSetPoint - OB)) { VelState = VelStates.down; } //approach velocity from above depth target if (pressure > (DepthSetPoint - OB) && pressure < (DepthSetPoint - IB) && parkTime > TimeSpan.Zero) { VelState = VelStates.downSlow; } //deadband where PVSetPoint = 0; this may also trigger enterPark state, but only if coming from transit if (pressure > (DepthSetPoint - IB) && pressure < (DepthSetPoint + IB) && parkTime > TimeSpan.Zero) { VelState = VelStates.stop; //move into park routine only if coming from transit moe if (MovingState == MovingStates.transit) { MovingState = MovingStates.enterPark; } } //approach from below depth target if (pressure > (DepthSetPoint + IB) && pressure < (DepthSetPoint + OB) && parkTime > TimeSpan.Zero) { VelState = VelStates.upSlow; } //below DepthSetPoint + Outterbound sbTemp.Clear(); sbTemp.Append((DepthSetPoint + OB).ToString()); if (pressure > (DepthSetPoint + OB)) { VelState = VelStates.up; } //sample state - triggers when passing target depth, but with no hold if (pressure < DepthSetPoint && parkTime == TimeSpan.Zero) { MovingState = MovingStates.sample; } //Exit CPFState.Descned if (depthTableNum >= MissionConfigFile.ascendTable.Length) { MovingState = MovingStates.transit; VelState = VelStates.up; } //check to see if we've hit the surface if (pressure < configFile.surfacePressure) { MovingState = MovingStates.exitAscend; Elmo.stopMotor(); runPVPID = false; runDepthPID = false; stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); CPFState = CPFStates.Initialize; stateTimedout = false; } if (stateTimedout) { stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Initialize; } break; case (CPFStates.Anchor): if (stateEntry) { basicStateEntry(); initialAnchorBP = bellowsPosition; } //normal state exit criteria goes here //if (true) //{ // stateEntry = true; // stateTimedout = false; // EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); // //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } // CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod); // CPFState = CPFStates.DeAnchor; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.DeAnchor; } break; case (CPFStates.DeAnchor): if (stateEntry) { basicStateEntry(); Elmo.extendBellows(configFile.deanchorJV); } //normal state exit criteria goes here if (lpfResults.LPFVel >= configFile.deanchorVelocityThreshold) { Elmo.stopMotor(); stateEntry = true; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod); //CPFState = CPFStates.RunEngrTest; CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; stateTimedout = false; } if (stateTimedout) { stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; } break; case (CPFStates.Surface): if (stateEntry) { basicStateEntry(); } //normal state exit criteria goes here //if (true) //{ // stateEntry = true; // stateTimedout = false; // EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); // //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(Logger.getPrefix(sbStateExit).ToString())); } // CPFStateTimer.Change(configFile.initializeTimeout, configFile.timeoutDefaultPeriod); // CPFState = CPFStates.Initialize; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Initialize; } break; case (CPFStates.EmergencyAscend): if (stateEntry) { basicStateEntry(); CPFStateTimer = null; EAOnSurface = false; //TODO stop everything else } if (bellowsPosition < (configFile.bellowsEAPosition - 1.0)) { if (!Elmo.extending) Elmo.extendBellows(configFile.EAJV); } else if (bellowsPosition > (configFile.bellowsEAPosition + 1.0)) { if (!Elmo.retracting) Elmo.retractBellows(configFile.EAJV); } else { 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): basicStateEntry(); break; case (CPFStates.Exit): basicStateEntry(); missionRun = false; //Make sure Elmo.stopMotor finishes Thread.Sleep(1000); break; } //if (MessageQueue.Count > 0) //{ // processMessageQueue(); //} if (sQueue.Count > 0) { processStructQ(); } writeSystemStateMessage(); Thread.Sleep(configFile.SMSleepPeriod); } EngrLogger.closeFile(); EngrLogger.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; public static double batteryBusAmps = double.NaN; public static double amps12V = double.NaN; public static double amps33V = double.NaN; private static double[] CN0254Volts = new double[8]; private static DGH.LPFResults lpfResults; private static readonly StringBuilder SBE41Header = new StringBuilder("SBE41 Message"); private static readonly StringBuilder ADuCHeader = new StringBuilder("ADuC Message"); private static readonly StringBuilder velPIDHeader = new StringBuilder("Vel PID Values"); private static readonly StringBuilder depthPIDHeader = new StringBuilder("Depth PID Values"); private static readonly StringBuilder sbPMQ = new StringBuilder(256); private static readonly StringBuilder sbConsoleCommand = new StringBuilder(32); private static readonly StringBuilder consoleCommandNONE = new StringBuilder("NONE"); private static readonly StringBuilder CN0254Header = new StringBuilder("CN0254 Message"); private static readonly StringBuilder MTSHeader = new StringBuilder("MTS Message"); private static readonly StringBuilder DGHHeader = new StringBuilder("DGH Message"); //private static void processMessageQueue() //{ // while (Program.MessageQueue.Count > 0) // { // 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 (runPVPID || runDepthPID) // { // if (runPVPID) // { // pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel); // if (verbosityLevel > 2) // { // EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(velPIDHeader), // EngrLogger.ColumnNums.PID_setPoint, pidLogValues.setPoint, // EngrLogger.ColumnNums.PID_PV, pidLogValues.PV, // EngrLogger.ColumnNums.PID_y1, pidLogValues.y1, // EngrLogger.ColumnNums.PID_y2, pidLogValues.y2, // EngrLogger.ColumnNums.PID_I, pidLogValues.I, // EngrLogger.ColumnNums.PID_v, pidLogValues.v, // EngrLogger.ColumnNums.PID_u, pidLogValues.u, // EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source // } // } // if (runDepthPID) // { // pidLogValues = BuoyancyControl.PIDDepth(DepthSetPoint, pressure); // if (verbosityLevel > 2) // { // EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(depthPIDHeader), // EngrLogger.ColumnNums.PID_setPoint, pidLogValues.setPoint, // EngrLogger.ColumnNums.PID_PV, pidLogValues.PV, // EngrLogger.ColumnNums.PID_y1, pidLogValues.y1, // EngrLogger.ColumnNums.PID_y2, pidLogValues.y2, // EngrLogger.ColumnNums.PID_I, pidLogValues.I, // EngrLogger.ColumnNums.PID_v, pidLogValues.v, // EngrLogger.ColumnNums.PID_u, pidLogValues.u, // EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source // } // } // } // if (verbosityLevel > 2) // { // EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(SBE41Header), // EngrLogger.ColumnNums.pressureTimestamp, SBE41.pressureTimestamp, // EngrLogger.ColumnNums.pressure, pressure, // EngrLogger.ColumnNums.platformLPFVel, lpfResults.LPFVel, // EngrLogger.ColumnNums.platformDiffVel, lpfResults.diffVel, // EngrLogger.ColumnNums.platformLPFAcc, lpfResults.LPFAcc, // EngrLogger.ColumnNums.pressureAverage, lpfResults.pressureAverage, // EngrLogger.ColumnNums.pressureVariance, lpfResults.pressureVariance); //TODO Should do this in a way that the message is time stamped at the source // } // if (lpfResults.LPFPressure > configFile.maxPressure) // { // Elmo.stopMotor(); // runPVPID = false; // runDepthPID = false; // EngrLogger.write(EngrLogger.ColumnNums.comment, // EngrLogger.getPrefix(DepthError)); // CPFStateTimer = null; // CPFState = CPFStates.EmergencyAscend; // } // if ((lpfResults.pressureVariance > -0.01) && (lpfResults.pressureVariance < 0.01) && // (pressure > configFile.anchorPressureMinimum) && (CPFState == CPFStates.Descend)) // { // EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BottomDetect)); // Elmo.stopMotor(); // runPVPID = false; // runDepthPID = false; // CPFStateTimer.Change(configFile.anchorTimeout, configFile.timeoutDefaultPeriod); // CPFState = CPFStates.Anchor; // } // writeSystemStateMessage(); // //TODO Check for anchor condition, if true transition to anchor // } // 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)); // EngrLogger.write(EngrLogger.ColumnNums.comment, sbPMQ); // } // } //} private static byte[] returnedByteArray = new byte[StructQueue.qRecordWidth]; private static StringBuilder returnedSB = new StringBuilder(StructQueue.qRecordWidth); private static DateTime timeNow; private static void processStructQ() { while (Program.sQueue.Count > 0) { lock (programLock) { qStructure = sQueue.Dequeue(); } switch (qStructure.qRecordType) { case (StructQueue.QRecordType.DGH): pressure = DGH.parsePressureSample(qStructure.byteArray); lpfResults = DGH.LPFPressure(pressure); //below is copied from SBE41 case; LPF code from SBE41 copied to DGH class as well. if (runPVPID || runDepthPID) { if (runPVPID) { pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.LPFVel); if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, velPIDHeader), EngrLogger.ColumnNums.PID_setPoint, pidLogValues.setPoint, EngrLogger.ColumnNums.PID_PV, pidLogValues.PV, EngrLogger.ColumnNums.PID_y1, pidLogValues.y1, EngrLogger.ColumnNums.PID_y2, pidLogValues.y2, EngrLogger.ColumnNums.PID_I, pidLogValues.I, EngrLogger.ColumnNums.PID_v, pidLogValues.v, EngrLogger.ColumnNums.PID_u, pidLogValues.u, EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } } if (runDepthPID) { pidLogValues = BuoyancyControl.PIDDepth(DepthSetPoint, pressure); if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, depthPIDHeader), EngrLogger.ColumnNums.PID_setPoint, pidLogValues.setPoint, EngrLogger.ColumnNums.PID_PV, pidLogValues.PV, EngrLogger.ColumnNums.PID_y1, pidLogValues.y1, EngrLogger.ColumnNums.PID_y2, pidLogValues.y2, EngrLogger.ColumnNums.PID_I, pidLogValues.I, EngrLogger.ColumnNums.PID_v, pidLogValues.v, EngrLogger.ColumnNums.PID_u, pidLogValues.u, EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } } } if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, DGHHeader), EngrLogger.ColumnNums.pressureTimestamp, qStructure.timeStamp, EngrLogger.ColumnNums.pressure, pressure, EngrLogger.ColumnNums.platformLPFVel, lpfResults.LPFVel, EngrLogger.ColumnNums.platformDiffVel, lpfResults.diffVel, EngrLogger.ColumnNums.platformLPFAcc, lpfResults.LPFAcc, EngrLogger.ColumnNums.pressureAverage, lpfResults.pressureAverage, EngrLogger.ColumnNums.pressureVariance, lpfResults.pressureVariance); //TODO Should do this in a way that the message is time stamped at the source Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } if (lpfResults.LPFPressure > configFile.maxPressure) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(DepthError)); CPFStateTimer = null; CPFState = CPFStates.EmergencyAscend; } if ((lpfResults.pressureVariance > -0.01) && (lpfResults.pressureVariance < 0.01) && (pressure > configFile.anchorPressureMinimum) && (CPFState == CPFStates.Descend)) { EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BottomDetect)); Elmo.stopMotor(); runPVPID = false; runDepthPID = false; CPFStateTimer.Change(configFile.anchorTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Anchor; } break; case (StructQueue.QRecordType.SBE41): pressure = SBE41.parse(qStructure.byteArray); //lpfResults = SBE41.LPFPressure(pressure); if (runPVPID || runDepthPID) { if (runPVPID) { pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel); if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, velPIDHeader), EngrLogger.ColumnNums.PID_setPoint, pidLogValues.setPoint, EngrLogger.ColumnNums.PID_PV, pidLogValues.PV, EngrLogger.ColumnNums.PID_y1, pidLogValues.y1, EngrLogger.ColumnNums.PID_y2, pidLogValues.y2, EngrLogger.ColumnNums.PID_I, pidLogValues.I, EngrLogger.ColumnNums.PID_v, pidLogValues.v, EngrLogger.ColumnNums.PID_u, pidLogValues.u, EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } } if (runDepthPID) { pidLogValues = BuoyancyControl.PIDDepth(DepthSetPoint, pressure); if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, depthPIDHeader), EngrLogger.ColumnNums.PID_setPoint, pidLogValues.setPoint, EngrLogger.ColumnNums.PID_PV, pidLogValues.PV, EngrLogger.ColumnNums.PID_y1, pidLogValues.y1, EngrLogger.ColumnNums.PID_y2, pidLogValues.y2, EngrLogger.ColumnNums.PID_I, pidLogValues.I, EngrLogger.ColumnNums.PID_v, pidLogValues.v, EngrLogger.ColumnNums.PID_u, pidLogValues.u, EngrLogger.ColumnNums.PID_uCPS, pidLogValues.uCPS); //TODO Should do this in a way that the message is time stamped at the source Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } } } if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, SBE41Header), EngrLogger.ColumnNums.pressureTimestamp, SBE41.pressureTimestamp, EngrLogger.ColumnNums.pressure, pressure, EngrLogger.ColumnNums.platformLPFVel, lpfResults.LPFVel, EngrLogger.ColumnNums.platformDiffVel, lpfResults.diffVel, EngrLogger.ColumnNums.platformLPFAcc, lpfResults.LPFAcc, EngrLogger.ColumnNums.pressureAverage, lpfResults.pressureAverage, EngrLogger.ColumnNums.pressureVariance, lpfResults.pressureVariance); //TODO Should do this in a way that the message is time stamped at the source Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } if (lpfResults.LPFPressure > configFile.maxPressure) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(DepthError)); CPFStateTimer = null; CPFState = CPFStates.EmergencyAscend; } if ((lpfResults.pressureVariance > -0.01) && (lpfResults.pressureVariance < 0.01) && (pressure > configFile.anchorPressureMinimum) && (CPFState == CPFStates.Descend)) { EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BottomDetect)); Elmo.stopMotor(); runPVPID = false; runDepthPID = false; CPFStateTimer.Change(configFile.anchorTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Anchor; } break; case (StructQueue.QRecordType.Optode): returnedByteArray = Optode.qHandler(qStructure); EngrLogger.qWriter(returnedByteArray); break; case (StructQueue.QRecordType.MTS): bellowsPosition = MTS.parse(qStructure.byteArray); if (verbosityLevel > 2) { //returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(qStructure.timeStamp, qStructure.state, MTSHeader), // EngrLogger.ColumnNums.bellowsPosition, bellowsPosition); //Array.Clear(returnedByteArray, 0, returnedByteArray.Length); //Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); //EngrLogger.qWriter(returnedByteArray); } if (CPFState != CPFStates.EmergencyAscend) { if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition)) { Elmo.stopMotor(); CPFStateTimer = null; runPVPID = false; runDepthPID = false; EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BPError)); CPFState = CPFStates.EmergencyAscend; } } break; case (StructQueue.QRecordType.CN0254): timeNow = DateTime.Now; CN0254Volts = CN0254.scanAll(); bellowsPosition = (CN0254Volts[0] * configFile.cn0254CH0Scale) + configFile.cn0254CH0Offset; batteryBusVolts = (CN0254Volts[1] * configFile.cn0254CH1Scale) + configFile.cn0254CH1Offset; batteryBusAmps = (CN0254Volts[2] * configFile.cn0254CH2Scale) + configFile.cn0254CH2Offset; amps12V = (CN0254Volts[3] * configFile.cn0254CH3Scale) + configFile.cn0254CH3Offset; amps33V = (CN0254Volts[4] * configFile.cn0254CH4Scale) + configFile.cn0254CH4Offset; if (verbosityLevel > 2) { returnedSB = EngrLogger.formatRecord(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(timeNow, CPFState, CN0254Header), EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts, EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps, EngrLogger.ColumnNums.Amps12V, amps12V, EngrLogger.ColumnNums.Amps33V, amps33V); Array.Clear(returnedByteArray, 0, returnedByteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(returnedSB.ToString()), returnedByteArray, returnedSB.Length); EngrLogger.qWriter(returnedByteArray); } if (CPFState != CPFStates.EmergencyAscend) { if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition)) { Elmo.stopMotor(); CPFStateTimer = null; runPVPID = false; runDepthPID = false; EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BPError)); CPFState = CPFStates.EmergencyAscend; } if (batteryBusVolts < configFile.minBatteryBusVolts) { Elmo.stopMotor(); CPFStateTimer = null; runPVPID = false; runDepthPID = false; EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(BBVError)); CPFState = CPFStates.EmergencyAscend; } } break; case (StructQueue.QRecordType.Console): processConsoleCommand(qStructure.byteArray); break; } //sbTemp.Clear(); //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); //Debug.Print(sbTemp.ToString()); } } private static void CPFStateTimerCallback(object o) { stateTimedout = true; } private static StructQueue.qStruct ADCQStruct = new StructQueue.qStruct() { qRecordType = StructQueue.QRecordType.CN0254, byteArray = new byte[StructQueue.qRecordWidth] }; private static object ADCTimerLock = new Object(); private static void ADCTimerCallback(object o) { ADCQStruct.timeStamp = DateTime.Now; ADCQStruct.state = CPFState; lock (ADCTimerLock) { Program.sQueue.Enqueue(ADCQStruct); } //readADC = true; } private static StringBuilder consoleCommandComment = new StringBuilder(); private static void processConsoleCommand(byte[] consoleCommandMessage) { sbConsoleCommand.Clear(); sbConsoleCommand.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, (byte)0) - 1))); consoleCommandComment.Clear(); consoleCommandComment.Append("Processing Console Command: "); consoleCommandComment.Append(sbConsoleCommand); EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(consoleCommandComment)); switch (sbConsoleCommand.ToString().ToUpper()) { case "DIR": { break; } case "DOWNENGR": { break; } case "DOWNMIS": { break; } case "UPDATAFILE": { break; } case "UPENGR": { uploadEngrFile = true; break; } case "GO": { surfaceOpsGo = true; break; } case "EXIT": { CPFState = CPFStates.Exit; break; } default: { break; } } sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); } private static void writeSystemStateMessage() { sbTemp.Clear(); sbTemp.Append("CPF State,"); sbTemp.Append("depthTableNum = "); sbTemp.Append(depthTableNum.ToString()); sbTemp.Append(","); if (runPVPID) sbTemp.Append("PVPID Running"); sbTemp.Append(","); if (runDepthPID) sbTemp.Append("DepthPID Running"); sbTemp.Append(","); if (parked) sbTemp.Append("Parked"); sbTemp.Append(","); if (Elmo.extending) sbTemp.Append("Extending"); sbTemp.Append(","); if (Elmo.retracting) sbTemp.Append("Retracting"); sbTemp.Append(","); if (Elmo.motorStopped) sbTemp.Append("MotorStopped"); sbTemp.Append(","); if (CPFState == CPFStates.Descend || CPFState == CPFStates.Ascend) { sbTemp.Append(Program.MovingStateNames[(int)Program.MovingState]); sbTemp.Append(","); sbTemp.Append(Program.VelStateNames[(int)Program.VelState]); sbTemp.Append(","); } //sbTemp.Append("DepthSetPoint: "); //sbTemp.Append(DepthSetPoint.ToString()); //sbTemp.Append(","); //sbTemp.Append("pressure: "); //sbTemp.Append(pressure.ToString()); //sbTemp.Append(","); EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(sbTemp), EngrLogger.ColumnNums.pressure, pressure, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts); } private static void basicStateEntry() { Elmo.stopMotor(); EngrLogger.qWriter(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); runPVPID = false; runDepthPID = false; stateEntry = false; stateTimedout = false; } } }