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.Processor; using BreakContinue.Diagnostics; //TODO List //*** Add watchdog timer //*** Make sure we're taking park samples regardless of which band we're in after we've achieved the park depth //*** Organize transition to CP mode so it happens as quickly as possible, i.e., check for proper response instead of preset fixed delays //*** Hold depth while transitioning to CP mode, maybe make hold depth if/then/else a method like the old DepthPID //*** Add try/catch/finally resulting in recovery mode starting in every method (that needs it) and bubbling up to main() //** Need a no wait way to dump CP data //** Change h value in PID loop when in CP mode //** Check Extend retract logic at beginning of ascend state //** Turn EA state into low power recovery mode state //** Continue implementing the errorHandler //** Add a deterministic way to check for correct response from SBE41 //** Get rid of Thread.Sleep anywhere in the Main loop //** Think about using bluetooth instead of USB for download/debug port //** Deal with the case on entering ascend state where desired depth is deeper than current depth //* Create methods and classes for things that get repeated like // queing a message // Error handler error counters // State exit and state timedout // Serial Instrument class with standard Serial port event handler and error handlers //* Deal with S> pressure response //* pre-Encode and make static UTF8 all constant strings and stringbuilders. Put them all in a text class. Maybe convert to byte array. //* Log instrument handler calls //* Investigate whether we reallly need a lock for methods in the same thread //* Add serial port error received error handlers //* Figure out a solid way to start and stop CTD, Buoyancy control, and everything else that needs stopping/starting //* Change state timedout functionality to force state transitions even if SM hangs up, maybe use execution constraints //* To get a line count: ctrl-shift-F find what: "^:b*[^:b#/]+.*$" (no quotes), Find options "use regular expressions", Look at these file types *.cs //* Maybe discard serial buffers at end of every serial port event handler //* Add execution constraints to first part of program (before state machine timeouts kick in) //* Harmonize the way anything that puts stuff on the queue formats the record. Maybe every class that puts something on the queue should format the whole record in one of the class methods as opposed to letting the processQueue case statement do it. //* Harmonize the way stuff is written to the engineering log //* Get rid of Depth PID stuff and moving state and velocity state //* Make all the serial port handlers look like the SBE41 handler and the optode EH //* Add comments describing public variables and methods to each class //* Think about getting rid of verbosity levels for engineering log. namespace CPF { 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, InitializeMission, InitializeProfile, 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, sendSBD, holdDepth, readOptode, getDirectory, exitProgram } private static EngrTests engrTest = EngrTests.none; private static Timer CPFStateTimer; private static Timer ADCTimer; private static Timer PTHTimer; 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 runPVPID = false; private static bool uploadEngrFile = false; private static bool SORecoveryMode = true; private static bool gotParkSample = false; private static bool checkStuckPressure = false; 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 mtAnchorStartTime; private static TimeSpan lastAnchorSampleTime; private static TimeSpan mtTimeNow; 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; private static double initialAnchorPressure = double.NaN; public static int depthTableNum = -1; private static int profileNum = 0; private static int verbosityLevel = 5; private static int engrTestSpeed = 0; private static int ABRetractThresholdCount = 0; public static int signalQuality = -1; 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); private static readonly StringBuilder testFileName = new StringBuilder(128); private static readonly StringBuilder UDF = new StringBuilder("Uploading Data File"); private static readonly StringBuilder WFCC = new StringBuilder("Waiting for Console Command"); private static readonly StringBuilder BBVError = new StringBuilder("Battery Bus Voltage below threshold, beginning Emergency Ascend"); private static readonly StringBuilder BPError = new StringBuilder("Bellows position exceeds limits, beginning Emergency Ascend"); private static readonly StringBuilder EPP = new StringBuilder("Exiting Park Period"); private static readonly StringBuilder toDeepError = new StringBuilder("Current depth exceeds max pressure, beginning Emergency Ascend"); private static readonly StringBuilder BottomDetect = new StringBuilder("Bottom detected based on pressure variance value"); private static readonly StringBuilder TimerNull = new StringBuilder("Surface Ops set state timer null after first profile"); private static readonly StringBuilder sbEAStateExit = new StringBuilder("Exiting Emergency Ascend State"); private static readonly StringBuilder pressureStuckError = new StringBuilder("Pressure isn't changing, beginning Emergency Ascend"); private static readonly StringBuilder exitAnchorTripped = new StringBuilder("Anchor state exit threshold tripped"); private static TimeSpan engrTestStartTime; private static TimeSpan engrTestCyclePeriod = new TimeSpan(0, 0, 30); private static bool firstEngrTest = true; private static bool engrTestExtend = true; // names for each of the ascend and descend substates private static StringBuilder sbAD_escendNameNone = new StringBuilder("None"); private static StringBuilder sbAD_escendNameDescending = new StringBuilder("Descending"); private static StringBuilder sbAD_escendNameAscending = new StringBuilder("Ascending"); private static StringBuilder sbAD_escendNameParkingDown = new StringBuilder("Parking Down"); private static StringBuilder sbAD_escendNameDownSlow = new StringBuilder("Down Slow"); private static StringBuilder sbAD_escendNameInBand = new StringBuilder("In Band"); private static StringBuilder sbAD_escendNameUpSlow = new StringBuilder("Up Slow"); private static StringBuilder sbAD_escendNameParkingUp = new StringBuilder("Parking Up"); private static StringBuilder sbAD_escendStateName = new StringBuilder(); private static int surfaceOpsSubstate = -1; private static TimeSpan surfaceOpsSubstateStartTime; private static TimeSpan surfaceOpsSubstate1Timeout = new TimeSpan(0, 0, 2); private static TimeSpan surfaceOpsSubstate2Timeout = new TimeSpan(0, 0, 5); private static TimeSpan surfaceOpsSubstate3Timeout = new TimeSpan(0, 0, 20); private static TimeSpan surfaceOpsSubstate4Timeout = new TimeSpan(0, 0, 10); private static int initMissionState = 0; private static TimeSpan initMissionStartTime; private static TimeSpan initMissionState1Timeout = new TimeSpan(0, 0, 30); private static TimeSpan initMissionState2Timeout = new TimeSpan(0, 1, 0); private static int startCPModeState = 0; private static TimeSpan startCPModeStartTime; private static TimeSpan startCPModeState1Timeout = new TimeSpan(0, 0, 30); private static TimeSpan startCPModeState2Timeout = new TimeSpan(0, 0, 1); private static TimeSpan startCPModeState3Timeout = new TimeSpan(0, 0, 2); public static void Main() { #region Debug.Print("Program Started"); Utility.SetLocalTime(RealTimeClock.GetDateTime()); //TODO Add synch to GPS CPFUtilities.setStateNames(); BTConsole.OpenConsolePort(); CPFState = CPFStates.NotRunning; CPFStateTimer = new Timer(new TimerCallback(CPFStateTimerCallback), null, 1000000, 1000000); EngrLogger.init(); sbTemp.Clear(); sbTemp.Append("Intialize Elmo"); EngrLogger.writeToColumns(sbTemp); Elmo.init(); sbTemp.Clear(); sbTemp.Append("Intializing PTH and starting PTH and ADC Timers"); EngrLogger.writeToColumns(sbTemp); PTH.initPTH(); ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 0, configFile.ADCSamplePeriod); PTHTimer = new Timer(new TimerCallback(PTHTimerCallback), null, 0, configFile.PTHSamplePeriod); sbTemp.Clear(); sbTemp.Append("Intializing Mission"); EngrLogger.writeToColumns(sbTemp); MissionConfigFile.init(); sbTemp.Clear(); sbTemp.Append("Intializing Instrument Handler"); EngrLogger.writeToColumns(sbTemp); InstrumentHandler.init(); sbTemp.Clear(); sbTemp.Append("Intialize SBE41 (takes up to 10 seconds)"); EngrLogger.writeToColumns(sbTemp); SBE41.Init(); processStructQ(); sbTemp.Clear(); sbTemp.Append("Intialize Optode"); EngrLogger.writeToColumns(sbTemp); Optode.init(); Thread.Sleep(2000); processStructQ(); //sbTemp.Clear(); //sbTemp.Append("Power On uBlox"); //EngrLogger.writeComment(sbTemp).ToString())); //uBlox.PowerOn("COM5", 19200); sbTemp.Clear(); sbTemp.Append("Initialize EVK7 GPS"); EngrLogger.writeToColumns(sbTemp); EVK7GPS.init(); sbTemp.Clear(); sbTemp.Append("Initialize A3LA Iridum Modem"); EngrLogger.writeToColumns(sbTemp); IModem.init(); //sbTemp.Clear(); //sbTemp.Append("Power On Iridium"); //EngrLogger.writeComment(); //Iridium.PowerOn("COM2", 19200); //Process anything that is still in the queue Thread.Sleep(3000); processStructQ(); //Comment out all but one of these //engrTest = EngrTests.stepBellows; //engrTest = EngrTests.runBellows; //engrTest = EngrTests.cycleBellows; //engrTest = EngrTests.readOptode; //engrTest = EngrTests.sendSBD; engrTest = EngrTests.noOps; //engrTest = EngrTests.getDirectory; //engrTest = EngrTests.holdDepth; //engrTest = EngrTests.none; //Uncomment these 2 to run enginering tests //CPFState = CPFStates.RunEngrTest; //CPFStateTimer.Change(configFile.RunEngrTestTimeout, configFile.timeoutDefaultPeriod); //Uncoment these 2 lines to jump to SurfaceOps //CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); //CPFState = CPFStates.SurfaceOps; //Uncomment these 2 lines to run the program normally CPFStateTimer.Change(configFile.InitializeMissionTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.InitializeMission; SORecoveryMode = configFile.RecoveryMode; #endregion while (missionRun) { //try { switch (CPFState) { case (CPFStates.RunEngrTest): #region if (stateEntry) { basicStateEntry(); firstEngrTest = true; } switch (engrTest) { case (EngrTests.none): Elmo.stopMotor(); stateEntry = true; stateTimedout = false; CPFState = CPFStates.InitializeProfile; EngrLogger.writeToColumns(sbStateExit); break; case (EngrTests.noOps): //don't do anything just monitor stuff Elmo.stopMotor(); runPVPID = false; break; case (EngrTests.sendSBD): Elmo.stopMotor(); runPVPID = false; sbTemp.Clear(); sbTemp.Append("Getting GPS PUBX Message"); EngrLogger.writeToColumns(sbTemp); EVK7GPS.getPUBX(); engrTest = EngrTests.noOps; break; case (EngrTests.readOptode): Optode.doSample(); break; case (EngrTests.getDirectory): EngrLogger.getDirectory(); testFileName.Clear(); testFileName.Append(@"\SD\2014-9-25T20-23-14"); BTConsole.uploadDataFileWhole(testFileName); engrTest = EngrTests.noOps; break; case (EngrTests.runBellows): if (firstEngrTest) { engrTestSpeed = 20000; //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): break; case (EngrTests.exitProgram): Elmo.stopMotor(); stateEntry = true; stateTimedout = false; CPFState = CPFStates.Exit; break; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.Exit; } break; #endregion case (CPFStates.InitializeMission): #region if (stateEntry) { basicStateEntry(); sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); } //Sequence through mission initialization steps switch (initMissionState) { case 0: //Get SBE41 settting //Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Sending ds command"); SBE41.displaySettings(); initMissionStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); initMissionState = 1; break; case 1: //Get Optode settings if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - initMissionStartTime) > initMissionState1Timeout) { //Debug.Print("\r\n" + DateTime.Now.TimeOfDay.ToString() + " Starting CTD Timer and getting Optode setup"); SBE41.startCommandModeTimer(); Optode.getAll(); initMissionStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); initMissionState = 2; } break; case 2: //Wait for Optode to finish sending settings if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - initMissionStartTime) > initMissionState2Timeout) { stateEntry = true; EngrLogger.writeToColumns(sbStateExit); CPFState = CPFStates.InitializeProfile; checkStuckPressure = true; } break; } break; #endregion case (CPFStates.InitializeProfile): #region if (stateEntry) { basicStateEntry(); profileNum = profileNum + 1; depthTableNum = 0; anchorPressureCount = 0; } stateEntry = true; EngrLogger.writeToColumns(sbStateExit); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.SOSetBellowsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOpsSetBellows; break; #endregion case (CPFStates.SurfaceOpsSetBellows): #region 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 if ((bellowsPosition <= configFile.SOSetBellowsPosition + 0.5) && (bellowsPosition >= configFile.SOSetBellowsPosition - 0.5)) //Normal state exit criteria { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateExit); if (profileNum == 1) { CPFStateTimer.Change(configFile.preMissionDelayTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.PreMissionDelay; } else { CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } break; #endregion case (CPFStates.PreMissionDelay): #region if (stateEntry) { basicStateEntry(); } if (surfaceOpsGo) { stateEntry = true; stateTimedout = false; surfaceOpsGo = false; EngrLogger.writeToColumns(sbStateExit); CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } if (stateTimedout) { stateEntry = true; stateTimedout = false; surfaceOpsGo = false; EngrLogger.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } break; #endregion case (CPFStates.SurfaceOps): #region if (stateEntry) { basicStateEntry(); timeSynched = false; telemetryDone = false; surfaceOpsGo = false; //Probably should send an error if surfaceOpsSubstate isn't already 0 surfaceOpsSubstate = 0; signalQuality = -1; Optode.doSample(); //After the first profile close the old engr log file and open a new one if (profileNum > 1) { lastFileName.Clear(); lastFileName.Append(EngrLogger.fileName); sbTemp.Clear(); sbTemp.Append("Closing old engr log file: "); sbTemp.Append(lastFileName); EngrLogger.writeToColumns(sbTemp); EngrLogger.closeFile(); EngrLogger.openFile(); sbTemp.Clear(); sbTemp.Append("Opened new engr log file: "); sbTemp.Append(EngrLogger.fileName); EngrLogger.writeToColumns(sbTemp); if (!SORecoveryMode) uploadEngrFile = true; } } //Sequence through steps to send SBD message and then upload last profile engr log switch (surfaceOpsSubstate) { case 0: //Clear modem buffers signalQuality = -1; IModem.sendSBDD2(); sbTemp.Clear(); sbTemp.Append("Sent SBDD2"); EngrLogger.writeToColumns(sbTemp); surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); surfaceOpsSubstate = 1; break; case 1: //Get PUBX message from GPS if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate1Timeout) { EVK7GPS.getPUBX(); sbTemp.Clear(); sbTemp.Append("Sent getPUBX"); EngrLogger.writeToColumns(sbTemp); surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); surfaceOpsSubstate = 2; } break; case 2: //Get signal quality from IModem if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate2Timeout) { IModem.sendCSQ(); sbTemp.Clear(); sbTemp.Append("Sent CSQ"); EngrLogger.writeToColumns(sbTemp); surfaceOpsSubstate = 3; surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); } break; case 3: //Load SBDWT buffer with PUBX message if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate3Timeout) { sbTemp.Clear(); sbTemp.Append("SQ = "); sbTemp.Append(signalQuality.ToString()); sbTemp.Append(","); sbTemp.Append(UTF8Encoding.UTF8.GetChars(pubxByteArray)); Debug.Print(DateTime.Now + "SBDWT message = " + sbTemp.ToString()); IModem.sendSBDWT(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString())); sbTemp.Clear(); sbTemp.Append("Sent SBDWT"); EngrLogger.writeToColumns(sbTemp); surfaceOpsSubstate = 4; surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); } break; case 4: //Send the message with SBDDI if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - surfaceOpsSubstateStartTime) > surfaceOpsSubstate4Timeout) { IModem.sendSBDI(); sbTemp.Clear(); sbTemp.Append("Sent SBDDI"); EngrLogger.writeToColumns(sbTemp); surfaceOpsSubstate = -1; signalQuality = -1; } break; default: break; } if (uploadEngrFile && (surfaceOpsSubstate == -1)) { if (profileNum > 1) { EngrLogger.writeToColumns(UDF); BTConsole.uploadDataFileWhole(lastFileName); //TODO The BTConsole might need to go in a separate thread uploadEngrFile = false; } else { sbTemp.Clear(); sbTemp.Append("Haven't done first profile, no engr log file yet"); BTConsole.SendLine(sbTemp); } } //Normal exit in SORecovery Mode only if surfaceOpsGo = true //Normal exit if not in SORecovery mode when (timeSynced AND telemetryDone) OR surfaceOPSgo = true //No matter what, wait for telemetry to finish sequence if (surfaceOpsSubstate == -1) { if ((SORecoveryMode && surfaceOpsGo) || (!SORecoveryMode && ((timeSynched && telemetryDone) || surfaceOpsGo))) { timeSynched = false; telemetryDone = false; surfaceOpsGo = false; stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateExit); CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastSetBellows; sbTemp.Clear(); sbTemp.Append("Free Memory = "); sbTemp.Append(Debug.GC(false).ToString()); EngrLogger.writeToColumns(sbTemp); } if (stateTimedout) { if (SORecoveryMode) { stateEntry = true; stateTimedout = false; surfaceOpsGo = false; EngrLogger.writeToColumns(sbStateTimedout); CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; sbTemp.Clear(); sbTemp.Append("Free Memory = "); sbTemp.Append(Debug.GC(false).ToString()); EngrLogger.writeToColumns(sbTemp); } else { timeSynched = false; telemetryDone = false; surfaceOpsGo = false; stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateTimedout); CPFStateTimer.Change(configFile.ABSetBellowsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastSetBellows; sbTemp.Clear(); sbTemp.Append("Free Memory = "); sbTemp.Append(Debug.GC(false).ToString()); EngrLogger.writeToColumns(sbTemp); } } } break; #endregion case (CPFStates.AutoballastSetBellows): #region if (stateEntry) { basicStateEntry(); } //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; EngrLogger.writeToColumns(sbStateExit); CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastRetract; } if (stateTimedout) { Elmo.stopMotor(); stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ABRetractTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.AutoballastRetract; } break; #endregion case (CPFStates.AutoballastRetract): #region if (stateEntry) { basicStateEntry(); ABRetractThresholdCount = 0; Elmo.retractBellows(configFile.ABRetractJV); } //Increment threshold counter on successive tests otherwise reset counter to 0 if (pressure >= configFile.ABDepthThreshold) ABRetractThresholdCount = ABRetractThresholdCount + 1; else ABRetractThresholdCount = 0; //Normal state exit criteria if (ABRetractThresholdCount >= 3) { Elmo.stopMotor(); stateEntry = true; EngrLogger.writeToColumns(sbStateExit); //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.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Descend; } break; #endregion case (CPFStates.Descend): #region if (stateEntry) { basicStateEntry(); //parked = false; depthTableNum = 0; gotParkSample = false; BuoyancyControl.resetPVPID(); runPVPID = true; //GM Moved these 2 lines from if block directly below to here. Seems like they only need to run once on state entry IB = configFile.IB; OB = configFile.OB; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameNone); MovingState = MovingStates.transit; VelState = VelStates.none; BTConsole.BTSend = false; } //if (depthTableNum < MissionConfigFile.descendTable.Length) //{ //} //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; //} depthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth; parkTime = MissionConfigFile.descendTable[depthTableNum].parkTime; if (parkTime == TimeSpan.Zero) //Not parking at target depth just taking a sample { sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendStateName); if (pressure < depthSetPoint) { PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); runPVPID = true; } if (pressure >= depthSetPoint) { InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; } } else if (parkTime > TimeSpan.Zero) //we are parking at this depth check to see which of the 5 depth bands we're in { if (pressure <= (depthSetPoint - OB)) //Above outer band { PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameParkingDown); } else if ((pressure > (depthSetPoint - OB)) && (pressure < (depthSetPoint - IB))) //In upper outer band { PVSetPoint = -1 * System.Math.Abs(configFile.approachVel); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameDownSlow); } else if ((pressure >= (depthSetPoint - IB)) && (pressure <= (depthSetPoint + IB))) //In inner band { PVSetPoint = 0.0; runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameInBand); if (!gotParkSample) //set up the sample and park time start times and take a sample { //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(parkTime.ToString()); EngrLogger.writeToColumns(sbTemp); InstrumentHandler.sampleInstruments(depthTableNum); gotParkSample = true; } else //check for next sample time and end of park time { mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); //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 ((mtTimeNow - mtParkStartTime) >= parkTime) { depthTableNum = depthTableNum + 1; EngrLogger.writeToColumns(EPP); } } } else if ((pressure > (depthSetPoint + IB)) && (pressure < (depthSetPoint + OB))) //in lower outer band { PVSetPoint = System.Math.Abs(configFile.approachVel); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameUpSlow); } else if (pressure >= depthSetPoint + OB) //Below lower band { PVSetPoint = 1.0 * System.Math.Abs(MissionConfigFile.descendTable[depthTableNum].PV); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameParkingUp); } } //Check to see if we're at the end of the descend table //Normal state exit if (depthTableNum >= MissionConfigFile.descendTable.Length) { Elmo.stopMotor(); runPVPID = false; stateEntry = true; EngrLogger.writeToColumns(sbStateExit); CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; sbAD_escendStateName.Clear(); MovingState = MovingStates.transit; VelState = VelStates.none; stateTimedout = false; } if (stateTimedout) { PVSetPoint = 0.0; runPVPID = false; stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); sbAD_escendStateName.Clear(); CPFState = CPFStates.Ascend; } break; #endregion case (CPFStates.Ascend): #region if (stateEntry) { basicStateEntry(); sbTemp.Clear(); depthTableNum = 0; IB = configFile.IB; OB = configFile.OB; gotParkSample = false; BuoyancyControl.resetPVPID(); PVSetPoint = 0.0; runPVPID = true; startCPModeState = 0; } depthSetPoint = MissionConfigFile.descendTable[depthTableNum].depth; parkTime = MissionConfigFile.descendTable[depthTableNum].parkTime; //Start CP Mode if (configFile.CPMode) { switch (startCPModeState) { case 0: //Stop SBE41 timer sbTemp.Clear(); sbTemp.Append("Stopping SBE41 Timer"); EngrLogger.writeToColumns(sbTemp); SBE41.stopTimer(); startCPModeStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); startCPModeState = 1; break; case 1: //Wakeup the SBE41 if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - startCPModeStartTime) > startCPModeState1Timeout) { sbTemp.Clear(); sbTemp.Append("Sending SBE41 CR/LF"); EngrLogger.writeToColumns(sbTemp); SBE41.sendCRLF(); startCPModeStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); startCPModeState = 2; } break; case 2: //Start CP mode if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - startCPModeStartTime) > startCPModeState2Timeout) { sbTemp.Clear(); sbTemp.Append("Sending StartProfile Command"); EngrLogger.writeToColumns(sbTemp); SBE41.startProfile(); startCPModeState = 3; } break; case 3: //Wait for CP Mode to start if ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - startCPModeStartTime) > startCPModeState3Timeout) { sbTemp.Clear(); sbTemp.Append("CP Profile should be started, probably"); EngrLogger.writeToColumns(sbTemp); startCPModeState = -1; } break; } } //If CPMode has started then start ascending if (startCPModeState == -1) { //Deal with the case where the first ascend station deeper than last descend station //2014 Oct 20 GM - I don't think this is correct for the first depth > start depth //if ((pressure < depthSetPoint) && (depthTableNum == 0)) //{ // PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); // runPVPID = true; //} //Not parking at target depth just taking a sample //else if (parkTime == TimeSpan.Zero) { sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameAscending); if (pressure > depthSetPoint) { PVSetPoint = System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPVPID = true; } if (pressure <= depthSetPoint) { InstrumentHandler.sampleInstruments(depthTableNum); depthTableNum = depthTableNum + 1; } } //we are parking at this depth check to see which of the 5 depth bands we're in else if (parkTime > TimeSpan.Zero) { //Above outer band if (pressure <= (depthSetPoint - OB)) { PVSetPoint = -1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameParkingDown); } //In upper outer band else if ((pressure > (depthSetPoint - OB)) && (pressure < (depthSetPoint - IB))) { PVSetPoint = -1 * System.Math.Abs(configFile.approachVel); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameDownSlow); } //In inner band else if ((pressure >= (depthSetPoint - IB)) && (pressure <= (depthSetPoint + IB))) { PVSetPoint = 0.0; runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameInBand); //set up the sample and park time start times and take a sample if (!gotParkSample) { //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(parkTime.ToString()); EngrLogger.writeToColumns(sbTemp); InstrumentHandler.sampleInstruments(depthTableNum); gotParkSample = true; } //check for next sample time and end of park time else { mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); //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 ((mtTimeNow - mtParkStartTime) >= parkTime) { depthTableNum = depthTableNum + 1; EngrLogger.writeToColumns(EPP); } } } //in lower outer band else if ((pressure > (depthSetPoint + IB)) && (pressure < (depthSetPoint + OB))) { PVSetPoint = System.Math.Abs(configFile.approachVel); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameUpSlow); } //Below lower band else if (pressure >= depthSetPoint + OB) { PVSetPoint = 1.0 * System.Math.Abs(MissionConfigFile.ascendTable[depthTableNum].PV); runPVPID = true; sbAD_escendStateName.Clear(); sbAD_escendStateName.Append(sbAD_escendNameParkingUp); } } //check to see if we've hit the surface if (pressure < (configFile.cpPressureCutoff + 0.5)) { BTConsole.BTSend = true; MovingState = MovingStates.exitAscend; Elmo.stopMotor(); if (configFile.CPMode) { SBE41.stopProfile(); Thread.Sleep(1000); SBE41.dumpData(); Thread.Sleep(5000); SBE41.sendCRLF(); Thread.Sleep(1000); SBE41.stopSBE41Timer = false; SBE41.restartTimer(500, configFile.SBE41SamplePeriod); } runPVPID = false; stateEntry = true; sbAD_escendStateName.Clear(); EngrLogger.writeToColumns(sbStateExit); CPFState = CPFStates.InitializeProfile; stateTimedout = false; } if (stateTimedout) { Elmo.stopMotor(); if (configFile.CPMode) { SBE41.stopProfile(); Thread.Sleep(1000); SBE41.dumpData(); Thread.Sleep(5000); SBE41.sendCRLF(); Thread.Sleep(1000); SBE41.stopSBE41Timer = false; SBE41.restartTimer(500, configFile.SBE41SamplePeriod); } runPVPID = false; stateEntry = true; stateTimedout = false; sbAD_escendStateName.Clear(); EngrLogger.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFState = CPFStates.InitializeProfile; } } break; #endregion case (CPFStates.Anchor): #region if (stateEntry) { basicStateEntry(); parkTime = MissionConfigFile.descendTable[MissionConfigFile.descendTable.Length - 1].parkTime; initialAnchorPressure = pressure; mtAnchorStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); lastAnchorSampleTime = mtAnchorStartTime; sbTemp.Append("Starting anchor period = "); sbTemp.Append(parkTime.ToString()); EngrLogger.writeToColumns(sbTemp); } mtTimeNow = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); //check to see if we need to sample if ((mtTimeNow - lastAnchorSampleTime) >= MissionConfigFile.parkSamplePeriod) { lastAnchorSampleTime = mtTimeNow; InstrumentHandler.sampleInstruments(depthTableNum); } //check to see if park time has expired if ((mtTimeNow - mtAnchorStartTime) >= parkTime) { stateEntry = false; stateTimedout = false; EngrLogger.writeToColumns(sbStateExit); CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.DeAnchor; } //Check to see if we aren't really parked, if not go back to descend as if nothing had happened if ((pressure - initialAnchorPressure) >= configFile.exitAnchorPressureThreshold) { stateEntry = false; stateTimedout = false; EngrLogger.writeToColumns(exitAnchorTripped); CPFStateTimer.Change(configFile.descendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Descend; } if (stateTimedout) { stateEntry = true; stateTimedout = false; EngrLogger.writeToColumns(sbStateTimedout); CPFStateTimer.Change(configFile.deAnchorTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.DeAnchor; } break; #endregion case (CPFStates.DeAnchor): #region if (stateEntry) { basicStateEntry(); Elmo.extendBellows(configFile.deanchorJV); } //normal state exit criteria goes here if (lpfResults.LPFVel >= configFile.deanchorVelocityThreshold) { Elmo.stopMotor(); stateEntry = true; EngrLogger.writeToColumns(sbStateExit); //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.writeToColumns(sbStateTimedout); //lock (programLock) { Program.MessageQueue.Enqueue(UTF8Encoding.UTF8.GetBytes(EngrLogger.getPrefix(sbStateTimedout).ToString())); } CPFStateTimer.Change(configFile.ascendTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Ascend; } break; #endregion case (CPFStates.Surface): #region break; #endregion case (CPFStates.EmergencyAscend): #region if (stateEntry) { basicStateEntry(); checkStuckPressure = false; } SORecoveryMode = true; 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(); stateEntry = true; stateTimedout = false; profileNum = profileNum + 1; EngrLogger.writeToColumns(sbEAStateExit); CPFStateTimer.Change(configFile.surfaceOpsTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.SurfaceOps; } break; #endregion case (CPFStates.ProcessErrors): #region basicStateEntry(); break; #endregion case (CPFStates.Exit): #region basicStateEntry(); //This calls Elmo.StopMotor missionRun = false; //Make sure Elmo.stopMotor finishes Thread.Sleep(1000); break; #endregion } writeSystemStateMessage(); if (sQueue.Count > 0) { processStructQ(); } //uBlox.Parse_NMEA_Messages(); Thread.Sleep(configFile.SMSleepPeriod); } //catch (Exception currentException) //{ // sbTemp.Clear(); // sbTemp.Append("*** Exception in State Machine loop: "); // sbTemp.Append(currentException.HResultToString()); // EngrLogger.writeToColumns(sbTemp); // CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod); // CPFState = CPFStates.EmergencyAscend; //} } EngrLogger.closeFile(); EngrLogger.unMount(); //Iridium.PowerOff(); } private static BuoyancyControl.PIDLogValues pidLogValues; private static byte[] dequeueMessage = new byte[256]; private static double pressure = double.NaN; 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 unParsedSBE41Header = new StringBuilder("Unparsed SBE41 Message"); private static readonly StringBuilder velPIDHeader = new StringBuilder("Vel 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 PTHHeader = new StringBuilder("PTH Message"); private static readonly StringBuilder OptodeHeader = new StringBuilder("Optode Message"); private static readonly StringBuilder EVK7Header = new StringBuilder("EVK7 GPS Message"); private static readonly StringBuilder IModemHeader = new StringBuilder("A3LA Message"); private static readonly StringBuilder DefaultHeader = new StringBuilder("Unknown Message: "); private static byte[] returnedByteArray = new byte[StructQueue.qRecordWidth]; private static byte[] sbdwtByteArray = new byte[1024]; private static byte[] pubxByteArray = new byte[512]; private static byte[] sbdwtHeader = new byte[9] { (byte)65, (byte)84, (byte)43, (byte)83, (byte)66, (byte)68, (byte)87, (byte)84, (byte)61 }; // AT+SBDWT= private static DateTime timeNow; private static PTH.PTHData pthData; private static int byteArrayLength = -1; private static int anchorPressureCount = 0; private static void processStructQ() { while (Program.sQueue.Count > 0) { lock (programLock) { qStructure = sQueue.Dequeue(); } switch (qStructure.qRecordType) { case (StructQueue.QRecordType.SBE41): #region byteArrayLength = Array.IndexOf(qStructure.byteArray, 0); //check for pressure only message from SBE41. This means message starts with a blank space and 8<=length=>10 if ((qStructure.byteArray[0] == 32) && (byteArrayLength <= 10) && (byteArrayLength >= 8)) { pressure = SBE41.parsePOnly(qStructure.byteArray); lpfResults = SBE41.LPFPressure(pressure); if (runPVPID) { if (runPVPID) { pidLogValues = BuoyancyControl.PIDPlatformVelocity(PVSetPoint, lpfResults.diffVel); if (verbosityLevel > 2) { EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, 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 (verbosityLevel > 2) { EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, SBE41Header, EngrLogger.ColumnNums.pressure, pressure, EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray, 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 } //Check depth limit if (lpfResults.LPFPressure > configFile.maxPressure) { Elmo.stopMotor(); runPVPID = false; EngrLogger.writeToColumns(toDeepError); CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.EmergencyAscend; } //Check for anchor condition if (CPFState == CPFStates.Descend) { //Might need to do something for the case where were wiggling around the anchorPressureMinimum if (pressure > configFile.anchorPressureMinimum) { if (lpfResults.pressureVariance < configFile.anchorPressureVarianceLimit) { anchorPressureCount = anchorPressureCount + 1; if (anchorPressureCount >= configFile.anchorPressureCountThreshold) { EngrLogger.writeToColumns(BottomDetect); Elmo.stopMotor(); runPVPID = false; stateEntry = true; CPFStateTimer.Change(configFile.anchorTimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.Anchor; } } else { if (anchorPressureCount > 0) anchorPressureCount = 0; } } } } else { //log any non-pressure only messages EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, unParsedSBE41Header, EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray); } //Check for SBE41 not updating pressure if (checkStuckPressure) { if (ErrorHandler.isPressureStuck(pressure)) { Elmo.stopMotor(); runPVPID = false; EngrLogger.writeToColumns(pressureStuckError); CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.EmergencyAscend; } } break; #endregion case (StructQueue.QRecordType.Optode): #region EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, OptodeHeader, EngrLogger.ColumnNums.TOpt, qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.EVK7): #region Array.Clear(pubxByteArray, 0, pubxByteArray.Length); Array.Copy(qStructure.byteArray, pubxByteArray, Array.IndexOf(qStructure.byteArray, 0)); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, EVK7Header, EngrLogger.ColumnNums.GPS, qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.IModem): #region EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, IModemHeader, EngrLogger.ColumnNums.IModem, qStructure.byteArray); if (surfaceOpsSubstate == 3) IModem.parseSQ(qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.PTH): #region pthData = PTH.getPTH(); timeNow = DateTime.Now; EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, CPFState, EngrLogger.ColumnNums.comment, PTHHeader, EngrLogger.ColumnNums.LPS331Pressure, pthData.LPS331Pressure, EngrLogger.ColumnNums.LPS331Temperature, pthData.LPS331Temperature, EngrLogger.ColumnNums.SHT21Humidity, pthData.SHT21Humidity, EngrLogger.ColumnNums.SHT21Temperature, pthData.SHT21Temperature); break; #endregion case (StructQueue.QRecordType.BTConsole): #region processConsoleCommand(qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.CN0254): #region CN0254Volts = CN0254.scanAll(); timeNow = DateTime.Now; 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.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, CPFState, EngrLogger.ColumnNums.comment, CN0254Header, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts, EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps, EngrLogger.ColumnNums.Amps12V, amps12V, EngrLogger.ColumnNums.Amps33V, amps33V); } if ((CPFState != CPFStates.EmergencyAscend) && (CPFState != CPFStates.NotRunning)) { if ((bellowsPosition > configFile.maxBellowsPosition) || (bellowsPosition < configFile.minBellowsPosition)) { Elmo.stopMotor(); runPVPID = false; EngrLogger.writeToColumns(BPError); CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.EmergencyAscend; } if (batteryBusVolts < configFile.minBatteryBusVolts) { Elmo.stopMotor(); runPVPID = false; EngrLogger.writeToColumns(BBVError); CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod); CPFState = CPFStates.EmergencyAscend; } } break; #endregion default: #region timeNow = DateTime.Now; sbTemp.Clear(); sbTemp.Append(DefaultHeader); sbTemp.Append(qStructure.qRecordType); sbTemp.Append(" "); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, CPFState, EngrLogger.ColumnNums.comment, DefaultHeader); break; #endregion } //sbTemp.Clear(); //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); //Debug.Print(sbTemp.ToString()); } } private static double lastPressure = double.NaN; private static bool firstPressure = true; private static double editPressure(double pressure) { double pressureDiff = double.NaN; if (firstPressure) { lastPressure = pressure; firstPressure = false; } else { pressureDiff = System.Math.Abs(pressure - lastPressure); if (pressureDiff > 1.0) { Debug.Print("Edited new pressure value = " + pressure.ToString()); pressure = lastPressure; } else lastPressure = pressure; } return (pressure); } 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) { lock (ADCTimerLock) { Program.sQueue.Enqueue(ADCQStruct); } } private static StructQueue.qStruct PTHQStruct = new StructQueue.qStruct() { qRecordType = StructQueue.QRecordType.PTH, byteArray = new byte[StructQueue.qRecordWidth] }; private static object PTHTimerLock = new Object(); private static void PTHTimerCallback(object o) { PTHQStruct.timeStamp = DateTime.Now; PTHQStruct.state = CPFState; lock (PTHTimerLock) { Program.sQueue.Enqueue(PTHQStruct); } } private static StringBuilder consoleCommandComment = new StringBuilder(); private static void processConsoleCommand(byte[] consoleCommandMessage) { sbConsoleCommand.Clear(); //Ignore command if it doesn't have a CR if (Array.IndexOf(consoleCommandMessage, 13) >= 0) { sbConsoleCommand.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, 13) - 1))); consoleCommandComment.Clear(); consoleCommandComment.Append("Processing Console Command: "); consoleCommandComment.Append(sbConsoleCommand); EngrLogger.writeToColumns(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 "RECOVERYTRUE": SORecoveryMode = true; break; case "RECOVERYFALSE": SORecoveryMode = false; break; case "EXIT": { CPFState = CPFStates.Exit; break; } default: { sbTemp.Clear(); sbTemp.Append("Invalid command, try again"); BTConsole.SendLine(sbTemp); break; } } } else { sbConsoleCommand.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, 0) - 1))); consoleCommandComment.Clear(); consoleCommandComment.Append("Received non-CR terminated Console Command: "); consoleCommandComment.Append(sbConsoleCommand); EngrLogger.writeToColumns(consoleCommandComment); } sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); } private static void writeSystemStateMessage() { timeNow = DateTime.Now; sbTemp.Clear(); sbTemp.Append("depthNum = "); sbTemp.Append(depthTableNum.ToString()); sbTemp.Append(","); if (runPVPID) sbTemp.Append("PVPID Running,"); sbTemp.Append(","); sbTemp.Append(sbAD_escendStateName); 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.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, CPFState, EngrLogger.ColumnNums.comment, sbTemp, EngrLogger.ColumnNums.pressure, pressure, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts); } private static void basicStateEntry() { Elmo.stopMotor(); EngrLogger.writeToColumns(sbStateEntry); runPVPID = false; stateEntry = false; stateTimedout = false; gotParkSample = false; sbAD_escendStateName.Clear(); } } }