//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();
}
