public void AddProgramData(ProgramControlData programData) { switch (programData.controlStatus) { case 1: case 2: case 12: ActivePoint(FlyProtocol.GetProgramControlStatusDescription(programData.controlStatus), true); break; } }
public void ParseData2(byte[] buffer, out List <NavData> navDataList, out List <AngleData> angleDataList, out List <ProgramControlData> programControlDataList, out List <ServoData> servoDataList) { navDataList = new List <NavData>(); angleDataList = new List <AngleData>(); programControlDataList = new List <ProgramControlData>(); servoDataList = new List <ServoData>(); if (buffer == null || buffer.Length <= Marshal.SizeOf(typeof(FlyHeader))) { return; } FlyPacket flyPacket = Tool.ByteToStruct <FlyPacket>(buffer, 0, Marshal.SizeOf(typeof(FlyPacket))); if (!Enumerable.SequenceEqual(flyPacket.header.syncHeader, FlyProtocol.syncHeader) || flyPacket.header.dataType != FlyProtocol.dataType) { return; } if (dataLength2 + flyPacket.header.dataLen.SwapUInt16() > dataBuffer2.Length) { dataLength2 = 0; return; } if (flyPacket.data.Length < flyPacket.header.dataLen.SwapUInt16()) { return; } Array.Copy(flyPacket.data, 0, dataBuffer2, dataLength2, flyPacket.header.dataLen.SwapUInt16()); dataLength2 += flyPacket.header.dataLen.SwapUInt16(); for (; searchPos2 < dataLength2; ++searchPos2) { if (EqualHeader(FlyProtocol.navHeader, dataBuffer2, searchPos2)) { if (searchPos2 + FlyProtocol.NavDataLengthWithPadding >= dataLength2) { break; } byte[] packetBuffer = GetPacketDataWithoutPadding(dataBuffer2, searchPos2, FlyProtocol.NavDataLengthWithPadding); NavData navData = Tool.ByteToStruct <NavData>(packetBuffer, 0, packetBuffer.Length); if (navData.crc != CalcNavCrc(packetBuffer) || navData.endChar != 0x21) { searchPos2++; continue; } navDataList.Add(navData); searchPos2 += FlyProtocol.NavDataLengthWithPadding; Array.Copy(dataBuffer2, searchPos2, dataBuffer2, 0, dataLength2 - searchPos2); dataLength2 -= searchPos2; searchPos2 = 0; } if (EqualHeader(FlyProtocol.angleHeader, dataBuffer2, searchPos2)) { if (searchPos2 + FlyProtocol.AngleDataLengthWithPadding >= dataLength2) { break; } byte[] packetBuffer = GetPacketDataWithoutPadding(dataBuffer2, searchPos2, FlyProtocol.AngleDataLengthWithPadding); AngleData angleData = Tool.ByteToStruct <AngleData>(packetBuffer, 0, packetBuffer.Length); if (angleData.crc != CalcAngelCrc(packetBuffer)) { searchPos2++; continue; } angleDataList.Add(angleData); searchPos2 += FlyProtocol.AngleDataLengthWithPadding; Array.Copy(dataBuffer2, searchPos2, dataBuffer2, 0, dataLength2 - searchPos2); dataLength2 -= searchPos2; searchPos2 = 0; } if (EqualHeader(FlyProtocol.programHeader, dataBuffer2, searchPos2)) { if (searchPos2 + FlyProtocol.ProgramDataLengthWithPadding >= dataLength2) { break; } byte[] packetBuffer = GetPacketDataWithoutPadding(dataBuffer2, searchPos2, FlyProtocol.ProgramDataLengthWithPadding); ProgramControlData programControlData = Tool.ByteToStruct <ProgramControlData>(packetBuffer, 0, packetBuffer.Length); if (programControlData.crc != CalcProgramData(packetBuffer) || programControlData.endChar != 0x21) { searchPos2++; continue; } programControlDataList.Add(programControlData); searchPos2 += FlyProtocol.ProgramDataLengthWithPadding; Array.Copy(dataBuffer2, searchPos2, dataBuffer2, 0, dataLength2 - searchPos2); dataLength2 -= searchPos2; searchPos2 = 0; } if (EqualHeader(FlyProtocol.servoHeader, dataBuffer2, searchPos2)) { if (searchPos2 + FlyProtocol.ServoDataLengthWithPadding >= dataLength2) { break; } byte[] packetBuffer = GetPacketDataWithoutPadding(dataBuffer2, searchPos2, FlyProtocol.ServoDataLengthWithPadding); ServoData servoData = Tool.ByteToStruct <ServoData>(packetBuffer, 0, packetBuffer.Length); if (servoData.crc != CalcServoData(packetBuffer) || servoData.endChar != 0x21) { searchPos2++; continue; } servoDataList.Add(servoData); searchPos2 += FlyProtocol.ServoDataLengthWithPadding; Array.Copy(dataBuffer2, searchPos2, dataBuffer2, 0, dataLength2 - searchPos2); dataLength2 -= searchPos2; searchPos2 = 0; } } }