Ejemplo n.º 1
0
 public void AddProgramData(ProgramControlData programData)
 {
     switch (programData.controlStatus)
     {
     case 1:
     case 2:
     case 12:
         ActivePoint(FlyProtocol.GetProgramControlStatusDescription(programData.controlStatus), true);
         break;
     }
 }
Ejemplo n.º 2
0
        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;
                }
            }
        }