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