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