|
|
@@ -0,0 +1,615 @@
|
|
|
+using System;
|
|
|
+using System.Collections.Generic;
|
|
|
+using System.Net;
|
|
|
+using System.Net.Sockets;
|
|
|
+using System.Text;
|
|
|
+using System.Threading.Tasks;
|
|
|
+using TeamAAS_VP.Enums;
|
|
|
+using TeamAAS_VP.Interfaces;
|
|
|
+
|
|
|
+namespace TeamAAS_VP.Core.ScrewDriver
|
|
|
+{
|
|
|
+ /*
|
|
|
+ 计划(伪代码):
|
|
|
+ 1. 为类添加 XML 文档注释,描述此驱动器的用途与协议相关信息。
|
|
|
+ 2. 为构造函数、属性、事件、公开方法(Connect/ConnectAsync/GetData/GetDataAsync/Dispose)添加注释,说明用途与返回值。
|
|
|
+ 3. 为内部重要方法(EnsureSocket/SendGetTspc/ReceiveOnce/UpdateConnectState/DataAnalysis/ToInt16/ToInt32N)添加注释,解释实现细节与特殊字节序处理。
|
|
|
+ 4. 在 DataAnalysis 中为每个解析字段添加注释,标明字节偏移与单位转换,保留对异常与边界检查的说明。
|
|
|
+ 5. 保持原有逻辑不变,只增加注释,保证与现有代码风格一致并能通过编译。
|
|
|
+ */
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// XYD 系列电动螺丝刀数据读取与解析器。
|
|
|
+ /// 通过 UDP 向设备发送 "GET_TSPC" 探测命令并接收包含 TSPB/TSPA 数据包的响应,
|
|
|
+ /// 解析设备返回的扭矩、角度、速度、时间等曲线和结果信息。
|
|
|
+ /// </summary>
|
|
|
+ public class XYD_ScrewDriver : IScrewDriver
|
|
|
+ {
|
|
|
+ private Socket _socket;
|
|
|
+ private IPEndPoint _endPoint;
|
|
|
+
|
|
|
+ // 协议最小数据长度(TSPB 固定头与描述长度)
|
|
|
+ private const int MinDataLength = 508;
|
|
|
+
|
|
|
+ #region 属性
|
|
|
+ public Guid Id { get; private set; }
|
|
|
+
|
|
|
+ private bool _isConnected;
|
|
|
+ /// <summary>
|
|
|
+ /// 当前连接状态(是否已与设备建立连接并成功接收过有效数据)。
|
|
|
+ /// </summary>
|
|
|
+ public bool IsConnected => _isConnected;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 螺丝刀设备 IP 地址。
|
|
|
+ /// </summary>
|
|
|
+ public string IPAdress { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 设备端口号。
|
|
|
+ /// </summary>
|
|
|
+ public int Port { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 螺丝刀品牌标识(XYD 固定)。
|
|
|
+ /// </summary>
|
|
|
+ public ElectricScrewdriverBrand Brand => ElectricScrewdriverBrand.XYD;
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 螺丝编号(设备返回的整型编号)。
|
|
|
+ /// </summary>
|
|
|
+ public int ScrewNum { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 曲线采样频率(单位:Hz 或协议定义的采样单位)。
|
|
|
+ /// </summary>
|
|
|
+ public int Frequency { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 结果时间戳(设备返回的年月日时分秒)。
|
|
|
+ /// </summary>
|
|
|
+ public DateTime ResultDateTime { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 锁止角(单位:度)。
|
|
|
+ /// </summary>
|
|
|
+ public double LockAngel { get; private set; }
|
|
|
+
|
|
|
+ public double StrokeAngle1 { get; private set; }
|
|
|
+ public double StrokeAngle2 { get; private set; }
|
|
|
+ public double StrokeAngle3 { get; private set; }
|
|
|
+ public double StrokeAngle4 { get; private set; }
|
|
|
+ public double StrokeAngle5 { get; private set; }
|
|
|
+
|
|
|
+ public double StrokeTorque1 { get; private set; }
|
|
|
+ public double StrokeTorque2 { get; private set; }
|
|
|
+ public double StrokeTorque3 { get; private set; }
|
|
|
+ public double StrokeTorque4 { get; private set; }
|
|
|
+ public double StrokeTorque5 { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 峰值扭矩(单位:N·m,协议返回值需乘以 0.001)。
|
|
|
+ /// </summary>
|
|
|
+ public double MaxTorque { get; private set; }
|
|
|
+
|
|
|
+ public double OverlockAngle { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 过锁扭矩(单位:N·m)。
|
|
|
+ /// </summary>
|
|
|
+ public double OverlockTorque { get; private set; }
|
|
|
+
|
|
|
+ public double SlopeCheck { get; private set; }
|
|
|
+ public double ClampingAngle { get; private set; }
|
|
|
+ public double ClampingAngleCheck { get; private set; }
|
|
|
+ public double FitAngle { get; private set; }
|
|
|
+ public double FitAngleCheck { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 夹紧扭矩(单位:N·m)。
|
|
|
+ /// </summary>
|
|
|
+ public double ClampingTorque { get; private set; }
|
|
|
+
|
|
|
+ public double ClampingTorqueCheck { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 过程最大扭矩(单位:N·m)。
|
|
|
+ /// </summary>
|
|
|
+ public double ProcessMaxTorque { get; private set; }
|
|
|
+
|
|
|
+ public double ProcessMaxTorqueCheck { get; private set; }
|
|
|
+ public double FitPointCoord { get; private set; }
|
|
|
+ public double FitAngle1 { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 贴合点扭矩(单位:N·m)。
|
|
|
+ /// </summary>
|
|
|
+ public double FitTorque { get; private set; }
|
|
|
+
|
|
|
+ public double SectionTooth { get; private set; }
|
|
|
+ public double SectionStroke1 { get; private set; }
|
|
|
+ public double SectionStroke2 { get; private set; }
|
|
|
+ public double SectionStroke3 { get; private set; }
|
|
|
+ public double SectionStroke4 { get; private set; }
|
|
|
+ public double SectionStroke5 { get; private set; }
|
|
|
+ public double SectionLockAngle { get; private set; }
|
|
|
+ public double SectionLockFinish1 { get; private set; }
|
|
|
+ public double SectionOverLock { get; private set; }
|
|
|
+ public double SectionFinalEnd { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 解析出的错误信息或结果说明(根据设备返回码映射)。
|
|
|
+ /// </summary>
|
|
|
+ public string ErrorInfo { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 本次结果是否判定为成功(设备返回结果码为 4 表示成功)。
|
|
|
+ /// </summary>
|
|
|
+ public bool Success { get; private set; }
|
|
|
+
|
|
|
+ public int ProgramNumber { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 总圈数(单位:圈,协议返回为度需除以 360)。
|
|
|
+ /// </summary>
|
|
|
+ public double Truns { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 结果扭矩值(单位:N·m)。
|
|
|
+ /// </summary>
|
|
|
+ public double TorqueValue { get; private set; }
|
|
|
+
|
|
|
+ public int FinalTime { get; private set; }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 曲线数据集合(解析后的时间、角度、扭矩、斜率等)。
|
|
|
+ /// </summary>
|
|
|
+ public List<WaveData> WaveDatas { get; } = new List<WaveData>();
|
|
|
+
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region 事件
|
|
|
+ /// <summary>
|
|
|
+ /// 连接状态变化事件,参数:sender, connected。
|
|
|
+ /// 当连接状态改变时触发(包括连接失败/成功与获取数据后状态更新)。
|
|
|
+ /// </summary>
|
|
|
+ public event Action<object, bool> ConnectStateChangedEvent;
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 构造函数,初始化目标设备的 IP 与端口。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="ip">设备 IP 地址</param>
|
|
|
+ /// <param name="port">设备 UDP 端口</param>
|
|
|
+ public XYD_ScrewDriver(string ip, int port)
|
|
|
+ {
|
|
|
+ IPAdress = ip;
|
|
|
+ Port = port;
|
|
|
+ Id=Guid.NewGuid();
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 确保内部 Socket 已创建并配置好超时与目标 EndPoint。
|
|
|
+ /// 如果已存在则直接返回。
|
|
|
+ /// </summary>
|
|
|
+ private void EnsureSocket()
|
|
|
+ {
|
|
|
+ if (_socket == null)
|
|
|
+ {
|
|
|
+ _socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
|
|
|
+ // 发送/接收超时,避免阻塞 UI 线程过久
|
|
|
+ _socket.SendTimeout = 1000;
|
|
|
+ _socket.ReceiveTimeout = 2000;
|
|
|
+ _endPoint = new IPEndPoint(System.Net.IPAddress.Parse(IPAdress), Port);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 同步连接到设备并进行一次探测(发送 GET_TSPC 并解析响应)。
|
|
|
+ /// 返回是否成功连接并解析到有效数据。
|
|
|
+ /// </summary>
|
|
|
+ public bool Connect()
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ EnsureSocket();
|
|
|
+ _socket.Connect(_endPoint);
|
|
|
+
|
|
|
+ // 发送探测命令并等待响应
|
|
|
+ if (!SendGetTspc())
|
|
|
+ {
|
|
|
+ UpdateConnectState(false);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ byte[] data = ReceiveOnce();
|
|
|
+ if (data == null || !DataAnalysis(data))
|
|
|
+ {
|
|
|
+ UpdateConnectState(false);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ UpdateConnectState(true);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ catch (Exception)
|
|
|
+ {
|
|
|
+ UpdateConnectState(false);
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 异步版本的 Connect。
|
|
|
+ /// </summary>
|
|
|
+ public Task<bool> ConnectAsync()
|
|
|
+ {
|
|
|
+ return Task.Run(() => Connect());
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 向设备请求数据并解析,最多尝试若干次(容错网络丢包)。
|
|
|
+ /// </summary>
|
|
|
+ public bool GetData()
|
|
|
+ {
|
|
|
+ if (_socket == null)
|
|
|
+ {
|
|
|
+ EnsureSocket();
|
|
|
+ }
|
|
|
+
|
|
|
+ // 允许多次重试以应对 UDP 丢包/超时
|
|
|
+ for (int i = 0; i < 10; i++)
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ if (!SendGetTspc())
|
|
|
+ continue;
|
|
|
+
|
|
|
+ byte[] data = ReceiveOnce();
|
|
|
+ if (data == null)
|
|
|
+ continue;
|
|
|
+
|
|
|
+ if (!DataAnalysis(data))
|
|
|
+ continue;
|
|
|
+
|
|
|
+ UpdateConnectState(true);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ catch (Exception)
|
|
|
+ {
|
|
|
+ UpdateConnectState(false);
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 异步版本的 GetData。
|
|
|
+ /// </summary>
|
|
|
+ public Task<bool> GetDataAsync()
|
|
|
+ {
|
|
|
+ return Task.Run(() => GetData());
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 发送协议探测命令 "GET_TSPC" 到设备。
|
|
|
+ /// 返回是否发送成功。
|
|
|
+ /// </summary>
|
|
|
+ private bool SendGetTspc()
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ var cmd = "GET_TSPC";
|
|
|
+ var bytes = Encoding.ASCII.GetBytes(cmd);
|
|
|
+ _socket.SendTo(bytes, _endPoint);
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ catch
|
|
|
+ {
|
|
|
+ // 发送失败通常来自网络/Socket 状态,调用方会做重试或断开处理
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 从 Socket 接收一次数据(单次 ReceiveFrom),并返回实际接收到的字节数组。
|
|
|
+ /// 超时或异常返回 null。
|
|
|
+ /// </summary>
|
|
|
+ private byte[] ReceiveOnce()
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ EndPoint remote = new IPEndPoint(System.Net.IPAddress.Any, 0);
|
|
|
+ byte[] buf = new byte[_socket.ReceiveBufferSize];
|
|
|
+ int len = _socket.ReceiveFrom(buf, ref remote);
|
|
|
+ if (len <= 0) return null;
|
|
|
+ var data = new byte[len];
|
|
|
+ Array.Copy(buf, 0, data, 0, len);
|
|
|
+ return data;
|
|
|
+ }
|
|
|
+ catch
|
|
|
+ {
|
|
|
+ // 接收异常(例如超时),返回 null 以触发上层重试逻辑
|
|
|
+ return null;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 更新连接状态并触发事件。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="connected">新的连接状态</param>
|
|
|
+ private void UpdateConnectState(bool connected)
|
|
|
+ {
|
|
|
+ _isConnected = connected;
|
|
|
+ ConnectStateChangedEvent?.Invoke(this, connected);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 释放资源实现(受保护,可由派生类覆盖)。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="disposing">是否为显式释放</param>
|
|
|
+ protected virtual void Dispose(bool disposing)
|
|
|
+ {
|
|
|
+ if (disposing)
|
|
|
+ {
|
|
|
+ if (_socket != null)
|
|
|
+ {
|
|
|
+ try
|
|
|
+ {
|
|
|
+ _socket.Shutdown(SocketShutdown.Both);
|
|
|
+ }
|
|
|
+ catch { }
|
|
|
+ _socket.Close();
|
|
|
+ _socket.Dispose();
|
|
|
+ _socket = null;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 释放所有托管资源并抑制终结化。
|
|
|
+ /// </summary>
|
|
|
+ public void Dispose()
|
|
|
+ {
|
|
|
+ Dispose(true);
|
|
|
+ GC.SuppressFinalize(this);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 解析设备返回的字节数据(TSPB + 可选的 TSPA 曲线数据)。
|
|
|
+ /// 对协议字段按固定偏移解析并进行单位转换。
|
|
|
+ /// 返回解析是否成功(数据完整且符合长度要求)。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="data">原始字节数据</param>
|
|
|
+ /// <returns>是否解析成功</returns>
|
|
|
+ private bool DataAnalysis(byte[] data)
|
|
|
+ {
|
|
|
+ if (data == null || data.Length < MinDataLength)
|
|
|
+ return false;
|
|
|
+
|
|
|
+ try
|
|
|
+ {
|
|
|
+ #region 注释(协议字段说明)
|
|
|
+ // 协议字节说明(基于设备文档):
|
|
|
+ // 1 - 4 “TSPB”
|
|
|
+ // 5 - 8 螺丝编号 (4 bytes, DCBA order)
|
|
|
+ // 9 - 10 曲线采样频率 (2 bytes)
|
|
|
+ // 11 - 22 结果日期 - 年 月 日 时 分 秒 (每项 2 bytes)
|
|
|
+ // 23 - 24 程序编号
|
|
|
+ // 25 - 26 总圈数 (单位:度)
|
|
|
+ // 27 锁止角度 (2 bytes)
|
|
|
+ // 29 结果扭矩 (2 bytes)
|
|
|
+ // 31 结果时间 (2 bytes)
|
|
|
+ // 33 锁附结果 (2 bytes)
|
|
|
+ // 35 ... 94 各阶段角度/扭矩等(按文档固定偏移)
|
|
|
+ // 205 TSPA 数据起始坐标(协议中以 1-base 或 0-base 可能不同)
|
|
|
+ // 207 TSPA 数据总数(样本点数)
|
|
|
+ // 随后按扭矩/速度/角度/斜率分别存放(每项为 2 bytes,sectionBytes = length * 2)
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ // 基本字段解析
|
|
|
+ ScrewNum = ToInt32N(data, 4);
|
|
|
+ Frequency = ToInt16(data, 8);
|
|
|
+
|
|
|
+ int year = ToInt16(data, 10);
|
|
|
+ int month = ToInt16(data, 12);
|
|
|
+ int day = ToInt16(data, 14);
|
|
|
+ int hour = ToInt16(data, 16);
|
|
|
+ int minute = ToInt16(data, 18);
|
|
|
+ int second = ToInt16(data, 20);
|
|
|
+ ResultDateTime = new DateTime(year, month, day, hour, minute, second);
|
|
|
+
|
|
|
+ ProgramNumber = ToInt16(data, 22);
|
|
|
+ // 设备返回的总圈数字段以度为单位,转换为圈
|
|
|
+ Truns = ToInt16(data, 24) / 360.0;
|
|
|
+ LockAngel = ToInt16(data, 26);
|
|
|
+ // 扭矩值以 mN·m(或协议指定单位)返回,乘以 0.001 转换为 N·m
|
|
|
+ TorqueValue = ToInt16(data, 28) * 0.001;
|
|
|
+ FinalTime = ToInt16(data, 30);
|
|
|
+
|
|
|
+ int finalResult = ToInt16(data, 32);
|
|
|
+ switch (finalResult)
|
|
|
+ {
|
|
|
+ case 1: ErrorInfo = "圈数异常"; break;
|
|
|
+ case 2: ErrorInfo = "区段行程NG"; break;
|
|
|
+ case 3: ErrorInfo = "超时"; break;
|
|
|
+ case 4: ErrorInfo = string.Empty; break;
|
|
|
+ case 5: ErrorInfo = "中断"; break;
|
|
|
+ case 6: ErrorInfo = "转矩过大"; break;
|
|
|
+ case 7: ErrorInfo = "反转"; break;
|
|
|
+ case 8: ErrorInfo = "锁止角不合格"; break;
|
|
|
+ case 9: ErrorInfo = "过锁异常"; break;
|
|
|
+ case 10: ErrorInfo = "斜率异常"; break;
|
|
|
+ case 11: ErrorInfo = "夹角异常"; break;
|
|
|
+ case 12: ErrorInfo = "贴合角异常"; break;
|
|
|
+ case 13: ErrorInfo = "夹紧力异常"; break;
|
|
|
+ case 14: ErrorInfo = "过程力异常"; break;
|
|
|
+ default: ErrorInfo = finalResult.ToString(); break;
|
|
|
+ }
|
|
|
+ Success = finalResult == 4;
|
|
|
+
|
|
|
+ // 阶段行程角度:协议中以度为单位,代码将其转换为圈(度/360)
|
|
|
+ StrokeAngle1 = ToInt16(data, 34) / 360.0;
|
|
|
+ StrokeAngle2 = ToInt16(data, 36) / 360.0;
|
|
|
+ StrokeAngle3 = ToInt16(data, 38) / 360.0;
|
|
|
+ StrokeAngle4 = ToInt16(data, 40) / 360.0;
|
|
|
+ StrokeAngle5 = ToInt16(data, 42) / 360.0;
|
|
|
+
|
|
|
+ // 阶段扭矩转换(单位缩放)
|
|
|
+ StrokeTorque1 = ToInt16(data, 44) * 0.001;
|
|
|
+ StrokeTorque2 = ToInt16(data, 46) * 0.001;
|
|
|
+ StrokeTorque3 = ToInt16(data, 48) * 0.001;
|
|
|
+ StrokeTorque4 = ToInt16(data, 50) * 0.001;
|
|
|
+ StrokeTorque5 = ToInt16(data, 52) * 0.001;
|
|
|
+
|
|
|
+ MaxTorque = ToInt16(data, 54) * 0.001;
|
|
|
+ OverlockAngle = ToInt16(data, 56);
|
|
|
+ OverlockTorque = ToInt16(data, 58) * 0.001;
|
|
|
+
|
|
|
+ SlopeCheck = ToInt16(data, 60);
|
|
|
+ ClampingAngle = ToInt16(data, 62);
|
|
|
+ ClampingAngleCheck = ToInt16(data, 64);
|
|
|
+ FitAngle = ToInt16(data, 66);
|
|
|
+ FitAngleCheck = ToInt16(data, 68);
|
|
|
+ ClampingTorque = ToInt16(data, 70) * 0.001;
|
|
|
+
|
|
|
+ ClampingTorqueCheck = ToInt16(data, 72);
|
|
|
+ ProcessMaxTorque = ToInt16(data, 74) * 0.001;
|
|
|
+ ProcessMaxTorqueCheck = ToInt16(data, 76);
|
|
|
+ FitPointCoord = ToInt16(data, 78);
|
|
|
+
|
|
|
+ FitAngle1 = ToInt16(data, 80);
|
|
|
+ FitTorque = ToInt16(data, 82) * 0.001;
|
|
|
+ SectionTooth = ToInt16(data, 84);
|
|
|
+
|
|
|
+ SectionStroke1 = ToInt16(data, 86);
|
|
|
+ SectionStroke2 = ToInt16(data, 88);
|
|
|
+ SectionStroke3 = ToInt16(data, 90);
|
|
|
+ SectionStroke4 = ToInt16(data, 92);
|
|
|
+ SectionStroke5 = ToInt16(data, 94);
|
|
|
+
|
|
|
+ SectionLockAngle = ToInt16(data, 96);
|
|
|
+ SectionLockFinish1 = ToInt16(data, 98);
|
|
|
+ SectionOverLock = ToInt16(data, 100);
|
|
|
+ SectionFinalEnd = ToInt16(data, 102);
|
|
|
+
|
|
|
+ // 计算 TSPA 曲线数据段长度(协议: 在偏移 504/506 存有开始/结束索引或总数)
|
|
|
+ int length = ToInt16(data, 506) - ToInt16(data, 504);
|
|
|
+ if (length <= 0)
|
|
|
+ {
|
|
|
+ // 无曲线数据,清空集合并返回成功
|
|
|
+ WaveDatas.Clear();
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+ int sectionBytes = length * 2;
|
|
|
+ int expectedTotal = 508 + sectionBytes * 4;
|
|
|
+ // 如果整体长度不足,则认为数据不完整,返回失败以触发重试
|
|
|
+ if (data.Length < expectedTotal)
|
|
|
+ return false;
|
|
|
+
|
|
|
+ var torques = new byte[sectionBytes];
|
|
|
+ var speeds = new byte[sectionBytes];
|
|
|
+ var angles = new byte[sectionBytes];
|
|
|
+ var slopes = new byte[sectionBytes];
|
|
|
+
|
|
|
+ // 按顺序从数据中拷贝各段(扭矩、速度、角度、斜率)
|
|
|
+ Array.Copy(data, 508, torques, 0, sectionBytes);
|
|
|
+ Array.Copy(data, 508 + sectionBytes, speeds, 0, sectionBytes);
|
|
|
+ Array.Copy(data, 508 + sectionBytes * 2, angles, 0, sectionBytes);
|
|
|
+ Array.Copy(data, 508 + sectionBytes * 3, slopes, 0, sectionBytes);
|
|
|
+
|
|
|
+ WaveDatas.Clear();
|
|
|
+ // 每 2 字节为一个样本,使用 ToInt16 解析(注意字节序)
|
|
|
+ for (int i = 0; i < sectionBytes; i += 2)
|
|
|
+ {
|
|
|
+ double angleVal = ToInt16(angles, i) / 360.0;
|
|
|
+ double torqueVal = ToInt16(torques, i) * 0.001;
|
|
|
+ double slopeVal = ToInt16(slopes, i);
|
|
|
+ // 时间:按采样频率换算(单位:秒),注意 i 为字节索引,样本索引为 i/2
|
|
|
+ double time = (Frequency * i) / 1000.0;
|
|
|
+
|
|
|
+ WaveDatas.Add(new WaveData
|
|
|
+ {
|
|
|
+ LockAngle = angleVal,
|
|
|
+ Torque = torqueVal,
|
|
|
+ Slopes = slopeVal,
|
|
|
+ Turns = angleVal,
|
|
|
+ Time = time
|
|
|
+ });
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ catch
|
|
|
+ {
|
|
|
+ // 任意解析或转换异常均视为解析失败
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 将指定数组中从 startIndex 开始的两个字节按设备协议的字节序解析为有符号 16 位值。
|
|
|
+ /// 协议采用“DCBA 风格(低字节在前,高字节在后)”,因此这里以小端方式合成。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="value">字节数组</param>
|
|
|
+ /// <param name="startIndex">起始索引</param>
|
|
|
+ /// <returns>解析出的 short 值,出错返回 0</returns>
|
|
|
+ private short ToInt16(byte[] value, int startIndex)
|
|
|
+ {
|
|
|
+ if (value == null || startIndex < 0 || startIndex + 1 >= value.Length)
|
|
|
+ return 0;
|
|
|
+
|
|
|
+ // data is DCBA style (little-endian word swapped)
|
|
|
+ return (short)((value[startIndex + 1] << 8) | value[startIndex]);
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 将指定数组中从 startIndex 开始的 4 个字节按设备协议解析为 32 位整型(DCBA 顺序)。
|
|
|
+ /// 注意字节顺序为设备特定格式,这里按原实现保持位移组合。
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="value">字节数组</param>
|
|
|
+ /// <param name="startIndex">起始索引</param>
|
|
|
+ /// <returns>解析出的 int 值,出错返回 0</returns>
|
|
|
+ private int ToInt32N(byte[] value, int startIndex)
|
|
|
+ {
|
|
|
+ if (value == null || startIndex < 0 || startIndex + 3 >= value.Length)
|
|
|
+ return 0;
|
|
|
+
|
|
|
+ // DCBA order -> 构造 32 位整数(高位在后面的索引)
|
|
|
+ return (value[startIndex + 3] << 24) | (value[startIndex + 2] << 16) | (value[startIndex + 1] << 8) | value[startIndex];
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 曲线数据结构,包含速度、扭矩、角度、圈数、斜率与时间。
|
|
|
+ /// </summary>
|
|
|
+ public struct WaveData
|
|
|
+ {
|
|
|
+ /// <summary>
|
|
|
+ /// 速度
|
|
|
+ /// </summary>
|
|
|
+ public double Speed { get; set; }
|
|
|
+ /// <summary>
|
|
|
+ /// 扭矩
|
|
|
+ /// </summary>
|
|
|
+ public double Torque { get; set; }
|
|
|
+ /// <summary>
|
|
|
+ /// 角度
|
|
|
+ /// </summary>
|
|
|
+ public double LockAngle { get; set; }
|
|
|
+ /// <summary>
|
|
|
+ /// 圈数
|
|
|
+ /// </summary>
|
|
|
+ public double Turns { get; set; }
|
|
|
+ /// <summary>
|
|
|
+ /// 斜率
|
|
|
+ /// </summary>
|
|
|
+ public double Slopes { get; set; }
|
|
|
+ /// <summary>
|
|
|
+ /// 时间(秒)
|
|
|
+ /// </summary>
|
|
|
+ public double Time { get; set; }
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|