darra-ethercat-master 2.0.5

//! CiA 402 PDO 映射高级功能
//!
//! 扫描从站 PDO 映射表，建立对象索引到 IOmap 偏移的快速查找，
//! 并提供 PDO 优先读写（回退到 SDO）接口。

use std::collections::HashMap;
use crate::data::error::{DarraError, Result};
use crate::utils::ffi;
use std::os::raw::c_int;

// ===================== PDO 映射条目 =====================

/// 单个 PDO 映射条目（对应一个对象子索引在 IOmap 中的位置）
#[derive(Debug, Clone)]
pub struct PdoMapEntry {
    /// 对象索引
    pub od_index: u16,
    /// 子索引
    pub sub_index: u8,
    /// 位长度
    pub bit_length: u8,
    /// 在从站 IOmap 中的字节偏移
    pub byte_offset: u32,
    /// 在字节内的位偏移
    pub bit_offset: u8,
    /// 方向：true = 输出 (RxPDO), false = 输入 (TxPDO)
    pub is_output: bool,
}

impl PdoMapEntry {
    /// 返回字节数（向上取整）
    pub fn byte_size(&self) -> u32 {
        (self.bit_length as u32 + 7) / 8
    }
}

// ===================== PDO 映射表 =====================

/// CiA 402 从站 PDO 映射缓存
///
/// 通过 `scan_pdo_mapping()` 构建，之后可用 `read` / `write`
/// 以 PDO 直接访问替代 SDO 访问，实现更低延迟的读写。
pub struct Cia402PdoMap {
    /// 主站索引
    master_index: u16,
    /// 从站索引
    slave_index: u16,
    /// 输出 PDO 映射 (RxPDO): key = (index, subindex)
    pub(crate) output_map: HashMap<(u16, u8), PdoMapEntry>,
    /// 输入 PDO 映射 (TxPDO): key = (index, subindex)
    pub(crate) input_map: HashMap<(u16, u8), PdoMapEntry>,
    /// 输出缓冲区起始指针（直接来自 IOmap）
    out_ptr: *mut u8,
    /// 输出缓冲区字节数
    out_size: i32,
    /// 输入缓冲区起始指针（直接来自 IOmap）
    in_ptr: *mut u8,
    /// 输入缓冲区字节数
    in_size: i32,
}

// 安全：指针来自 DLL 管理的共享内存，生命周期与主站一致
unsafe impl Send for Cia402PdoMap {}
unsafe impl Sync for Cia402PdoMap {}

impl Cia402PdoMap {
    /// 扫描从站 PDO 映射，返回映射表实例
    ///
    /// 读取 0x1600/0x1A00 系列对象的映射参数，
    /// 以及从站 IOmap 的 IO 指针，建立完整映射缓存。
    pub fn scan(master_index: u16, slave_index: u16) -> Result<Self> {
        // 获取 IOmap 指针
        let mut out_size: c_int = 0;
        let mut out_ptr: *mut u8 = std::ptr::null_mut();
        let mut in_size: c_int = 0;
        let mut in_ptr: *mut u8 = std::ptr::null_mut();
        let io_ok = unsafe {
            ffi::GetIO(master_index, slave_index,
                       &mut out_size, &mut out_ptr,
                       &mut in_size, &mut in_ptr)
        };
        if io_ok == 0 {
            return Err(DarraError::PdoFailed("获取从站 IOmap 失败".into()));
        }

        let mut output_map = HashMap::new();
        let mut input_map = HashMap::new();

        // 扫描 RxPDO 分配 (0x1C12) 以确定有哪些 RxPDO
        let rxpdo_count = Self::read_u8_sdo(master_index, slave_index, 0x1C12, 0)
            .unwrap_or(0);

        let mut out_byte_offset: u32 = 0;
        for sm_idx in 1..=rxpdo_count {
            // 0x1C12:sm_idx = 引用的 PDO 索引 (如 0x1600)
            let pdo_idx = match Self::read_u16_sdo(master_index, slave_index, 0x1C12, sm_idx) {
                Ok(v) => v,
                Err(_) => continue,
            };
            // 读取该 PDO 的映射条目数
            let map_count = Self::read_u8_sdo(master_index, slave_index, pdo_idx, 0)
                .unwrap_or(0);
            for map_sub in 1..=map_count {
                let map_val = match Self::read_u32_sdo(master_index, slave_index, pdo_idx, map_sub) {
                    Ok(v) => v,
                    Err(_) => continue,
                };
                // 映射值格式：bits[31:16]=索引, bits[15:8]=子索引, bits[7:0]=位长度
                let obj_idx   = ((map_val >> 16) & 0xFFFF) as u16;
                let obj_sub   = ((map_val >> 8)  & 0xFF) as u8;
                let bit_len   = (map_val & 0xFF) as u8;
                if obj_idx == 0 || bit_len == 0 { continue; }
                let entry = PdoMapEntry {
                    od_index:    obj_idx,
                    sub_index:   obj_sub,
                    bit_length:  bit_len,
                    byte_offset: out_byte_offset,
                    bit_offset:  0,
                    is_output:   true,
                };
                out_byte_offset += (bit_len as u32 + 7) / 8;
                output_map.insert((obj_idx, obj_sub), entry);
            }
        }

        // 扫描 TxPDO 分配 (0x1C13)
        let txpdo_count = Self::read_u8_sdo(master_index, slave_index, 0x1C13, 0)
            .unwrap_or(0);

        let mut in_byte_offset: u32 = 0;
        for sm_idx in 1..=txpdo_count {
            let pdo_idx = match Self::read_u16_sdo(master_index, slave_index, 0x1C13, sm_idx) {
                Ok(v) => v,
                Err(_) => continue,
            };
            let map_count = Self::read_u8_sdo(master_index, slave_index, pdo_idx, 0)
                .unwrap_or(0);
            for map_sub in 1..=map_count {
                let map_val = match Self::read_u32_sdo(master_index, slave_index, pdo_idx, map_sub) {
                    Ok(v) => v,
                    Err(_) => continue,
                };
                let obj_idx   = ((map_val >> 16) & 0xFFFF) as u16;
                let obj_sub   = ((map_val >> 8)  & 0xFF) as u8;
                let bit_len   = (map_val & 0xFF) as u8;
                if obj_idx == 0 || bit_len == 0 { continue; }
                let entry = PdoMapEntry {
                    od_index:    obj_idx,
                    sub_index:   obj_sub,
                    bit_length:  bit_len,
                    byte_offset: in_byte_offset,
                    bit_offset:  0,
                    is_output:   false,
                };
                in_byte_offset += (bit_len as u32 + 7) / 8;
                input_map.insert((obj_idx, obj_sub), entry);
            }
        }

        Ok(Self {
            master_index,
            slave_index,
            output_map,
            input_map,
            out_ptr,
            out_size,
            in_ptr,
            in_size,
        })
    }

    /// 通过 PDO 读取输入对象（若不在映射中则回退到 SDO）
    pub fn read_u32(&self, od_index: u16, sub_index: u8) -> Result<u32> {
        if let Some(entry) = self.input_map.get(&(od_index, sub_index)) {
            // 直接从 IOmap 读取（零拷贝）
            let off = entry.byte_offset as usize;
            let sz  = entry.byte_size() as usize;
            if !self.in_ptr.is_null() && off + sz <= self.in_size as usize {
                let val = unsafe {
                    let ptr = self.in_ptr.add(off);
                    match sz {
                        1 => *ptr as u32,
                        2 => u16::from_le_bytes([*ptr, *ptr.add(1)]) as u32,
                        4 => u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)]),
                        _ => 0,
                    }
                };
                return Ok(val);
            }
        }
        // 回退到 SDO 读取
        Self::read_u32_sdo(self.master_index, self.slave_index, od_index, sub_index)
    }

    /// 通过 PDO 写入输出对象（若不在映射中则回退到 SDO）
    pub fn write_u32(&self, od_index: u16, sub_index: u8, value: u32) -> Result<()> {
        if let Some(entry) = self.output_map.get(&(od_index, sub_index)) {
            let off = entry.byte_offset as usize;
            let sz  = entry.byte_size() as usize;
            if !self.out_ptr.is_null() && off + sz <= self.out_size as usize {
                unsafe {
                    let ptr = self.out_ptr.add(off);
                    let bytes = value.to_le_bytes();
                    std::ptr::copy_nonoverlapping(bytes.as_ptr(), ptr, sz.min(4));
                }
                return Ok(());
            }
        }
        // 回退到 SDO 写入
        let bytes = value.to_le_bytes();
        let ok = unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, od_index, sub_index,
                              0, bytes.as_ptr(), 4)
        };
        if ok != 0 { Ok(()) } else { Err(DarraError::SdoWriteFailed { index: od_index, subindex: sub_index, abort_code: None }) }
    }

    /// 通过 PDO 读取 u16
    pub fn read_u16(&self, od_index: u16, sub_index: u8) -> Result<u16> {
        Ok(self.read_u32(od_index, sub_index)? as u16)
    }

    /// 通过 PDO 写入 u16
    pub fn write_u16(&self, od_index: u16, sub_index: u8, value: u16) -> Result<()> {
        if let Some(entry) = self.output_map.get(&(od_index, sub_index)) {
            let off = entry.byte_offset as usize;
            if !self.out_ptr.is_null() && off + 2 <= self.out_size as usize {
                unsafe {
                    let bytes = value.to_le_bytes();
                    std::ptr::copy_nonoverlapping(bytes.as_ptr(), self.out_ptr.add(off), 2);
                }
                return Ok(());
            }
        }
        let bytes = value.to_le_bytes();
        let ok = unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, od_index, sub_index,
                              0, bytes.as_ptr(), 2)
        };
        if ok != 0 { Ok(()) } else { Err(DarraError::SdoWriteFailed { index: od_index, subindex: sub_index, abort_code: None }) }
    }

    /// 获取 RxPDO（输出）映射条目列表
    pub fn output_entries(&self) -> impl Iterator<Item = &PdoMapEntry> {
        self.output_map.values()
    }

    /// 获取 TxPDO（输入）映射条目列表
    pub fn input_entries(&self) -> impl Iterator<Item = &PdoMapEntry> {
        self.input_map.values()
    }

    /// 检查指定对象是否已映射到 RxPDO
    pub fn is_output_mapped(&self, od_index: u16, sub_index: u8) -> bool {
        self.output_map.contains_key(&(od_index, sub_index))
    }

    /// 检查指定对象是否已映射到 TxPDO
    pub fn is_input_mapped(&self, od_index: u16, sub_index: u8) -> bool {
        self.input_map.contains_key(&(od_index, sub_index))
    }

    // ===================== 内部辅助 SDO 读取 =====================

    fn read_u8_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u8> {
        let mut size: c_int = 0;
        let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
        if ptr.is_null() || size < 1 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
        }
        let val = unsafe { *ptr };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(val)
    }

    fn read_u16_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u16> {
        let mut size: c_int = 0;
        let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
        if ptr.is_null() || size < 2 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
        }
        let val = unsafe { u16::from_le_bytes([*ptr, *ptr.add(1)]) };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(val)
    }

    fn read_u32_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u32> {
        let mut size: c_int = 0;
        let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
        if ptr.is_null() || size < 4 {
            if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
            return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
        }
        let val = unsafe {
            u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)])
        };
        unsafe { ffi::FreeMemory(ptr as *mut _) };
        Ok(val)
    }
}

impl std::fmt::Debug for Cia402PdoMap {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        f.debug_struct("Cia402PdoMap")
            .field("master_index", &self.master_index)
            .field("slave_index", &self.slave_index)
            .field("output_entries", &self.output_map.len())
            .field("input_entries", &self.input_map.len())
            .finish()
    }
}

// ===================== CiA 402 标准对象索引 =====================

/// 控制字 (RxPDO)
pub const OD_CONTROLWORD: u16 = 0x6040;
/// 状态字 (TxPDO)
pub const OD_STATUSWORD: u16 = 0x6041;
/// 操作模式设置
pub const OD_MODES_OF_OPERATION: u16 = 0x6060;
/// 操作模式显示
pub const OD_MODES_OF_OPERATION_DISPLAY: u16 = 0x6061;
/// 目标位置
pub const OD_TARGET_POSITION: u16 = 0x607A;
/// 实际位置
pub const OD_POSITION_ACTUAL: u16 = 0x6064;
/// 目标速度
pub const OD_TARGET_VELOCITY: u16 = 0x60FF;
/// 实际速度
pub const OD_VELOCITY_ACTUAL: u16 = 0x606C;
/// 目标转矩
pub const OD_TARGET_TORQUE: u16 = 0x6071;
/// 实际转矩
pub const OD_TORQUE_ACTUAL: u16 = 0x6077;
/// 最大轮廓速度
pub const OD_MAX_PROFILE_VELOCITY: u16 = 0x6080;
/// 轮廓速度
pub const OD_PROFILE_VELOCITY: u16 = 0x6081;
/// 轮廓加速度
pub const OD_PROFILE_ACCELERATION: u16 = 0x6083;
/// 轮廓减速度
pub const OD_PROFILE_DECELERATION: u16 = 0x6084;
/// 快速停止减速度
pub const OD_QUICK_STOP_DECELERATION: u16 = 0x6085;
/// 回零方法
pub const OD_HOMING_METHOD: u16 = 0x6098;
/// 回零速度
pub const OD_HOMING_SPEEDS: u16 = 0x6099;
/// 回零偏移
pub const OD_HOME_OFFSET: u16 = 0x607C;
/// 转矩偏移
pub const OD_TORQUE_OFFSET: u16 = 0x60B2;
/// 软件位置限制
pub const OD_SOFTWARE_POSITION_LIMIT: u16 = 0x607D;
/// 支持的驱动模式位掩码
pub const OD_SUPPORTED_DRIVE_MODES: u16 = 0x6502;
/// 极性
pub const OD_POLARITY: u16 = 0x607E;
/// 运动轮廓类型
pub const OD_MOTION_PROFILE_TYPE: u16 = 0x6086;
/// Touch Probe 功能控制
pub const OD_TOUCH_PROBE_FUNCTION: u16 = 0x60B8;
/// Touch Probe 1 状态
pub const OD_TOUCH_PROBE_STATUS: u16 = 0x60B9;
/// Touch Probe 1 正边沿位置
pub const OD_TOUCH_PROBE_POS_EDGE: u16 = 0x60BA;
/// Touch Probe 1 负边沿位置
pub const OD_TOUCH_PROBE_NEG_EDGE: u16 = 0x60BB;
/// 回零加速度
pub const OD_HOMING_ACCELERATION: u16 = 0x609A;
/// 数字输入
pub const OD_DIGITAL_INPUTS: u16 = 0x60FD;
/// 数字输出
pub const OD_DIGITAL_OUTPUTS: u16 = 0x60FE;
/// 最大转矩 (千分之额定转矩)
pub const OD_MAX_TORQUE: u16 = 0x6072;
/// 电机额定转矩 (单位 mNm)
pub const OD_MOTOR_RATED_TORQUE: u16 = 0x6076;
/// 位置偏移 (CSP 模式附加偏移)
pub const OD_POSITION_OFFSET: u16 = 0x60B0;
/// 速度偏移 (CSV 模式附加偏移)
pub const OD_VELOCITY_OFFSET: u16 = 0x60B1;
/// 插补时间周期 (子索引1=值, 子索引2=指数)
pub const OD_INTERPOLATION_TIME_PERIOD: u16 = 0x60C2;
/// 快速停止选项码
pub const OD_QUICK_STOP_OPTION_CODE: u16 = 0x605A;
/// 正方向力矩限制 (千分之额定转矩)
pub const OD_POSITIVE_TORQUE_LIMIT: u16 = 0x60E0;
/// 负方向力矩限制 (千分之额定转矩)
pub const OD_NEGATIVE_TORQUE_LIMIT: u16 = 0x60E1;
/// 支持的回零方法列表
pub const OD_SUPPORTED_HOMING_METHODS: u16 = 0x60E3;
/// TxPDO 数据无效标志
pub const OD_TXPDO_DATA_INVALID: u16 = 0x603E;
/// 同步设置
pub const OD_SYNCHRONIZATION_SETTINGS: u16 = 0x60D9;
/// 驱动同步状态
pub const OD_DRIVE_SYNC_STATUS: u16 = 0x60DA;
/// 错误码
pub const OD_ERROR_CODE: u16 = 0x603F;

// ===================== 控制字常量 =====================

/// Shutdown 命令 (-> Ready to switch on)
pub const CW_SHUTDOWN: u16 = 0x06;
/// Switch On 命令 (-> Switched on)
pub const CW_SWITCH_ON: u16 = 0x07;
/// Enable Operation 命令 (-> Operation enabled)
pub const CW_ENABLE_OPERATION: u16 = 0x0F;
/// Disable Voltage 命令 (-> Switch on disabled)
pub const CW_DISABLE_VOLTAGE: u16 = 0x00;
/// Quick Stop 命令 (-> Quick stop active)
pub const CW_QUICK_STOP: u16 = 0x02;
/// Fault Reset 命令 (上升沿清除故障)
pub const CW_FAULT_RESET: u16 = 0x80;
/// Halt 位 (Bit 8, 暂停运动但不禁用)
pub const CW_HALT: u16 = 0x0100;

// ===================== 状态字位定义 =====================

/// Bit 0: Ready to switch on
pub const SW_READY_TO_SWITCH_ON: u16 = 0x0001;
/// Bit 1: Switched on
pub const SW_SWITCHED_ON: u16 = 0x0002;
/// Bit 2: Operation enabled
pub const SW_OPERATION_ENABLED: u16 = 0x0004;
/// Bit 3: Fault
pub const SW_FAULT: u16 = 0x0008;
/// Bit 4: Voltage enabled
pub const SW_VOLTAGE_ENABLED: u16 = 0x0010;
/// Bit 5: Quick stop (0=activated)
pub const SW_QUICK_STOP: u16 = 0x0020;
/// Bit 6: Switch on disabled
pub const SW_SWITCH_ON_DISABLED: u16 = 0x0040;
/// Bit 7: Warning
pub const SW_WARNING: u16 = 0x0080;
/// Bit 9: Remote
pub const SW_REMOTE: u16 = 0x0200;
/// Bit 10: Target reached
pub const SW_TARGET_REACHED: u16 = 0x0400;
/// Bit 11: Internal limit active
pub const SW_INTERNAL_LIMIT: u16 = 0x0800;
/// Bit 12: Set-point acknowledge (PP) / Homing attained (HM)
pub const SW_OP_MODE_SPECIFIC_1: u16 = 0x1000;
/// Bit 13: Following error (CSP) / Homing error (HM)
pub const SW_OP_MODE_SPECIFIC_2: u16 = 0x2000;

// ===================== CiA 402 驱动器状态 =====================

/// CiA 402 驱动器状态机状态
#[repr(u8)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum StateCiA402 {
    /// 初始化中
    NotReadyToSwitchOn = 0,
    /// 驱动禁用
    SwitchOnDisabled = 1,
    /// 准备就绪
    ReadyToSwitchOn = 2,
    /// 已开启
    SwitchedOn = 3,
    /// 运行使能
    OperationEnabled = 4,
    /// 快速停止
    QuickStopActive = 5,
    /// 故障反应中
    FaultReactionActive = 6,
    /// 故障
    Fault = 7,
    /// 未知状态
    Unknown = 99,
}

impl StateCiA402 {
    /// 从状态字解析驱动器状态
    pub fn from_statusword(sw: u16) -> Self {
        if (sw & SW_FAULT) != 0 {
            if (sw & 0x0F) == 0x0F {
                return Self::FaultReactionActive;
            }
            return Self::Fault;
        }
        let mask = sw & 0x006F; // Bit 0,1,2,3,5,6
        match mask {
            0x0000 => Self::NotReadyToSwitchOn,
            0x0040 => Self::SwitchOnDisabled,
            0x0021 => Self::ReadyToSwitchOn,
            0x0023 => Self::SwitchedOn,
            0x0027 => Self::OperationEnabled,
            0x0007 => Self::QuickStopActive,
            _ => Self::Unknown,
        }
    }

    /// 获取状态描述
    pub fn description(&self) -> &'static str {
        match self {
            Self::NotReadyToSwitchOn => "初始化中",
            Self::SwitchOnDisabled => "驱动禁用",
            Self::ReadyToSwitchOn => "准备就绪",
            Self::SwitchedOn => "已开启",
            Self::OperationEnabled => "运行使能",
            Self::QuickStopActive => "快速停止",
            Self::FaultReactionActive => "故障反应中",
            Self::Fault => "故障",
            Self::Unknown => "未知",
        }
    }
}

impl std::fmt::Display for StateCiA402 {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{}", self.description())
    }
}

// ===================== CiA 402 操作模式 =====================

/// CiA 402 操作模式
#[repr(i8)]
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ModeCiA402 {
    /// 轮廓位置模式 (Profile Position)
    PP = 1,
    /// 速度模式 (Velocity)
    VL = 2,
    /// 轮廓速度模式 (Profile Velocity)
    PV = 3,
    /// 轮廓转矩模式 (Profile Torque)
    PT = 4,
    /// 回零模式 (Homing)
    HM = 6,
    /// 插补位置模式 (Interpolated Position)
    IP = 7,
    /// 周期同步位置模式 (Cyclic Synchronous Position)
    CSP = 8,
    /// 周期同步速度模式 (Cyclic Synchronous Velocity)
    CSV = 9,
    /// 周期同步转矩模式 (Cyclic Synchronous Torque)
    CST = 10,
    /// 周期同步转矩加速度模式
    CSTCA = 11,
}

impl ModeCiA402 {
    /// 获取模式在 SupportedDriveModes (0x6502) 中的位索引
    pub fn supported_bit(&self) -> Option<u32> {
        match self {
            Self::PP => Some(0),
            Self::VL => Some(1),
            Self::PV => Some(2),
            Self::PT => Some(3),
            Self::HM => Some(5),
            Self::IP => Some(6),
            Self::CSP => Some(7),
            Self::CSV => Some(8),
            Self::CST => Some(9),
            Self::CSTCA => Some(10),
        }
    }

    /// 获取模式描述
    pub fn description(&self) -> &'static str {
        match self {
            Self::PP => "轮廓位置 (PP)",
            Self::VL => "速度 (VL)",
            Self::PV => "轮廓速度 (PV)",
            Self::PT => "轮廓转矩 (PT)",
            Self::HM => "回零 (HM)",
            Self::IP => "插补位置 (IP)",
            Self::CSP => "周期同步位置 (CSP)",
            Self::CSV => "周期同步速度 (CSV)",
            Self::CST => "周期同步转矩 (CST)",
            Self::CSTCA => "周期同步转矩加速度 (CSTCA)",
        }
    }
}

impl std::fmt::Display for ModeCiA402 {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        write!(f, "{}", self.description())
    }
}

// ===================== CiA 402 驱动器实例 =====================

/// CiA 402 驱动器协议实例
///
/// 封装 CiA 402 状态机管理、使能流程、操作模式切换、运动参数等功能。
/// 通过 PDO 映射表优先访问, SDO 回退。
///
/// # 使用示例
/// ```no_run
/// let pdo_map = Cia402PdoMap::scan(0, 1).unwrap();
/// let drv = CiA402Instance::new(0, 1, pdo_map);
///
/// // 非阻塞使能 (每个 PDO 周期调用)
/// if drv.enable() == StateCiA402::OperationEnabled {
///     drv.set_target_position(10000);
///     drv.new_setpoint(10000, false);
/// }
/// ```
pub struct CiA402Instance {
    /// 主站索引
    master_index: u16,
    /// 从站索引
    slave_index: u16,
    /// PDO 映射表
    pdo_map: Cia402PdoMap,
}

impl CiA402Instance {
    /// 创建 CiA 402 驱动器实例
    pub fn new(master_index: u16, slave_index: u16, pdo_map: Cia402PdoMap) -> Self {
        Self { master_index, slave_index, pdo_map }
    }

    /// 获取从站索引
    pub fn slave_index(&self) -> u16 {
        self.slave_index
    }

    /// 获取 PDO 映射表引用
    pub fn pdo_map(&self) -> &Cia402PdoMap {
        &self.pdo_map
    }

    /// 初始化 PDO 偏移缓存
    ///
    /// 重新扫描 PDO 映射 (0x1C12/0x1C13), 更新内部映射表。
    /// 应在 SafeOp/OP 状态下调用一次。
    pub fn initialize_pdo_offsets(&mut self) -> Result<()> {
        self.pdo_map = Cia402PdoMap::scan(self.master_index, self.slave_index)?;
        Ok(())
    }

    /// 重置 PDO 偏移缓存
    ///
    /// 清空映射表, 后续访问将回退到 SDO。
    /// 状态降级时调用, 下次进入 SafeOp+ 应重新 initialize_pdo_offsets。
    pub fn reset_pdo_offsets(&mut self) {
        self.pdo_map.output_map.clear();
        self.pdo_map.input_map.clear();
    }

    // ===================== SDO 辅助读写 =====================

    fn sdo_read_u16(&self, index: u16, sub: u8) -> u16 {
        self.pdo_map.read_u16(index, sub).unwrap_or(0)
    }

    fn sdo_read_i32(&self, index: u16, sub: u8) -> i32 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0) as i32
    }

    fn sdo_read_u32(&self, index: u16, sub: u8) -> u32 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0)
    }

    fn sdo_read_u8(&self, index: u16, sub: u8) -> u8 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0) as u8
    }

    fn sdo_read_i8(&self, index: u16, sub: u8) -> i8 {
        self.pdo_map.read_u32(index, sub).unwrap_or(0) as i8
    }

    fn sdo_read_i16(&self, index: u16, sub: u8) -> i16 {
        self.pdo_map.read_u16(index, sub).unwrap_or(0) as i16
    }

    fn sdo_write_u16(&self, index: u16, sub: u8, val: u16) {
        let _ = self.pdo_map.write_u16(index, sub, val);
    }

    fn sdo_write_u32(&self, index: u16, sub: u8, val: u32) {
        let _ = self.pdo_map.write_u32(index, sub, val);
    }

    fn sdo_write_i32(&self, index: u16, sub: u8, val: i32) {
        let _ = self.pdo_map.write_u32(index, sub, val as u32);
    }

    fn sdo_write_u8(&self, index: u16, sub: u8, val: u8) {
        // SDO 写 1 字节
        let bytes = [val];
        unsafe {
            ffi::SDOwrite_raw(self.master_index, self.slave_index, index, sub,
                              0, bytes.as_ptr(), 1);
        }
    }

    fn sdo_write_i8(&self, index: u16, sub: u8, val: i8) {
        self.sdo_write_u8(index, sub, val as u8);
    }

    fn sdo_write_i16(&self, index: u16, sub: u8, val: i16) {
        self.sdo_write_u16(index, sub, val as u16);
    }

    // ===================== 状态读取 =====================

    /// 读取当前状态字 (PDO 优先)
    pub fn statusword(&self) -> u16 {
        self.sdo_read_u16(OD_STATUSWORD, 0)
    }

    /// 读取或写入控制字
    pub fn controlword(&self) -> u16 {
        self.sdo_read_u16(OD_CONTROLWORD, 0)
    }

    /// 写入控制字
    pub fn set_controlword(&self, value: u16) {
        self.sdo_write_u16(OD_CONTROLWORD, 0, value);
    }

    /// 解析当前驱动器状态
    pub fn state_drive(&self) -> StateCiA402 {
        StateCiA402::from_statusword(self.statusword())
    }

    /// 目标已到达 (Bit 10)
    pub fn target_reached(&self) -> bool {
        (self.statusword() & SW_TARGET_REACHED) != 0
    }

    /// 是否有故障 (Bit 3)
    pub fn has_fault(&self) -> bool {
        (self.statusword() & SW_FAULT) != 0
    }

    /// 是否有警告 (Bit 7)
    pub fn has_warning(&self) -> bool {
        (self.statusword() & SW_WARNING) != 0
    }

    /// 远程模式 (Bit 9)
    pub fn is_remote(&self) -> bool {
        (self.statusword() & SW_REMOTE) != 0
    }

    // ===================== 操作模式 =====================

    /// 读取当前操作模式 (0x6061)
    pub fn operation_mode(&self) -> i8 {
        self.sdo_read_i8(OD_MODES_OF_OPERATION_DISPLAY, 0)
    }

    /// 设置操作模式 (0x6060)
    pub fn set_operation_mode(&self, mode: ModeCiA402) {
        self.sdo_write_i8(OD_MODES_OF_OPERATION, 0, mode as i8);
    }

    // ===================== 使能/停止 (非阻塞) =====================

    /// 使能驱动器: 状态机推进逻辑由 native DLL (CiA402_Enable) 承载,
    /// 内部按 CiA 402-2 完成 SwitchOnDisabled→ReadyToSwitchOn→SwitchedOn→OperationEnabled
    /// 全套时序、Fault 复位、QuickStop 恢复 (含 0x605A 选项码判断), 不在 Rust 层暴露状态机 IP.
    ///
    /// # 参数
    /// - `max_retries`: 最大重试次数
    ///
    /// # 返回值
    /// - `true` = 已使能到 OperationEnabled
    /// - `false` = 失败 / 超时
    pub fn enable(&self, max_retries: i32) -> bool {
        // native CiA402_Enable 返回 BOOL: 非 0 = 成功, 0 = 失败
        unsafe { ffi::CiA402_Enable(self.master_index, self.slave_index, max_retries) != 0 }
    }

    /// 使能驱动器 (默认重试 10 次)
    pub fn enable_default(&self) -> bool {
        self.enable(10)
    }

    /// 禁用运行 (非阻塞, OperationEnabled -> SwitchedOn)
    pub fn disable_operation(&self) {
        self.set_controlword(CW_SWITCH_ON);
    }

    /// 禁用伺服 (非阻塞, -> SwitchOnDisabled), 完全断电
    pub fn disable(&self) {
        self.set_controlword(CW_DISABLE_VOLTAGE);
    }

    /// 快速停止 (非阻塞, -> QuickStopActive)
    pub fn quick_stop(&self) {
        self.set_controlword(CW_QUICK_STOP);
    }

    /// 清除故障 (发送 Fault Reset 上升沿)
    ///
    /// 按 ETG.6010 §5.2 要求, Bit7 必须产生 0->1 上升沿才能触发故障复位。
    /// 先清除 Bit7 确保低电平, 再置位 Bit7 产生上升沿。
    /// 警告: 包含 1ms sleep, PDO 回调中请分两个周期分别写 0x00 和 CW_FAULT_RESET。
    pub fn fault_reset(&self) {
        // 先写 0x00 确保 Bit7 为 0 (低电平)
        self.set_controlword(0x00);
        // 插入短暂延时确保两次写入不在同一 PDO 周期
        std::thread::sleep(std::time::Duration::from_millis(1));
        // 再写 0x80 置位 Bit7 产生上升沿
        self.set_controlword(CW_FAULT_RESET);
    }

    // ===================== 运动参数 =====================

    /// 实际位置 (0x6064)
    pub fn position_actual(&self) -> i32 {
        self.sdo_read_i32(OD_POSITION_ACTUAL, 0)
    }

    /// 实际速度 (0x606C)
    pub fn velocity_actual(&self) -> i32 {
        self.sdo_read_i32(OD_VELOCITY_ACTUAL, 0)
    }

    /// 实际转矩 (0x6077), 单位千分之额定转矩
    pub fn torque_actual(&self) -> i16 {
        self.sdo_read_i16(OD_TORQUE_ACTUAL, 0)
    }

    // ===================== 目标设置 =====================

    /// 设置目标位置 (0x607A)
    pub fn set_target_position(&self, value: i32) {
        self.sdo_write_i32(OD_TARGET_POSITION, 0, value);
    }

    /// 获取目标位置
    pub fn target_position(&self) -> i32 {
        self.sdo_read_i32(OD_TARGET_POSITION, 0)
    }

    /// 设置目标速度 (0x60FF)
    pub fn set_target_velocity(&self, value: i32) {
        self.sdo_write_i32(OD_TARGET_VELOCITY, 0, value);
    }

    /// 获取目标速度
    pub fn target_velocity(&self) -> i32 {
        self.sdo_read_i32(OD_TARGET_VELOCITY, 0)
    }

    /// 设置目标转矩 (0x6071), 单位千分之额定转矩
    pub fn set_target_torque(&self, value: i16) {
        self.sdo_write_i16(OD_TARGET_TORQUE, 0, value);
    }

    /// 获取目标转矩
    pub fn target_torque(&self) -> i16 {
        self.sdo_read_i16(OD_TARGET_TORQUE, 0)
    }

    // ===================== 轮廓参数 =====================

    /// 轮廓速度 (0x6081)
    pub fn profile_velocity(&self) -> u32 {
        self.sdo_read_u32(OD_PROFILE_VELOCITY, 0)
    }
    /// 设置轮廓速度
    pub fn set_profile_velocity(&self, value: u32) {
        self.sdo_write_u32(OD_PROFILE_VELOCITY, 0, value);
    }

    /// 轮廓加速度 (0x6083)
    pub fn profile_acceleration(&self) -> u32 {
        self.sdo_read_u32(OD_PROFILE_ACCELERATION, 0)
    }
    /// 设置轮廓加速度
    pub fn set_profile_acceleration(&self, value: u32) {
        self.sdo_write_u32(OD_PROFILE_ACCELERATION, 0, value);
    }

    /// 轮廓减速度 (0x6084)
    pub fn profile_deceleration(&self) -> u32 {
        self.sdo_read_u32(OD_PROFILE_DECELERATION, 0)
    }
    /// 设置轮廓减速度
    pub fn set_profile_deceleration(&self, value: u32) {
        self.sdo_write_u32(OD_PROFILE_DECELERATION, 0, value);
    }

    /// 快速停止减速度 (0x6085)
    pub fn quick_stop_deceleration(&self) -> u32 {
        self.sdo_read_u32(OD_QUICK_STOP_DECELERATION, 0)
    }
    /// 设置快速停止减速度
    pub fn set_quick_stop_deceleration(&self, value: u32) {
        self.sdo_write_u32(OD_QUICK_STOP_DECELERATION, 0, value);
    }

    /// 极性 (0x607E)
    pub fn polarity(&self) -> u8 {
        self.sdo_read_u8(OD_POLARITY, 0)
    }
    /// 设置极性
    pub fn set_polarity(&self, value: u8) {
        self.sdo_write_u8(OD_POLARITY, 0, value);
    }

    /// 运动轮廓类型 (0x6086)
    pub fn motion_profile_type(&self) -> i16 {
        self.sdo_read_i16(OD_MOTION_PROFILE_TYPE, 0)
    }
    /// 设置运动轮廓类型
    pub fn set_motion_profile_type(&self, value: i16) {
        self.sdo_write_i16(OD_MOTION_PROFILE_TYPE, 0, value);
    }

    /// 软件位置最小限制 (0x607D:01)
    pub fn software_position_limit_min(&self) -> i32 {
        self.sdo_read_i32(OD_SOFTWARE_POSITION_LIMIT, 1)
    }
    /// 设置软件位置最小限制
    pub fn set_software_position_limit_min(&self, value: i32) {
        self.sdo_write_i32(OD_SOFTWARE_POSITION_LIMIT, 1, value);
    }

    /// 软件位置最大限制 (0x607D:02)
    pub fn software_position_limit_max(&self) -> i32 {
        self.sdo_read_i32(OD_SOFTWARE_POSITION_LIMIT, 2)
    }
    /// 设置软件位置最大限制
    pub fn set_software_position_limit_max(&self, value: i32) {
        self.sdo_write_i32(OD_SOFTWARE_POSITION_LIMIT, 2, value);
    }

    // ===================== 回零参数 =====================

    /// 回零方法 (0x6098)
    pub fn homing_method(&self) -> i8 {
        self.sdo_read_i8(OD_HOMING_METHOD, 0)
    }
    /// 设置回零方法
    pub fn set_homing_method(&self, value: i8) {
        self.sdo_write_i8(OD_HOMING_METHOD, 0, value);
    }

    /// 回零偏移 (0x607C)
    pub fn home_offset(&self) -> i32 {
        self.sdo_read_i32(OD_HOME_OFFSET, 0)
    }
    /// 设置回零偏移
    pub fn set_home_offset(&self, value: i32) {
        self.sdo_write_i32(OD_HOME_OFFSET, 0, value);
    }

    /// 回零搜索速度 (0x6099:01)
    pub fn homing_speed_search(&self) -> u32 {
        self.sdo_read_u32(OD_HOMING_SPEEDS, 1)
    }
    /// 设置回零搜索速度
    pub fn set_homing_speed_search(&self, value: u32) {
        self.sdo_write_u32(OD_HOMING_SPEEDS, 1, value);
    }

    /// 回零零脉冲速度 (0x6099:02)
    pub fn homing_speed_zero(&self) -> u32 {
        self.sdo_read_u32(OD_HOMING_SPEEDS, 2)
    }
    /// 设置回零零脉冲速度
    pub fn set_homing_speed_zero(&self, value: u32) {
        self.sdo_write_u32(OD_HOMING_SPEEDS, 2, value);
    }

    /// 回零加速度 (0x609A)
    pub fn homing_acceleration(&self) -> u32 {
        self.sdo_read_u32(OD_HOMING_ACCELERATION, 0)
    }
    /// 设置回零加速度
    pub fn set_homing_acceleration(&self, value: u32) {
        self.sdo_write_u32(OD_HOMING_ACCELERATION, 0, value);
    }

    // ===================== Touch Probe =====================

    /// 配置 Touch Probe 功能 (0x60B8)
    pub fn configure_touch_probe(&self, function: u16) {
        self.sdo_write_u16(OD_TOUCH_PROBE_FUNCTION, 0, function);
    }

    /// Touch Probe 状态 (0x60B9)
    pub fn touch_probe_status(&self) -> u16 {
        self.sdo_read_u16(OD_TOUCH_PROBE_STATUS, 0)
    }

    /// Touch Probe 1 正边沿捕获位置 (0x60BA)
    pub fn touch_probe_positive_edge(&self) -> i32 {
        self.sdo_read_i32(OD_TOUCH_PROBE_POS_EDGE, 0)
    }

    /// Touch Probe 1 负边沿捕获位置 (0x60BB)
    pub fn touch_probe_negative_edge(&self) -> i32 {
        self.sdo_read_i32(OD_TOUCH_PROBE_NEG_EDGE, 0)
    }

    // ===================== 数字 IO =====================

    /// 数字输入状态 (0x60FD, 只读)
    pub fn digital_inputs(&self) -> u32 {
        self.sdo_read_u32(OD_DIGITAL_INPUTS, 0)
    }

    /// 数字输出控制 (0x60FE:01)
    pub fn digital_outputs(&self) -> u32 {
        self.sdo_read_u32(OD_DIGITAL_OUTPUTS, 1)
    }
    /// 设置数字输出
    pub fn set_digital_outputs(&self, value: u32) {
        self.sdo_write_u32(OD_DIGITAL_OUTPUTS, 1, value);
    }

    // ===================== 驱动器信息 =====================

    /// 支持的驱动模式位掩码 (0x6502, 只读)
    pub fn supported_drive_modes(&self) -> u32 {
        self.sdo_read_u32(OD_SUPPORTED_DRIVE_MODES, 0)
    }

    /// 检查驱动器是否支持指定操作模式
    pub fn is_mode_supported(&self, mode: ModeCiA402) -> bool {
        let supported = self.supported_drive_modes();
        if let Some(bit) = mode.supported_bit() {
            (supported & (1u32 << bit)) != 0
        } else {
            false
        }
    }

    // ===================== 扩展运动参数 =====================

    /// 最大转矩 (0x6072, 千分之额定转矩)
    pub fn max_torque(&self) -> u16 {
        self.sdo_read_u16(OD_MAX_TORQUE, 0)
    }
    /// 设置最大转矩
    pub fn set_max_torque(&self, value: u16) {
        self.sdo_write_u16(OD_MAX_TORQUE, 0, value);
    }

    /// 电机额定转矩 (0x6076, 单位 mNm)
    pub fn motor_rated_torque(&self) -> u32 {
        self.sdo_read_u32(OD_MOTOR_RATED_TORQUE, 0)
    }
    /// 设置电机额定转矩
    pub fn set_motor_rated_torque(&self, value: u32) {
        self.sdo_write_u32(OD_MOTOR_RATED_TORQUE, 0, value);
    }

    /// 位置偏移 (0x60B0, CSP 模式下叠加到目标位置)
    pub fn position_offset(&self) -> i32 {
        self.sdo_read_i32(OD_POSITION_OFFSET, 0)
    }
    /// 设置位置偏移
    pub fn set_position_offset(&self, value: i32) {
        self.sdo_write_i32(OD_POSITION_OFFSET, 0, value);
    }

    /// 速度偏移 (0x60B1, CSV 模式下叠加到目标速度)
    pub fn velocity_offset(&self) -> i32 {
        self.sdo_read_i32(OD_VELOCITY_OFFSET, 0)
    }
    /// 设置速度偏移
    pub fn set_velocity_offset(&self, value: i32) {
        self.sdo_write_i32(OD_VELOCITY_OFFSET, 0, value);
    }

    /// 转矩偏移 (0x60B2, CST 模式下叠加到目标转矩)
    pub fn torque_offset(&self) -> i16 {
        self.sdo_read_i16(OD_TORQUE_OFFSET, 0)
    }
    /// 设置转矩偏移
    pub fn set_torque_offset(&self, value: i16) {
        self.sdo_write_i16(OD_TORQUE_OFFSET, 0, value);
    }

    /// 插补时间周期值 (0x60C2:01)
    pub fn interpolation_time_period_value(&self) -> u8 {
        self.sdo_read_u8(OD_INTERPOLATION_TIME_PERIOD, 1)
    }
    /// 设置插补时间周期值
    pub fn set_interpolation_time_period_value(&self, value: u8) {
        self.sdo_write_u8(OD_INTERPOLATION_TIME_PERIOD, 1, value);
    }

    /// 插补时间周期指数 (0x60C2:02, 实际周期 = Value * 10^Index 秒)
    pub fn interpolation_time_period_index(&self) -> i8 {
        self.sdo_read_i8(OD_INTERPOLATION_TIME_PERIOD, 2)
    }
    /// 设置插补时间周期指数
    pub fn set_interpolation_time_period_index(&self, value: i8) {
        self.sdo_write_i8(OD_INTERPOLATION_TIME_PERIOD, 2, value);
    }

    /// 快速停止选项码 (0x605A, 决定快速停止后的行为)
    pub fn quick_stop_option_code(&self) -> i16 {
        self.sdo_read_i16(OD_QUICK_STOP_OPTION_CODE, 0)
    }
    /// 设置快速停止选项码
    pub fn set_quick_stop_option_code(&self, value: i16) {
        self.sdo_write_i16(OD_QUICK_STOP_OPTION_CODE, 0, value);
    }

    /// 正方向力矩限制 (0x60E0, 千分之额定转矩)
    pub fn positive_torque_limit(&self) -> u16 {
        self.sdo_read_u16(OD_POSITIVE_TORQUE_LIMIT, 0)
    }
    /// 设置正方向力矩限制
    pub fn set_positive_torque_limit(&self, value: u16) {
        self.sdo_write_u16(OD_POSITIVE_TORQUE_LIMIT, 0, value);
    }

    /// 负方向力矩限制 (0x60E1, 千分之额定转矩)
    pub fn negative_torque_limit(&self) -> u16 {
        self.sdo_read_u16(OD_NEGATIVE_TORQUE_LIMIT, 0)
    }
    /// 设置负方向力矩限制
    pub fn set_negative_torque_limit(&self, value: u16) {
        self.sdo_write_u16(OD_NEGATIVE_TORQUE_LIMIT, 0, value);
    }

    /// 支持的回零方法列表 (0x60E3, 子索引0=数量, 子索引1..N=方法编号)
    pub fn supported_homing_methods(&self) -> Vec<i8> {
        let count = self.sdo_read_u8(OD_SUPPORTED_HOMING_METHODS, 0);
        let mut methods = Vec::new();
        for i in 1..=count {
            methods.push(self.sdo_read_i8(OD_SUPPORTED_HOMING_METHODS, i));
        }
        methods
    }

    /// TxPDO 数据是否无效 (0x603E:00, 非零表示数据无效)
    pub fn txpdo_data_invalid(&self) -> bool {
        self.sdo_read_u8(OD_TXPDO_DATA_INVALID, 0) != 0
    }

    /// 同步设置 (0x60D9:01, 同步使能位掩码)
    pub fn synchronization_settings(&self) -> u16 {
        self.sdo_read_u16(OD_SYNCHRONIZATION_SETTINGS, 1)
    }
    /// 设置同步设置
    pub fn set_synchronization_settings(&self, value: u16) {
        self.sdo_write_u16(OD_SYNCHRONIZATION_SETTINGS, 1, value);
    }

    /// 驱动同步状态 (0x60DA:00, 只读, 指示驱动器是否已同步到主站时钟)
    pub fn drive_sync_status(&self) -> u16 {
        self.sdo_read_u16(OD_DRIVE_SYNC_STATUS, 0)
    }

    // ===================== 快速运动控制 =====================

    /// PP 模式: 发送新定位命令
    ///
    /// - `position`: 目标位置
    /// - `relative`: true = 相对定位, false = 绝对定位
    pub fn new_setpoint(&self, position: i32, relative: bool) {
        self.set_target_position(position);
        let mut cw = CW_ENABLE_OPERATION | 0x10; // Bit 4 = New Setpoint
        if relative { cw |= 0x40; } // Bit 6 = Relative
        self.set_controlword(cw);
    }

    /// PP 模式: 清除 NewSetpoint 标志 (Bit4=0)
    pub fn clear_new_setpoint(&self) {
        self.set_controlword(CW_ENABLE_OPERATION);
    }

    /// HM 模式: 启动回零
    pub fn start_homing(&self) {
        self.set_controlword(CW_ENABLE_OPERATION | 0x10); // Bit 4 = Homing Start
    }

    /// 回零完成 (Bit 12)
    pub fn homing_attained(&self) -> bool {
        (self.statusword() & SW_OP_MODE_SPECIFIC_1) != 0
    }

    /// 回零错误 (Bit 13)
    pub fn homing_error(&self) -> bool {
        (self.statusword() & SW_OP_MODE_SPECIFIC_2) != 0
    }

    // ===================== 轮廓参数验证 =====================

    /// 检查驱动器轮廓参数是否满足 PP/PV 模式运动需求
    ///
    /// 仅在 PP(1) 或 PV(3) 模式下检测, 返回警告消息列表 (空 = 通过)。
    pub fn validate_profile_parameters(&self, configured_mode: Option<i8>) -> Vec<String> {
        let mut warnings = Vec::new();

        let mode = configured_mode.unwrap_or_else(|| self.sdo_read_i8(OD_MODES_OF_OPERATION, 0));

        // 仅 PP(1) 和 PV(3) 依赖轨迹生成器
        if mode != 1 && mode != 3 {
            return warnings;
        }

        let mode_name = if mode == 1 { "PP" } else { "PV" };
        let checks: [(u16, &str); 3] = [
            (OD_MAX_PROFILE_VELOCITY, "Max Profile Velocity (0x6080)"),
            (OD_PROFILE_ACCELERATION, "Profile Acceleration (0x6083)"),
            (OD_PROFILE_DECELERATION, "Profile Deceleration (0x6084)"),
        ];

        for (index, name) in &checks {
            let val = self.sdo_read_u32(*index, 0);
            if val == 0 {
                warnings.push(format!(
                    "{} 模式: {} = 0, 驱动器无法生成运动曲线, 请在 Startup 中配置此参数",
                    mode_name, name
                ));
            }
        }

        warnings
    }
}

impl std::fmt::Display for CiA402Instance {
    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
        let sw = self.statusword();
        let state = StateCiA402::from_statusword(sw);
        let mode = self.operation_mode();
        write!(f, "从站 {}: CiA402 [{}, 模式={}, SW=0x{:04X}]",
            self.slave_index, state, crate::statics::print::cia402_mode_description(mode), sw)
    }
}