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