//Returns array of bytes from MDACommand structure to send to MOOG byte[] getBytes(MDACommand str) { int size = Marshal.SizeOf(str); byte[] arr = new byte[size]; IntPtr ptr = Marshal.AllocHGlobal(size); Marshal.StructureToPtr(str, ptr, true); Marshal.Copy(ptr, arr, 0, size); Marshal.FreeHGlobal(ptr); return arr; }
/*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 }
byte[] flipStrBytes(MDACommand str) { byte[] arr = getBytes(str); byte[] new_arr = new byte[arr.Length]; for (int j = 0; j < arr.Length; j = j + 4) { for (int i = 0; i < 4; i++) { new_arr[i + j] = arr[3 - i + j]; } //for } //for return new_arr; }
//Returns MDACommand structure from array of bytes MDACommand fromBytes(byte[] arr) { MDACommand str = new MDACommand(); int size = Marshal.SizeOf(str); IntPtr ptr = Marshal.AllocHGlobal(size); Marshal.Copy(arr, 0, ptr, size); str = (MDACommand)Marshal.PtrToStructure(ptr, str.GetType()); Marshal.FreeHGlobal(ptr); return str; }
private void UpdateCreatedData() { byte LocalRequestedState = 0; MDACommand localMDA = new MDACommand(); InputUpdate = true; t_prescribedDataStopwatch.Start(); localMDA = simulateData.UpdateData(0); byte[] recvBytes = getBytes(localMDA); InputDataMutex.WaitOne(); InputBytes = recvBytes; InputDataMutex.ReleaseMutex(); UpdateMutex.WaitOne(); LocalRequestedState = RequestedState; UpdateMutex.ReleaseMutex(); while (InputUpdate) { localMDA = simulateData.UpdateData(1.0f * t_prescribedDataStopwatch.ElapsedMilliseconds / 1000); recvBytes = getBytes(localMDA); if (LocalRequestedState == MOOG_NEWMDA && MoogElapsedSendState.ElapsedMilliseconds >= MoogSendTimeOutMS) { //Send updated state SendState(MOOGPlatformRcvUDP, MOOGhostname, MOOGdestport, recvBytes); MoogSendStateCount++; MoogElapsedSendState.Reset(); MoogElapsedSendState.Start(); } InputDataMutex.WaitOne(); InputBytes = recvBytes; InputDataMutex.ReleaseMutex(); UpdateMutex.WaitOne(); LocalRequestedState = RequestedState; UpdateMutex.ReleaseMutex(); } t_prescribedDataStopwatch.Reset(); }
public ControlGUI() { if (!InputBox.ShowDialog("Enter Password to Access MOOG Control", "Password Prompt")) { MessageBox.Show("Incorrect Password: Closing Application", "Incorrect Password"); Environment.Exit(0); return; } InitializeComponent(); //C# automatic form Stuff CurrentMDACommand = new MDACommand(); InputBytes = getBytes(CurrentMDACommand); InitLabelArrays(); InitializeThread(); this.splitContainer1.SplitterDistance = crossbowCheck.Checked ? 0 : 180; cmb_Connection.Items.AddRange(SerialPort.GetPortNames()); try { _imuConnection = new XSens(ImuMutex); } catch (SystemException ex) { MessageBox.Show(ex.Message); ImuConnectButton.Enabled = false; } }
//Sends requested MDA File private void SendMDAFileChange(System.Net.Sockets.UdpClient PlatformSendUDP, string hostname, int port) { MDACommand platformCommand = new MDACommand(); platformCommand.MCW = BitConverter.ToUInt32(new byte[4] {MOOG_NEWMDAFILE, RequestedMDAFile, 0, 0 }, 0); byte[] sendBytes = flipStrBytes(platformCommand); try { PlatformSendUDP.Send(sendBytes, sendBytes.Length, hostname, port); //PlatformSendUDP.Send(sendBytes, sendBytes.Length); } catch (System.Exception ex) { //Cause failure of some sorts. //errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), ex.Message); MessageBox.Show(ex.Message, "Send Command Error"); } }
private void UpdateCommandDataDisplay(MDACommand LocalMDACommand) { AxText.BeginInvoke(new InvokeDelegateFloat(UpdateAx), LocalMDACommand.a_x); AyText.BeginInvoke(new InvokeDelegateFloat(UpdateAy), LocalMDACommand.a_y); AzText.BeginInvoke(new InvokeDelegateFloat(UpdateAz), LocalMDACommand.a_z); ArollText.BeginInvoke(new InvokeDelegateFloat(UpdateAroll), LocalMDACommand.a_roll); ApitchText.BeginInvoke(new InvokeDelegateFloat(UpdateApitch), LocalMDACommand.a_pitch); AyawText.BeginInvoke(new InvokeDelegateFloat(UpdateAyaw), LocalMDACommand.a_yaw); VrollText.BeginInvoke(new InvokeDelegateFloat(UpdateVroll), LocalMDACommand.v_roll); VpitchText.BeginInvoke(new InvokeDelegateFloat(UpdateVpitch), LocalMDACommand.v_pitch); VyawText.BeginInvoke(new InvokeDelegateFloat(UpdateVyaw), LocalMDACommand.v_yaw); RollText.BeginInvoke(new InvokeDelegateFloat(UpdateRoll), LocalMDACommand.roll); PitchText.BeginInvoke(new InvokeDelegateFloat(UpdatePitch), LocalMDACommand.pitch); YawText.BeginInvoke(new InvokeDelegateFloat(UpdateYaw), LocalMDACommand.yaw); TimeOffsetText.BeginInvoke(new InvokeDelegateTextString(UpdateTextBoxValue), TimeOffsetText, recvDeltaTime.ToString("0.000")); }
//Sends desired command private void SendCommand(byte command, System.Net.Sockets.UdpClient PlatformSendUDP, string hostname, int port) { MDACommand platformCommand = new MDACommand(); platformCommand.MCW = (uint) command; byte[] sendBytes = flipStrBytes(platformCommand); try { PlatformSendUDP.Send(sendBytes, sendBytes.Length, hostname, port); //PlatformSendUDP.Send(sendBytes, sendBytes.Length); } catch (System.Exception ex) { //Cause failure of some sorts. //errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), ex.Message); MessageBox.Show(ex.Message,"Send Command Error"); } }
private void UpdateCommandDataDisplay(MDACommand LocalMDACommand) { AxText.BeginInvoke(new InvokeDelegateFloat(UpdateAx), LocalMDACommand.a_x); AyText.BeginInvoke(new InvokeDelegateFloat(UpdateAy), LocalMDACommand.a_y); AzText.BeginInvoke(new InvokeDelegateFloat(UpdateAz), LocalMDACommand.a_z); ArollText.BeginInvoke(new InvokeDelegateFloat(UpdateAroll), LocalMDACommand.a_roll); ApitchText.BeginInvoke(new InvokeDelegateFloat(UpdateApitch), LocalMDACommand.a_pitch); AyawText.BeginInvoke(new InvokeDelegateFloat(UpdateAyaw), LocalMDACommand.a_yaw); VrollText.BeginInvoke(new InvokeDelegateFloat(UpdateVroll), LocalMDACommand.v_roll); VpitchText.BeginInvoke(new InvokeDelegateFloat(UpdateVpitch), LocalMDACommand.v_pitch); VyawText.BeginInvoke(new InvokeDelegateFloat(UpdateVyaw), LocalMDACommand.v_yaw); RollText.BeginInvoke(new InvokeDelegateFloat(UpdateRoll), LocalMDACommand.roll); PitchText.BeginInvoke(new InvokeDelegateFloat(UpdatePitch), LocalMDACommand.pitch); YawText.BeginInvoke(new InvokeDelegateFloat(UpdateYaw), LocalMDACommand.yaw); }
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 }
public ControlGUI() { InitializeComponent(); //C# automatic form Stuff CurrentMDACommand = new MDACommand(); InitLabelArrays(); InitializeThread(); }
/*Function running on InputUDPThread that retrieves *UDP datagrams being sent by UDP meant for the platform */ private void UpdateUDPInputData() { IPEndPoint RemoteIpEndPoint = new IPEndPoint(IPAddress.Any, 0); System.Net.Sockets.UdpClient InputUDPClient = new System.Net.Sockets.UdpClient(5345); while (InputUpdate) { if (InputUDPClient.Available > 0) { try { byte[] recvBytes = InputUDPClient.Receive(ref RemoteIpEndPoint); InputDataMutex.WaitOne(); CurrentMDACommand = fromBytes(recvBytes); InputDataMutex.ReleaseMutex(); //UpdateCommandDataDisplay(CurrentMDACommand); } catch (System.Exception ex) { errorBar.BeginInvoke(new InvokeDelegateString(ErrorBarMessage), ex.Message); break; } } //if } //while InputUDPClient.Close(); }