private PartData ParsePartData(Transform trans, PartUnit partUnit, string customValue) { PartData partData; switch (partUnit.type) { case PartType.Bone: { var data = new BoneData(); for (int i = 0; i < trans.childCount; i++) { var t = trans.GetChild(i); if (!t.name.StartsWith(ModelFBXParser.GroupMark)) { continue; } var group = t; for (int j = 0; j < @group.childCount; j++) { var child = @group.GetChild(j); data.num = customValue; var childName = ModelFBXParser.ToLegalName(child.name); data.items.Add(childName); } break; } partData = data; break; } case PartType.MC: { var data = new MCData(); data.type = 1; partData = data; break; } case PartType.Wire: { var data = new WireData(); data.num = customValue; partData = data; break; } case PartType.Servo: { var data = new ServoData(); data.id = int.Parse(customValue); data.type = 1; data.angle = 120; partData = data; break; } case PartType.Motor: { var data = new MotorData(); data.id = int.Parse(customValue); partData = data; break; } case PartType.Sensor: { var data = new SensorData(); data.id = int.Parse(customValue); partData = data; break; } default: { partData = new PartData(); break; } } partData.name = trans.name; partData.source = partUnit.source; partData.transform = new DTransform(); partData.transform.position = trans.localPosition; partData.transform.angle = trans.localEulerAngles; partData.transform.scale = GetScale(trans, partData.partType); partData.parent = GetParentName(trans); return(partData); }
public void AddServoData(ServoData servoData) { }
/// <summary> /// constructor /// </summary> public ServoClient(string id, IotHttpClient client, IotNode parent) : base(id, client, parent) { ServoData = new ServoData(); }
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; } } }