/*Function running on PlatformUDPThread that communicates *with MOOG platform by UDP datagrams*/ private void PlatformCommuncation() { Stopwatch freqTimer = new Stopwatch(); Stopwatch elapsedTimer = new Stopwatch(); Stopwatch recvTimer = new Stopwatch(); double currentFrequency = 0.0; int sendCount = 0; MDACommand platformCommand = new MDACommand(); platformCommand.MCW = (uint) MOOG_ENGAGE; byte[] sendBytes = flipStrBytes(platformCommand); MOOGhostname = MOOGRemoteIpEndPoint.Address.ToString(); MOOGdestport = MOOGRemoteIpEndPoint.Port; //Platform sends and receives on different ports MOOGPlatformRcvUDP.Send(sendBytes, sendBytes.Length, MOOGhostname,MOOGdestport); //bool bSetMode = false; byte lastMDAFile = 101; freqTimer.Start(); elapsedTimer.Start(); recvTimer.Start(); #region Loop while (PlatformUpdate) { #region RcvPlatformPacket if (MOOGPlatformRcvUDP.Available > 0) { try { byte[] recvBytes = MOOGPlatformRcvUDP.Receive(ref MOOGRemoteIpEndPoint); MOOGResponse platformResp = fromMOOGBytes(recvBytes); //check for faults? //Update Status //UpdateFeedbackState(platformResp); MOOGMutex.WaitOne(); CurrentMOOGResponse = platformResp; MOOGMutex.ReleaseMutex(); byte[] RespStatus = BitConverter.GetBytes(platformResp.status); if (RespStatus[1] != lastMDAFile && errorBar != null) { lastMDAFile = RespStatus[1]; try { int index = Array.FindIndex(MOOG_MDAOPTIONS, item => item == lastMDAFile); MDAFileBox.BeginInvoke(new InvokeDelegateComboIndex(UpdateComboBoxIndex), MDAFileBox, index); //MDAFileBox.BeginInvoke(new InvokeDelegateComboString(UpdateComboBoxValue), MDAFileBox, lastMDAFile.ToString("000")); //errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), "Current MDA File: MDA" + lastMDAFile.ToString("000") + ".in"); } catch (System.Exception ex) { MessageBox.Show(ex.Message, "Change of MDA File Error"); } } PlatformFault = platformResp.fault; PlatformIO = platformResp.io_info; PlatformState = (byte)(platformResp.status % 16); //Update displayed state if (PlatformState != PlatformLastState && StatusText != null) { try { StatusText.BeginInvoke(new InvokeDelegateState(UpdatePlatformState), PlatformState); } catch (System.Exception ex) { MessageBox.Show(ex.Message, "Status Update Error"); } } // Reset Timer recvTimer.Reset(); recvTimer.Start(); } catch (System.Exception ex) { MessageBox.Show(ex.Message, "Platform Receiving Communication Error"); break; } } //if else if (recvTimer.ElapsedMilliseconds > 10000 && PlatformState != (byte)MachineStates.DISABLED) { // Haven't heard from the platform in a long while PlatformState = (byte) MachineStates.INHIBITED; StatusText.BeginInvoke(new InvokeDelegateState(UpdatePlatformState), PlatformState); // Reset Timer recvTimer.Reset(); recvTimer.Start(); } //else #endregion UpdateMutex.WaitOne(); byte LocalRequestedState = RequestedState; UpdateMutex.ReleaseMutex(); #region SendPlatformPacket //Only run if significant time has passed if (elapsedTimer.ElapsedMilliseconds >= MoogSendTimeOutMS || (LocalRequestedState == MOOG_NEWMDA && MoogElapsedSendState.ElapsedMilliseconds >= MoogSendTimeOutMS)) { try { if (LocalRequestedState == MOOG_DISABLE) { //Send Disable Command SendCommand(MOOG_DISABLE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } switch (PlatformState) { case (byte)MachineStates.ENGAGED: //Send updated state if (LocalRequestedState == MOOG_NEWMDA && MoogElapsedSendState.ElapsedMilliseconds >= MoogSendTimeOutMS) { //Send updated state InputDataMutex.WaitOne(); byte[] localBytes = InputBytes; InputDataMutex.ReleaseMutex(); SendState(MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport,localBytes); //SendCommand(MOOG_MDAMODE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); MoogSendStateCount++; MoogElapsedSendState.Reset(); MoogElapsedSendState.Start(); } else if (LocalRequestedState == MOOG_PARK) { //Send Park command SendCommand(MOOG_PARK, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } else if (LocalRequestedState == MOOG_ENGAGE) { //Send Engage Command SendCommand(MOOG_ENGAGE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); }//if else if (LocalRequestedState == MOOG_MDAMODE) { SendCommand(MOOG_MDAMODE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } //Keep alive //Display feedback data break; case (byte)MachineStates.POWERUP: //Send MDA Mode command if (LocalRequestedState == MOOG_INHIBIT) { //Send Inhibit Command SendCommand(MOOG_INHIBIT, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } else if (LocalRequestedState == MOOG_NEWMDAFILE) { //Request new mda file SendMDAFileChange(MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); if (RequestedMDAFile == lastMDAFile) { UpdateMutex.WaitOne(); RequestedState = MOOG_MDAMODE; UpdateMutex.ReleaseMutex(); } } else //Keep connection alive { SendCommand(MOOG_MDAMODE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } break; case (byte)MachineStates.PARKING: if (LocalRequestedState == MOOG_PARK) { //Send Park command SendCommand(MOOG_PARK, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } break; case (byte)MachineStates.IDLE: //Send updated state if (LocalRequestedState == MOOG_ENGAGE) { //Send Engage Command SendCommand(MOOG_ENGAGE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); }//if else if (LocalRequestedState == MOOG_INHIBIT) { //Send Inhibit Command SendCommand(MOOG_INHIBIT, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } else if (LocalRequestedState == MOOG_NEWMDAFILE) { //Request new mda file SendMDAFileChange(MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); if (RequestedMDAFile == lastMDAFile) { UpdateMutex.WaitOne(); RequestedState = MOOG_MDAMODE; UpdateMutex.ReleaseMutex(); } } else //Keep connection alive { SendCommand(MOOG_MDAMODE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } break; case (byte)MachineStates.STANDBY: if (LocalRequestedState == MOOG_PARK) { //Send Park command SendCommand(MOOG_PARK, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } else if (LocalRequestedState == MOOG_ENGAGE) { //Send Engage Command //SendState(PlatformRcvUDP, hostname, dest_port); SendCommand(MOOG_ENGAGE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); }//if else //Keep connection alive { SendCommand(MOOG_MDAMODE, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } break; case (byte)MachineStates.FAULT2: case (byte)MachineStates.INHIBITED: //Need to tell user to issue Reset Command if (LocalRequestedState == MOOG_RESET) { //Send Reset command SendCommand(MOOG_RESET, MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport); } break; } } catch (System.Exception ex) { MessageBox.Show(ex.Message, "Platform Sending Communication Error"); break; } #region UpdateTimeout sendCount++; if (sendCount >= 500) { //Ensures Consistent Platform Communications long elapsedTime = freqTimer.ElapsedMilliseconds; currentFrequency = 1000.0 * sendCount / elapsedTime; sendCount = 0; if (currentFrequency > 100.0) //Going far too fast { MoogSendTimeOutMS += 3; } else if (currentFrequency > 70.0) //Going too fast { MoogSendTimeOutMS += 2; } else if (currentFrequency > 62.0) //Going too fast { MoogSendTimeOutMS += 1; } else if (currentFrequency < 58.0) //Can speed up a bit { MoogSendTimeOutMS -= 1; } //Update Send State Frequency if (MoogSendStateCount > 100) { currentFrequency = 1000.0 * MoogSendStateCount / elapsedTime; errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), "Send State Freq: " + currentFrequency.ToString("000.00") + " Hz"); meanDeltaTime = sendDeltaTime / MoogSendStateCount; SendDeltaTimeTextbox.BeginInvoke(new InvokeDelegateTextString(UpdateTextBoxValue), SendDeltaTimeTextbox, meanDeltaTime.ToString("0.000")); } MoogSendStateCount = 0; sendDeltaTime = 0; freqTimer.Reset(); freqTimer.Start(); } #endregion elapsedTimer.Reset(); elapsedTimer.Start(); } //if timer elapsed #endregion }//while #endregion }
public void UpdateFeedbackState(MOOGResponse platformResp) { PPitchText.BeginInvoke(new InvokeDelegateFloat(UpdatePPitch), platformResp.pitch); PRollText.BeginInvoke(new InvokeDelegateFloat(UpdatePRoll), platformResp.roll); PYawText.BeginInvoke(new InvokeDelegateFloat(UpdatePYaw), platformResp.yaw); PSurgeText.BeginInvoke(new InvokeDelegateFloat(UpdatePSurge), platformResp.surge); PSwayText.BeginInvoke(new InvokeDelegateFloat(UpdatePSway), platformResp.lateral); PHeaveText.BeginInvoke(new InvokeDelegateFloat(UpdatePHeave), platformResp.heave); }
//Returns MOOGResponse structure from array of bytes MOOGResponse fromMOOGBytes(byte[] arr) { MOOGResponse str = new MOOGResponse(); str.fault = BitConverter.ToUInt32(flipBytes(arr,0), 0); str.io_info = BitConverter.ToUInt32(flipBytes(arr, 4), 0); str.status = BitConverter.ToUInt32(flipBytes(arr, 8), 0); str.roll = BitConverter.ToSingle(flipBytes(arr, 12), 0); str.pitch = BitConverter.ToSingle(flipBytes(arr, 16), 0); str.heave = BitConverter.ToSingle(flipBytes(arr, 20), 0); str.surge = BitConverter.ToSingle(flipBytes(arr, 24), 0); str.yaw = BitConverter.ToSingle(flipBytes(arr, 28), 0); str.lateral = BitConverter.ToSingle(flipBytes(arr, 32), 0); str.spare = 0; return str; }
private void PlatformCommuncationTest() { IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Parse("255.255.255.255"), 991); //System.Net.Sockets.UdpClient PlatformSendUDP = new System.Net.Sockets.UdpClient(991); System.Net.Sockets.UdpClient PlatformRcvUDP = new System.Net.Sockets.UdpClient(992); MDACommand platformCommand = new MDACommand(); platformCommand.MCW = (uint) MOOG_ENGAGE; byte[] sendBytes = flipStrBytes(platformCommand); string hostname = RemoteIpEndPoint.Address.ToString(); int dest_port = RemoteIpEndPoint.Port; //Platform sends and receives on different ports PlatformRcvUDP.Send(sendBytes, sendBytes.Length, hostname,dest_port); //bool bSetMode = false; while (PlatformUpdate) { //Wait for platform request if (PlatformRcvUDP.Available > 0) { try { byte[] recvBytes = PlatformRcvUDP.Receive(ref RemoteIpEndPoint); MOOGResponse platformResp = fromMOOGBytes(recvBytes); // string statusMessage = String.Format("Platform: {0}:{1}", hostname, dest_port); // errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), statusMessage); //check for faults if (platformResp.fault > 0) { //What's the fault? } //if //Update Status //UpdateFeedbackState(platformResp); MOOGMutex.WaitOne(); CurrentMOOGResponse = platformResp; MOOGMutex.ReleaseMutex(); PlatformFault = platformResp.fault; PlatformIO = platformResp.io_info; PlatformState = (byte)(platformResp.status % 16); UpdateMutex.WaitOne(); byte LocalRequestedState = RequestedState; UpdateMutex.ReleaseMutex(); if (LocalRequestedState == MOOG_DISABLE) { //Send Disable Command SendCommand(MOOG_DISABLE, PlatformRcvUDP, hostname, dest_port); } switch (PlatformState) { case (byte)MachineStates.ENGAGED: //Send updated state if (LocalRequestedState == MOOG_ENGAGE) { //Send updated state SendState(PlatformRcvUDP, hostname, dest_port); } else if (LocalRequestedState == MOOG_PARK) { //Send Park command SendCommand(MOOG_PARK, PlatformRcvUDP, hostname, dest_port); } //Display feedback data break; case (byte)MachineStates.POWERUP: //Send MDA Mode command SendCommand(MOOG_MDAMODE, PlatformRcvUDP, hostname, dest_port); break; case (byte)MachineStates.PARKING: if (LocalRequestedState == MOOG_PARK) { //Send Park command SendCommand(MOOG_PARK, PlatformRcvUDP, hostname, dest_port); } break; case (byte)MachineStates.IDLE: //Send updated state if (LocalRequestedState == MOOG_ENGAGE) { //Send Engage Command SendCommand(MOOG_ENGAGE, PlatformRcvUDP, hostname, dest_port); }//if else //Keep connection alive { SendCommand(MOOG_MDAMODE, PlatformRcvUDP, hostname, dest_port); } break; case (byte)MachineStates.STANDBY: if (LocalRequestedState == MOOG_PARK) { //Send Park command SendCommand(MOOG_PARK, PlatformRcvUDP, hostname, dest_port); } else if (LocalRequestedState == MOOG_ENGAGE) { //Send Engage Command SendState(PlatformRcvUDP, hostname, dest_port); }//if break; case (byte)MachineStates.FAULT2: case (byte)MachineStates.INHIBITED: //Need to tell user to issue Reset Command if (LocalRequestedState == MOOG_RESET) { //Send Reset command SendCommand(MOOG_RESET, PlatformRcvUDP, hostname, dest_port); } break; case (byte)MachineStates.DISABLED: //Warn user a hard reset is required break; } //Update displayed state if (PlatformState != PlatformLastState) { StatusText.BeginInvoke(new InvokeDelegateState(UpdatePlatformState), PlatformState); } } catch (System.Exception ex) { errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), ex.Message); break; } } //if else { //Attempt to re-engage communication SendCommand(MOOG_MDAMODE, PlatformRcvUDP, hostname, dest_port); } }//while }