feat(fanuc): 添加协议编解码与状态页" -m "* 固化 10010 状态帧、10012 命令帧和 60015 J519 包编解码

* 扩展 ControllerClient 兼容层的执行参数和运行时编排
  * 新增 /status 页面与 /api/status/snapshot 状态快照接口
  * 补充 FANUC 协议、客户端和状态接口的最小验证测试
  * 更新 README、兼容要求和真机 Socket 通信实现计划
This commit is contained in:
2026-04-24 21:26:25 +08:00
parent 8a20d9f507
commit a78e6761cb
25 changed files with 3773 additions and 55 deletions

View File

@@ -0,0 +1,127 @@
using System.Buffers.Binary;
namespace Flyshot.Runtime.Fanuc.Protocol;
/// <summary>
/// 表示 FANUC TCP 10010 状态通道中的单个状态帧。
/// </summary>
public sealed class FanucStateFrame
{
private readonly double[] _pose;
private readonly double[] _jointOrExtensionValues;
private readonly uint[] _tailWords;
/// <summary>
/// 初始化状态帧解析结果。
/// </summary>
/// <param name="messageId">状态帧消息号或序号。</param>
/// <param name="pose">控制器回传的笛卡尔位姿。</param>
/// <param name="jointOrExtensionValues">控制器回传的关节或扩展轴状态。</param>
/// <param name="tailWords">状态帧尾部状态槽位。</param>
public FanucStateFrame(
uint messageId,
IEnumerable<double> pose,
IEnumerable<double> jointOrExtensionValues,
IEnumerable<uint> tailWords)
{
MessageId = messageId;
_pose = pose?.ToArray() ?? throw new ArgumentNullException(nameof(pose));
_jointOrExtensionValues = jointOrExtensionValues?.ToArray() ?? throw new ArgumentNullException(nameof(jointOrExtensionValues));
_tailWords = tailWords?.ToArray() ?? throw new ArgumentNullException(nameof(tailWords));
}
/// <summary>
/// 获取状态帧消息号或序号。
/// </summary>
public uint MessageId { get; }
/// <summary>
/// 获取控制器回传的笛卡尔位姿。
/// </summary>
public IReadOnlyList<double> Pose => _pose;
/// <summary>
/// 获取控制器回传的关节或扩展轴状态。
/// </summary>
public IReadOnlyList<double> JointOrExtensionValues => _jointOrExtensionValues;
/// <summary>
/// 获取状态帧尾部状态槽位。
/// </summary>
public IReadOnlyList<uint> TailWords => _tailWords;
}
/// <summary>
/// 提供 FANUC TCP 10010 状态通道固定帧解析能力。
/// </summary>
public static class FanucStateProtocol
{
/// <summary>
/// FANUC 状态通道抓包确认的完整帧长度。
/// </summary>
public const int StateFrameLength = 90;
/// <summary>
/// 解析 TCP 10010 状态通道中的单个完整状态帧。
/// </summary>
/// <param name="frame">完整状态帧。</param>
/// <returns>状态帧解析结果。</returns>
public static FanucStateFrame ParseFrame(ReadOnlySpan<byte> frame)
{
ValidateFrame(frame);
var pose = new double[6];
var jointOrExtensionValues = new double[9];
var tailWords = new uint[4];
// 状态帧采用固定布局,偏移来自抓包与 StateServer 逆向结论。
for (var index = 0; index < pose.Length; index++)
{
pose[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(11 + (index * sizeof(float)), sizeof(float)));
}
for (var index = 0; index < jointOrExtensionValues.Length; index++)
{
jointOrExtensionValues[index] = BinaryPrimitives.ReadSingleBigEndian(frame.Slice(35 + (index * sizeof(float)), sizeof(float)));
}
for (var index = 0; index < tailWords.Length; index++)
{
tailWords[index] = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(71 + (index * sizeof(uint)), sizeof(uint)));
}
return new FanucStateFrame(
BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint))),
pose,
jointOrExtensionValues,
tailWords);
}
/// <summary>
/// 校验状态帧的长度、magic 和长度字段。
/// </summary>
/// <param name="frame">完整状态帧。</param>
private static void ValidateFrame(ReadOnlySpan<byte> frame)
{
if (frame.Length != StateFrameLength)
{
throw new InvalidDataException("FANUC 状态帧长度不符合 TCP 10010 固定帧布局。");
}
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 状态帧长度字段与实际长度不一致。");
}
}
}