我首先将OUT-OF-BOX demo里面的.BIN文件烧入板子里面,在mmwave Demo visual 里面配置参数,就实现可视化演示了,现在的问题 在前面的工作的基础上,我要在unity上实现雷达实时采集数据并进可视化演示。我自己已经写一个脚本:using UnityEngine;
using System.IO.Ports;
using System;
using System.Threading;
using System.Collections.Generic;
using System.Text;
using System.Linq;
public class AWR6843ISK_ODS_RadarController : MonoBehaviour
{
[Header("串口配置 - AWR6843ISK-ODS")]
public string configPort = "COM4"; // 配置口
public string dataPort = "COM5"; // 数据口
public int configBaudRate = 115200;
public int dataBaudRate = 921600; // ODS数据量较大,使用更高波特率
[Header("ODS优化配置")]
[TextArea(20, 35)]
public string configCommands = @"sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 5 0 // ODS: 3Tx4Rx模式
adcCfg 2 1
adcbufCfg -1 0 1 1 1
// 优化检测参数 - 提高灵敏度
profileCfg 0 60 20 7 60 0 0 70 1 256 5209 0 0 30
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 4
frameCfg 0 2 32 0 50 1 0 // 调整帧率
lowPower 0 0
guiMonitor -1 1 1 0 0 0 1
// ODS广角FOV
aoaFovCfg -1 -60 60 -15 15
// 降低CFAR阈值提高检测灵敏度
cfarCfg -1 0 2 4 2 3 0 8 0.2
cfarCfg -1 1 0 3 2 3 1 8 0.2
multiObjBeamForming -1 1 0.3
clutterRemoval -1 0 // 关闭杂波消除以提高检测率
calibDcRangeSig -1 0 -5 8 256
compRangeBiasAndRxChanPhase 0.0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
measureRangeBiasAndRxChanPhase 0 1.5 0.2
CQRxSatMonitor 0 3 5 121 0
CQSigImgMonitor 0 127 4
analogMonitor 0 0
cfarFovCfg -1 0 0 3.0 // 缩小检测范围提高灵敏度
cfarFovCfg -1 1 -1 1.5
calibData 0 0 0
sensorStart";
[Header("3D点云可视化设置")]
public bool enable3DVisualization = true;
public Material pointMaterial;
public Color closePointColor = Color.green;
public Color farPointColor = Color.red;
public float pointSize = 0.1f;
public float maxVisualizationRange = 20f;
public bool showPointCloud = true;
public bool showTargetCubes = true;
public bool showRadarFOV = true;
[Header("数据解析设置")]
public bool enableRawDataDebug = true;
public bool enableTLVParsing = true;
public int debugDataLength = 32;
[Header("ODS监控设置")]
public bool enableDataMonitoring = true;
public bool enableObjectTracking = true;
public float monitorUpdateInterval = 0.3f;
[Header("ODS性能指标")]
public int objectsDetected = 0;
public float avgDetectionRange = 0f;
public float maxDetectionRange = 0f;
public int pointCloudCount = 0;
// 3D可视化相关
private List<GameObject> pointCloudObjects = new List<GameObject>();
private List<GameObject> targetCubes = new List<GameObject>();
private GameObject pointCloudContainer;
private GameObject targetContainer;
private LineRenderer fovRenderer;
private Mesh pointMesh;
private SerialPort configSerialPort;
private SerialPort dataSerialPort;
private bool isConfigured = false;
private bool isMonitoring = false;
private string lastResponse = "";
private string monitorData = "";
private float lastMonitorUpdateTime = 0f;
// ODS数据统计
private int totalBytesReceived = 0;
private int framesReceived = 0;
private int dataErrors = 0;
private float dataRate = 0f;
private Queue<float> dataRateHistory = new Queue<float>();
private const int HISTORY_SIZE = 60;
// 数据解析
private byte[] dataBuffer = new byte[16384];
private int bufferPosition = 0;
private DateTime lastPacketTime;
private readonly object dataLock = new object();
// 目标跟踪
private List<RadarObject> detectedObjects = new List<RadarObject>();
private List<PointCloudPoint> currentPointCloud = new List<PointCloudPoint>();
[System.Serializable]
public class RadarObject
{
public float x;
public float y;
public float z;
public float velocity;
public float snr;
public float range;
public float azimuth;
public float elevation;
public DateTime detectionTime;
public Vector3 position => new Vector3(x, y, z);
}
[System.Serializable]
public class PointCloudPoint
{
public float x;
public float y;
public float z;
public float velocity;
public float snr;
public Color color;
public Vector3 position => new Vector3(x, y, z);
}
// 数据包格式检测
private bool useAlternativePacketFormat = false;
private int packetsProcessed = 0;
void Start()
{
for (int i = 0; i < HISTORY_SIZE; i++)
{
dataRateHistory.Enqueue(0f);
}
lastPacketTime = DateTime.Now;
detectedObjects = new List<RadarObject>();
currentPointCloud = new List<PointCloudPoint>();
Initialize3DVisualization();
}
void Initialize3DVisualization()
{
pointCloudContainer = new GameObject("PointCloudContainer");
pointCloudContainer.transform.SetParent(this.transform);
targetContainer = new GameObject("TargetContainer");
targetContainer.transform.SetParent(this.transform);
CreateFOVVisualization();
CreatePointMesh();
for (int i = 0; i < 2000; i++)
{
GameObject point = CreatePointObject();
pointCloudObjects.Add(point);
point.SetActive(false);
}
for (int i = 0; i < 100; i++)
{
GameObject cube = CreateTargetCube();
targetCubes.Add(cube);
cube.SetActive(false);
}
}
void CreatePointMesh()
{
pointMesh = new Mesh();
Vector3[] vertices = new Vector3[4] {
new Vector3(-0.5f, -0.5f, 0),
new Vector3(0.5f, -0.5f, 0),
new Vector3(-0.5f, 0.5f, 0),
new Vector3(0.5f, 0.5f, 0)
};
int[] triangles = new int[6] { 0, 2, 1, 2, 3, 1 };
pointMesh.vertices = vertices;
pointMesh.triangles = triangles;
}
GameObject CreatePointObject()
{
GameObject point = new GameObject("Point");
point.transform.SetParent(pointCloudContainer.transform);
MeshFilter meshFilter = point.AddComponent<MeshFilter>();
meshFilter.mesh = pointMesh;
MeshRenderer renderer = point.AddComponent<MeshRenderer>();
renderer.material = pointMaterial != null ? pointMaterial : CreateDefaultPointMaterial();
return point;
}
GameObject CreateTargetCube()
{
GameObject cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
cube.transform.SetParent(targetContainer.transform);
cube.transform.localScale = Vector3.one * 0.3f;
Renderer renderer = cube.GetComponent<Renderer>();
renderer.material.color = Color.yellow;
return cube;
}
void CreateFOVVisualization()
{
GameObject fovObj = new GameObject("FOVVisualization");
fovObj.transform.SetParent(this.transform);
fovRenderer = fovObj.AddComponent<LineRenderer>();
fovRenderer.material = new Material(Shader.Find("Sprites/Default"));
fovRenderer.startColor = Color.blue;
fovRenderer.endColor = Color.cyan;
fovRenderer.startWidth = 0.02f;
fovRenderer.endWidth = 0.02f;
fovRenderer.useWorldSpace = false;
UpdateFOVVisualization();
}
void UpdateFOVVisualization()
{
if (!showRadarFOV || fovRenderer == null) return;
int segments = 16;
float maxRange = maxVisualizationRange;
List<Vector3> points = new List<Vector3>();
for (int i = 0; i <= segments; i++)
{
float angle = i * 360f / segments;
float rad = angle * Mathf.Deg2Rad;
float x = Mathf.Sin(rad) * maxRange;
float z = Mathf.Cos(rad) * maxRange;
points.Add(new Vector3(x, 0, z));
}
fovRenderer.positionCount = points.Count;
fovRenderer.SetPositions(points.ToArray());
}
Material CreateDefaultPointMaterial()
{
Material mat = new Material(Shader.Find("Standard"));
mat.color = Color.white;
mat.enableInstancing = true;
return mat;
}
void Update()
{
if (Time.time - lastMonitorUpdateTime > monitorUpdateInterval && isMonitoring)
{
UpdateODSMonitorDisplay();
lastMonitorUpdateTime = Time.time;
}
if (enable3DVisualization)
{
UpdatePointCloudVisualization();
UpdateTargetVisualization();
}
}
void UpdatePointCloudVisualization()
{
if (!showPointCloud) return;
lock (dataLock)
{
foreach (var point in pointCloudObjects)
{
point.SetActive(false);
}
for (int i = 0; i < Math.Min(currentPointCloud.Count, pointCloudObjects.Count); i++)
{
var pointData = currentPointCloud[i];
var pointObj = pointCloudObjects[i];
pointObj.transform.localPosition = pointData.position;
pointObj.transform.localScale = Vector3.one * pointSize;
Renderer renderer = pointObj.GetComponent<Renderer>();
float distance = pointData.position.magnitude;
float colorFactor = Mathf.Clamp01(distance / maxVisualizationRange);
pointData.color = Color.Lerp(closePointColor, farPointColor, colorFactor);
renderer.material.color = pointData.color;
pointObj.SetActive(true);
}
}
}
void UpdateTargetVisualization()
{
if (!showTargetCubes) return;
lock (dataLock)
{
foreach (var cube in targetCubes)
{
cube.SetActive(false);
}
for (int i = 0; i < Math.Min(detectedObjects.Count, targetCubes.Count); i++)
{
var target = detectedObjects[i];
var cube = targetCubes[i];
cube.transform.localPosition = target.position;
Renderer renderer = cube.GetComponent<Renderer>();
float speedFactor = Mathf.Clamp01(Mathf.Abs(target.velocity) / 10f);
renderer.material.color = Color.Lerp(Color.green, Color.red, speedFactor);
cube.SetActive(true);
}
}
}
void OnGUI()
{
GUIStyle bigButton = new GUIStyle(GUI.skin.button);
bigButton.fontSize = 14;
bigButton.padding = new RectOffset(8, 8, 8, 8);
GUIStyle titleStyle = new GUIStyle(GUI.skin.box);
titleStyle.alignment = TextAnchor.UpperLeft;
titleStyle.fontSize = 14;
titleStyle.fontStyle = FontStyle.Bold;
titleStyle.normal.textColor = Color.cyan;
GUIStyle textStyle = new GUIStyle(GUI.skin.box);
textStyle.alignment = TextAnchor.UpperLeft;
textStyle.fontSize = 11;
textStyle.normal.textColor = Color.white;
GUIStyle monitorStyle = new GUIStyle(GUI.skin.box);
monitorStyle.alignment = TextAnchor.UpperLeft;
monitorStyle.fontSize = 10;
monitorStyle.normal.textColor = Color.green;
// 左侧控制面板
GUI.BeginGroup(new Rect(10, 10, 300, Screen.height - 20));
GUI.Box(new Rect(0, 0, 300, 40), "AWR6843ISK-ODS 3D雷达控制器", titleStyle);
string portInfo = $"配置口: {configPort} ({configBaudRate} baud)\n";
portInfo += $"数据口: {dataPort} ({dataBaudRate} baud)\n";
portInfo += $"状态: {(isConfigured ? "已配置" : "未配置")}\n";
portInfo += $"监控: {(isMonitoring ? "运行中" : "停止")}";
GUI.Box(new Rect(0, 50, 300, 80), portInfo, textStyle);
if (GUI.Button(new Rect(0, 140, 145, 40), "发送ODS配置", bigButton))
{
SendODSConfiguration();
}
if (GUI.Button(new Rect(155, 140, 145, 40), "停止传感器", bigButton))
{
SendCommand("sensorStop");
}
if (GUI.Button(new Rect(0, 190, 145, 40), "开始监控", bigButton))
{
StartODSDataMonitoring();
}
if (GUI.Button(new Rect(155, 190, 145, 40), "停止监控", bigButton))
{
StopDataMonitoring();
}
// 调试控制
GUI.Box(new Rect(0, 240, 300, 30), "数据解析设置", titleStyle);
enableRawDataDebug = GUI.Toggle(new Rect(10, 280, 140, 20), enableRawDataDebug, "原始数据调试");
enableTLVParsing = GUI.Toggle(new Rect(160, 280, 140, 20), enableTLVParsing, "TLV解析");
// 3D可视化控制
GUI.Box(new Rect(0, 310, 300, 30), "3D可视化控制", titleStyle);
showPointCloud = GUI.Toggle(new Rect(10, 350, 140, 20), showPointCloud, "显示点云");
showTargetCubes = GUI.Toggle(new Rect(160, 350, 140, 20), showTargetCubes, "显示目标");
showRadarFOV = GUI.Toggle(new Rect(10, 380, 140, 20), showRadarFOV, "显示FOV");
// 参数调节
GUI.Label(new Rect(10, 410, 100, 20), "点大小:");
pointSize = GUI.HorizontalSlider(new Rect(80, 415, 100, 20), pointSize, 0.01f, 0.5f);
pointSize = float.Parse(GUI.TextField(new Rect(190, 410, 40, 20), pointSize.ToString("F2")));
GUI.Label(new Rect(10, 440, 100, 20), "最大距离:");
maxVisualizationRange = GUI.HorizontalSlider(new Rect(80, 445, 100, 20), maxVisualizationRange, 5f, 50f);
maxVisualizationRange = float.Parse(GUI.TextField(new Rect(190, 440, 40, 20), maxVisualizationRange.ToString("F0")));
// 数据包格式控制
GUI.Box(new Rect(0, 480, 300, 30), "数据包格式设置", titleStyle);
useAlternativePacketFormat = GUI.Toggle(new Rect(10, 520, 280, 20), useAlternativePacketFormat, "使用替代数据包格式 (09 80 12 02)");
// 性能统计
string stats = $"ODS性能统计:\n";
stats += $"检测目标: {objectsDetected} 个\n";
stats += $"点云数量: {pointCloudCount} 点\n";
stats += $"平均距离: {avgDetectionRange:F2} m\n";
stats += $"最远距离: {maxDetectionRange:F2} m\n";
stats += $"数据速率: {dataRate:F1} KB/s\n";
stats += $"总帧数: {framesReceived}\n";
stats += $"处理包数: {packetsProcessed}";
GUI.Box(new Rect(0, 550, 300, 120), stats, textStyle);
// 响应信息
GUI.Box(new Rect(0, 680, 300, 150), "配置响应:\n" + lastResponse, textStyle);
GUI.EndGroup();
// 右侧监控信息
GUI.BeginGroup(new Rect(Screen.width - 310, 10, 300, Screen.height - 20));
GUI.Box(new Rect(0, 0, 300, 200), "ODS数据监控:\n" + monitorData, monitorStyle);
// 目标信息
string objectInfo = $"目标详细信息:\n";
lock (dataLock)
{
for (int i = 0; i < Math.Min(6, detectedObjects.Count); i++)
{
var obj = detectedObjects[i];
objectInfo += $"{i + 1}: ({obj.x:F1}, {obj.y:F1}, {obj.z:F1})m\n";
objectInfo += $" 速度: {obj.velocity:F1}m/s 信噪比: {obj.snr:F1}dB\n";
}
}
if (detectedObjects.Count == 0) objectInfo += "暂无检测目标";
GUI.Box(new Rect(0, 210, 300, 250), objectInfo, textStyle);
GUI.EndGroup();
}
void SendODSConfiguration()
{
Thread configThread = new Thread(() => {
try
{
lastResponse = "正在连接AWR6843ISK-ODS配置口...\n";
configSerialPort = new SerialPort(configPort, configBaudRate);
configSerialPort.ReadTimeout = 2000;
configSerialPort.WriteTimeout = 2000;
configSerialPort.NewLine = "\n";
configSerialPort.Open();
lastResponse += "✓ ODS配置口已打开\n";
Thread.Sleep(1000);
string[] commands = configCommands.Split(new[] { '\r', '\n' }, StringSplitOptions.RemoveEmptyEntries);
lastResponse += $"开始发送 {commands.Length} 条ODS配置命令...\n\n";
int commandCount = 0;
foreach (string cmd in commands)
{
string trimmedCmd = cmd.Trim();
if (string.IsNullOrEmpty(trimmedCmd) || trimmedCmd.StartsWith("//"))
continue;
configSerialPort.WriteLine(trimmedCmd);
lastResponse += $"[{++commandCount}] > {trimmedCmd}\n";
Thread.Sleep(100);
try
{
StringBuilder responseBuilder = new StringBuilder();
DateTime startTime = DateTime.Now;
while ((DateTime.Now - startTime).TotalMilliseconds < 1500)
{
if (configSerialPort.BytesToRead > 0)
{
string line = configSerialPort.ReadLine();
responseBuilder.AppendLine(line);
if (line.Contains("Done") || line.Contains("Success"))
{
lastResponse += $" ✓ 成功\n";
break;
}
else if (line.Contains("Error") || line.Contains("ERROR"))
{
lastResponse += $" ✗ 错误: {line}\n";
dataErrors++;
break;
}
}
Thread.Sleep(10);
}
if (responseBuilder.Length > 0)
{
lastResponse += $" 响应: {responseBuilder.ToString().Trim()}\n";
}
}
catch (TimeoutException)
{
lastResponse += " ⚠ 无响应\n";
}
}
lastResponse += $"\n✓ ODS配置完成! 共发送 {commandCount} 条命令\n";
lastResponse += "✓ 传感器已启动,等待数据流...\n";
isConfigured = true;
if (enableDataMonitoring)
{
Thread.Sleep(2000);
StartODSDataMonitoring();
}
configSerialPort.Close();
}
catch (Exception e)
{
lastResponse += $"\n✗ ODS配置失败: {e.Message}\n";
isConfigured = false;
}
});
configThread.IsBackground = true;
configThread.Start();
}
void StartODSDataMonitoring()
{
if (isMonitoring) return;
Thread monitorThread = new Thread(() => {
try
{
dataSerialPort = new SerialPort(dataPort, dataBaudRate);
dataSerialPort.ReadTimeout = 100;
dataSerialPort.WriteTimeout = 100;
dataSerialPort.Open();
isMonitoring = true;
lastResponse += $"✓ 开始监控数据口 {dataPort}\n";
lastResponse += "✓ 启用真实雷达数据解析...\n";
byte[] readBuffer = new byte[4096];
lastPacketTime = DateTime.Now;
bufferPosition = 0;
packetsProcessed = 0;
while (isMonitoring && dataSerialPort != null && dataSerialPort.IsOpen)
{
try
{
if (dataSerialPort.BytesToRead > 0)
{
int bytesToRead = Math.Min(readBuffer.Length, dataSerialPort.BytesToRead);
int bytesRead = dataSerialPort.Read(readBuffer, 0, bytesToRead);
ProcessRealRadarData(readBuffer, bytesRead);
Thread.Sleep(1);
}
else
{
Thread.Sleep(5);
}
}
catch (TimeoutException) { }
catch (Exception ex)
{
dataErrors++;
lastResponse += $"✗ 数据读取错误: {ex.Message}\n";
Thread.Sleep(100);
}
}
}
catch (Exception e)
{
lastResponse += $"✗ 监控启动失败: {e.Message}\n";
isMonitoring = false;
}
});
monitorThread.IsBackground = true;
monitorThread.Start();
}
void ProcessRealRadarData(byte[] data, int length)
{
totalBytesReceived += length;
framesReceived++;
DateTime currentTime = DateTime.Now;
double timeDiff = (currentTime - lastPacketTime).TotalSeconds;
if (timeDiff > 0)
{
dataRate = (float)(length / timeDiff / 1024.0);
lastPacketTime = currentTime;
}
if (dataRateHistory.Count >= HISTORY_SIZE)
dataRateHistory.Dequeue();
dataRateHistory.Enqueue(dataRate);
// 将新数据添加到缓冲区
lock (dataLock)
{
if (bufferPosition + length > dataBuffer.Length)
{
Array.Copy(dataBuffer, 0, dataBuffer, length, bufferPosition);
bufferPosition = 0;
}
Array.Copy(data, 0, dataBuffer, bufferPosition, length);
bufferPosition += length;
}
// 解析完整的数据包
ParseCompletePackets();
}
void ParseCompletePackets()
{
lock (dataLock)
{
int offset = 0;
bool packetFound = false;
while (offset < bufferPosition - 16) // 至少需要16字节来检测包格式
{
packetFound = false;
// 方法1: 检测标准TLV格式 (02 01 04 03)
if (!packetFound && dataBuffer[offset] == 0x02 && dataBuffer[offset + 1] == 0x01 &&
dataBuffer[offset + 2] == 0x04 && dataBuffer[offset + 3] == 0x03)
{
packetFound = TryParseStandardTLVFormat(offset);
}
// 方法2: 检测图片中的格式 (09 80 12 02) - 这是关键修改!
if (!packetFound && useAlternativePacketFormat &&
dataBuffer[offset] == 0x09 && dataBuffer[offset + 1] == 0x80 &&
dataBuffer[offset + 2] == 0x12 && dataBuffer[offset + 3] == 0x02)
{
packetFound = TryParseAlternativeFormat(offset);
}
// 方法3: 通用数据包检测
if (!packetFound)
{
packetFound = TryParseGenericFormat(offset);
}
if (packetFound)
{
packetsProcessed++;
break; // 处理一个包后跳出,等待下一帧
}
else
{
offset++;
}
}
// 移除已处理的数据
if (offset > 0 && offset < bufferPosition)
{
int remaining = bufferPosition - offset;
Array.Copy(dataBuffer, offset, dataBuffer, 0, remaining);
bufferPosition = remaining;
}
else if (offset >= bufferPosition)
{
bufferPosition = 0;
}
}
}
bool TryParseStandardTLVFormat(int offset)
{
try
{
uint packetLength = BitConverter.ToUInt32(dataBuffer, offset + 4);
if (offset + packetLength <= bufferPosition && packetLength > 0 && packetLength < 10000)
{
ProcessTLVPacket(dataBuffer, offset + 8, (int)packetLength - 8);
return true;
}
}
catch (Exception ex)
{
Debug.LogWarning($"标准TLV格式解析错误: {ex.Message}");
}
return false;
}
bool TryParseAlternativeFormat(int offset)
{
try
{
// 基于图片中的包头格式 09 80 12 02 进行解析
// 假设这是一种变体TLV格式
uint packetLength = BitConverter.ToUInt32(dataBuffer, offset + 4);
if (offset + packetLength <= bufferPosition && packetLength > 8 && packetLength < 10000)
{
// 尝试解析为点云数据
if (TryParsePointCloudData(dataBuffer, offset + 8, (int)packetLength - 8))
{
return true;
}
// 尝试解析为目标数据
if (TryParseTargetData(dataBuffer, offset + 8, (int)packetLength - 8))
{
return true;
}
}
}
catch (Exception ex)
{
Debug.LogWarning($"替代格式解析错误: {ex.Message}");
}
return false;
}
bool TryParseGenericFormat(int offset)
{
try
{
// 通用数据包检测:基于数据长度和结构特征
if (offset + 20 > bufferPosition) return false;
// 检查可能的数据包长度
for (int lenOffset = 4; lenOffset <= 8; lenOffset += 4)
{
if (offset + lenOffset + 4 <= bufferPosition)
{
uint potentialLength = BitConverter.ToUInt32(dataBuffer, offset + lenOffset);
if (potentialLength >= 20 && potentialLength <= 5000 &&
offset + potentialLength <= bufferPosition)
{
// 进一步验证数据包结构
if (ValidateGenericPacket(dataBuffer, offset, (int)potentialLength))
{
ProcessGenericRadarData(dataBuffer, offset, (int)potentialLength);
return true;
}
}
}
}
}
catch (Exception ex)
{
Debug.LogWarning($"通用格式解析错误: {ex.Message}");
}
return false;
}
bool ValidateGenericPacket(byte[] data, int offset, int length)
{
// 简单验证:检查数据包中是否有合理的浮点数范围
int validFloatCount = 0;
int totalFloatCount = 0;
for (int i = offset + 8; i < offset + length - 4; i += 4)
{
if (i + 4 <= data.Length)
{
float value = BitConverter.ToSingle(data, i);
totalFloatCount++;
// 检查是否为合理的雷达数据范围
if (Math.Abs(value) < 1000f && !float.IsNaN(value) && !float.IsInfinity(value))
{
validFloatCount++;
}
}
}
// 如果大部分浮点数都在合理范围内,认为是有效数据包
return totalFloatCount > 0 && (float)validFloatCount / totalFloatCount > 0.5f;
}
void ProcessTLVPacket(byte[] data, int start, int length)
{
if (length < 8) return;
try
{
uint tlvType = BitConverter.ToUInt32(data, start);
uint tlvLength = BitConverter.ToUInt32(data, start + 4);
if (start + 8 + tlvLength > data.Length) return;
switch (tlvType)
{
case 1: // 检测点
ParseDetectedPoints(data, start + 8, (int)tlvLength);
break;
case 7: // 检测目标
ParseDetectedObjects(data, start + 8, (int)tlvLength);
break;
case 9: // 跟踪目标
ParseTrackedObjects(data, start + 8, (int)tlvLength);
break;
default:
if (enableRawDataDebug)
{
Debug.Log($"未处理的TLV类型: {tlvType}, 长度: {tlvLength}");
}
break;
}
}
catch (Exception ex)
{
Debug.LogWarning($"TLV包解析错误: {ex.Message}");
}
}
bool TryParsePointCloudData(byte[] data, int start, int length)
{
try
{
// 点云数据通常有特定的结构
// 尝试解析为16字节每点的格式 (x, y, z, velocity 各4字节)
if (length >= 16 && length % 16 == 0)
{
int pointCount = length / 16;
ParsePointCloudFromBytes(data, start, length, pointCount);
return true;
}
// 尝试解析为20字节每点的格式 (包含SNR等信息)
if (length >= 20 && length % 20 == 0)
{
int pointCount = length / 20;
ParsePointCloudFromBytes(data, start, length, pointCount);
return true;
}
}
catch (Exception ex)
{
Debug.LogWarning($"点云数据解析错误: {ex.Message}");
}
return false;
}
bool TryParseTargetData(byte[] data, int start, int length)
{
try
{
// 目标数据通常包含目标数量字段
if (length >= 4)
{
int targetCount = BitConverter.ToInt32(data, start);
int targetSize = 20; // 假设每个目标20字节
if (length >= 4 + targetCount * targetSize)
{
ParseTargetData(data, start, length, targetCount, targetSize);
return true;
}
}
}
catch (Exception ex)
{
Debug.LogWarning($"目标数据解析错误: {ex.Message}");
}
return false;
}
void ProcessGenericRadarData(byte[] data, int start, int length)
{
try
{
// 先尝试解析为点云
if (TryParsePointCloudData(data, start, length))
{
return;
}
// 再尝试解析为目标
if (TryParseTargetData(data, start, length))
{
return;
}
// 如果都无法解析,尝试启发式解析
ParseHeuristicData(data, start, length);
}
catch (Exception ex)
{
Debug.LogWarning($"通用雷达数据解析错误: {ex.Message}");
}
}
void ParsePointCloudFromBytes(byte[] data, int start, int length, int pointCount)
{
lock (dataLock)
{
currentPointCloud.Clear();
for (int i = 0; i < pointCount; i++)
{
int pointStart = start + (i * 16);
if (pointStart + 16 <= data.Length)
{
PointCloudPoint point = new PointCloudPoint();
try
{
point.x = BitConverter.ToSingle(data, pointStart);
point.y = BitConverter.ToSingle(data, pointStart + 4);
point.z = BitConverter.ToSingle(data, pointStart + 8);
point.velocity = BitConverter.ToSingle(data, pointStart + 12);
point.snr = 20.0f; // 默认值
// 验证数据合理性
if (IsValidRadarPoint(point))
{
currentPointCloud.Add(point);
}
}
catch
{
// 忽略解析错误的点
}
}
}
pointCloudCount = currentPointCloud.Count;
}
}
void ParseTargetData(byte[] data, int start, int length, int targetCount, int targetSize)
{
lock (dataLock)
{
detectedObjects.Clear();
float totalRange = 0f;
maxDetectionRange = 0f;
for (int i = 0; i < targetCount; i++)
{
int targetStart = start + 4 + (i * targetSize);
if (targetStart + targetSize <= data.Length)
{
RadarObject obj = new RadarObject();
try
{
obj.x = BitConverter.ToSingle(data, targetStart);
obj.y = BitConverter.ToSingle(data, targetStart + 4);
obj.z = BitConverter.ToSingle(data, targetStart + 8);
obj.velocity = BitConverter.ToSingle(data, targetStart + 12);
obj.snr = BitConverter.ToSingle(data, targetStart + 16);
obj.range = Mathf.Sqrt(obj.x * obj.x + obj.y * obj.y + obj.z * obj.z);
obj.azimuth = Mathf.Atan2(obj.x, obj.z) * Mathf.Rad2Deg;
obj.elevation = Mathf.Atan2(obj.y, Mathf.Sqrt(obj.x * obj.x + obj.z * obj.z)) * Mathf.Rad2Deg;
obj.detectionTime = DateTime.Now;
if (IsValidRadarTarget(obj))
{
detectedObjects.Add(obj);
totalRange += obj.range;
if (obj.range > maxDetectionRange) maxDetectionRange = obj.range;
}
}
catch
{
// 忽略解析错误的目标
}
}
}
objectsDetected = detectedObjects.Count;
if (objectsDetected > 0) avgDetectionRange = totalRange / objectsDetected;
}
}
void ParseHeuristicData(byte[] data, int start, int length)
{
// 启发式解析:尝试识别数据模式
if (length < 16) return;
// 检查数据中是否有明显的点云模式
int potentialPoints = length / 16;
if (potentialPoints > 0)
{
ParsePointCloudFromBytes(data, start, length, potentialPoints);
}
}
void ParseDetectedPoints(byte[] data, int start, int length)
{
if (length < 4) return;
int pointCount = BitConverter.ToInt32(data, start);
ParsePointCloudFromBytes(data, start + 4, length - 4, pointCount);
}
void ParseDetectedObjects(byte[] data, int start, int length)
{
if (length < 4) return;
int targetCount = BitConverter.ToInt32(data, start);
ParseTargetData(data, start, length, targetCount, 20);
}
void ParseTrackedObjects(byte[] data, int start, int length)
{
ParseDetectedObjects(data, start, length); // 暂时使用相同逻辑
}
bool IsValidRadarPoint(PointCloudPoint point)
{
float distance = point.position.magnitude;
return distance > 0.1f && distance < maxVisualizationRange * 2f &&
!float.IsNaN(point.x) && !float.IsNaN(point.y) && !float.IsNaN(point.z) &&
Math.Abs(point.x) < 100f && Math.Abs(point.y) < 100f && Math.Abs(point.z) < 100f;
}
bool IsValidRadarTarget(RadarObject target)
{
return target.range > 0.1f && target.range < maxVisualizationRange * 2f &&
!float.IsNaN(target.x) && !float.IsNaN(target.y) && !float.IsNaN(target.z) &&
Math.Abs(target.velocity) < 100f;
}
void UpdateODSMonitorDisplay()
{
StringBuilder monitorBuilder = new StringBuilder();
monitorBuilder.AppendLine($"● 数据流: {(isMonitoring ? "活跃" : "停止")}");
monitorBuilder.AppendLine($"📊 速率: {dataRate:F1} KB/s");
monitorBuilder.AppendLine($"🎯 目标: {objectsDetected} 个");
monitorBuilder.AppendLine($"✨ 点云: {pointCloudCount} 点");
monitorBuilder.AppendLine($"📏 平均距离: {avgDetectionRange:F2} m");
monitorBuilder.AppendLine($"⚡ 帧数: {framesReceived}");
monitorBuilder.AppendLine($"❌ 错误: {dataErrors}");
monitorBuilder.AppendLine($"📦 处理包: {packetsProcessed}");
if (dataRate > 1.0f)
{
if (pointCloudCount > 0 || objectsDetected > 0)
{
monitorBuilder.AppendLine("✅ 数据解析正常");
}
else
{
monitorBuilder.AppendLine("⚠️ 有数据流但无目标");
monitorBuilder.AppendLine("尝试切换数据包格式");
}
}
else if (dataRate > 0.1f)
{
monitorBuilder.AppendLine("⚠️ 数据流较慢");
}
else
{
monitorBuilder.AppendLine("❎ 检查雷达连接");
}
// 添加数据包头信息
if (enableRawDataDebug && bufferPosition >= 8)
{
monitorBuilder.AppendLine($"\n最新包头:");
for (int i = 0; i < Math.Min(8, bufferPosition); i++)
{
monitorBuilder.Append($"{dataBuffer[i]:X2} ");
}
// 显示当前使用的数据包格式
monitorBuilder.AppendLine($"\n格式: {(useAlternativePacketFormat ? "替代(09 80 12 02)" : "标准(02 01 04 03)")}");
}
monitorData = monitorBuilder.ToString();
UpdateFOVVisualization();
}
void StopDataMonitoring()
{
isMonitoring = false;
if (dataSerialPort != null && dataSerialPort.IsOpen)
{
dataSerialPort.Close();
}
lastResponse += "✓ 数据监控已停止\n";
lock (dataLock)
{
objectsDetected = 0;
pointCloudCount = 0;
avgDetectionRange = 0f;
maxDetectionRange = 0f;
currentPointCloud.Clear();
detectedObjects.Clear();
bufferPosition = 0;
}
}
void SendCommand(string command)
{
Thread cmdThread = new Thread(() => {
try
{
using (SerialPort port = new SerialPort(configPort, configBaudRate))
{
port.Open();
port.WriteLine(command);
Thread.Sleep(500);
lastResponse = $"已发送: {command}\n";
StringBuilder responseBuilder = new StringBuilder();
DateTime startTime = DateTime.Now;
while ((DateTime.Now - startTime).TotalMilliseconds < 3000)
{
if (port.BytesToRead > 0)
{
responseBuilder.AppendLine(port.ReadLine());
}
Thread.Sleep(100);
}
if (responseBuilder.Length > 0)
{
lastResponse += "响应:\n" + responseBuilder.ToString();
}
port.Close();
}
}
catch (Exception e)
{
lastResponse = $"命令失败: {e.Message}";
}
});
cmdThread.IsBackground = true;
cmdThread.Start();
}
void OnDestroy()
{
isMonitoring = false;
if (configSerialPort != null && configSerialPort.IsOpen)
{
try { configSerialPort.Close(); } catch { }
}
if (dataSerialPort != null && dataSerialPort.IsOpen)
{
try { dataSerialPort.Close(); } catch { }
}
if (pointCloudContainer != null) Destroy(pointCloudContainer);
if (targetContainer != null) Destroy(targetContainer);
if (pointMesh != null) Destroy(pointMesh);
}
public void ResetStatistics()
{
totalBytesReceived = 0;
framesReceived = 0;
dataErrors = 0;
packetsProcessed = 0;
lock (dataLock)
{
detectedObjects.Clear();
currentPointCloud.Clear();
bufferPosition = 0;
}
dataRateHistory.Clear();
for (int i = 0; i < HISTORY_SIZE; i++)
{
dataRateHistory.Enqueue(0f);
}
}
}运行结果没有接受到点云数据,是因为数据包格式无法识别,导致无法解析。请你先认真分析我上面的话,根据上一段聊天内容,认真分析,在我脚本上帮我解决这些问题。
最新发布