//#define OVERRIDE_PRESSURE //#define NO_CTD //#define DEBUG_ANNOUNCE //#define FAKE_CN0254_VOLTS //#define DEBUG_NO_STATE_TIMEOUT //#define SIM_PROFILE //#define ADD_RANDP 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; //This is a bogus change for git educational purposes namespace CPF { public class Program { public enum CPFStates : int { SMNotRunning = 1, initMission, initProfile, descend, ascend, surfaceOpsSetBellows, surfaceOps, preMissionDelay, dumpCPData, ABRetractFast, ABRetractSlow, park, anchor, deAnchor, startCPMode, recoveryMode, exit, cpfStatesLength } public static StateBase[] cpfState = new StateBase[(int)CPFStates.cpfStatesLength]; //TODO Think about moving to SV class. //would need to impliment interface Program-SV interface to allow setting with this current protection level //OR //just make public setable... public static CPFStates CurrentState { get; protected set; } public static CPFStates PastState { get; protected set; } public static CPFStates NextState { get; protected set; } public static bool missionRun { get; set; } public static bool timedoutTrue = true; public static bool timedoutFalse = false; public static I2CDevice g400I2C = new I2CDevice(null); //The only timer that fires something directly is the SBE41CommandModeTimer private static Timer PTHTimer; private static Timer ADCTimer; public static int dumpCPExceptionCounter = 0; private static double parsedPressure = double.NaN; public static double MaxPressure { get; set; } private static TimeSpan SMCycleStartTime; private static TimeSpan SMCyclePeriod; public static DateTime missionStartTime = new DateTime(); private static StringBuilder sbTemp = new StringBuilder(512); public static StringBuilder missionFileName = new StringBuilder("mission60mNoHold.mis"); public static StructQueue sQueue = new StructQueue(); private static StructQueue.qStruct qStructure = new StructQueue.qStruct(); public static BTConsole btConsole; private static Register resetControllerStatus = new Register(0xFFFFFE04); #if(GC_INFO) private static uint freeMem = 0; private static int[] deltaMem = {0,0}; #endif public static void Main() { try { Debug.Print("Program Started"); missionRun = true; //String nmeaString = "PUBX,00,195913.00,3646.20222,N,12152.07890,W,0.000,G3,4.2,5.6,1.404,45.02,-0.078,,0.91,1.34,0.82,9,0,0*68"; //EVK7GPS.parsePUBX00(UTF8Encoding.UTF8.GetBytes(nmeaString)); //SurfaceOps.buildSBDWTStatusMsg(); //TODO implement Watchdog! GHI.Processor.Watchdog.Disable(); #region STATES cpfState[(int)Program.CPFStates.SMNotRunning] = new InitMission("SM Not Running"); cpfState[(int)Program.CPFStates.initMission] = new InitMission("Init Mission"); cpfState[(int)Program.CPFStates.initProfile] = new InitProfile("Init Profile"); cpfState[(int)Program.CPFStates.descend] = new Descend("Descend"); cpfState[(int)Program.CPFStates.ascend] = new Ascend("Ascend"); cpfState[(int)Program.CPFStates.surfaceOpsSetBellows] = new SurfaceOpsSetBellows("Surface Ops Set Bellows"); cpfState[(int)Program.CPFStates.surfaceOps] = new SurfaceOps("Surface Ops"); cpfState[(int)Program.CPFStates.preMissionDelay] = new PreMissionDelay("Pre-Mission Delay"); cpfState[(int)Program.CPFStates.dumpCPData] = new DumpCPData("Dump CP Data"); cpfState[(int)Program.CPFStates.ABRetractFast] = new ABRetractFast("AB Retract Fast"); cpfState[(int)Program.CPFStates.ABRetractSlow] = new ABRetractSlow("AB Retract Slow"); cpfState[(int)Program.CPFStates.park] = new Park("Park"); cpfState[(int)Program.CPFStates.anchor] = new Anchor("Anchor"); cpfState[(int)Program.CPFStates.deAnchor] = new DeAnchor("DeAnchor"); cpfState[(int)Program.CPFStates.startCPMode] = new StartCPMode("Start CP Mode"); cpfState[(int)Program.CPFStates.recoveryMode] = new RecoveryMode("RecoveryMode"); cpfState[(int)Program.CPFStates.exit] = new Exit("Exit"); CurrentState = CPFStates.SMNotRunning; NextState = CurrentState; PastState = (CPFStates)0; #endregion #region initialize //Create BTConsole btConsole = new BTConsole(); btConsole.OpenConsolePort(); //Set system time Debug.Print("Synching system time to GPS"); EVK7GPS.setSystemTime(EVK7GPS.sendI2CCmd("$PUBX,04*37\r\n")); //Utility.SetLocalTime(RealTimeClock.GetDateTime()); //TODO can more initialization be moved into SM (initMission?). Starting SBE41 ASAP can give depth-control. Help protect float from romboot when UW //Start ENGR Logger Debug.Print("Starting EngrLogger.Init"); if (EngrLogger.init()) { sbTemp.Clear(); sbTemp.Append("Engr Logger Initialized"); EngrLogger.writeToColumns(sbTemp); } else { Debug.Print("EngrLogger.Init Failed"); SV.RecoveryMode = true; } sbTemp.Clear(); sbTemp.Append("Initializing FLBB"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); FLBB.init(); sbTemp.Clear(); sbTemp.Append("Initializing Iridium Modem"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); IModem.init(); //IModem.sendPAPUBX04(); //Thread.Sleep(2000); //processStructQ(); //This just puts a file on the mass storage device to mark the beginning of a mission to make it easy to spot reboots //EngrLogger.writeMissionStartFile(resetControllerStatusValue); //Create missions sbTemp.Clear(); sbTemp.Append("Creating Mission"); EngrLogger.writeToColumns(sbTemp); Mission.createStandardMission(100, 200, 1, 0); SV.RecoveryMode = Mission.recoveryMode; //GM Added 2015Aug04 SV.MaxProfiles = Mission.maxProfiles; sbTemp.Clear(); sbTemp.Append("Maximum number of profiles = "); sbTemp.Append(SV.MaxProfiles.ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); SV.MissionTimeoutTS = new TimeSpan(Mission.missionTimeout.Days, Mission.missionTimeout.Hours, Mission.missionTimeout.Minutes, Mission.missionTimeout.Seconds); missionStartTime = DateTime.Now; sbTemp.Clear(); sbTemp.Append("Mission Timeout = "); sbTemp.Append(SV.MissionTimeoutTS.ToString()); sbTemp.Append(" Mission Start Time: "); sbTemp.Append(missionStartTime.ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); Mission.BTSendMission(); //Initialize ports & electronics sbTemp.Clear(); sbTemp.Append("Initializing SBE41"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); SBE41.init(); sbTemp.Clear(); sbTemp.Append("Initializing Elmo"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); Elmo.init(); sbTemp.Clear(); sbTemp.Append("Initializing Optode"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); Optode.init(); Thread.Sleep(1000); sbTemp.Clear(); sbTemp.Append("Initializing EVK7 GPS"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); EVK7GPS.init(); sbTemp.Clear(); sbTemp.Append("Initializing Pressure, Temperature, Humidity sensor"); EngrLogger.writeToColumns(sbTemp); sbTemp.Clear(); PTH.init(); //TODO Can move to specific classes PTHTimer = new Timer(new TimerCallback(PTHTimerCallback), null, 0, configFile.PTHSamplePeriod); ADCTimer = new Timer(new TimerCallback(ADCTimerCallback), null, 0, configFile.ADCSamplePeriod); BuoyancyControl.init(); //TODO this should all be moved into CN0254 class, but since ADC will soon be changed, leaving here to make progress for (int i = 0; i < 3; i++) { //CN0254Volts = CN0254.scanAll(); #if(FAKE_CN0254_VOLTS) CN0254Volts[0] = 2.5; //bellowsPosition CN0254Volts[1] = 4; //batteryBusVolts #endif //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; //Thread.Sleep(1000); } #endregion processStructQ(); //This is the top level state machine processor. It doesn't know anything //about what happens in any state. It just makes sure the right state is running. CurrentState = CPFStates.initMission; NextState = CurrentState; PastState = CPFStates.SMNotRunning; #region State Machine while (missionRun) { try { #if (!DEBUG_NO_STATE_TIMEOUT) ExecutionConstraint.Install(cpfState[(int)CurrentState].ECTimeout, 0); #else ExecutionConstraint.Install(1000*60*60,0); //60 minute timeout #endif do { SMCycleStartTime = Microsoft.SPOT.Hardware.Utility.GetMachineTime(); //Do the state entry actions one time if (SV.StateEntry) cpfState[(int)CurrentState].doStateEntryActions(); //Do the state actions and check events for the next state cpfState[(int)CurrentState].doStateActions(); NextState = cpfState[(int)CurrentState].checkEvents(); processStructQ(); //check CPF health, (potentially) overwrite NextState if problem exists NextState = StateBase.checkGlobalIssues(); if (NextState != CurrentState) cpfState[(int)CurrentState].doExitActions(timedoutFalse); writeSystemStateMessage(); SMCyclePeriod = Microsoft.SPOT.Hardware.Utility.GetMachineTime() - SMCycleStartTime; while ((Microsoft.SPOT.Hardware.Utility.GetMachineTime() - SMCycleStartTime) < configFile.SMSleepPeriod) Thread.Sleep(100); } while (NextState == CurrentState); } catch (Exception currentException) { //Since we're using execution constraints as the state timeout timer this kind of exception //isn't necessarily an error but does force a state timout action if (currentException is ConstraintException) { #if (!DEBUG_NO_STATE_TIMEOUT) cpfState[(int)CurrentState].doExitActions(timedoutTrue); NextState = cpfState[(int)CurrentState].doTimeoutAction(); #endif } //All other exceptions are caught here else { sbTemp.Clear(); sbTemp.Append("*** Exception in State Machine loop: "); sbTemp.Append(currentException.Message); sbTemp.Append(";"); sbTemp.Append(currentException.InnerException); sbTemp.Append(";"); sbTemp.Append(currentException.StackTrace); EngrLogger.writeToColumns(sbTemp); for (int i = 0; i < 5; i++) { btConsole.SendLine(sbTemp); Thread.Sleep(1000); } if ( CurrentState == CPFStates.dumpCPData) { dumpCPExceptionCounter++; if (dumpCPExceptionCounter <= 3) { EngrLogger.writeToColumns("Exception in dumpCPData, trying again"); NextState = CPFStates.dumpCPData; } else { EngrLogger.writeToColumns("Exception in dumpCPData, giving up"); NextState = CPFStates.recoveryMode; } } else { NextState = CPFStates.recoveryMode; } } } ExecutionConstraint.Install(-1, 0); PastState = CurrentState; CurrentState = NextState; } #endregion #region Last Catch Block } catch (Exception currentException) { sbTemp.Clear(); sbTemp.Append("*** Exception in CPF Program Initialization: "); sbTemp.Append(currentException.Message); sbTemp.Append(";"); sbTemp.Append(currentException.InnerException); sbTemp.Append(";"); sbTemp.Append(currentException.StackTrace); Debug.Print(sbTemp.ToString()); if (EngrLogger.fileOpen) { EngrLogger.writeToColumns(sbTemp); } if (BTConsole.IsOpen) { for (int i = 0; i < 5; i++) { btConsole.SendLine(sbTemp); Thread.Sleep(1000); } } } #endregion } //TODO move the below two methods to BTConsole, Instrument classes, and StructQueue class //Static variables relevant to queue processor #region Queue Static Variables private static BuoyancyControl.PIDLogValues pidLogValues; private static byte[] dequeueMessage = new byte[256]; private static byte[] pubx04ByteArray = new byte[] { (byte)'P', (byte)'U', (byte)'B', (byte)'X', (byte)',', (byte)'0', (byte)'4', }; private static double simPressure = 0.0; 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]; public 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 SBE41ErrorHeader = new StringBuilder("*** Error in 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 FLBBHeader = new StringBuilder("FLBB 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 readonly StringBuilder BPError = new StringBuilder("Bellows position exceeds limits, start Emergency Ascend. Pos = "); private static byte[] returnedByteArray = new byte[StructQueue.byteArrayWidth]; private static byte[] sbdwtByteArray = new byte[1024]; public 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 byte[] sbdrtHeader = UTF8Encoding.UTF8.GetBytes("SBDRT:"); private static byte[] sbdRecoveryTrue = UTF8Encoding.UTF8.GetBytes("$RECOVERYTRUE"); private static byte[] sbdRecoveryFalse = UTF8Encoding.UTF8.GetBytes("$RECOVERYFALSE"); private static byte[] sbdGo = UTF8Encoding.UTF8.GetBytes("$GO"); private static byte[] sbdMaxProfiles = UTF8Encoding.UTF8.GetBytes("$MAXPROFILES"); private static byte[] sbdMissionTimeout = UTF8Encoding.UTF8.GetBytes("$MISSIONTIMEOUT"); private static byte[] sbdParkTime = UTF8Encoding.UTF8.GetBytes("$PARKTIME"); private static byte[] sbdParkPressure = UTF8Encoding.UTF8.GetBytes("$PARKPRESSURE"); private static DateTime timeNow; private static PTH.PTHData pthData; public static PTH.PTHData PTHData { get { return pthData; } set { pthData = value; } } public static EVK7GPS.Position currentPosition; private static int byteArrayLength = -1; private static int msgLength = 0; private static Random randNum = new Random(); private static EnergyMonitor.EnergyMonitorData EMData = new EnergyMonitor.EnergyMonitorData(); private static StringBuilder sbTemp2 = new StringBuilder(512); #endregion private static void processStructQ() { while (Program.sQueue.Count > 0) { // I don't believe this lock is effective because there is only one way that the CPF program can get to this line //and that's through the main program thread calling processStructQ(). //Ultimatly what we want to avoid is multiple threads (interrupt-dirven serial event handlers in our case) from //accessing the queue at the same time. This lock does not accomplish that. The lock needs to be internal //to the code within Dequeue(), if that's what we indeed want to lock. qStructure = sQueue.Dequeue(); switch (qStructure.qRecordType) { case (StructQueue.QRecordType.SBE41): #region if (qStructure.errorCode > 0) //No error in SBE41 IO //parse and log except "S>fp" and "SBE41 ...") { if ((qStructure.byteArray[0] == 83) && (qStructure.byteArray[1] == 62) && (qStructure.byteArray[2] == 102) && (qStructure.byteArray[3] == 112)) { } else if ((qStructure.byteArray[0] == 83) && (qStructure.byteArray[1] == 66) && (qStructure.byteArray[2] == 69) && (qStructure.byteArray[3] == 32) && (qStructure.byteArray[4] == 52) && (qStructure.byteArray[5] == 49)) { } else { byteArrayLength = Array.IndexOf(qStructure.byteArray, 0); //Parse message, only fp and CP mode PTS responses will return a value, all other messages should return .NaN parsedPressure = SBE41.parse(qStructure.byteArray); if ((parsedPressure > -5) && (parsedPressure < 5000)) { SV.Pressure = parsedPressure; if (parsedPressure > MaxPressure) MaxPressure = parsedPressure; SV.PressureTimeStamp = DateTime.Now; #region OVERRIDE_PRESSURE #if(OVERRIDE_PRESSURE) OVERRIDE_PRESSURE(); #endif #endregion #region SIM_PROFILE #if(SIM_PROFILE) SIM_PROFILE(); #endif #endregion #region ADD_RANDP #if(ADD_RANDP) pressure = pressure + ((randNum.NextDouble() - 0.5) / 500.0); #endif #endregion lpfResults = SBE41.LPFPressure(SV.Pressure, qStructure.timeStamp.Ticks / 10000000.0 , false); if (SV.RunPVPID) { if (SV.RunPVPID) { pidLogValues = BuoyancyControl.PIDPlatformVelocity(Profile.PVSetPoint, lpfResults.diffVelocity); if (configFile.verbosityLevel > 2) { EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, velPIDHeader, EngrLogger.ColumnNums.pressure, SV.Pressure, //added by LB 7/24/15 because otherwise plotting PID vals is a pain EngrLogger.ColumnNums.PID_SP, 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, EngrLogger.ColumnNums.PID_h, pidLogValues.h); //TODO Should do this in a way that the message is time stamped at the source } } } if (configFile.verbosityLevel > 2) { EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, SBE41Header, EngrLogger.ColumnNums.pressure, SV.Pressure, EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray, EngrLogger.ColumnNums.SBE41Pressure, lpfResults.Pressure, EngrLogger.ColumnNums.SBE41LPFPressure, lpfResults.LPFPressure, EngrLogger.ColumnNums.SBE41LPFVelocity, lpfResults.LPFVelocity, EngrLogger.ColumnNums.SBE41DiffVelocity, lpfResults.diffVelocity, EngrLogger.ColumnNums.SBE41Diff2Velocity, lpfResults.diff2Velocity, EngrLogger.ColumnNums.SBE41Slope, lpfResults.slope, EngrLogger.ColumnNums.CTDSamplePeriod, SBE41.ctdSamplePeriod); //TODO Should do this in a way that the message is time stamped at the source } } else { //log any non-pressure messages") EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, unParsedSBE41Header, EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray); } } //Check for expected response if (SBE41.waitingForResponse) SBE41.checkResponse(qStructure.byteArray); } else //Error in SBE 41 IO { sbTemp.Clear(); sbTemp.Append(SBE41ErrorHeader); sbTemp.Append(" Error number = "); sbTemp.Append(qStructure.errorCode.ToString()); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, sbTemp, EngrLogger.ColumnNums.CTDSalinity, qStructure.byteArray); } break; #endregion case (StructQueue.QRecordType.Optode): #region for (int i = 0; i < qStructure.byteArray.Length; i++) { if (qStructure.byteArray[i] == 9) qStructure.byteArray[i] = 44; } EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, OptodeHeader, EngrLogger.ColumnNums.pressure, SV.Pressure, EngrLogger.ColumnNums.TOpt, qStructure.byteArray); //check for expectedResponse anywhere in the SBE41 response message msgLength = qStructure.byteArray.Length; if (Optode.waitingForResponse) { if (msgLength > Optode.expectedResponseLength) { for (int j = 0; j < msgLength - Optode.expectedResponseLength; j++) { for (int k = 0; k < Optode.expectedResponseLength; k++) { if (qStructure.byteArray[j + k] != Optode.expectedResponse[k]) { Optode.gotResponse = false; break; } } } Optode.gotResponse = true; } } break; #endregion case (StructQueue.QRecordType.FLBB): #region timeNow = DateTime.Now; if (Array.IndexOf(qStructure.byteArray, (byte)'\t') > -1) { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); try { sbTemp.Replace('\t', ','); } catch { sbTemp2.Clear(); sbTemp2.Append("Program /t .Replace error location 1 string: "); sbTemp2.Append(sbTemp); EngrLogger.writeToColumns(sbTemp2); } Array.Clear(qStructure.byteArray, 0, qStructure.byteArray.Length); Array.Copy(UTF8Encoding.UTF8.GetBytes(sbTemp.ToString()), qStructure.byteArray, sbTemp.Length); } EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, FLBBHeader, EngrLogger.ColumnNums.pressure, SV.Pressure, EngrLogger.ColumnNums.FLBB, qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.EVK7): #region //Set system time if message is PUBX,04 if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, pubx04ByteArray) >= 0) { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EVK7GPS.setSystemTime(sbTemp); } else //it has to be a PUBX,00 message (at least until we add more message type) { if (qStructure.errorCode >= 0) { Array.Clear(pubxByteArray, 0, pubxByteArray.Length); Array.Copy(qStructure.byteArray, pubxByteArray, Array.IndexOf(qStructure.byteArray, 0)); EVK7GPS.parsePUBX00(pubxByteArray); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, EVK7Header, EngrLogger.ColumnNums.GPS, qStructure.byteArray); } else //If error code is < 0 we've got an error { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EngrLogger.writeToColumns(sbTemp); } } break; #endregion case (StructQueue.QRecordType.IModem): #region //TODO Should this really be here? if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, pubx04ByteArray) >= 0) { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EVK7GPS.setSystemTime(sbTemp); } if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdRecoveryTrue) >= 0) { EngrLogger.writeToColumns("Received SBD: RecoveryTrue"); processConsoleCommand(UTF8Encoding.UTF8.GetBytes("$RECOVERYTRUE\r\n")); } else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdRecoveryFalse) >= 0) { EngrLogger.writeToColumns("Received SBD: RecoveryFalse"); processConsoleCommand(UTF8Encoding.UTF8.GetBytes("$RECOVERYFALSE\r\n")); } else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdGo) >= 0) //TODO Add a don't go command { EngrLogger.writeToColumns("Received SBD: Go"); processConsoleCommand(UTF8Encoding.UTF8.GetBytes("$GO\r\n")); } else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdMaxProfiles) >= 0) //TODO Add a don't go command { EngrLogger.writeToColumns("Received SBD: MaxProfiles"); processConsoleCommand(qStructure.byteArray); } else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdMissionTimeout) >= 0) //TODO Add a don't go command { EngrLogger.writeToColumns("Received SBD: MissionTimeout"); processConsoleCommand(qStructure.byteArray); } else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdParkTime) >= 0) //TODO Add a don't go command { EngrLogger.writeToColumns("Received SBD: ParkTime"); processConsoleCommand(qStructure.byteArray); } else if (GHI.Utilities.Arrays.Contains(qStructure.byteArray, sbdParkPressure) >= 0) //TODO Add a don't go command { EngrLogger.writeToColumns("Received SBD: ParkPressure"); processConsoleCommand(qStructure.byteArray); } EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, qStructure.timeStamp, EngrLogger.ColumnNums.state, qStructure.state, EngrLogger.ColumnNums.comment, IModemHeader, EngrLogger.ColumnNums.IModem, qStructure.byteArray); if (SurfaceOps.Substate == (int)IModem.commands.csq) { IModem.parseSQ(qStructure.byteArray); } //Check for expected response if (IModem.waitingForResponse && !IModem.gotResponse) IModem.checkResponse(qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.PTH): #region //If error code == 0 then get a PTH set of samples if (qStructure.errorCode >= 0) { pthData = PTH.getPTH(); timeNow = DateTime.Now; EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, Program.CurrentState, EngrLogger.ColumnNums.comment, PTHHeader, EngrLogger.ColumnNums.LPS331Pressure, pthData.LPS331Pressure, EngrLogger.ColumnNums.LPS331Temperature, pthData.LPS331Temperature, EngrLogger.ColumnNums.SHT21Humidity, pthData.SHT21Humidity, EngrLogger.ColumnNums.SHT21Temperature, pthData.SHT21Temperature); } else //If error code is < 0 we've got an error { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EngrLogger.writeToColumns(sbTemp); } break; #endregion case (StructQueue.QRecordType.Error): #region if (qStructure.errorCode >= 0) { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EngrLogger.writeToColumns(sbTemp); } else //If error code is < 0 we've got an error { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); EngrLogger.writeToColumns(sbTemp); } break; #endregion case (StructQueue.QRecordType.BTConsole): #region processConsoleCommand(qStructure.byteArray); break; #endregion case (StructQueue.QRecordType.ADC): #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; bellowsPosition = ADC2485.readBellowsPosition(); EMData = EnergyMonitor.readEM(); batteryBusVolts = EMData.voltage; batteryBusAmps = EMData.current; amps12V = -100.0; amps33V = -100.0; if (configFile.verbosityLevel > 5) { EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, Program.CurrentState, 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(FAKE_CN0254_VOLTS) bellowsPosition = 68.0; batteryBusVolts = 30.0; #endif 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, Program.CurrentState, EngrLogger.ColumnNums.comment, UTF8Encoding.UTF8.GetBytes(sbTemp.ToString())); break; #endregion } //sbTemp.Clear(); //sbTemp.Append(UTF8Encoding.UTF8.GetChars(qStructure.byteArray)); //Debug.Print(sbTemp.ToString()); } } public static bool DirectoryValid { get; set; } public static int uploadEngrFileNum = -1; private static void processConsoleCommand(byte[] consoleCommandMessage) { const byte byteToken = 44; int tokenIndex = -1; double argValue = -1; //Ignore command if it doesn't have a CR if (Array.IndexOf(consoleCommandMessage, 13) >= 0) { BTConsole.BTSend = true; sbTemp.Clear(); sbTemp.Append("Processing Console Command: "); sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, Array.IndexOf(consoleCommandMessage, 13))); EngrLogger.writeToColumns(sbTemp); //Check if the command has an argument tokenIndex = Array.IndexOf(consoleCommandMessage, byteToken); //If it has an argument, process it here if (tokenIndex > 0) { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, tokenIndex + 1, (Array.IndexOf(consoleCommandMessage, 13) - tokenIndex - 1))); try { argValue = double.Parse(sbTemp.ToString()); sbTemp.Clear(); sbTemp.Append("Console arg value = "); sbTemp.Append(argValue.ToString()); Debug.Print(sbTemp.ToString()); } catch { sbTemp.Clear(); sbTemp.Append("Error parsing console command argument: "); sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, (Array.IndexOf(consoleCommandMessage, 13) - 1))); EngrLogger.writeToColumns(sbTemp); } sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, tokenIndex - 1)); switch (sbTemp.ToString().ToUpper()) { case "SIMP": simPressure = argValue; sbTemp.Clear(); sbTemp.Append("Sim P = "); sbTemp.Append(simPressure.ToString()); Debug.Print(sbTemp.ToString()); break; case "UPENGR": SV.UploadEngrFile = true; uploadEngrFileNum = (int)argValue; break; case "MAXPROFILES": if (((int)argValue > 0) && ((int)argValue < 200)) { sbTemp.Clear(); sbTemp.Append("Setting Max Number of Profiles to: "); sbTemp.Append(((float)argValue).ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); SV.MaxProfiles = (int)argValue; } else EngrLogger.writeToColumns("MaxProfiles value out of range"); break; case "MISSIONTIMEOUT": if (((int)argValue > 0) && ((int)argValue < 120)) { SV.MissionTimeoutTS = new TimeSpan(0, (int)argValue, 0, 0); sbTemp.Clear(); sbTemp.Append("Set MissionTimeout hours to: "); sbTemp.Append(SV.MissionTimeoutTS.ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); } else EngrLogger.writeToColumns("MissionTimeout value out of range"); break; case "PARKTIME": if (((int)argValue > 0) && ((int)argValue < (2 * 24 * 3600))) { Mission.descendTable[0].parkTime = new TimeSpan(0, 0, (int)argValue, 0); sbTemp.Clear(); sbTemp.Append("Park time minutes to: "); sbTemp.Append(Mission.descendTable[0].parkTime.ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); } else EngrLogger.writeToColumns("ParkTime value out of range"); break; case "PARKPRESSURE": if (((float)argValue > 10) && ((float)argValue < 300)) { sbTemp.Clear(); sbTemp.Append("Setting Park Pressure to: "); sbTemp.Append(((float)argValue).ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); Mission.descendTable[0].pressure = (float)argValue; } else EngrLogger.writeToColumns("ParkPressure value out of range"); break; case "SETKP": if (((float)argValue > 1) && ((float)argValue < 300)) { sbTemp.Clear(); sbTemp.Append("Setting Kp to: "); sbTemp.Append(((float)argValue).ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); BuoyancyControl.PlatVel.K = (double)argValue; BuoyancyControl.calcPIDConstants(); } else EngrLogger.writeToColumns("Kp value out of range"); break; case "SETABRETRACTJV": if (((float)argValue > 1) && ((float)argValue < 5000)) { sbTemp.Clear(); sbTemp.Append("Setting AB Retract Slow JV to: "); sbTemp.Append(((float)argValue).ToString()); EngrLogger.writeToColumns(sbTemp.ToString()); configFile.ABRetractJV = (int)argValue; } else EngrLogger.writeToColumns("Kp value out of range"); break; } } else //process commands without an argument { sbTemp.Clear(); sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 1, (Array.IndexOf(consoleCommandMessage, 13) - 1))); switch (sbTemp.ToString().ToUpper()) { case "GETDIR": EngrLogger.getDirectory(); DirectoryValid = true; break; case "GO": SV.SurfaceOpsGo = true; break; case "RECOVERYTRUE": SV.RecoveryMode = true; sbTemp.Clear(); sbTemp.Append("Recovery mode set true"); EngrLogger.writeToColumns(sbTemp); break; case "RECOVERYFALSE": SV.RecoveryMode = false; sbTemp.Clear(); sbTemp.Append("Recovery mode set false"); EngrLogger.writeToColumns(sbTemp); break; case "EXIT": CurrentState = Program.CPFStates.exit; break; #if(OVERRIDE_PRESSURE) case "RANDON": simRAND_ON = true; break; case "RANDOFF": simRAND_ON = false; break; #endif default: sbTemp.Clear(); sbTemp.Append("Invalid command, try again"); btConsole.SendLine(sbTemp); break; } } } else { sbTemp.Clear(); sbTemp.Append("Received non-CR terminated Console Command: "); sbTemp.Append(UTF8Encoding.UTF8.GetChars(consoleCommandMessage, 0, (Array.IndexOf(consoleCommandMessage, 0) - 1))); EngrLogger.writeToColumns(sbTemp); } sbConsoleCommand.Clear(); sbConsoleCommand.Append("NONE"); } private static StructQueue.qStruct PTHQStruct = new StructQueue.qStruct() { qRecordType = StructQueue.QRecordType.PTH, byteArray = new byte[StructQueue.byteArrayWidth] }; private static void PTHTimerCallback(object o) { PTHQStruct.timeStamp = DateTime.Now; PTHQStruct.state = CurrentState; PTHQStruct.errorCode = 0; Program.sQueue.Enqueue(PTHQStruct); } private static StructQueue.qStruct ADCQStruct = new StructQueue.qStruct() { qRecordType = StructQueue.QRecordType.ADC, byteArray = new byte[StructQueue.byteArrayWidth] }; private static void ADCTimerCallback(object o) { Program.sQueue.Enqueue(ADCQStruct); } private static void writeSystemStateMessage() { timeNow = DateTime.Now; sbTemp.Clear(); if (SV.RecoveryMode) sbTemp.Append("RecMode true, "); else sbTemp.Append("RecMode false, "); if (SV.RunPVPID) sbTemp.Append("PID On, "); else sbTemp.Append("PID Off, "); if (Elmo.extending) sbTemp.Append("Ext'g, "); if (Elmo.retracting) sbTemp.Append("Ret'g, "); if (Elmo.motorStopped) sbTemp.Append("Stopped, "); if (Elmo.isValveOpen()) sbTemp.Append("Open, "); else sbTemp.Append("Closed, "); if (CurrentState == CPFStates.park) sbTemp.Append("Park, "); sbTemp.Append("Scend sub: "); sbTemp.Append(Profile.ProfileState); sbTemp.Append(", "); sbTemp.Append("depthNum = "); sbTemp.Append(SV.PressureTableNum.ToString()); EngrLogger.writeToColumns(EngrLogger.ColumnNums.dateTime, timeNow, EngrLogger.ColumnNums.state, Program.CurrentState, EngrLogger.ColumnNums.comment, sbTemp, EngrLogger.ColumnNums.pressure, SV.Pressure, EngrLogger.ColumnNums.bellowsPosition, bellowsPosition, EngrLogger.ColumnNums.batteryBusVolts, batteryBusVolts, EngrLogger.ColumnNums.batteryBusAmps, batteryBusAmps, EngrLogger.ColumnNums.Amps12V, amps12V); } #region NO_CTD #if(NO_CTD) OVERRIDE_PRESSURE(); #endif #endregion #region TESTS #region OVERRIDE_PRESSURE #if(OVERRIDE_PRESSURE) private static bool simRAND_ON = true; private static void OVERRIDE_PRESSURE() { if (simRAND_ON) SV.Pressure = simPressure + ((randNum.NextDouble() - 0.5) / 10.0); else SV.Pressure = simPressure; SV.runPVPID = false; sbTemp.Clear(); sbTemp.Append("Simulating pressure from console, runPVPID: false, P = "); sbTemp.Append(pressure.ToString("f2")); Debug.Print(sbTemp.ToString()); } #endif #endregion #endregion } }