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 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, RunEngrTest, Initialize, SurfaceOpsSetBellows, PreMissionDelay, SurfaceOps, AutoballastSetBellows, AutoballastRetract, Descend, Anchor, DeAnchor, Ascend, Surface, EmergencyAscend, ProcessErrors, Exit, lastState } public static CPFStates CPFState; 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 sQ = new StructQueue(); private static StructQueue.qStruct qS = 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 double PVSetPoint = 0.0; private static double DepthSetPoint = 0.0; public static int depthTableNum = -1; private static int profileNum = 0; private static int verbosityLevel = 2; 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 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(); 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(); 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) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; firstEngrTest = true; stateTimedout = false; } //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; lock (programLock) { Program.MessageQueue.Enqueue(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; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Initialize; } break; case (CPFStates.Initialize): lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } runPVPID = false; runDepthPID = false; profileNum = profileNum + 1; depthTableNum = 0; sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); stateEntry = true; stateTimedout = false; 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) { runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; } //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; lock (programLock) { Program.MessageQueue.Enqueue(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; 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) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; } if (surfaceOpsGo) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); } CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } if (stateTimedout) { stateEntry = true; stateTimedout = false; 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) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; stateTimedout = false; //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(); 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; lock (programLock) { Program.MessageQueue.Enqueue(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; 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) { runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.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(); stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); } CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastRetract; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; 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) { runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } Elmo.retractBellows(configFile.ABRetractJV); stateEntry = false; } //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; lock (programLock) { Program.MessageQueue.Enqueue(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; 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) { 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; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; } //A positive park time means a transition to park if (!parked) { if (pressure >= MissionConfigFile.descendTable[depthTableNum].depth) { //check to see if this is a park station //if it is, take a sample and set park = true if (MissionConfigFile.descendTable[depthTableNum].parkTime > TimeSpan.Zero) { PVSetPoint = 0.0; DepthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth; runPVPID = false; BuoyancyControl.resetDepthPID(); runDepthPID = true; //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(EngrLogger.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; if (depthTableNum < MissionConfigFile.descendTable.Length) PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); } } } else //If we are parked { 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; if (depthTableNum < MissionConfigFile.descendTable.Length) PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); //runDepthPID = false; //BuoyancyControl.resetPVPID(); runPVPID = true; parked = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); } } } //check to see if this is that last descend station if (depthTableNum >= MissionConfigFile.descendTable.Length) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); } CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; stateTimedout = false; } if (stateTimedout) { PVSetPoint = 0.0; runPVPID = false; runDepthPID = false; stateEntry = true; stateTimedout = false; 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) { lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } parked = false; depthTableNum = 0; BuoyancyControl.resetPVPID(); BuoyancyControl.resetDepthPID(); runDepthPID = false; runPVPID = true; stateEntry = false; //Check to see if we need to descend to first ascend depth if (pressure > MissionConfigFile.ascendTable[depthTableNum].depth) { //Make sure the PV set point is positive regardless of typos etc. PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); descendToFirstAscend = false; } else { //need to descend to first ascend station PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); descendToFirstAscend = true; } } //Do all this if were not at the last depth in the ascend table if we are, just check for the surface if (depthTableNum < MissionConfigFile.ascendTable.Length - 1) { //First deal with descend to first ascend station if (descendToFirstAscend) { if (pressure >= MissionConfigFile.ascendTable[depthTableNum].depth) { InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; if (depthTableNum < MissionConfigFile.ascendTable.Length) PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); descendToFirstAscend = false; } } else { if (!parked) { //check to see if we've passed the target depth if (pressure <= MissionConfigFile.ascendTable[depthTableNum].depth) { //First check to see if it is a park depth if (MissionConfigFile.ascendTable[depthTableNum].parkTime > TimeSpan.Zero) { PVSetPoint = 0.0; //DepthSetPoint = MissionConfigFile.ascendTable[depthTableNum].depth; //runPVPID = false; //BuoyancyControl.resetDepthPID(); //runDepthPID = true; //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(EngrLogger.getPrefix(sbTemp).ToString())); } parked = true; //Now take a sample but don't move on to next sample depth until park period is over InstrumentHandler.sampleInstruments(depthTableNum); } //If it isn't a park depth, take a sample and increment the depth table number else { InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; if (depthTableNum < MissionConfigFile.ascendTable.Length) PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); } } } else { //If we are parked, check the time for sampling and to see if park time has ended 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; //runDepthPID = false; //BuoyancyControl.resetPVPID(); runPVPID = true; if (depthTableNum < MissionConfigFile.ascendTable.Length) PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); parked = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(EPP).ToString())); } } } } //At this point we're aimed at the next sample depth } else { //check to see if we've hit the surface if (pressure < configFile.surfacePressure) { Elmo.stopMotor(); runPVPID = false; runDepthPID = false; stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateExit).ToString())); } CPFState = CPFStates.Initialize; stateTimedout = false; } } if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Initialize; } break; case (CPFStates.Anchor): if (stateEntry) { runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } stateEntry = false; initialAnchorBP = bellowsPosition; } //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, configFile.timeoutDefaultPeriod); // CPFState = CPFStates.DeAnchor; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; 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) { runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } Elmo.extendBellows(configFile.deanchorJV); stateEntry = false; } //normal state exit criteria goes here if (lpfResults.LPFVel >= configFile.deanchorVelocityThreshold) { Elmo.stopMotor(); stateEntry = true; lock (programLock) { Program.MessageQueue.Enqueue(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; 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) { runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.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, configFile.timeoutDefaultPeriod); // CPFState = CPFStates.Initialize; //} if (stateTimedout) { stateEntry = true; stateTimedout = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Initialize; } break; case (CPFStates.EmergencyAscend): if (stateEntry) { CPFStateTimer = null; Elmo.stopMotor(); runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.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 + 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): runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } break; case (CPFStates.Exit): Elmo.stopMotor(); runPVPID = false; runDepthPID = false; lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateEntry).ToString())); } missionRun = false; //Make sure Elmo.stopMotor finishes Thread.Sleep(1000); break; } if (MessageQueue.Count > 0) { processMessageQueue(); } if(sQ.Count > 0) { processSQ(); } //It might be more consistent if the ADC results were returned on the queue if (readADC) { 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) { EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(CN0254Header), //TODO Should do this in a way that the message is time stamped at the source EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts, EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps, EngrLogger.ColumnNums.Amps12V, amps12V, EngrLogger.ColumnNums.Amps33V, amps33V); } 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; } } readADC = false; } 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 SBE41.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 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 (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); } } while (Program.MessageQueue.Count > 0); } private static void processSQ() { lock (programLock) { qS = sQ.Dequeue(); } qS.qDel(qS); //sbTemp.Clear(); //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qS.byteArray)); //Debug.Print(sbTemp.ToString()); } private static void CPFStateTimerCallback(object o) { stateTimedout = true; } private static void ADCTimerCallback(object o) { readADC = 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); 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("depthNum = "); 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(","); EngrLogger.write(EngrLogger.ColumnNums.comment, EngrLogger.getPrefix(sbTemp), EngrLogger.ColumnNums.pressure, pressure, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts); } } }