///////////////////////////////
// DATA PARSER
///////////////////////////////
public unsafe void Update()
{
///////////////////////////////
// Process Serial Data
///////////////////////////////
commMgr.Update(10);
///////////////////////////////
// Process Ethernet Data
///////////////////////////////
if (udp == null)
{
return;
}
// Get data from UDP
while (udp.Available > 0)
{
// get data
IPEndPoint endP = new IPEndPoint(IPAddress.Any, 0); // any port of the sender
byte[] bytes = udp.Receive(ref endP);
// process data
ReceivedPacketsCounter++;
if (PacketIsValid(bytes))
{
// remove header
byte[] withoutHeader = bytes.Skip(3).ToArray();
switch (bytes[2])
{
case 0x20:
// data
SCommEthData commData = (SCommEthData)Comm.FromBytes(withoutHeader, new SCommEthData());
MainSystemData = commData;
// filter data
formMain.FilteredRoll = 0.9998 * formMain.FilteredRoll + 0.0002 * MainSystemData.dRoll;
formMain.FilteredPitch = 0.9998 * formMain.FilteredPitch + 0.0002 * MainSystemData.dPitch;
formMain.FilteredYaw = 0.9998 * formMain.FilteredYaw + 0.0002 * MainSystemData.dYaw;
for (int i = 0; i != 10; i++)
{
formMain.FilteredTuningData[i] = 0.999 * formMain.FilteredTuningData[i] + 0.001 * Math.Abs(commData.TuningData[i] - formMain.FilteredTuningLastData[i]);
formMain.FilteredTuningLastData[i] = commData.TuningData[i];
}
logStream.Write(withoutHeader, 0, withoutHeader.Length);
break;
case 0x62: // get params data
SParameters p = (SParameters)Comm.FromBytes(withoutHeader, new SParameters());
// Display
formMain.DisplayParams(p);
break;
}
}
}
// send PING
if (++PingCounter > 20)
{
PingCounter = 0;
byte[] lp = BitConverter.GetBytes(LocalPort); // send port
SendData(0x10, lp);
}
// GUI update
formMain.DisplaySystemData(ReceivedPacketsCounter, MainSystemData);
// Update assist now
byte[] toSend = assistNow.Update(MainSystemData.AssistNextChunkToSend);
if (toSend != null)
{
SendData(0x30, toSend);
}
}
public void WriteParams(SParameters p)
{
// Send
byte[] toSend = Comm.GetBytes(p);
formMain.SendData(0x60, toSend);
}
