private bool IsAccZero(AngleData angleData)
{
if (angleData.ax == 0 && angleData.ay == 0 && angleData.az == 0)
{
return(true);
}
return(false);
}
public void AddAngleData(AngleData angelData)
{
if (lastAngleData != null)
{
if (IsAccZero(lastAngleData.Value) && !IsAccZero(angelData))
{
ActivePoint(FlyProtocol.GetPoint(PROGRAM_CONTROL_STATUS.STATUS_LEVEL1_SHUTDOWN).Value, true);
}
}
lastAngleData = angelData;
}
/// <summary>
/// 点の回転
/// </summary>
/// <param name="point">対象座標</param>
/// <param name="axis">回転軸</param>
/// <param name="angle">角度</param>
/// <returns></returns>
public static PointD PointRotation(PointD point, PointD axis, AngleData angle)
{
PointD rotate = new PointD();
double ad = axis.X - point.X;
double bd = axis.Y - point.Y;
rotate.X = ((Math.Cos(angle.Radian) * ad + -1 * Math.Sin(angle.Radian) * bd) + axis.X);
rotate.Y = ((Math.Sin(angle.Radian) * ad + Math.Cos(angle.Radian) * bd) + axis.Y);
return(rotate);
}
public SerialCommunication()
{
stream = null;
angleHelpArray = new AngleData[6];
angleHelpArray[0] = new AngleData(15, 2, 56.83f);
angleHelpArray[1] = new AngleData(10, 2, 38.1f);
angleHelpArray[2] = new AngleData(8, 2, 31.92f);
angleHelpArray[3] = new AngleData(6, 2, 22.14f);
angleHelpArray[4] = new AngleData(5, 2, 16.7f);
angleHelpArray[5] = new AngleData(3, 2, 11.5f);
//angleHelpArray[6] = new AngleData(3,1,5.7f);
posHelpArray = new PosData[6];
posHelpArray[0] = new PosData(25, 3, 0.206f);
posHelpArray[1] = new PosData(20, 3, 0.177f);
posHelpArray[2] = new PosData(15, 3, 0.138f);
posHelpArray[3] = new PosData(10, 3, 0.0975f);
posHelpArray[4] = new PosData(6, 3, 0.064f);
posHelpArray[5] = new PosData(3, 3, 0.039f);
}
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;
}
}
}
