✨ feat(fanuc): 打通飞拍轨迹完整执行链路
* 增加 J519 稠密发送采样校验与保姿回发逻辑 * 调整 saveTrajectory 导出与 sequence buffer 行为 * 补充 10010 解析脚本、ICSP 说明和回归测试
This commit is contained in:
199
analysis/fanuc_10010_dissector.lua
Normal file
199
analysis/fanuc_10010_dissector.lua
Normal file
@@ -0,0 +1,199 @@
|
||||
local plugin_info = {
|
||||
version = "1.1.0",
|
||||
author = "OpenAI Codex",
|
||||
description = "FANUC TCP 10010 状态帧解析器",
|
||||
}
|
||||
|
||||
set_plugin_info(plugin_info)
|
||||
|
||||
local fanuc10010_proto = Proto("fanuc10010_state_frame", "FANUC 10010 State Frame")
|
||||
|
||||
local FRAME_LENGTH = 90
|
||||
local HEADER_LENGTH = 3
|
||||
local TRAILER_LENGTH = 3
|
||||
local LENGTH_OFFSET = 3
|
||||
local MESSAGE_ID_OFFSET = 7
|
||||
local POSE_OFFSET = 11
|
||||
local POSE_COUNT = 6
|
||||
local JOINT_OFFSET = 35
|
||||
local JOINT_COUNT = 9
|
||||
local TAIL_OFFSET = 71
|
||||
local TAIL_COUNT = 4
|
||||
|
||||
local POSE_NAMES = { "X", "Y", "Z", "W", "P", "R" }
|
||||
local JOINT_NAMES = { "J1", "J2", "J3", "J4", "J5", "J6", "Ext1", "Ext2", "Ext3" }
|
||||
local HEADER_MAGIC = "doz"
|
||||
local TRAILER_MAGIC = "zod"
|
||||
|
||||
local fields = {
|
||||
frame = ProtoField.bytes("fanuc10010.frame", "原始状态帧"),
|
||||
header = ProtoField.string("fanuc10010.header", "帧头 Magic"),
|
||||
declared_length = ProtoField.uint32("fanuc10010.length", "声明长度", base.DEC),
|
||||
message_id = ProtoField.uint32("fanuc10010.message_id", "消息号", base.DEC),
|
||||
pose = ProtoField.none("fanuc10010.pose", "笛卡尔位姿"),
|
||||
joints = ProtoField.none("fanuc10010.joints", "关节与扩展轴"),
|
||||
joint_degrees = ProtoField.none("fanuc10010.joint_degrees", "Joint Degrees"),
|
||||
tail = ProtoField.none("fanuc10010.tail", "尾部状态字"),
|
||||
trailer = ProtoField.string("fanuc10010.trailer", "帧尾 Magic"),
|
||||
expert_bad_length = ProtoField.string("fanuc10010.expert.bad_length", "长度异常"),
|
||||
expert_bad_magic = ProtoField.string("fanuc10010.expert.bad_magic", "Magic 异常"),
|
||||
}
|
||||
|
||||
for index = 1, POSE_COUNT do
|
||||
fields["pose_" .. index] = ProtoField.float(
|
||||
"fanuc10010.pose_" .. index,
|
||||
"Pose " .. POSE_NAMES[index])
|
||||
end
|
||||
|
||||
for index = 1, JOINT_COUNT do
|
||||
fields["joint_" .. index] = ProtoField.float(
|
||||
"fanuc10010.joint_" .. index,
|
||||
JOINT_NAMES[index] .. " (raw)")
|
||||
end
|
||||
|
||||
for index = 1, 6 do
|
||||
fields["joint_deg_" .. index] = ProtoField.float(
|
||||
"fanuc10010.joint_deg_" .. index,
|
||||
JOINT_NAMES[index] .. " (deg)")
|
||||
end
|
||||
|
||||
for index = 1, TAIL_COUNT do
|
||||
fields["tail_" .. index] = ProtoField.uint32(
|
||||
"fanuc10010.tail_" .. index,
|
||||
"Tail[" .. (index - 1) .. "]",
|
||||
base.DEC)
|
||||
end
|
||||
|
||||
fanuc10010_proto.fields = fields
|
||||
|
||||
local function read_f32_be(tvb, offset)
|
||||
return tvb(offset, 4):tvb():range(0, 4):float()
|
||||
end
|
||||
|
||||
local function read_u32_be(tvb, offset)
|
||||
return tvb(offset, 4):uint()
|
||||
end
|
||||
|
||||
local function read_ascii(tvb, offset, length)
|
||||
return tvb(offset, length):string()
|
||||
end
|
||||
|
||||
local function radians_to_degrees(value)
|
||||
return value * 180.0 / math.pi
|
||||
end
|
||||
|
||||
local function add_error(subtree, range, field, message)
|
||||
local item = subtree:add(field, range, message)
|
||||
item:add_expert_info(PI_MALFORMED, PI_ERROR, message)
|
||||
end
|
||||
|
||||
local function dissect_single_frame(tvb, pinfo, tree, frame_offset)
|
||||
local remaining = tvb:len() - frame_offset
|
||||
local candidate_length = math.min(remaining, FRAME_LENGTH)
|
||||
local frame_range = tvb(frame_offset, candidate_length)
|
||||
local subtree = tree:add(fanuc10010_proto, frame_range, "FANUC 10010 状态帧")
|
||||
subtree:add(fields.frame, frame_range)
|
||||
|
||||
if remaining < FRAME_LENGTH then
|
||||
add_error(subtree, frame_range, fields.expert_bad_length, "剩余字节不足 90B，无法组成完整状态帧")
|
||||
return remaining
|
||||
end
|
||||
|
||||
local header = read_ascii(tvb, frame_offset, HEADER_LENGTH)
|
||||
local declared_length = read_u32_be(tvb, frame_offset + LENGTH_OFFSET)
|
||||
local message_id = read_u32_be(tvb, frame_offset + MESSAGE_ID_OFFSET)
|
||||
local trailer = read_ascii(tvb, frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH)
|
||||
|
||||
subtree:add(fields.header, tvb(frame_offset, HEADER_LENGTH), header)
|
||||
subtree:add(fields.declared_length, tvb(frame_offset + LENGTH_OFFSET, 4), declared_length)
|
||||
subtree:add(fields.message_id, tvb(frame_offset + MESSAGE_ID_OFFSET, 4), message_id)
|
||||
|
||||
local pose_tree = subtree:add(fields.pose, tvb(frame_offset + POSE_OFFSET, POSE_COUNT * 4))
|
||||
local pose_values = {}
|
||||
for index = 1, POSE_COUNT do
|
||||
local field_offset = frame_offset + POSE_OFFSET + ((index - 1) * 4)
|
||||
local value = read_f32_be(tvb, field_offset)
|
||||
pose_values[index] = value
|
||||
pose_tree:add(fields["pose_" .. index], tvb(field_offset, 4), value)
|
||||
end
|
||||
|
||||
local joint_tree = subtree:add(fields.joints, tvb(frame_offset + JOINT_OFFSET, JOINT_COUNT * 4))
|
||||
local joint_values = {}
|
||||
for index = 1, JOINT_COUNT do
|
||||
local field_offset = frame_offset + JOINT_OFFSET + ((index - 1) * 4)
|
||||
local value = read_f32_be(tvb, field_offset)
|
||||
joint_values[index] = value
|
||||
joint_tree:add(fields["joint_" .. index], tvb(field_offset, 4), value)
|
||||
end
|
||||
|
||||
-- 单独保留一组角度显示，便于对照原始关节弧度值。
|
||||
local joint_degree_tree = subtree:add(fields.joint_degrees, tvb(frame_offset + JOINT_OFFSET, 6 * 4))
|
||||
for index = 1, 6 do
|
||||
local field_offset = frame_offset + JOINT_OFFSET + ((index - 1) * 4)
|
||||
joint_degree_tree:add(
|
||||
fields["joint_deg_" .. index],
|
||||
tvb(field_offset, 4),
|
||||
radians_to_degrees(joint_values[index]))
|
||||
end
|
||||
|
||||
local tail_tree = subtree:add(fields.tail, tvb(frame_offset + TAIL_OFFSET, TAIL_COUNT * 4))
|
||||
local tail_values = {}
|
||||
for index = 1, TAIL_COUNT do
|
||||
local field_offset = frame_offset + TAIL_OFFSET + ((index - 1) * 4)
|
||||
local value = read_u32_be(tvb, field_offset)
|
||||
tail_values[index] = value
|
||||
tail_tree:add(fields["tail_" .. index], tvb(field_offset, 4), value)
|
||||
end
|
||||
|
||||
subtree:add(fields.trailer, tvb(frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH), trailer)
|
||||
|
||||
if header ~= HEADER_MAGIC then
|
||||
add_error(subtree, tvb(frame_offset, HEADER_LENGTH), fields.expert_bad_magic, "帧头不是 doz")
|
||||
end
|
||||
|
||||
if trailer ~= TRAILER_MAGIC then
|
||||
add_error(subtree, tvb(frame_offset + FRAME_LENGTH - TRAILER_LENGTH, TRAILER_LENGTH), fields.expert_bad_magic, "帧尾不是 zod")
|
||||
end
|
||||
|
||||
if declared_length ~= FRAME_LENGTH then
|
||||
add_error(subtree, tvb(frame_offset + LENGTH_OFFSET, 4), fields.expert_bad_length, "长度字段不是 90")
|
||||
end
|
||||
|
||||
local summary = string.format(
|
||||
"MsgId=%u X=%.3f Y=%.3f Z=%.3f J1=%.6f rad / %.3f deg J2=%.6f rad / %.3f deg Tail=[%u,%u,%u,%u]",
|
||||
message_id,
|
||||
pose_values[1], pose_values[2], pose_values[3],
|
||||
joint_values[1], radians_to_degrees(joint_values[1]),
|
||||
joint_values[2], radians_to_degrees(joint_values[2]),
|
||||
tail_values[1], tail_values[2], tail_values[3], tail_values[4])
|
||||
|
||||
subtree:set_text("FANUC 10010 状态帧, " .. summary)
|
||||
pinfo.cols.info:append(" | " .. summary)
|
||||
|
||||
return FRAME_LENGTH
|
||||
end
|
||||
|
||||
function fanuc10010_proto.dissector(tvb, pinfo, tree)
|
||||
if tvb:len() == 0 then
|
||||
return 0
|
||||
end
|
||||
|
||||
pinfo.cols.protocol = "FANUC10010"
|
||||
|
||||
local offset = 0
|
||||
while offset < tvb:len() do
|
||||
local consumed = dissect_single_frame(tvb, pinfo, tree, offset)
|
||||
if consumed <= 0 then
|
||||
break
|
||||
end
|
||||
|
||||
offset = offset + consumed
|
||||
if consumed < FRAME_LENGTH then
|
||||
break
|
||||
end
|
||||
end
|
||||
|
||||
return tvb:len()
|
||||
end
|
||||
|
||||
DissectorTable.get("tcp.port"):add(10010, fanuc10010_proto)
|
||||
Reference in New Issue
Block a user