private void UpdateDataOscillating(SonarInput data)
{
data = new SonarInput(data.Distance, data.Angle - 90f);
int direction = MathTools.Sign(data.Angle - m_LastAngle);
lock (m_Data)
{
if (direction != m_LastDirection)
{
m_Data.Clear();
m_LastDirection = direction;
}
m_Data.Add(data);
}
m_LastAngle = data.Angle;
}
private void UpdateDataRevolving(SonarInput data)
{
if (m_InitialCall)
{
m_Origin = data;
m_InitialCall = false;
}
data = new SonarInput(data.Distance, data.Angle - 90f);
Vector3 crossProduct = MathTools.CrossProduct(data.Direction, m_Origin.Direction);
lock (m_Data)
{
if ((m_LastCrossProduct.z < 0f && crossProduct.z >= 0f) || (m_LastCrossProduct.z > 0f && crossProduct.z <= 0f))
{
m_Data.Clear();
}
m_Data.Add(data);
}
m_LastCrossProduct = crossProduct;
}
/*private static void OnDataReceived(object sender, SerialDataReceivedEventArgs e)
* {
* }*/
private static void Read()
{
while (s_IsActive)
{
try
{
do
{
//DebugStringBuilder debug = new DebugStringBuilder("RECV: ");//
int packetSize = Marshal.SizeOf(typeof(SonarData));
int packetCount = s_SerialPort.BytesToRead / packetSize;
//debug.Append("TotalSize={0}, PacketCount={1}", s_SerialPort.BytesToRead, packetCount);//
if (packetCount <= 0)
{
break;
}
int packetChunkSize = packetCount * packetSize;
byte[] packetBuffer = new byte[packetChunkSize];
//debug.Append(", PacketsSize={0}", packetChunkSize);//
int readSize = s_SerialPort.Read(packetBuffer, 0, packetChunkSize);
//debug.AppendLine(", ReadSize={0}", readSize);//
if (readSize % packetSize != 0)
{
break;
}
IEnumerable <IEnumerable <byte> > bytes = packetBuffer.Split(packetSize);
unsafe
{
foreach (var packet in bytes)
{
fixed(byte *pointer = packet.ToArray())
{
//int checksumSize = Marshal.SizeOf(((SonarData*)pointer)->Header.Checksum);//
//debug.Append("PacketCRC={0}, LocalCRC={1}", ((SonarData*)pointer)->Header.Checksum, CRC16.Generate((byte*)(pointer + checksumSize), packetSize - checksumSize));//
if (Packet.Verify((Packet.Header *)pointer, packetSize))
{
Data = (SonarData *)pointer;
OnUpdate?.Invoke(Data);
//debug.AppendLine(", VALID");//
//debug.AppendLine(", {0}", Data);//
}
/*else
* {
* //debug.AppendLine(", FAIL");//
* //debug.LogMode = DebugLogMode.Warning;//
* }*/
}
}
}
//debug.Print();//
} while(false);
} // try
catch (TimeoutException) { }
Thread.Sleep(s_Settings.LoopDelay);
}
}
