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