yunxiao.zhu
69fa3edd89
* 在 FanucCommandProtocol/Client 中补齐速度倍率、TCP 位姿和 IO 的封包/解析，并引入 FanucIoTypes 字符串到枚举映射 * FanucControllerRuntime 在非仿真模式下接入真机命令通道，本地 缓存仅作为兜底，TCP 操作扩展为 7 维 Pose * FanucStateClient 增加帧超时检测、退避自动重连和诊断状态接口, 超时或重连期间不再把陈旧帧当作当前机器人状态 * FanucStateProtocol 锁定 90B 帧字段为 pose[6]、joint[6]、 external_axes[3] 和 raw_tail_words[4],并保留状态字诊断槽位 * ICspPlanner 增加 global_scale > 1.0 失败判定,self-adapt-icsp 内部禁用该判定以保留补点重试链路 * 同步更新 README/AGENTS/计划文档的 todo 状态和实现说明
622 lines
21 KiB
C#
622 lines
21 KiB
C#
using System.Buffers.Binary;
|
||
using System.Text;
|
||
|
||
namespace Flyshot.Runtime.Fanuc.Protocol;
|
||
|
||
/// <summary>
|
||
/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
|
||
/// </summary>
|
||
public static class FanucCommandMessageIds
|
||
{
|
||
/// <summary>
|
||
/// 获取控制器程序状态的消息号。
|
||
/// </summary>
|
||
public const uint GetProgramStatus = 0x2003;
|
||
|
||
/// <summary>
|
||
/// 复位控制器的消息号。
|
||
/// </summary>
|
||
public const uint ResetRobot = 0x2100;
|
||
|
||
/// <summary>
|
||
/// 启动控制器程序的消息号。
|
||
/// </summary>
|
||
public const uint StartProgram = 0x2102;
|
||
|
||
/// <summary>
|
||
/// 停止控制器程序的消息号。
|
||
/// </summary>
|
||
public const uint StopProgram = 0x2103;
|
||
|
||
/// <summary>
|
||
/// 读取控制器 TCP 的消息号。
|
||
/// </summary>
|
||
public const uint GetTcp = 0x2200;
|
||
|
||
/// <summary>
|
||
/// 设置控制器 TCP 的消息号。
|
||
/// </summary>
|
||
public const uint SetTcp = 0x2201;
|
||
|
||
/// <summary>
|
||
/// 读取控制器速度倍率的消息号。
|
||
/// </summary>
|
||
public const uint GetSpeedRatio = 0x2206;
|
||
|
||
/// <summary>
|
||
/// 设置控制器速度倍率的消息号。
|
||
/// </summary>
|
||
public const uint SetSpeedRatio = 0x2207;
|
||
|
||
/// <summary>
|
||
/// 读取控制器 IO 的消息号。
|
||
/// </summary>
|
||
public const uint GetIo = 0x2208;
|
||
|
||
/// <summary>
|
||
/// 设置控制器 IO 的消息号。
|
||
/// </summary>
|
||
public const uint SetIo = 0x2209;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
|
||
/// </summary>
|
||
public static class FanucIoTypes
|
||
{
|
||
/// <summary>
|
||
/// FANUC 数字输入 DI。
|
||
/// </summary>
|
||
public const uint DigitalInput = 1;
|
||
|
||
/// <summary>
|
||
/// FANUC 数字输出 DO。
|
||
/// </summary>
|
||
public const uint DigitalOutput = 2;
|
||
|
||
/// <summary>
|
||
/// FANUC 机器人输入 RI。
|
||
/// </summary>
|
||
public const uint RobotInput = 8;
|
||
|
||
/// <summary>
|
||
/// FANUC 机器人输出 RO。
|
||
/// </summary>
|
||
public const uint RobotOutput = 9;
|
||
|
||
/// <summary>
|
||
/// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
|
||
/// </summary>
|
||
/// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
|
||
/// <returns>命令通道使用的 IO 类型数值。</returns>
|
||
public static uint FromName(string ioType)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(ioType))
|
||
{
|
||
throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
|
||
}
|
||
|
||
return ioType.Trim().ToUpperInvariant() switch
|
||
{
|
||
"DI" or "KIOTYPEDI" => DigitalInput,
|
||
"DO" or "KIOTYPEDO" => DigitalOutput,
|
||
"RI" or "KIOTYPERI" => RobotInput,
|
||
"RO" or "KIOTYPERO" => RobotOutput,
|
||
_ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
|
||
};
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
|
||
/// </summary>
|
||
public sealed class FanucCommandResultResponse
|
||
{
|
||
/// <summary>
|
||
/// 初始化命令结果响应。
|
||
/// </summary>
|
||
/// <param name="messageId">响应对应的消息号。</param>
|
||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||
public FanucCommandResultResponse(uint messageId, uint resultCode)
|
||
{
|
||
MessageId = messageId;
|
||
ResultCode = resultCode;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取响应对应的消息号。
|
||
/// </summary>
|
||
public uint MessageId { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的结果码。
|
||
/// </summary>
|
||
public uint ResultCode { get; }
|
||
|
||
/// <summary>
|
||
/// 获取当前响应是否表示成功。
|
||
/// </summary>
|
||
public bool IsSuccess => ResultCode == 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 表示 FANUC TCP 10012 速度倍率响应。
|
||
/// </summary>
|
||
public sealed class FanucSpeedRatioResponse
|
||
{
|
||
/// <summary>
|
||
/// 初始化速度倍率响应。
|
||
/// </summary>
|
||
/// <param name="messageId">响应对应的消息号。</param>
|
||
/// <param name="ratioInt">控制器返回的整数百分比。</param>
|
||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||
public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
|
||
{
|
||
MessageId = messageId;
|
||
RatioInt = ratioInt;
|
||
ResultCode = resultCode;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取响应对应的消息号。
|
||
/// </summary>
|
||
public uint MessageId { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的整数百分比。
|
||
/// </summary>
|
||
public uint RatioInt { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的结果码。
|
||
/// </summary>
|
||
public uint ResultCode { get; }
|
||
|
||
/// <summary>
|
||
/// 获取转换后的 0.0 到 1.0 速度倍率。
|
||
/// </summary>
|
||
public double Ratio => RatioInt / 100.0;
|
||
|
||
/// <summary>
|
||
/// 获取当前响应是否表示成功。
|
||
/// </summary>
|
||
public bool IsSuccess => ResultCode == 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 表示 FANUC TCP 10012 TCP 位姿响应。
|
||
/// </summary>
|
||
public sealed class FanucTcpResponse
|
||
{
|
||
/// <summary>
|
||
/// 初始化 TCP 位姿响应。
|
||
/// </summary>
|
||
/// <param name="messageId">响应对应的消息号。</param>
|
||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||
/// <param name="tcpId">控制器返回的 TCP ID。</param>
|
||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||
public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
|
||
{
|
||
MessageId = messageId;
|
||
ResultCode = resultCode;
|
||
TcpId = tcpId;
|
||
Pose = pose.ToArray();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取响应对应的消息号。
|
||
/// </summary>
|
||
public uint MessageId { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的结果码。
|
||
/// </summary>
|
||
public uint ResultCode { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的 TCP ID。
|
||
/// </summary>
|
||
public uint TcpId { get; }
|
||
|
||
/// <summary>
|
||
/// 获取 7 维 TCP 位姿。
|
||
/// </summary>
|
||
public IReadOnlyList<double> Pose { get; }
|
||
|
||
/// <summary>
|
||
/// 获取当前响应是否表示成功。
|
||
/// </summary>
|
||
public bool IsSuccess => ResultCode == 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 表示 FANUC TCP 10012 IO 读取响应。
|
||
/// </summary>
|
||
public sealed class FanucIoResponse
|
||
{
|
||
/// <summary>
|
||
/// 初始化 IO 读取响应。
|
||
/// </summary>
|
||
/// <param name="messageId">响应对应的消息号。</param>
|
||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||
/// <param name="numericValue">控制器返回的 float IO 数值。</param>
|
||
public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
|
||
{
|
||
MessageId = messageId;
|
||
ResultCode = resultCode;
|
||
NumericValue = numericValue;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取响应对应的消息号。
|
||
/// </summary>
|
||
public uint MessageId { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的结果码。
|
||
/// </summary>
|
||
public uint ResultCode { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的原始数值。
|
||
/// </summary>
|
||
public double NumericValue { get; }
|
||
|
||
/// <summary>
|
||
/// 获取按布尔 IO 解释后的值。
|
||
/// </summary>
|
||
public bool Value => Math.Abs(NumericValue) > double.Epsilon;
|
||
|
||
/// <summary>
|
||
/// 获取当前响应是否表示成功。
|
||
/// </summary>
|
||
public bool IsSuccess => ResultCode == 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 表示 FANUC TCP 10012 程序状态响应。
|
||
/// </summary>
|
||
public sealed class FanucProgramStatusResponse
|
||
{
|
||
/// <summary>
|
||
/// 初始化程序状态响应。
|
||
/// </summary>
|
||
/// <param name="messageId">响应对应的消息号。</param>
|
||
/// <param name="resultCode">控制器返回的结果码。</param>
|
||
/// <param name="programStatus">控制器程序状态。</param>
|
||
public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
|
||
{
|
||
MessageId = messageId;
|
||
ResultCode = resultCode;
|
||
ProgramStatus = programStatus;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取响应对应的消息号。
|
||
/// </summary>
|
||
public uint MessageId { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器返回的结果码。
|
||
/// </summary>
|
||
public uint ResultCode { get; }
|
||
|
||
/// <summary>
|
||
/// 获取控制器程序状态值。
|
||
/// </summary>
|
||
public uint ProgramStatus { get; }
|
||
|
||
/// <summary>
|
||
/// 获取当前响应是否表示成功。
|
||
/// </summary>
|
||
public bool IsSuccess => ResultCode == 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
|
||
/// </summary>
|
||
public static class FanucCommandProtocol
|
||
{
|
||
/// <summary>
|
||
/// 将无业务体命令封装为 TCP 10012 二进制帧。
|
||
/// </summary>
|
||
/// <param name="messageId">命令消息号。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackEmptyCommand(uint messageId)
|
||
{
|
||
return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将程序名命令封装为 TCP 10012 二进制帧。
|
||
/// </summary>
|
||
/// <param name="messageId">命令消息号。</param>
|
||
/// <param name="programName">控制器程序名。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackProgramCommand(uint messageId, string programName)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(programName))
|
||
{
|
||
throw new ArgumentException("程序名不能为空。", nameof(programName));
|
||
}
|
||
|
||
var programNameBytes = Encoding.ASCII.GetBytes(programName);
|
||
var body = new byte[sizeof(uint) + programNameBytes.Length];
|
||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
|
||
programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));
|
||
|
||
return PackFrame(messageId, body);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装读取速度倍率命令。
|
||
/// </summary>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackGetSpeedRatioCommand()
|
||
{
|
||
return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
|
||
/// </summary>
|
||
/// <param name="ratio">目标速度倍率。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackSetSpeedRatioCommand(double ratio)
|
||
{
|
||
if (double.IsNaN(ratio) || double.IsInfinity(ratio))
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
|
||
}
|
||
|
||
var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
|
||
var body = new byte[sizeof(uint)];
|
||
BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
|
||
return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装读取 TCP 位姿命令。
|
||
/// </summary>
|
||
/// <param name="tcpId">目标 TCP ID。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackGetTcpCommand(uint tcpId)
|
||
{
|
||
var body = new byte[sizeof(uint)];
|
||
BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
|
||
return PackFrame(FanucCommandMessageIds.GetTcp, body);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装设置 TCP 位姿命令。
|
||
/// </summary>
|
||
/// <param name="tcpId">目标 TCP ID。</param>
|
||
/// <param name="pose">7 维 TCP 位姿。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
|
||
{
|
||
ArgumentNullException.ThrowIfNull(pose);
|
||
if (pose.Count != 7)
|
||
{
|
||
throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
|
||
}
|
||
|
||
var body = new byte[sizeof(uint) + sizeof(float) * 7];
|
||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
|
||
for (int i = 0; i < 7; i++)
|
||
{
|
||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
|
||
}
|
||
|
||
return PackFrame(FanucCommandMessageIds.SetTcp, body);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
|
||
/// </summary>
|
||
/// <param name="ioType">IO 类型数值。</param>
|
||
/// <param name="ioIndex">IO 索引。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
|
||
{
|
||
if (ioIndex < 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||
}
|
||
|
||
var body = new byte[sizeof(uint) * 2];
|
||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||
return PackFrame(FanucCommandMessageIds.GetIo, body);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
|
||
/// </summary>
|
||
/// <param name="ioType">IO 类型数值。</param>
|
||
/// <param name="ioIndex">IO 索引。</param>
|
||
/// <param name="value">目标 IO 布尔值。</param>
|
||
/// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
|
||
public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
|
||
{
|
||
if (ioIndex < 0)
|
||
{
|
||
throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
|
||
}
|
||
|
||
var body = new byte[sizeof(uint) * 2 + sizeof(float)];
|
||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
|
||
BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
|
||
BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
|
||
return PackFrame(FanucCommandMessageIds.SetIo, body);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析只携带结果码的 TCP 10012 响应帧。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>命令结果响应。</returns>
|
||
public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
|
||
{
|
||
var messageId = ValidateAndReadMessageId(frame);
|
||
var body = GetBody(frame);
|
||
if (body.Length < sizeof(uint))
|
||
{
|
||
throw new InvalidDataException("FANUC 命令响应体长度不足。");
|
||
}
|
||
|
||
return new FanucCommandResultResponse(
|
||
messageId,
|
||
BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>速度倍率响应。</returns>
|
||
public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
|
||
{
|
||
var messageId = ValidateAndReadMessageId(frame);
|
||
var body = GetBody(frame);
|
||
if (body.Length < sizeof(uint) * 2)
|
||
{
|
||
throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
|
||
}
|
||
|
||
// GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
|
||
var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||
return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析 GetTCP 的 TCP 10012 响应帧。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>TCP 位姿响应。</returns>
|
||
public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
|
||
{
|
||
var messageId = ValidateAndReadMessageId(frame);
|
||
var body = GetBody(frame);
|
||
if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
|
||
{
|
||
throw new InvalidDataException("FANUC TCP 响应体长度不足。");
|
||
}
|
||
|
||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||
var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||
var pose = new double[7];
|
||
for (int i = 0; i < pose.Length; i++)
|
||
{
|
||
pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
|
||
}
|
||
|
||
return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析 GetIO 的 TCP 10012 响应帧。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>IO 读取响应。</returns>
|
||
public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
|
||
{
|
||
var messageId = ValidateAndReadMessageId(frame);
|
||
var body = GetBody(frame);
|
||
if (body.Length < sizeof(uint) + sizeof(float))
|
||
{
|
||
throw new InvalidDataException("FANUC IO 响应体长度不足。");
|
||
}
|
||
|
||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||
var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
|
||
return new FanucIoResponse(messageId, resultCode, ioValue);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 解析 GetProgStatus 的 TCP 10012 响应帧。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>程序状态响应。</returns>
|
||
public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
|
||
{
|
||
var messageId = ValidateAndReadMessageId(frame);
|
||
var body = GetBody(frame);
|
||
if (body.Length < sizeof(uint) * 2)
|
||
{
|
||
throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
|
||
}
|
||
|
||
// 抓包样本中的字段顺序为 result_code 后接 prog_status。
|
||
var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
|
||
var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
|
||
return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按 FANUC 命令通道 framing 规则封装完整帧。
|
||
/// </summary>
|
||
/// <param name="messageId">命令消息号。</param>
|
||
/// <param name="body">业务体。</param>
|
||
/// <returns>完整命令帧。</returns>
|
||
internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
|
||
{
|
||
var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
|
||
var frame = new byte[frameLength];
|
||
|
||
frame[0] = (byte)'d';
|
||
frame[1] = (byte)'o';
|
||
frame[2] = (byte)'z';
|
||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
|
||
BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
|
||
body.CopyTo(frame.AsSpan(11));
|
||
frame[^3] = (byte)'z';
|
||
frame[^2] = (byte)'o';
|
||
frame[^1] = (byte)'d';
|
||
|
||
return frame;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 校验完整帧并读取消息号。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>响应消息号。</returns>
|
||
private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
|
||
{
|
||
if (frame.Length < 14)
|
||
{
|
||
throw new InvalidDataException("FANUC 命令帧长度不足。");
|
||
}
|
||
|
||
if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
|
||
{
|
||
throw new InvalidDataException("FANUC 命令帧头 magic 不正确。");
|
||
}
|
||
|
||
if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
|
||
{
|
||
throw new InvalidDataException("FANUC 命令帧尾 magic 不正确。");
|
||
}
|
||
|
||
var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
|
||
if (declaredLength != frame.Length)
|
||
{
|
||
throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
|
||
}
|
||
|
||
return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取完整帧中的业务体切片。
|
||
/// </summary>
|
||
/// <param name="frame">完整响应帧。</param>
|
||
/// <returns>业务体切片。</returns>
|
||
private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
|
||
{
|
||
return frame.Slice(11, frame.Length - 14);
|
||
}
|
||
}
|