/*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
}
