✨ feat(runtime): 完善 FANUC 命令参数与状态通道重连
* 在 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 状态和实现说明
This commit is contained in:
@@ -39,6 +39,64 @@ public sealed class FanucProtocolTests
|
||||
Assert.Equal(1u, statusResponse.ProgramStatus);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 的速度倍率、TCP 和 IO 请求体字段顺序与逆向文档一致。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_PacksParameterCommandBodies()
|
||||
{
|
||||
var setTcpFrame = FanucCommandProtocol.PackSetTcpCommand(1, [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]);
|
||||
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000000e000022067a6f64"),
|
||||
FanucCommandProtocol.PackGetSpeedRatioCommand());
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001200002207000000507a6f64"),
|
||||
FanucCommandProtocol.PackSetSpeedRatioCommand(0.8));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001200002200000000017a6f64"),
|
||||
FanucCommandProtocol.PackGetTcpCommand(1));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a000000160000220800000002000000077a6f64"),
|
||||
FanucCommandProtocol.PackGetIoCommand(FanucIoTypes.DigitalOutput, 7));
|
||||
Assert.Equal(
|
||||
Convert.FromHexString("646f7a0000001a0000220900000002000000073f8000007a6f64"),
|
||||
FanucCommandProtocol.PackSetIoCommand(FanucIoTypes.DigitalOutput, 7, true));
|
||||
Assert.Equal(FanucCommandMessageIds.SetTcp, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(7, 4)));
|
||||
Assert.Equal(1u, BinaryPrimitives.ReadUInt32BigEndian(setTcpFrame.AsSpan(11, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(15, 4)));
|
||||
Assert.Equal(1.0f, BinaryPrimitives.ReadSingleBigEndian(setTcpFrame.AsSpan(39, 4)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10012 参数响应解析使用各自不同的字段顺序。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void CommandProtocol_ParsesParameterResponses()
|
||||
{
|
||||
var speedRatioResponse = FanucCommandProtocol.ParseSpeedRatioResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetSpeedRatio,
|
||||
Convert.FromHexString("0000005000000000")));
|
||||
var tcpResponse = FanucCommandProtocol.ParseTcpResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetTcp,
|
||||
Convert.FromHexString("00000000000000013f80000040000000404000000000000000000000000000003f800000")));
|
||||
var ioResponse = FanucCommandProtocol.ParseIoResponse(
|
||||
FanucCommandProtocol.PackFrame(
|
||||
FanucCommandMessageIds.GetIo,
|
||||
Convert.FromHexString("000000003f800000")));
|
||||
|
||||
Assert.True(speedRatioResponse.IsSuccess);
|
||||
Assert.Equal(0.8, speedRatioResponse.Ratio, precision: 6);
|
||||
Assert.True(tcpResponse.IsSuccess);
|
||||
Assert.Equal(1u, tcpResponse.TcpId);
|
||||
Assert.Equal([1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], tcpResponse.Pose);
|
||||
Assert.True(ioResponse.IsSuccess);
|
||||
Assert.True(ioResponse.Value);
|
||||
Assert.Equal(1.0, ioResponse.NumericValue, precision: 6);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10010 状态帧可以从抓包样本解析出尾部状态槽位。
|
||||
/// </summary>
|
||||
@@ -52,6 +110,52 @@ public sealed class FanucProtocolTests
|
||||
Assert.Equal(6, frame.Pose.Count);
|
||||
Assert.Equal(9, frame.JointOrExtensionValues.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], frame.TailWords);
|
||||
Assert.Equal(frame.Pose, frame.CartesianPose);
|
||||
Assert.Equal(frame.JointOrExtensionValues.Take(6), frame.JointDegrees);
|
||||
Assert.Equal(frame.JointOrExtensionValues.Skip(6), frame.ExternalAxes);
|
||||
Assert.Equal(frame.TailWords, frame.RawTailWords);
|
||||
Assert.Equal(2u, frame.StatusWord0);
|
||||
Assert.Equal(0u, frame.StatusWord1);
|
||||
Assert.Equal(0u, frame.StatusWord2);
|
||||
Assert.Equal(1u, frame.StatusWord3);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 pcap 中多条唯一 TCP 10010 状态帧都符合固定 90B 布局。
|
||||
/// </summary>
|
||||
[Theory]
|
||||
[InlineData("646f7a0000005a0000000040eac85a43b2ef4043aba8e9421ed9c1c2828105c2ed981f3fbdbda0bed4764ebe92aacc3efd9f0a3f317ce9be5d4580000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
[InlineData("646f7a0000005a00000000415aab64440a5302439adef542b39739c293c441431d50423fcdb7003d862fe3beca5730bf60eab23f148e403f89269d000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
[InlineData("646f7a0000005a000000004221b6f9440b9ce043a129ac42b292bac29cba78431bddcb3fc743213d90268dbeba5351bf64bc1b3f0cbdf73f826864000000000000000000000000000000020000000000000000000000017a6f64")]
|
||||
public void StateProtocol_ParsesMultipleCapturedPcapFrames(string frameHex)
|
||||
{
|
||||
var frameBytes = Convert.FromHexString(frameHex);
|
||||
|
||||
var frame = FanucStateProtocol.ParseFrame(frameBytes);
|
||||
|
||||
Assert.Equal(FanucStateProtocol.StateFrameLength, frameBytes.Length);
|
||||
Assert.Equal(6, frame.CartesianPose.Count);
|
||||
Assert.Equal(6, frame.JointDegrees.Count);
|
||||
Assert.Equal(3, frame.ExternalAxes.Count);
|
||||
Assert.Equal([2u, 0u, 0u, 1u], frame.RawTailWords);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 验证 TCP 10010 状态帧会拒绝损坏的长度和 magic,避免后台循环缓存坏帧。
|
||||
/// </summary>
|
||||
[Fact]
|
||||
public void StateProtocol_RejectsMalformedStateFrames()
|
||||
{
|
||||
var validFrame = Convert.FromHexString(
|
||||
"646f7a0000005a000000004388a23243f1ed7f43e9de6bc265031ec2b33cc3c278e0153f8742f53c3f128dbc929529bc7861d63cb0184c3c1ca1a7000000000000000000000000000000020000000000000000000000017a6f64");
|
||||
var wrongMagic = validFrame.ToArray();
|
||||
wrongMagic[0] = 0;
|
||||
var wrongLength = validFrame.ToArray();
|
||||
wrongLength[6] = 0x59;
|
||||
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(validFrame.AsSpan(0, validFrame.Length - 1)));
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongMagic));
|
||||
Assert.Throws<InvalidDataException>(() => FanucStateProtocol.ParseFrame(wrongLength));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
Reference in New Issue
Block a user