✨ feat(fanuc): 添加协议编解码与状态页" -m "* 固化 10010 状态帧、10012 命令帧和 60015 J519 包编解码
* 扩展 ControllerClient 兼容层的执行参数和运行时编排 * 新增 /status 页面与 /api/status/snapshot 状态快照接口 * 补充 FANUC 协议、客户端和状态接口的最小验证测试 * 更新 README、兼容要求和真机 Socket 通信实现计划
This commit is contained in:
127
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal file
127
src/Flyshot.Runtime.Fanuc/Protocol/FanucStateProtocol.cs
Normal 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 状态帧长度字段与实际长度不一致。");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user