//#define NO_IMODEM //#define NO_EVK7GPS using System; using Microsoft.SPOT; using System.Text; using System.Threading; namespace CPF { /*StateBase - from which all other states inherit * Includes sbTemp and SV for inherited states to use */ abstract public class StateBase { //ABSTRACT METHODS public abstract int doStateEntryActions(); public abstract int doStateActions(); public abstract Program.CPFStates checkEvents(); public abstract int doExitActions(bool timedOut); public abstract Program.CPFStates doTimeoutAction(); protected static int ABRetractThresholdCount = 0; //INHERITED METHODS protected static int basicStateEntry() { sbTemp.Clear(); sbTemp.Append("State Entry"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); Elmo.stopMotor(); //EngrLogger.writeToColumns(sbStateEntry); SV.RunPVPID = false; SV.CheckStuckPressure = true; ActionReturn = 0; SV.StateEntry = false; //Start at the beginning of the pressure table unless transitioning from park //Added GM 2015Oct22 if (Program.PastState != Program.CPFStates.park) SV.PressureTableNum = 0; return (1); } protected static int basicStateExit(bool timedOut) { //timedOut exit actions if (timedOut) { sbTemp.Clear(); sbTemp.Append("State timed out"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); } //normal state exit actions else { sbTemp.Clear(); sbTemp.Append("Normal state exit"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); SV.CheckStuckPressure = true; Elmo.stopMotor(); } SV.StateEntry = true; return (1); } //Evaluates return code of a given action, and if successful increments currentAtion. //If action is incomplete, or fails, returns initial value of currentAction. //Errors are to be handled higher up because this is a common and inherited method that is context agnostic. protected static int checkReturn(int returnVal, int currentAction) { if (returnVal > 0) //action has completed sucessfully, or is still in process { return (returnVal); } else if (returnVal == 0) { return (currentAction); } else { return ((int)currentAction); } } protected static bool exceedABRetractPressureThreshold() { if (SV.Pressure >= configFile.ABPressureThreshold) ABRetractThresholdCount = ABRetractThresholdCount + 1; else ABRetractThresholdCount = 0; //if 3 sucessive counts below threshold have been recorded, transistion if (ABRetractThresholdCount >= 3) { return (true); } //otherwise stay else { return (false); } } public static Program.CPFStates checkGlobalIssues() { //Check depth limit if (SV.Pressure > configFile.maxPressure) { sbTemp.Clear(); sbTemp.Append("Too deep. Moving to RECOVERY MODE"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return (Program.CPFStates.recoveryMode); } //Check mission timeout if (!SV.MissionTimedOut) { if (DateTime.Now - Program.missionStartTime > SV.MissionTimeoutTS) { SV.MissionTimedOut = true; sbTemp.Clear(); sbTemp.Append("Mission timed out. Moving to RECOVERY MODE"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return (Program.CPFStates.recoveryMode); } } #if(!MIN_HW_TEST) //Check bellows position #region Check bellows limits if ((Program.CurrentState != Program.CPFStates.initMission) && (Program.CurrentState != Program.CPFStates.exit)) { if ((Program.bellowsPosition > configFile.bellowsUpperLimit) || (Program.bellowsPosition < configFile.bellowsLowerLimit)) { if (Program.bellowsPosition >= configFile.bellowsUpperLimit) SV.BellowsAtUpperLimit = true; if (Program.bellowsPosition <= configFile.bellowsLowerLimit) SV.BellowsAtLowerLimit = true; sbTemp.Clear(); sbTemp.Append("Bellows position exceeds limits, Pos = "); sbTemp.Append(Program.bellowsPosition.ToString()); if ((Program.bellowsPosition >= configFile.bellowsUpperLimit) && Elmo.extending) { Elmo.stopMotor(); SV.RunPVPID = false; sbTemp.Append(" *Trying to pass limit, stopping Motor"); } if ((Program.bellowsPosition <= configFile.bellowsLowerLimit) && Elmo.retracting) { Elmo.stopMotor(); SV.RunPVPID = false; sbTemp.Append(" *Trying to pass limit, stopping Motor"); } EngrLogger.writeToColumns(sbTemp); //GM 2015 June 03 Maybe we shouldn't be going into emergency ascend just because the bellows is at it's limit //CPFStateTimer.Change(configFile.EATimeout, configFile.timeoutDefaultPeriod); //CPFState = CPFStates.SetRecoveryMode; } else { SV.BellowsAtUpperLimit = false; SV.BellowsAtLowerLimit = false; } } #endregion //Check Battery voltage //TODO Move batteryBusVolts to CN0254 class //if (Program.batteryBusVolts < configFile.minBatteryBusVolts) //{ // Elmo.stopMotor(); // SV.RunPVPID = false; // EngrLogger.writeToColumns("Battery Bus Voltage below threshold, beginning RECOVERYMODE"); // return (Program.CPFStates.recoveryMode); //} if (SV.ForceRecoveryMode) { Elmo.stopMotor(); SV.RunPVPID = false; EngrLogger.writeToColumns("SV Force recovery mode = true"); return (Program.CPFStates.recoveryMode); } //Check SBE41 Stuck Pressure // TODO Gene, you may want to check stuck pressure in SurfaceOps (or just reset SBE like Dana does?). I observed a stuck error in the tank on July 28 2015, // and it would have been more graceful to reset the SBE in SurfaceOps than let it go to ABSetFast only to discover pressure // was stuck, and move to recovery mode. -Laughlin #region Check Stuck Pressure if (SV.CheckStuckPressure) { if (ErrorHandler.isPressureStuck(SV.Pressure)) { Elmo.stopMotor(); SV.RunPVPID = false; SV.SurfaceOpsGo = false; EngrLogger.writeToColumns("Pressure isn't changing. Moving to RECOVERY MODE"); return (Program.CPFStates.recoveryMode); } } #endregion //TODO //Check if float surfaced when in "underwater" state //TODO //check for large delta in internal housing pressure #endif return (Program.NextState); } //COMMON PROPERTIES //This is the execution constraint time out that the state must complete in public int ECTimeout;// { get; protected set; } public static int ActionReturn { get; protected set; } //flag indicates whether data is to be dumped off SBE public static bool DumpCPData { get; protected set; } public readonly StringBuilder stateName = new StringBuilder(32); protected static StringBuilder sbTemp = new StringBuilder(128); } //Exit - finalize all writes to disk and exit program public class Exit : StateBase { public Exit(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 2 * 60 * 1000; //2 min } private enum Action : int { exit = 1, lastAction, size = lastAction } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); Program.missionRun = false; return (1); } public override int doStateActions() { //ensure all data is written to disk EngrLogger.sdVol.FlushAll(); return (1); } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); } Program.missionRun = false; return (1); } public override Program.CPFStates checkEvents() { return (Program.CPFStates.exit); } public override Program.CPFStates doTimeoutAction() { return (Program.CPFStates.exit); } } //InitializeMission - jogs bellows, inits SBE41, optode, FLBB public class InitMission : StateBase { public InitMission(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 5 * 60 * 1000; //5 minutes } private enum Action : int { //list sequential actions here readBPandBatteryVandI = 1, checkSBE41FastPressure, exerciseValve, moveBellowsInsideLimits, checkBellowsMovement, wakeSBE41, sendSBE41_DS, sendSBE41_DC, startCommandModeTimer, sendOptodeGetAll, sendOptodeDoSample, lastAction, size = lastAction } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); //state-specific entry actions below currentAction = (Action)1; SV.CheckStuckPressure = false; return (1); } // IF/ELSE blocks are preserved in switch statement and not factored into inherited methods because 1)actions are not always sequential, and 2) how we handle errors may be context dependant, so errors should be handled at point where currentAction is decided/switched public override int doStateActions() { switch (currentAction) { #region Sequential state actions case (Action.readBPandBatteryVandI): //do action, and store its status. ActionReturn = readBPandBatteryVandI(); break; case (Action.checkSBE41FastPressure): SBE41.sendFastPressure(); ActionReturn = (int)Action.exerciseValve; break; case (Action.exerciseValve): Elmo.closeValve(); Thread.Sleep(2000); if (Elmo.isValveOpen()) EngrLogger.writeToColumns("Closed valve isValveOpen = true"); else EngrLogger.writeToColumns("Closed valve isValveOpen = false"); Elmo.openValve(); Thread.Sleep(2000); if (Elmo.isValveOpen()) EngrLogger.writeToColumns("Opened valve isValveOpen = true"); else EngrLogger.writeToColumns("Opened valve isValveOpen = false"); Elmo.closeValve(); Thread.Sleep(2000); if (Elmo.isValveOpen()) EngrLogger.writeToColumns("Closed valve isValveOpen = true"); else EngrLogger.writeToColumns("Closed valve isValveOpen = false"); ActionReturn = (int)Action.moveBellowsInsideLimits; break; case (Action.moveBellowsInsideLimits): Elmo.openValve(); ActionReturn = moveBellowsInsideLimits(); break; case (Action.checkBellowsMovement): //do action, and store its status. ActionReturn = checkBellowsMovement(); break; case (Action.wakeSBE41): SBE41.sendCRLF(); ActionReturn = (int)Action.sendSBE41_DS; break; case (Action.sendSBE41_DS): ActionReturn = sendSBE41_DS(); break; case (Action.sendSBE41_DC): ActionReturn = sendSBE41_DC(); break; case (Action.startCommandModeTimer): ActionReturn = startCommandModeTimer(); break; case (Action.sendOptodeGetAll): ActionReturn = sendOptodeGetAll(); break; case Action.sendOptodeDoSample: Optode.doSample(); ActionReturn = (int)Action.lastAction; break; case (Action.lastAction): break; #endregion } //examine the return of the action, this will either 1) adjust currentAction, 2) keep it the same (action isn't complete), or 3) call handleErrors method currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { if (currentAction == Action.lastAction) return (Program.CPFStates.initProfile); else return (Program.CPFStates.initMission); } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); } SV.CheckStuckPressure = true; return (1); } public override Program.CPFStates doTimeoutAction() { //move to next state return (Program.CPFStates.initProfile); } //=====State-specific methods below===== //methods should return an int in the range [-1:Action.lastAction]. // -1: Error // 0: Action not complete // >0: an integar cast of the next action enumeration //TODO build logic that actually checks if the bellows is moving! //Should moveBellowsInsideLimits be a method that exists within Elmo class? //CallCount is just for initial testing, implement fully/cleanly private static int checkBellowsMovement() { Elmo.exerciseBellows(); return ((int)Action.wakeSBE41); } private static EnergyMonitor.EnergyMonitorData EMData = new EnergyMonitor.EnergyMonitorData(); private static DateTime timeNow = new DateTime(); private static int readBPandBatteryVandI() { double bellowsPosition = double.NaN; timeNow = DateTime.Now; EMData = EnergyMonitor.readEM(); bellowsPosition = ADC2485.readBellowsPosition(); sbTemp.Clear(); sbTemp.Append("Bellows Position = "); sbTemp.Append(bellowsPosition.ToString("f3")); sbTemp.Append(" Battery Voltage = "); sbTemp.Append(EMData.voltage.ToString("f2")); sbTemp.Append(" and Current = "); sbTemp.Append(EMData.current.ToString("f3")); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, Program.CurrentState, EngrLogger.ColumnNums.comment, sbTemp, EngrLogger.ColumnNums.batteryBusVolts, EMData.voltage, EngrLogger.ColumnNums.batteryBusAmps, EMData.current); return ((int)Action.checkSBE41FastPressure); } private static int moveBellowsInsideLimits() { //Just in case the bellows was outside the limits, put it inside the limits if (Program.bellowsPosition > (configFile.bellowsUpperLimit - 1.0)) { sbTemp.Clear(); sbTemp.Append("Retracting bellows inside limits"); EngrLogger.writeToColumns(sbTemp); if (!Elmo.retracting) Elmo.retractBellows(10000); return (0); //action not complete, return 0 } else if (Program.bellowsPosition < (configFile.bellowsLowerLimit + 1.0)) { sbTemp.Clear(); sbTemp.Append("Extending bellows inside limits"); EngrLogger.writeToColumns(sbTemp); if (!Elmo.extending) Elmo.extendBellows(10000); return (0); //action not complete, return 0 } Elmo.stopMotor(); sbTemp.Clear(); sbTemp.Append("Bellows inside limits"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); //return int of the next action to be done return ((int)Action.checkBellowsMovement); } private static int sendSBE41_DS() { int returnVal = -1; //skips over this command #region NO_CTD #if(NO_CTD) return((int)Action.sendOptodeGetAll); #endif #endregion sbTemp.Clear(); sbTemp.Append("Sending SBE41 ds command"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); returnVal = SBE41.waitForResponse(SBE41.commands.ds); if (returnVal == 0) return ((int)Action.sendSBE41_DS); else if (returnVal == 1) return ((int)Action.sendSBE41_DC); else sbTemp.Clear(); sbTemp.Append("Error in WFR(DS) command"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.startCommandModeTimer); } private static int sendSBE41_DC() { int returnVal = -1; //skips over this command #region NO_CTD #if(NO_CTD) return((int)Action.sendOptodeGetAll); #endif #endregion sbTemp.Clear(); sbTemp.Append("Sending SBE41 dc command"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); returnVal = SBE41.waitForResponse(SBE41.commands.dc); if (returnVal == 0) return ((int)Action.sendSBE41_DC); else if (returnVal == 1) return ((int)Action.startCommandModeTimer); else sbTemp.Clear(); sbTemp.Append("Error in WFR(DC) command"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.startCommandModeTimer); } private static int startCommandModeTimer() { SBE41.startSBE41CommandModeTimer(); return ((int)Action.sendOptodeGetAll); } private static int sendOptodeGetAll() { int returnVal = -1; sbTemp.Clear(); sbTemp.Append("Sending Optode Get All command"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); returnVal = Optode.waitForResponse(Optode.commands.getAll); if (returnVal == 0) return ((int)Action.sendOptodeGetAll); else if (returnVal == 1) return ((int)Action.sendOptodeDoSample); else sbTemp.Clear(); sbTemp.Append("Optode error, moving to last action"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.lastAction); } private static int initFLBB() { sbTemp.Clear(); sbTemp.Append("Initializing FLBB"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); FLBB.init(); //go to int of next action return ((int)Action.checkSBE41FastPressure); } } //InitializeProfile - sets execuition timeout constraint public class InitProfile : StateBase { public InitProfile(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 60 * 1000; //60 seconds } private enum Action : int { initProfileActions = 1, lastAction, size = lastAction } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); currentAction = (Action)1; //set these values before every profile SV.PressureTableNum = 0; Program.dumpCPExceptionCounter = 0; Program.MaxPressure = -100.0; SBE41.LPFPressure(SV.Pressure, SV.PressureTimeStamp.Ticks / 10000000.0, true); return (1); } public override int doStateActions() { switch (currentAction) { #region Sequential state actions case (Action.initProfileActions): //These actions are so simple I just lump them into one case block GM 2015Oct28 SBE41.sendStopProfile(); Thread.Sleep(1000); EngrLogger.writeToColumns("Resetting SBE41 with qsr"); SBE41.resetSBE41(); SV.ProfileNum++; if (SV.ProfileNum > SV.MaxProfiles) { EngrLogger.writeToColumns("Exceeded max profiles, setting recovery mode true"); SV.RecoveryMode = true; } //Set system time EngrLogger.writeToColumns("Synching system time to GPS"); EVK7GPS.setSystemTime(EVK7GPS.sendI2CCmd("$PUBX,04*37\r\n")); ActionReturn = (int)Action.lastAction; break; case (Action.lastAction): break; #endregion } //examine the return of the action, continue currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { if (currentAction == Action.lastAction) return (Program.CPFStates.surfaceOpsSetBellows); else return (Program.CPFStates.initProfile); } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); } return (1); } public override Program.CPFStates doTimeoutAction() { return (Program.CPFStates.surfaceOpsSetBellows); } } public class SurfaceOpsSetBellows : StateBase { public SurfaceOpsSetBellows(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 60 * 60 * 1000; //minutes } private enum Action : int { setBellows = 1, lastAction, size = lastAction //this is an abstract way to get the length of the Action enumeration for every class } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); sbTemp.Clear(); sbTemp.Append("Inflating bellows to Surface Ops position = "); if (SV.RecoveryMode) sbTemp.Append(configFile.RMSetBellowsPosition.ToString()); else sbTemp.Append(configFile.SOSetBellowsPosition.ToString()); sbTemp.Append(" at "); sbTemp.Append(configFile.SOSetBellowsJV.ToString()); sbTemp.Append(" cps"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); currentAction = (Action)1; SV.CheckStuckPressure = false; return (1); } public override int doStateActions() { switch (currentAction) { case (Action.setBellows): //Action return is the next action to be run. //If == 0, current action is not complete stay here //if > 0, Action return is the enumeration index for the next action //if == -1, there was an error ActionReturn = surfaceOpsSetBellows(); break; case (Action.lastAction): break; } //This is the method that call the concrete state error handler //Action return == 0 or > 0 are just re-returned currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { //This is the method generally just looks for state transitions and might looks for events that might change variables put something on the queue or ??? //is bellows at required position? if ((currentAction == Action.lastAction) && (SV.ProfileNum == 1)) return (Program.CPFStates.preMissionDelay); else if ((currentAction == Action.lastAction) && (SV.ProfileNum > 1)) return (Program.CPFStates.dumpCPData); else return (Program.CPFStates.surfaceOpsSetBellows); } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); } SV.CheckStuckPressure = true; return (1); } public override Program.CPFStates doTimeoutAction() { if ((currentAction == Action.lastAction) && (SV.ProfileNum == 1)) return (Program.CPFStates.preMissionDelay); else if ((currentAction == Action.lastAction) && (SV.ProfileNum > 1)) return (Program.CPFStates.dumpCPData); //if for some reason got stuck in setting bellows, still need to get to DumpCPData to offload data and restart SBE41 Command timer else if ((SV.ProfileNum > 1)) return (Program.CPFStates.dumpCPData); else return (Program.CPFStates.surfaceOps); } private static double setBellowsPosition = double.NaN; private static int surfaceOpsSetBellows() { int returnVal = 0; if (SV.RecoveryMode) setBellowsPosition = configFile.RMSetBellowsPosition; else setBellowsPosition = configFile.SOSetBellowsPosition; if (Program.bellowsPosition < setBellowsPosition) returnVal = Elmo.moveBellowsToPosition(setBellowsPosition, configFile.SOSetBellowsJV, 0.5); //TODO replace generic moveBellowsToPosition with specific, less error prone logic here and one other place // it is possible for the elmo to run the bellows outside of the window which moveBellowsToPosition will return // (int)1, so here we guard against that, and allow to exit bellows inflation routine. Said behavior observed in TT // July 28 2015. if (Program.bellowsPosition > setBellowsPosition) return ((int)Action.lastAction); if (returnVal == 0) return ((int)Action.setBellows); else return ((int)Action.lastAction); } } public class SurfaceOps : StateBase { public SurfaceOps(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 10 * 60 * 1000; //minutes } public enum Action : int { //list sequential actions here sendSBDD2 = 1, sendCREG, readGpsI2C, sendCSQ, buildSBDWTMsg, sendSBDWT, loadSBDWT, sendSBDIX, uploadEngrLog, sendFLBBRunCmd, lastAction, size = lastAction } private static Action currentAction; private static bool sendAckSBD = false; public override int doStateEntryActions() { basicStateEntry(); Elmo.closeValve(); Array.Clear(Program.pubxByteArray, 0, Program.pubxByteArray.Length); currentAction = (Action)1; //State-specific entry items below Optode.doSample(); //FLBB.sendRunCmd(); //Log file operations if (SV.ProfileNum > 1) { startNewEngrLog(); sbTemp.Clear(); sbTemp.Append("Profile Number = "); sbTemp.Append(SV.ProfileNum.ToString()); EngrLogger.writeToColumns(sbTemp); } SV.CheckStuckPressure = true; //GM Changed to true 2015Oct28 return (1); } public static int sbdiMsgNum = 0; public static int sbdiTryNum = 0; private static int wfrReturn = 0; public override int doStateActions() { switch (currentAction) { #region SurOps Action Sequence case (Action.sendSBDD2): //ActionReturn = sendSBDD2(); //TODO remove once waitForResponse has been implimented //surfaceOpsSubstateStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); if (IModem.waitForResponse(IModem.commands.sbdd2) == 0) { // Debug.Print("Waiting for sbdd2 Response"); } else ActionReturn = (int)Action.sendCREG; break; case (Action.sendCREG): if (IModem.waitForResponse(IModem.commands.creg) == 0) { // Debug.Print("Waiting for creg Response"); } else ActionReturn = (int)Action.readGpsI2C; break; case (Action.readGpsI2C): sbdiMsgNum = sbdiMsgNum + 1; sbdiTryNum = 0; //Use this for EVK7 GPS I2C interface ActionReturn = sendI2CCmd(); //Use this for A3LA GPS serial interface //if (IModem.waitForResponse(IModem.commands.papubx00) <= 0) // Debug.Print("Waiting for PA=PUBX00 Response"); //else // ActionReturn = (int)Action.sendCSQ; break; case (Action.sendCSQ): if (IModem.waitForResponse(IModem.commands.csq) == 0) { // Debug.Print("Waiting for csq Response"); } else ActionReturn = (int)Action.buildSBDWTMsg; break; case (Action.buildSBDWTMsg): buildSBDWTStatusMsg(); ActionReturn = (int)Action.sendSBDWT; break; case (Action.sendSBDWT): //Send the SBDWT command first //This 2 step method allows for the full 1920 byte message to be sent if (IModem.waitForResponse(IModem.commands.sendSBDWT) == 0) { //Debug.Print("Waiting for send SBDWT Response"); } else ActionReturn = (int)Action.loadSBDWT; break; case (Action.loadSBDWT): //Now load the sbdwt buffer with the message //This 2 step method allows for the full 1920 byte message to be sent if (IModem.waitForResponse(IModem.commands.loadSBDWT) == 0) { //Debug.Print("Waiting for load SBDWT Response"); } else ActionReturn = (int)Action.sendSBDIX; break; case (Action.sendSBDIX): wfrReturn = IModem.waitForResponse(IModem.commands.sbdix); if (wfrReturn == 0) { //Debug.Print("Waiting for sbdix Response"); } else { sbdiTryNum++; if (wfrReturn > 0) { if (IModem.sbdixMTStatus == 1) { sendAckSBD = true; IModem.sendSBDRT(); } } if ( ((sbdiTryNum >= 4) || ((IModem.sbdixMOStatus >= 0) && (IModem.sbdixMOStatus <= 4))) && (IModem.sbdixMTQueued <= 0)) { if (sendAckSBD) ActionReturn = (int)Action.buildSBDWTMsg; else ActionReturn = (int)Action.sendFLBBRunCmd; } else ActionReturn = (int)Action.sendSBDIX; } break; case (Action.sendFLBBRunCmd): ActionReturn = sendFLBBRunCmd(); break; case (Action.lastAction): //wait here until timeout, or $GO command currentAction = Action.lastAction; break; default: break; #endregion } currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); //upload logs if requested uploadEngrLog(); return (1); } public override Program.CPFStates checkEvents() { //Exit surfaceOps no matter what if we get a $go if (SV.SurfaceOpsGo) { Optode.doSample(); FLBB.sendRunCmd(); SV.SurfaceOpsGo = false; return (Program.CPFStates.ABRetractFast); } //don't exit SOps unless float has finished surface GPS/IModem routine if (currentAction == Action.lastAction) { //if we're not in SV.RecoveryMode, continue to ABRetract Fast if (!SV.RecoveryMode) //GM 2105Aug04 added || (!SV.Recovery) mode to force transition when done with telemetry { Optode.doSample(); FLBB.sendRunCmd(); return (Program.CPFStates.ABRetractFast); } //if not in SV.RecoveryMode, and have synched time and telemetered data, continue to ABRetractFast //TODO impliment code that actually changes timeSynced and telemetryDone. This wasn't done in old code. //TODO GM I'm pretty sure this won't be needed anymore since I added it to the test above. If it is it will need the Optode.DoSample and FLBB.sendRunCmd calls if (!SV.RecoveryMode && ((timeSynced && telemetryDone))) return (Program.CPFStates.ABRetractFast); } //default is to stay in SurfaceOps return (Program.CPFStates.surfaceOps); } public override Program.CPFStates doTimeoutAction() { IModem.waitingForResponse = false; if (SV.RecoveryMode) return (Program.CPFStates.surfaceOps); else { return (Program.CPFStates.ABRetractFast); } } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); //reset the current action to top of action list currentAction = (Action)1; //reset these so next time in SO they can be evaluated timeSynced = false; telemetryDone = false; SV.SurfaceOpsGo = false; } else { basicStateExit(false); SV.SurfaceOpsGo = false; } SV.CheckStuckPressure = true; return (1); } public static void buildSBDWTStatusMsg() { //Header try { IModem.SBDWTMsg.Clear(); IModem.SBDWTMsg.Append(DateTime.Now.ToString("yyyy-MM-dd")); IModem.SBDWTMsg.Append("T"); IModem.SBDWTMsg.Append(DateTime.Now.ToString("HH:mm:ss")); IModem.SBDWTMsg.Append("Z,"); //Position IModem.SBDWTMsg.Append("Lat,"); IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.latBytes)); IModem.SBDWTMsg.Append(",Lon,"); IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.lonBytes)); IModem.SBDWTMsg.Append(",Num Sats,"); IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(EVK7GPS.numSatsBytes)); if (sendAckSBD) { IModem.SBDWTMsg.Append(",ACK SBD"); sendAckSBD = false; } else IModem.SBDWTMsg.Append(",-"); IModem.SBDWTMsg.Append(",SQ,"); IModem.SBDWTMsg.Append(IModem.SignalQuality.ToString()); IModem.SBDWTMsg.Append(",MO Stat,-,"); //Mission parameters IModem.SBDWTMsg.Append(",Mission TO,"); IModem.SBDWTMsg.Append(SV.MissionTimeoutTS.ToString()); IModem.SBDWTMsg.Append(",Park Time,"); IModem.SBDWTMsg.Append(Mission.descendTable[0].parkTime.ToString()); IModem.SBDWTMsg.Append(",Park P,"); IModem.SBDWTMsg.Append(Mission.descendTable[0].pressure.ToString()); //Status IModem.SBDWTMsg.Append(",Prof Num,"); IModem.SBDWTMsg.Append(SV.ProfileNum.ToString()); IModem.SBDWTMsg.Append(" of "); IModem.SBDWTMsg.Append(SV.MaxProfiles.ToString()); IModem.SBDWTMsg.Append(",Max P,"); IModem.SBDWTMsg.Append(Program.MaxPressure.ToString("f3")); IModem.SBDWTMsg.Append(",RecMode,"); IModem.SBDWTMsg.Append(SV.RecoveryMode.ToString()); IModem.SBDWTMsg.Append(",BBVolts,"); IModem.SBDWTMsg.Append(Program.batteryBusVolts.ToString("f3")); IModem.SBDWTMsg.Append(",Can P,"); IModem.SBDWTMsg.Append(Program.PTHData.LPS331Pressure.ToString("f3")); IModem.SBDWTMsg.Append(",Can Hum,"); IModem.SBDWTMsg.Append(Program.PTHData.SHT21Humidity.ToString("f3")); //TODO get rid of this when parsed lat and lon is verified IModem.SBDWTMsg.Append(","); IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(Program.pubxByteArray)); //Debug.Print(DateTime.Now + "SBDWT message = " + sbTemp.ToString()); } catch { IModem.SBDWTMsg.Clear(); IModem.SBDWTMsg.Append(UTF8Encoding.UTF8.GetChars(Program.pubxByteArray)); } } private static int sendSBDD2() { #if(!NO_IMODEM) if (IModem.sendSBDD2() > 0) { //EngrLogger.quickWTC("Sent SBDD2"); return ((int)Action.readGpsI2C); } else //error return (-1); #else sbTemp.Clear(); sbTemp.Append("No IMODEM attached, pretend SBDD2"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.readGpsI2C); #endif } private static int sendI2CCmd() { #if(!NO_EVK7GPS) EVK7GPS.sendI2CCmd("$PUBX,00*33\r\n"); #else sbTemp.Clear(); sbTemp.Append("No GPS, pretend send GPS"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return((int)Action.sendCSQ); #endif return ((int)Action.sendCSQ); } private static int sendFLBBRunCmd() { if (FLBB.sendRunCmd() > 0) return ((int)Action.lastAction); else return (-1); } private static int uploadEngrLog() { if ((SV.UploadLastEngrFile || SV.UploadEngrFile)) { if (SV.UploadLastEngrFile) { sbTemp.Clear(); sbTemp.Append(UDF); sbTemp.Append(lastFileName); sbTemp.Append("Upload last file temporarily disabled"); //TODO this seems to crash the program for large data files EngrLogger.writeToColumns(sbTemp); //Program.btConsole.uploadDataFile(lastFileName); //TODO The BTConsole might need to go in a separate thread SV.UploadLastEngrFile = false; } else { if (Program.DirectoryValid) { if (Program.uploadEngrFileNum == 0) { EngrLogger.writeToColumns("Can't upload file number 0 it is the currently open file"); } else { if ((Program.uploadEngrFileNum > 0) && (Program.uploadEngrFileNum < (EngrLogger.numFilesOnDisk - 1))) Program.btConsole.uploadDataFile(EngrLogger.sbFileNames[Program.uploadEngrFileNum]); } SV.UploadEngrFile = false; } else { sbTemp.Clear(); sbTemp.Append("Need a valid directory, execute $GETDIR command"); Program.btConsole.SendLine(sbTemp); SV.UploadEngrFile = false; } } } else { //No logs to upload } return (1); } private static int startNewEngrLog() { 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); Program.DirectoryValid = false; SV.UploadLastEngrFile = true; return (1); } private static readonly StringBuilder UDF = new StringBuilder("Uploading Data File name = "); private static readonly StringBuilder lastFileName = new StringBuilder(128); //SurfaceOps is the only place these are used; don't need to be in SV. private static bool timeSynced = false; private static bool telemetryDone = false; //TODO: Think about whether there is a better way to do this... //added to allow ProcessSQ to examine currentAction. Legacy code only processed IModem q messages if //surfaceOps was in sendCSQ substate. public static int Substate { get { return (int)currentAction; } } //added from legacy code and implimented in lieu of listen for response on GPS/A3LA } public class PreMissionDelay : StateBase { private enum Action : int { wait = 1, lastAction, size = lastAction } private static Action currentAction; public PreMissionDelay(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 1 * 10 * 1000; } public override int doStateEntryActions() { basicStateEntry(); currentAction = (Action)1; return (1); } public override int doStateActions() { switch (currentAction) { case (Action.wait): ActionReturn = wait(); break; case (Action.lastAction): //cycle back to wait, even tho we shouldn't get here currentAction = Action.wait; break; } currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { if (SV.SurfaceOpsGo) return (Program.CPFStates.surfaceOps); else return (Program.CPFStates.preMissionDelay); } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); } return (1); } public override Program.CPFStates doTimeoutAction() { return (Program.CPFStates.surfaceOps); } private static int wait() { return ((int)Action.wait); } } public class DumpCPData : StateBase { public DumpCPData(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 15 * 60 * 1000; //15 minutes } private enum Action : int { sendCRLF = 1, sendBinaverage, sendDA, sendDAH, restartCommandTimer, lastAction, size = lastAction } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); Elmo.closeValve(); currentAction = (Action)1; //disable CheckStuckPressure to prevent blocking of upload; SV.CheckStuckPressure = false; return (1); } public override int doStateActions() { switch (currentAction) { case (Action.sendCRLF): ActionReturn = sendCRLFWait(); break; case(Action.sendBinaverage): ActionReturn = sendBinaverage(); break; case (Action.sendDA): ActionReturn = sendDAWait(); break; case (Action.sendDAH): ActionReturn = sendDAHWait(); break; case (Action.restartCommandTimer): ActionReturn = restartCommandTimer(); break; case (Action.lastAction): break; } currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { if (currentAction == Action.lastAction) { return (Program.CPFStates.surfaceOps); } else { return (Program.CPFStates.dumpCPData); } } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); } SV.CheckStuckPressure = true; return (1); } public override Program.CPFStates doTimeoutAction() { return (Program.CPFStates.surfaceOps); } private static int sendCRLFWait() { int returnVal = SBE41.waitForResponse(SBE41.commands.crlf); if (returnVal == 1) { sbTemp.Clear(); sbTemp.Append("Got CR/LF response"); EngrLogger.writeToColumns(sbTemp); return ((int)Action.sendBinaverage); } else if (returnVal == 0) { sbTemp.Clear(); sbTemp.Append("Didn't get CR/LF response, trying again"); EngrLogger.writeToColumns(sbTemp); return ((int)Action.sendCRLF); } else { sbTemp.Clear(); sbTemp.Append("Didn't get CR/LF response, exiting dumpCPData"); EngrLogger.writeToColumns(sbTemp); return ((int)Action.restartCommandTimer); } } private static int sendDDWait() { EngrLogger.writeToColumns("Sending dd wait command"); int returnValue = SBE41.waitForResponse(SBE41.commands.dd); if (returnValue == 1) { sbTemp.Clear(); sbTemp.Append("Good dd response; Upload sucessful"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.restartCommandTimer); } else if (returnValue == 0) { sbTemp.Clear(); sbTemp.Append("Trying again for dd response"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.sendDA); } else { sbTemp.Clear(); sbTemp.Append("Didn't get dd response, restarting command timer"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.restartCommandTimer); } } private static int sendBinaverage() { EngrLogger.writeToColumns("Sending binaverage wait command"); int returnValue = SBE41.waitForResponse(SBE41.commands.binaverage); if (returnValue == 1) { sbTemp.Clear(); sbTemp.Append("Got good binaverage response"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.sendDA); } else if (returnValue == 0) { sbTemp.Clear(); sbTemp.Append("Trying again for binaverage response"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.sendBinaverage); } else { sbTemp.Clear(); sbTemp.Append("Didn't get binaverage response, restarting command timer"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.restartCommandTimer); } } private static int sendDAWait() { EngrLogger.writeToColumns("Sending da wait command"); int returnValue = SBE41.waitForResponse(SBE41.commands.da); if (returnValue == 1) { sbTemp.Clear(); sbTemp.Append("Good da response; Upload sucessful"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.sendDAH); } else if (returnValue == 0) { sbTemp.Clear(); sbTemp.Append("Trying again for da response"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.sendDA); } else { sbTemp.Clear(); sbTemp.Append("Didn't get da response, restarting command timer"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.restartCommandTimer); } } private static int sendDAHWait() { EngrLogger.writeToColumns("Sending dah wait command"); int returnValue = SBE41.waitForResponse(SBE41.commands.dah); if (returnValue == 1) { sbTemp.Clear(); sbTemp.Append("Good dah response; Upload sucessful"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.restartCommandTimer); } else if (returnValue == 0) { sbTemp.Clear(); sbTemp.Append("Trying again for dah response"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.sendDAH); } else { sbTemp.Clear(); sbTemp.Append("Didn't get dah response, restarting command timer"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); return ((int)Action.restartCommandTimer); } } private static int restartCommandTimer() { SBE41.sendCRLF(); //don't need to wait for response here (According to GM legacy code) Thread.Sleep(1000); SBE41.stopSBE41Timer = false; SBE41.ctdSamplePeriod = configFile.SBE41SamplePeriod / 1000.0; BuoyancyControl.calcPIDConstants(); SBE41.restartTimer(500, configFile.SBE41SamplePeriod); return ((int)Action.lastAction); } } public class ABRetractFast : StateBase { public ABRetractFast(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 5 * 60 * 1000; //5 minutes } private enum Action : int { setBellows = 1, lastAction, size = lastAction } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); ABRetractThresholdCount = 0; sbTemp.Clear(); sbTemp.Append("Retractring bellows fast to bellows position = "); sbTemp.Append(configFile.ABSetBellowsPosition.ToString()); sbTemp.Append(" at "); sbTemp.Append(configFile.ABSetBellowsJV.ToString()); sbTemp.Append(" cps"); EngrLogger.writeToColumns(sbTemp); SBE41.LPFPressure(SV.Pressure, DateTime.Now.Ticks / 10000000.0, true); BuoyancyControl.resetPVPID(); currentAction = (Action)1; return (1); } public override int doStateActions() { switch (currentAction) { case (Action.setBellows): ActionReturn = setBellowsABFast(); break; case (Action.lastAction): //do nothing here break; default: break; } currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { if (currentAction == Action.lastAction) { return (Program.CPFStates.ABRetractSlow); } else if (exceedABRetractPressureThreshold()) { return (Program.CPFStates.ABRetractSlow); } //stay in absetFast else { return (Program.CPFStates.ABRetractFast); } } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); } else { basicStateExit(false); }; return (1); } public override Program.CPFStates doTimeoutAction() { return (Program.CPFStates.ABRetractSlow); } private static int setBellowsABFast() { int returnVal = 0; returnVal = Elmo.moveBellowsToPosition(configFile.ABSetBellowsPosition, configFile.ABSetBellowsJV, 0.5); if (returnVal == 0) return ((int)Action.setBellows); else if (returnVal == -1) return (-1); else return ((int)Action.lastAction); } } public class ABRetractSlow : StateBase { public ABRetractSlow(string name) { stateName.Clear(); stateName.Append(name); ECTimeout = 5 * 60 * 1000; //5 min } private enum Action : int { setBellows = 1, lastAction, size = lastAction } private static Action currentAction; public override int doStateEntryActions() { basicStateEntry(); sbTemp.Clear(); sbTemp.Append("Retractring bellows slow to pressure = "); sbTemp.Append(configFile.ABPressureThreshold.ToString()); sbTemp.Append(" at "); sbTemp.Append(configFile.ABRetractJV.ToString()); sbTemp.Append(" cps"); EngrLogger.writeToColumns(sbTemp); ABRetractThresholdCount = 0; currentAction = (Action)1; return (1); } public override int doStateActions() { switch (currentAction) { case (Action.setBellows): ActionReturn = retractBellowsSlow(); break; case (Action.lastAction): //do nothing here break; default: break; } currentAction = (Action)checkReturn(ActionReturn, (int)currentAction); return (1); } public override Program.CPFStates checkEvents() { //TODO use the same exceedABRetractPressureThreshold method that retractFast uses //Increment threshold counter on successive tests otherwise reset counter to 0 if (SV.Pressure >= configFile.ABPressureThreshold) ABRetractThresholdCount = ABRetractThresholdCount + 1; else ABRetractThresholdCount = 0; //if 3 sucessive counts below threshold have been recorded, transistion to descend if (ABRetractThresholdCount >= 3) { return (Program.CPFStates.descend); } //otherwise stay else { return (Program.CPFStates.ABRetractSlow); } } public override int doExitActions(bool timedOut) { if (timedOut) { basicStateExit(true); //reset privates ABRetractThresholdCount = 0; } else { basicStateExit(false); ABRetractThresholdCount = 0; } return (1); } public override Program.CPFStates doTimeoutAction() { return (Program.CPFStates.descend); } private static int retractBellowsSlow() { if (!Elmo.retracting) { Elmo.retractBellows(configFile.ABRetractJV); } return ((int)Action.setBellows); } } }