FlyShotHost/analysis/fanuc_10010_dissector.lua

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)