use std::collections::HashMap;
use std::sync::{Arc, Mutex};
use std::time::SystemTime;
use crate::data::error::{DarraError, Result};
use crate::utils::ffi::{self, FsoeConfig, SafeMdpConfig, FsoeStatus, FsoeState, FsoeError};
pub struct FsoeCrc16;
impl FsoeCrc16 {
const TABLE: [u16; 256] = Self::build_table();
const fn build_table() -> [u16; 256] {
let mut table = [0u16; 256];
let mut i = 0usize;
while i < 256 {
let mut crc = (i as u16) << 8;
let mut j = 0;
while j < 8 {
if (crc & 0x8000) != 0 {
crc = (crc << 1) ^ 0x1021;
} else {
crc <<= 1;
}
j += 1;
}
table[i] = crc;
i += 1;
}
table
}
pub fn compute(data: &[u8]) -> u16 {
let mut crc: u16 = 0xFFFF;
for &byte in data {
let idx = (((crc >> 8) as u8) ^ byte) as usize;
crc = (crc << 8) ^ Self::TABLE[idx];
}
crc
}
pub fn update(crc: u16, data: &[u8]) -> u16 {
let mut crc = crc;
for &byte in data {
let idx = (((crc >> 8) as u8) ^ byte) as usize;
crc = (crc << 8) ^ Self::TABLE[idx];
}
crc
}
pub fn verify(data: &[u8]) -> bool {
if data.len() < 2 {
return false;
}
let payload = &data[..data.len() - 2];
let expected = u16::from_le_bytes([data[data.len() - 2], data[data.len() - 1]]);
Self::compute(payload) == expected
}
pub fn append(data: &[u8]) -> Vec<u8> {
let crc = Self::compute(data);
let mut out = data.to_vec();
out.extend_from_slice(&crc.to_le_bytes());
out
}
}
#[derive(Debug, Clone)]
pub struct FsoeConnectionInfo {
pub slave_index: u16,
pub config: FsoeConfig,
pub last_status: Option<FsoeStatus>,
}
pub struct SafetyManager {
master_index: u16,
connections: HashMap<u16, FsoeConnectionInfo>,
}
impl SafetyManager {
pub fn new(master_index: u16) -> Self {
Self {
master_index,
connections: HashMap::new(),
}
}
pub fn add_connection(&mut self, slave_index: u16, config: FsoeConfig) -> Result<()> {
let mut cfg = config;
let ret = unsafe {
ffi::FSoEInitConnection(self.master_index, slave_index, &mut cfg)
};
if ret == 0 {
return Err(DarraError::Other(format!(
"FSoE 连接初始化失败 (从站: {})", slave_index
)));
}
self.connections.insert(slave_index, FsoeConnectionInfo {
slave_index,
config: cfg,
last_status: None,
});
Ok(())
}
pub fn remove_connection(&mut self, slave_index: u16) -> Result<()> {
let ret = unsafe {
ffi::FSoECloseConnection(self.master_index, slave_index)
};
self.connections.remove(&slave_index);
if ret == 0 {
Err(DarraError::Other(format!(
"FSoE 连接关闭失败 (从站: {})", slave_index
)))
} else {
Ok(())
}
}
pub fn refresh_all_status(&mut self) -> HashMap<u16, FsoeStatus> {
let mut result = HashMap::new();
let keys: Vec<u16> = self.connections.keys().copied().collect();
for slave in keys {
let mut status = FsoeStatus {
state: FsoeState::Reset,
last_error: FsoeError::None,
error_count: 0,
frames_sent: 0,
frames_received: 0,
crc_errors: 0,
watchdog_errors: 0,
watchdog_expired: 0,
in_failsafe: 0,
toggle_bit: 0,
initialized: 0,
};
let ret = unsafe {
ffi::FSoEGetStatus(self.master_index, slave, &mut status)
};
if ret != 0 {
if let Some(info) = self.connections.get_mut(&slave) {
info.last_status = Some(status);
}
result.insert(slave, status);
}
}
result
}
pub fn all_in_data(&self) -> HashMap<u16, Vec<u8>> {
let mut result = HashMap::new();
for (&slave, info) in &self.connections {
let size = info.config.safe_input_size as usize;
if size == 0 { continue; }
let mut buf = vec![0u8; size];
let mut actual_size = size as u32;
let ret = unsafe {
ffi::FSoEReadSafeInput(self.master_index, slave, buf.as_mut_ptr(), &mut actual_size)
};
if ret != 0 {
buf.truncate(actual_size as usize);
result.insert(slave, buf);
}
}
result
}
pub fn any_in_failsafe(&mut self) -> bool {
let statuses = self.refresh_all_status();
statuses.values().any(|s| s.in_failsafe != 0)
}
pub fn enter_all_failsafe(&self) -> Vec<(u16, Result<()>)> {
let mut results = Vec::new();
for (&slave, info) in &self.connections {
let size = info.config.safe_output_size as usize;
let zeros = vec![0u8; size];
let ret = unsafe {
ffi::FSoESetFailsafeOutput(self.master_index, slave, zeros.as_ptr(), size as u32)
};
let result = if ret != 0 {
Ok(())
} else {
Err(DarraError::Other(format!(
"从站 {} 进入失效安全失败 (返回码: {})", slave, ret
)))
};
results.push((slave, result));
}
results
}
pub fn process_cycle(&self) {
unsafe { ffi::FSoEProcessCycle(self.master_index) };
}
pub fn connection_count(&self) -> usize {
self.connections.len()
}
pub fn dll_connection_count(&self) -> u16 {
unsafe { ffi::FSoEGetConnectionCount(self.master_index) }
}
pub fn is_slave_capable(&self, slave_index: u16) -> bool {
unsafe { ffi::FSoEIsSlaveCapable(self.master_index, slave_index) != 0 }
}
pub fn state(&self, slave_index: u16) -> Option<FsoeState> {
let raw = unsafe { ffi::FSoEGetState(self.master_index, slave_index) };
match raw {
0x100 => Some(FsoeState::Reset),
0x101 => Some(FsoeState::Session),
0x102 => Some(FsoeState::Connection),
0x103 => Some(FsoeState::Parameter),
0x104 => Some(FsoeState::Data),
0x105 => Some(FsoeState::Failsafe),
_ => None,
}
}
pub fn write_safe_output(&self, slave_index: u16, data: &[u8]) -> Result<()> {
let ret = unsafe {
ffi::FSoEWriteSafeOutput(self.master_index, slave_index, data.as_ptr(), data.len() as u32)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!("FSoE 安全输出写入失败 (从站: {})", slave_index)))
}
}
pub fn download_parameters(&self, slave_index: u16, params: &[u8]) -> Result<u32> {
let mut sra_crc: u32 = 0;
let ret = unsafe {
ffi::FSoEDownloadParameters(
self.master_index, slave_index,
params.as_ptr(), params.len() as u32, &mut sra_crc,
)
};
if ret != 0 { Ok(sra_crc) } else {
Err(DarraError::Other(format!("FSoE 参数下载失败 (从站: {})", slave_index)))
}
}
pub fn reset_connection(&self, slave_index: u16) -> Result<()> {
let ret = unsafe { ffi::FSoEReset(self.master_index, slave_index) };
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!("FSoE 连接重置失败 (从站: {})", slave_index)))
}
}
pub fn clear_error(&self, slave_index: u16) {
unsafe { ffi::FSoEClearError(self.master_index, slave_index) };
}
pub fn find_by_address(&self, safety_address: u16) -> Option<u16> {
for (&slave, info) in &self.connections {
if info.config.safety_address == safety_address {
return Some(slave);
}
}
None
}
pub fn find_by_connection_id(&self, connection_id: u16) -> Option<u16> {
for (&slave, info) in &self.connections {
if info.config.connection_id == connection_id {
return Some(slave);
}
}
None
}
pub fn find_connection_by_address(&self, safety_address: u16) -> Option<u16> {
self.find_by_address(safety_address)
}
pub fn bind_safe_input(&mut self, slave_index: u16, safety_address: u16,
input_size: u16, watchdog_ms: u32) -> Result<()> {
let config = FsoeConfig {
connection_id: slave_index + 1,
safety_address,
watchdog_time_ms: watchdog_ms,
safe_input_size: input_size,
safe_output_size: 0,
pdo_input_offset: 0,
pdo_output_offset: 0,
};
self.add_connection(slave_index, config)
}
pub fn bind_safe_output(&mut self, slave_index: u16, safety_address: u16,
output_size: u16, watchdog_ms: u32) -> Result<()> {
let config = FsoeConfig {
connection_id: slave_index + 1,
safety_address,
watchdog_time_ms: watchdog_ms,
safe_input_size: 0,
safe_output_size: output_size,
pdo_input_offset: 0,
pdo_output_offset: 0,
};
self.add_connection(slave_index, config)
}
pub fn bind_safe_io(&mut self, slave_index: u16,
safe_input_offset: u32, safe_input_size: u16,
safe_output_offset: u32, safe_output_size: u16,
safety_address: u16, watchdog_ms: u32) -> Result<()> {
let config = FsoeConfig {
connection_id: slave_index + 1,
safety_address,
watchdog_time_ms: watchdog_ms,
safe_input_size,
safe_output_size,
pdo_input_offset: safe_input_offset,
pdo_output_offset: safe_output_offset,
};
self.add_connection(slave_index, config)
}
pub fn bind_mdp_drive_axis(&mut self, slave_index: u16, axis_index: u16,
safety_address: u16, watchdog_ms: u32) -> Result<()> {
let conn_index = axis_index + 1;
let mdp = SafeMdp::new(self.master_index, slave_index);
let config = SafeMdpConfig {
connection_id: conn_index,
safety_address,
watchdog_time_ms: watchdog_ms,
safe_input_size: 0,
safe_output_size: 0,
pdo_input_offset: 0,
pdo_output_offset: 0,
module_number: axis_index,
module_profile: 790,
axis_number: axis_index,
connection_type: 0,
};
mdp.init_connection(conn_index, config)
}
pub fn write_output_frame(&self, slave_index: u16, frame_data: &[u8]) -> bool {
if frame_data.is_empty() {
return false;
}
if !self.connections.contains_key(&slave_index) {
return false;
}
let ret = unsafe {
ffi::FSoEWriteSafeOutput(
self.master_index, slave_index,
frame_data.as_ptr(), frame_data.len() as u32,
)
};
ret != 0
}
pub fn read_input_frame(&self, slave_index: u16, buffer_size: usize) -> Option<Vec<u8>> {
let info = self.connections.get(&slave_index)?;
let read_size = if buffer_size > 0 {
buffer_size
} else {
info.config.safe_input_size as usize
};
if read_size == 0 {
return None;
}
let mut buf = vec![0u8; read_size];
let mut actual_size = buf.len() as u32;
let ret = unsafe {
ffi::FSoEReadSafeInput(
self.master_index, slave_index,
buf.as_mut_ptr(), &mut actual_size,
)
};
if ret != 0 && actual_size > 0 {
buf.truncate(actual_size as usize);
Some(buf)
} else {
None
}
}
pub fn status_summary(&self) -> String {
if self.connections.is_empty() {
return "未初始化".to_string();
}
let mut lines = vec![format!("FSoE Manager: 连接数={}", self.connections.len())];
for (&slave, info) in &self.connections {
let state = self.state(slave)
.map(|s| format!("{:?}", s))
.unwrap_or_else(|| "未知".to_string());
lines.push(format!(" 连接[从站={}]: 状态={}, 地址=0x{:04X}",
slave, state, info.config.safety_address));
}
lines.join("\n")
}
}
impl Drop for SafetyManager {
fn drop(&mut self) {
let keys: Vec<u16> = self.connections.keys().copied().collect();
for slave in keys {
unsafe { ffi::FSoECloseConnection(self.master_index, slave) };
}
}
}
pub struct SafeMdp {
master_index: u16,
slave_index: u16,
}
impl SafeMdp {
pub fn new(master_index: u16, slave_index: u16) -> Self {
Self { master_index, slave_index }
}
pub fn detect_connections(&self) -> u16 {
unsafe { ffi::SafeMdpDetectConnections(self.master_index, self.slave_index) }
}
pub fn slave_connection_count(&self) -> u16 {
unsafe { ffi::SafeMdpGetSlaveConnectionCount(self.master_index, self.slave_index) }
}
pub fn device_address(&self) -> Result<u16> {
let mut safety_addr: u16 = 0;
let ret = unsafe {
ffi::SafeMdpGetDeviceAddress(self.master_index, self.slave_index, &mut safety_addr)
};
if ret != 0 { Ok(safety_addr) } else {
Err(DarraError::Other(format!(
"读取 MDP 设备地址失败 (从站: {})", self.slave_index
)))
}
}
pub fn init_connection(&self, connection_index: u16, config: SafeMdpConfig) -> Result<()> {
let mut cfg = config;
let ret = unsafe {
ffi::SafeMdpInitConnection(
self.master_index, self.slave_index, connection_index, &mut cfg,
)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 初始化失败 (从站: {})", connection_index, self.slave_index
)))
}
}
pub fn close_connection(&self, connection_index: u16) -> Result<()> {
let ret = unsafe {
ffi::SafeMdpCloseConnection(self.master_index, self.slave_index, connection_index)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 关闭失败 (从站: {})", connection_index, self.slave_index
)))
}
}
pub fn status(&self, connection_index: u16) -> Result<FsoeStatus> {
let mut status = FsoeStatus {
state: FsoeState::Reset,
last_error: FsoeError::None,
error_count: 0,
frames_sent: 0,
frames_received: 0,
crc_errors: 0,
watchdog_errors: 0,
watchdog_expired: 0,
in_failsafe: 0,
toggle_bit: 0,
initialized: 0,
};
let ret = unsafe {
ffi::SafeMdpGetStatus(
self.master_index, self.slave_index, connection_index, &mut status,
)
};
if ret != 0 { Ok(status) } else {
Err(DarraError::Other(format!(
"获取 FSoE MDP 连接 {} 状态失败 (从站: {})", connection_index, self.slave_index
)))
}
}
pub fn request_state(&self, connection_index: u16, target_state: FsoeState) -> Result<()> {
let ret = unsafe {
ffi::SafeMdpRequestState(
self.master_index, self.slave_index, connection_index, target_state as i32,
)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 状态请求失败", connection_index
)))
}
}
pub fn state(&self, connection_index: u16) -> Option<FsoeState> {
let raw = unsafe {
ffi::SafeMdpGetState(self.master_index, self.slave_index, connection_index)
};
match raw {
0x100 => Some(FsoeState::Reset),
0x101 => Some(FsoeState::Session),
0x102 => Some(FsoeState::Connection),
0x103 => Some(FsoeState::Parameter),
0x104 => Some(FsoeState::Data),
0x105 => Some(FsoeState::Failsafe),
_ => None,
}
}
pub fn reset(&self, connection_index: u16) -> Result<()> {
let ret = unsafe {
ffi::SafeMdpReset(self.master_index, self.slave_index, connection_index)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 重置失败", connection_index
)))
}
}
pub fn write_safe_output(&self, connection_index: u16, data: &[u8]) -> Result<()> {
let ret = unsafe {
ffi::SafeMdpWriteSafeOutput(
self.master_index, self.slave_index, connection_index,
data.as_ptr(), data.len() as u32,
)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 安全输出写入失败", connection_index
)))
}
}
pub fn read_safe_input(&self, connection_index: u16, max_size: usize) -> Result<Vec<u8>> {
let mut buf = vec![0u8; max_size];
let mut actual_size = max_size as u32;
let ret = unsafe {
ffi::SafeMdpReadSafeInput(
self.master_index, self.slave_index, connection_index,
buf.as_mut_ptr(), &mut actual_size,
)
};
if ret != 0 {
buf.truncate(actual_size as usize);
Ok(buf)
} else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 安全输入读取失败", connection_index
)))
}
}
pub fn download_parameters(&self, connection_index: u16, params: &[u8]) -> Result<u32> {
let mut sra_crc: u32 = 0;
let ret = unsafe {
ffi::SafeMdpDownloadParameters(
self.master_index, self.slave_index, connection_index,
params.as_ptr(), params.len() as u32, &mut sra_crc,
)
};
if ret != 0 { Ok(sra_crc) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 参数下载失败", connection_index
)))
}
}
pub fn set_failsafe_output(&self, connection_index: u16, data: &[u8]) -> Result<()> {
let ret = unsafe {
ffi::SafeMdpSetFailsafeOutput(
self.master_index, self.slave_index, connection_index,
data.as_ptr(), data.len() as u32,
)
};
if ret != 0 { Ok(()) } else {
Err(DarraError::Other(format!(
"FSoE MDP 连接 {} 失效安全输出设置失败", connection_index
)))
}
}
pub fn check_watchdog(&self, connection_index: u16) -> bool {
unsafe {
ffi::SafeMdpCheckWatchdog(self.master_index, self.slave_index, connection_index) != 0
}
}
pub fn last_error(&self, connection_index: u16) -> i32 {
unsafe {
ffi::SafeMdpGetLastError(self.master_index, self.slave_index, connection_index)
}
}
pub fn clear_error(&self, connection_index: u16) {
unsafe {
ffi::SafeMdpClearError(self.master_index, self.slave_index, connection_index)
};
}
pub fn module_comm_param(&self, module_number: u16) -> Result<Vec<u8>> {
let mut buf = vec![0u8; 256];
let mut size: u32 = 256;
let ret = unsafe {
ffi::SafeMdpGetModuleCommParam(
self.master_index, self.slave_index, module_number,
buf.as_mut_ptr(), &mut size,
)
};
if ret != 0 {
buf.truncate(size as usize);
Ok(buf)
} else {
Err(DarraError::Other(format!(
"读取 MDP 模块 {} 通信参数失败", module_number
)))
}
}
pub fn module_diagnosis(&self, module_number: u16) -> Result<(u16, u16)> {
let mut conn_state: u16 = 0;
let mut conn_diag: u16 = 0;
let ret = unsafe {
ffi::SafeMdpGetModuleDiagnosis(
self.master_index, self.slave_index, module_number,
&mut conn_state, &mut conn_diag,
)
};
if ret != 0 { Ok((conn_state, conn_diag)) } else {
Err(DarraError::Other(format!(
"读取 MDP 模块 {} 诊断数据失败", module_number
)))
}
}
}
impl std::fmt::Debug for SafeMdp {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
f.debug_struct("SafeMdp")
.field("master_index", &self.master_index)
.field("slave_index", &self.slave_index)
.finish()
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[repr(u8)]
pub enum FsoeCommand {
Reset = 0x2A,
Session = 0x4E,
Connection = 0x64,
Parameter = 0x52,
Failsafe = 0x08,
Data = 0x36,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum FsoeFailsafeReason {
WatchdogTimeout,
CrcError,
CommunicationError,
ApplicationRequest,
SlaveRequest,
MasterRequest,
RecoveryToData,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum FsoeConnectionMode {
Single,
Multiple,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[repr(u16)]
pub enum FsoeConnectionType {
Master = 0,
Slave = 1,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[repr(u16)]
pub enum SafeModuleProfile {
DigitalInput = 190,
DigitalInOut = 195,
DigitalOutput = 290,
DriveConnection = 790,
Master = 6900,
}
#[derive(Clone, Debug)]
pub struct FsoeConnectionConfig {
pub connection_id: u16,
pub safety_address: u16,
pub watchdog_time_ms: u32,
pub safe_input_size: u16,
pub safe_output_size: u16,
pub pdo_input_offset: u32,
pub pdo_output_offset: u32,
}
impl Default for FsoeConnectionConfig {
fn default() -> Self {
Self {
connection_id: 0,
safety_address: 0,
watchdog_time_ms: 100,
safe_input_size: 0,
safe_output_size: 0,
pdo_input_offset: 0,
pdo_output_offset: 0,
}
}
}
#[derive(Clone, Debug)]
pub struct FsoeConnectionStatus {
pub state: FsoeState,
pub last_error: FsoeError,
pub error_count: u32,
pub frames_sent: u32,
pub frames_received: u32,
pub crc_errors: u32,
pub watchdog_errors: u32,
pub watchdog_expired: bool,
pub in_failsafe: bool,
pub toggle_bit: u8,
pub is_initialized: bool,
}
#[derive(Clone, Debug)]
pub struct FsoeCommParameters {
pub connection_id: u16,
pub safety_address: u16,
pub watchdog_time_ms: u32,
pub comm_param_length: u16,
pub app_param_length: u16,
pub version: String,
pub connection_type: u16,
}
impl Default for FsoeCommParameters {
fn default() -> Self {
Self {
connection_id: 0,
safety_address: 0,
watchdog_time_ms: 0,
comm_param_length: 0,
app_param_length: 0,
version: "01".to_string(),
connection_type: 0,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[repr(u16)]
pub enum FsoeDiagnosisBits {
NoError = 0x0000,
WrongCommand = 0x0001,
UnknownCommand = 0x0002,
WrongConnectionId = 0x0003,
WrongCrc = 0x0004,
WatchdogExpired = 0x0005,
WrongFsoeAddress = 0x0006,
WrongData = 0x0007,
WrongCommParamLength = 0x0008,
WrongCommParam = 0x0009,
WrongApplParamLength = 0x000A,
WrongApplParam = 0x000B,
UnexpectedSessionCmd = 0x000C,
FailsafeDataReceived = 0x000D,
ErrorMask = 0x000F,
ErrorInSlave = 0x0010,
SlaveInFailsafe = 0x0020,
ConnectionInStartup = 0x0040,
MasterInFailsafe = 0x0080,
}
pub struct FsoeConstants;
impl FsoeConstants {
pub const MAX_CONNECTIONS: u32 = 64;
pub const MAX_SAFE_DATA_SIZE: u32 = 64;
pub const MAX_CRC_SEGMENTS: u32 = 32;
pub const FRAME_HEADER_SIZE: u32 = 3;
pub const FRAME_CRC_SIZE: u32 = 2;
pub const CRC16_POLY: u16 = 0x1021;
pub const CRC16_INITIAL: u16 = 0xFFFF;
pub const SRA_CRC32_POLY: u32 = 0x814141AB;
pub const DEFAULT_WATCHDOG_TIME_MS: u32 = 100;
pub const COMMAND_TYPE_MASK: u8 = 0x0F;
pub const TOGGLE_BIT_MASK: u8 = 0x80;
pub const MASTER_RESET_MASK: u8 = 0x40;
pub const SLAVE_RESET_MASK: u8 = 0x20;
}
pub struct FsoeHelper;
impl FsoeHelper {
pub fn get_command_type(command: u8) -> Option<FsoeCommand> {
match command {
0x2A => Some(FsoeCommand::Reset),
0x4E => Some(FsoeCommand::Session),
0x64 => Some(FsoeCommand::Connection),
0x52 => Some(FsoeCommand::Parameter),
0x08 => Some(FsoeCommand::Failsafe),
0x36 => Some(FsoeCommand::Data),
_ => None,
}
}
pub fn get_toggle_bit(command: u8) -> bool {
let _ = command;
false
}
pub fn is_master_reset_request(command: u8) -> bool {
command == FsoeCommand::Reset as u8
}
pub fn is_slave_reset_request(command: u8) -> bool {
command == FsoeCommand::Reset as u8
}
pub fn build_command(
cmd_type: FsoeCommand,
toggle_bit: bool,
master_reset: bool,
slave_reset: bool,
) -> u8 {
let _ = (toggle_bit, master_reset, slave_reset);
cmd_type as u8
}
pub fn is_valid_state_transition(current: FsoeState, target: FsoeState) -> bool {
match current {
FsoeState::Reset => matches!(target, FsoeState::Session),
FsoeState::Session => matches!(target, FsoeState::Connection | FsoeState::Reset),
FsoeState::Connection => matches!(target, FsoeState::Parameter | FsoeState::Reset),
FsoeState::Parameter => matches!(target, FsoeState::Data | FsoeState::Reset),
FsoeState::Data => matches!(target, FsoeState::Failsafe | FsoeState::Reset),
FsoeState::Failsafe => matches!(target, FsoeState::Data | FsoeState::Reset),
}
}
pub fn get_error_description(error: FsoeError) -> &'static str {
match error {
FsoeError::None => "无错误",
FsoeError::WrongCommand => "错误的命令",
FsoeError::UnknownCommand => "未知命令",
FsoeError::WrongConnId => "连接ID不匹配",
FsoeError::Crc => "CRC校验失败",
FsoeError::Watchdog => "看门狗超时",
FsoeError::WrongAddress => "错误的FSoE地址",
FsoeError::WrongData => "无效数据",
FsoeError::CommParamLength => "通信参数长度错误",
FsoeError::CommParam => "通信参数错误",
FsoeError::AppParamLength => "应用参数长度错误",
FsoeError::AppParam => "应用参数错误",
FsoeError::UnexpectedSession => "意外的会话命令",
FsoeError::FailsafeData => "收到失效安全数据",
FsoeError::NotInitialized => "FSoE未初始化",
FsoeError::MaxConnections => "达到最大连接数",
FsoeError::InvalidState => "无效状态转换",
}
}
}
#[derive(Clone, Debug)]
pub struct FsoeStateChangedEventArgs {
pub slave_index: u16,
pub connection_index: u16,
pub old_state: FsoeState,
pub new_state: FsoeState,
}
#[derive(Clone, Debug)]
pub struct FsoeErrorEventArgs {
pub slave_index: u16,
pub connection_index: u16,
pub error: FsoeError,
pub current_state: FsoeState,
}
impl FsoeErrorEventArgs {
pub fn description(&self) -> &'static str {
FsoeHelper::get_error_description(self.error)
}
}
#[derive(Clone, Debug)]
pub struct FsoeFailsafeEventArgs {
pub slave_index: u16,
pub connection_index: u16,
pub reason: FsoeFailsafeReason,
pub entering_failsafe: bool,
}
#[derive(Clone, Debug)]
pub struct FsoeDataUpdatedEventArgs {
pub slave_index: u16,
pub connection_index: u16,
pub is_input: bool,
pub data: Vec<u8>,
}
#[derive(Clone, Debug)]
pub struct FsoeDataExchangeEventArgs {
pub slave_index: u16,
pub current_state: FsoeState,
pub safe_inputs: Vec<u8>,
pub safe_outputs: Vec<u8>,
pub in_failsafe: bool,
pub cycle_count: u32,
pub timestamp: SystemTime,
}
pub type FsoeDataExchangeCallback =
Arc<dyn Fn(&FsoeDataExchangeEventArgs) + Send + Sync>;
pub struct FsoeDataExchangeDispatcher {
callbacks: Arc<Mutex<Vec<FsoeDataExchangeCallback>>>,
cycle_count: Arc<Mutex<u32>>,
}
impl FsoeDataExchangeDispatcher {
pub fn new() -> Self {
Self {
callbacks: Arc::new(Mutex::new(Vec::new())),
cycle_count: Arc::new(Mutex::new(0)),
}
}
pub fn add_data_exchanged_callback(&self, cb: FsoeDataExchangeCallback) {
if let Ok(mut list) = self.callbacks.lock() {
list.push(cb);
}
}
pub fn remove_data_exchanged_callback(&self, cb: &FsoeDataExchangeCallback) {
if let Ok(mut list) = self.callbacks.lock() {
list.retain(|c| !Arc::ptr_eq(c, cb));
}
}
pub fn fire_data_exchanged(
&self,
slave_index: u16,
current_state: FsoeState,
safe_inputs: &[u8],
safe_outputs: &[u8],
in_failsafe: bool,
) {
let cycle = {
let mut c = match self.cycle_count.lock() {
Ok(g) => g,
Err(_) => return,
};
*c = c.wrapping_add(1);
*c
};
let snapshot: Vec<FsoeDataExchangeCallback> = match self.callbacks.lock() {
Ok(list) => list.clone(),
Err(_) => return,
};
if snapshot.is_empty() {
return;
}
let args = FsoeDataExchangeEventArgs {
slave_index,
current_state,
safe_inputs: safe_inputs.to_vec(),
safe_outputs: safe_outputs.to_vec(),
in_failsafe,
cycle_count: cycle,
timestamp: SystemTime::now(),
};
for cb in &snapshot {
let _ = std::panic::catch_unwind(std::panic::AssertUnwindSafe(|| cb(&args)));
}
}
}
impl Default for FsoeDataExchangeDispatcher {
fn default() -> Self {
Self::new()
}
}
pub struct FsoeEventCallbacks {
pub on_state_changed: Option<Box<dyn Fn(&FsoeStateChangedEventArgs) + Send + Sync>>,
pub on_error: Option<Box<dyn Fn(&FsoeErrorEventArgs) + Send + Sync>>,
pub on_failsafe: Option<Box<dyn Fn(&FsoeFailsafeEventArgs) + Send + Sync>>,
pub on_safe_data_updated: Option<Box<dyn Fn(&FsoeDataUpdatedEventArgs) + Send + Sync>>,
}
impl FsoeEventCallbacks {
pub fn new() -> Self {
Self {
on_state_changed: None,
on_error: None,
on_failsafe: None,
on_safe_data_updated: None,
}
}
pub fn set_on_state_changed<F>(&mut self, f: F)
where
F: Fn(&FsoeStateChangedEventArgs) + Send + Sync + 'static,
{
self.on_state_changed = Some(Box::new(f));
}
pub fn set_on_error<F>(&mut self, f: F)
where
F: Fn(&FsoeErrorEventArgs) + Send + Sync + 'static,
{
self.on_error = Some(Box::new(f));
}
pub fn set_on_failsafe<F>(&mut self, f: F)
where
F: Fn(&FsoeFailsafeEventArgs) + Send + Sync + 'static,
{
self.on_failsafe = Some(Box::new(f));
}
pub fn set_on_safe_data_updated<F>(&mut self, f: F)
where
F: Fn(&FsoeDataUpdatedEventArgs) + Send + Sync + 'static,
{
self.on_safe_data_updated = Some(Box::new(f));
}
pub fn fire_state_changed(&self, args: &FsoeStateChangedEventArgs) {
if let Some(ref cb) = self.on_state_changed {
cb(args);
}
}
pub fn fire_error(&self, args: &FsoeErrorEventArgs) {
if let Some(ref cb) = self.on_error {
cb(args);
}
}
pub fn fire_failsafe(&self, args: &FsoeFailsafeEventArgs) {
if let Some(ref cb) = self.on_failsafe {
cb(args);
}
}
pub fn fire_safe_data_updated(&self, args: &FsoeDataUpdatedEventArgs) {
if let Some(ref cb) = self.on_safe_data_updated {
cb(args);
}
}
}
impl Default for FsoeEventCallbacks {
fn default() -> Self {
Self::new()
}
}
#[derive(Clone, Debug)]
pub struct SafeModuleCommParam {
pub version: [u8; 2],
pub safety_address: u16,
pub connection_id: u16,
pub watchdog_time: u32,
pub unique_device_id: [u8; 6],
pub connection_type: u16,
pub comm_param_length: u16,
pub appl_param_length: u16,
pub sra_crc: u32,
}
impl Default for SafeModuleCommParam {
fn default() -> Self {
Self {
version: [0; 2],
safety_address: 0,
connection_id: 0,
watchdog_time: 0,
unique_device_id: [0; 6],
connection_type: 0,
comm_param_length: 0,
appl_param_length: 0,
sra_crc: 0,
}
}
}
#[derive(Clone, Debug, Default)]
pub struct SafeModuleDiag {
pub connection_state: u16,
pub connection_diagnosis: u16,
}
#[derive(Clone, Debug, Default)]
pub struct SafeModuleInfo {
pub connection_id: u16,
pub device_type: u16,
pub vendor_id: u32,
pub product_code: u32,
pub revision_number: u32,
pub serial_number: u32,
}
pub struct SafeMdpIndex;
impl SafeMdpIndex {
pub const AXIS_INDEX_OFFSET: u16 = 0x0800;
pub const CONFIG_AREA_BASE: u16 = 0x8000;
pub const DEVICE_FSOE_ADDRESS: u16 = 0xF980;
pub const DIAG_AREA_BASE: u16 = 0xA000;
pub const DRIVE_CONFIG_AREA_BASE: u16 = 0xE800;
pub const DRIVE_DIAG_AREA_BASE: u16 = 0xEA00;
pub const DRIVE_INFO_AREA_BASE: u16 = 0xE900;
pub const DRIVE_INPUT_AREA_BASE: u16 = 0xE600;
pub const DRIVE_OUTPUT_AREA_BASE: u16 = 0xE700;
pub const INFO_AREA_BASE: u16 = 0x9000;
pub const INPUT_AREA_BASE: u16 = 0x6000;
pub const MODULE_INDEX_OFFSET: u16 = 0x0010;
pub const OUTPUT_AREA_BASE: u16 = 0x7000;
pub const PARAM_SET_MAPPING_BASE: u16 = 0x1E00;
pub const RX_PDO_MAPPING_BASE: u16 = 0x1600;
pub const TX_PDO_MAPPING_BASE: u16 = 0x1A00;
pub fn get_axis_index(base_index: u16, axis_number: u16) -> u16 {
base_index + axis_number * Self::AXIS_INDEX_OFFSET
}
pub fn get_module_index(base_index: u16, module_number: u16) -> u16 {
base_index + module_number * Self::MODULE_INDEX_OFFSET
}
}
#[derive(Clone, Debug)]
pub struct SafeModuleConfig {
pub module_number: u16,
pub axis_number: u16,
pub profile: SafeModuleProfile,
pub safety_address: u16,
pub connection_id: u16,
pub connection_type: FsoeConnectionType,
pub safe_input_size: u16,
pub safe_output_size: u16,
pub pdo_input_offset: u32,
pub pdo_output_offset: u32,
pub watchdog_time_ms: u32,
}
impl Default for SafeModuleConfig {
fn default() -> Self {
Self {
module_number: 0,
axis_number: 0,
profile: SafeModuleProfile::DigitalInOut,
safety_address: 0,
connection_id: 0,
connection_type: FsoeConnectionType::Slave,
safe_input_size: 0,
safe_output_size: 0,
pdo_input_offset: 0,
pdo_output_offset: 0,
watchdog_time_ms: 100,
}
}
}
impl SafeModuleConfig {
pub fn to_connection_config(&self) -> FsoeConnectionConfig {
FsoeConnectionConfig {
connection_id: self.connection_id,
safety_address: self.safety_address,
watchdog_time_ms: self.watchdog_time_ms,
safe_input_size: self.safe_input_size,
safe_output_size: self.safe_output_size,
pdo_input_offset: self.pdo_input_offset,
pdo_output_offset: self.pdo_output_offset,
}
}
}
#[derive(Clone, Debug)]
pub struct SafeMdpDeviceConfig {
pub connection_mode: FsoeConnectionMode,
pub device_safety_address: u16,
pub is_drive: bool,
pub axis_count: u16,
pub modules: Vec<SafeModuleConfig>,
}
impl Default for SafeMdpDeviceConfig {
fn default() -> Self {
Self {
connection_mode: FsoeConnectionMode::Single,
device_safety_address: 0,
is_drive: false,
axis_count: 1,
modules: Vec::new(),
}
}
}
impl SafeMdpDeviceConfig {
pub fn add_module(&mut self, profile: SafeModuleProfile, safety_address: u16) -> &mut SafeModuleConfig {
let idx = self.modules.len() as u16;
self.modules.push(SafeModuleConfig {
module_number: idx,
profile,
safety_address,
connection_id: 0x0001 + idx,
..Default::default()
});
self.modules.last_mut().unwrap()
}
pub fn add_drive_axis(&mut self, axis_number: u16, safety_address: u16) -> &mut SafeModuleConfig {
let idx = self.modules.len() as u16;
self.modules.push(SafeModuleConfig {
module_number: idx,
axis_number,
profile: SafeModuleProfile::DriveConnection,
safety_address,
connection_id: 0x0001 + idx,
..Default::default()
});
self.modules.last_mut().unwrap()
}
pub fn get_connection_count(&self) -> usize {
match self.connection_mode {
FsoeConnectionMode::Single => 1,
FsoeConnectionMode::Multiple => self.modules.len(),
}
}
}
#[derive(Clone, Debug)]
pub struct FsoePdoLayout {
pub command_offset: usize,
pub flags_offset: usize,
pub connection_id_offset: usize,
pub sequence_offset: usize,
pub data_offset: usize,
pub data_length: usize,
pub crc_length: usize,
}
impl Default for FsoePdoLayout {
fn default() -> Self {
Self {
command_offset: 0,
flags_offset: 1,
connection_id_offset: 2,
sequence_offset: 4,
data_offset: 6,
data_length: 0,
crc_length: 2,
}
}
}
impl FsoePdoLayout {
pub fn total_length(&self) -> usize {
self.data_offset + self.data_length + self.crc_length
}
pub fn validate(&self) -> Result<()> {
if self.data_offset < self.sequence_offset {
return Err(DarraError::Other(
"DataOffset 必须大于等于 SequenceOffset".to_string(),
));
}
if self.crc_length == 0 {
return Err(DarraError::Other(
"CRC 长度必须大于 0".to_string(),
));
}
Ok(())
}
}
#[derive(Clone, Debug)]
pub struct FsoePdoFrame {
pub command: FsoeCommand,
pub flags: u8,
pub connection_id: u16,
pub sequence: u16,
pub data: Vec<u8>,
pub crc: u16,
}
impl Default for FsoePdoFrame {
fn default() -> Self {
Self {
command: FsoeCommand::Reset,
flags: 0,
connection_id: 0,
sequence: 0,
data: Vec::new(),
crc: 0,
}
}
}
impl FsoePdoFrame {
pub fn to_bytes(&mut self, layout: &FsoePdoLayout, crc_func: Option<fn(&[u8]) -> u16>) -> Result<Vec<u8>> {
layout.validate()?;
if self.data.len() != layout.data_length {
return Err(DarraError::Other(format!(
"Data 长度必须为 {} 字节，但收到 {}",
layout.data_length,
self.data.len()
)));
}
let total = layout.total_length();
let mut buf = vec![0u8; total];
buf[layout.command_offset] = self.command as u8;
buf[layout.flags_offset] = self.flags;
buf[layout.connection_id_offset] = (self.connection_id & 0xFF) as u8;
buf[layout.connection_id_offset + 1] = ((self.connection_id >> 8) & 0xFF) as u8;
buf[layout.sequence_offset] = (self.sequence & 0xFF) as u8;
buf[layout.sequence_offset + 1] = ((self.sequence >> 8) & 0xFF) as u8;
buf[layout.data_offset..layout.data_offset + layout.data_length]
.copy_from_slice(&self.data);
if let Some(f) = crc_func {
self.crc = f(&buf[..total - layout.crc_length]);
}
let crc_offset = layout.data_offset + layout.data_length;
buf[crc_offset] = (self.crc & 0xFF) as u8;
buf[crc_offset + 1] = ((self.crc >> 8) & 0xFF) as u8;
Ok(buf)
}
pub fn from_bytes(data: &[u8], layout: &FsoePdoLayout, crc_func: Option<fn(&[u8]) -> u16>) -> Result<Self> {
layout.validate()?;
let total = layout.total_length();
if data.len() < total {
return Err(DarraError::Other(format!(
"数据长度不足，期望 {} 字节，实际 {}",
total,
data.len()
)));
}
let command = match data[layout.command_offset] {
0x2A => FsoeCommand::Reset,
0x4E => FsoeCommand::Session,
0x64 => FsoeCommand::Connection,
0x52 => FsoeCommand::Parameter,
0x08 => FsoeCommand::Failsafe,
0x36 => FsoeCommand::Data,
v => return Err(DarraError::Other(format!("无效的 FSoE 命令: 0x{:02X}", v))),
};
let flags = data[layout.flags_offset];
let connection_id = u16::from_le_bytes([
data[layout.connection_id_offset],
data[layout.connection_id_offset + 1],
]);
let sequence = u16::from_le_bytes([
data[layout.sequence_offset],
data[layout.sequence_offset + 1],
]);
let frame_data = data[layout.data_offset..layout.data_offset + layout.data_length].to_vec();
let crc_offset = layout.data_offset + layout.data_length;
let crc = u16::from_le_bytes([data[crc_offset], data[crc_offset + 1]]);
if let Some(f) = crc_func {
let computed = f(&data[..total - layout.crc_length]);
if computed != crc {
return Err(DarraError::Other(format!(
"FSoE CRC 校验失败：期望 0x{:04X}，计算值 0x{:04X}",
crc, computed
)));
}
}
Ok(Self {
command,
flags,
connection_id,
sequence,
data: frame_data,
crc,
})
}
}
pub struct FsoeCrc16Configurable {
polynomial: u16,
initial: u16,
xor_out: u16,
}
impl FsoeCrc16Configurable {
pub fn new(polynomial: u16, initial: u16, xor_out: u16) -> Self {
Self { polynomial, initial, xor_out }
}
pub fn compute(&self, data: &[u8]) -> u16 {
let mut crc = self.initial;
for &b in data {
crc ^= (b as u16) << 8;
for _ in 0..8 {
if (crc & 0x8000) != 0 {
crc = (crc << 1) ^ self.polynomial;
} else {
crc <<= 1;
}
}
}
(crc ^ self.xor_out) & 0xFFFF
}
}
impl Default for FsoeCrc16Configurable {
fn default() -> Self {
Self::new(0x1021, 0xFFFF, 0x0000)
}
}
pub fn fsoe_crc16_ccitt_false() -> FsoeCrc16Configurable {
FsoeCrc16Configurable::new(0x1021, 0xFFFF, 0x0000)
}
pub fn fsoe_crc16(data: &[u8]) -> u16 {
FsoeCrc16::compute(data)
}
pub fn fsoe_crc16_fast(data: &[u8]) -> u16 {
FsoeCrc16::compute(data)
}
pub fn validate_conn_id(conn_id: u16) -> bool {
unsafe { ffi::FSoEValidateConnId(conn_id) != 0 }
}
pub fn is_connection_id_available(conn_id: u16) -> bool {
validate_conn_id(conn_id)
}
impl crate::abstractions::MailboxProtocol for SafeMdp {
fn protocol_type(&self) -> u8 { 0x08 }
fn protocol_name(&self) -> &'static str { "FSoE" }
fn is_supported(&self) -> bool {
self.detect_connections() > 0
}
fn statistics(&self) -> crate::abstractions::MailboxStatistics {
let mut stats = ffi::EcMbxStatsC::default();
let rc = unsafe {
ffi::mbx_get_stats_by_master(
self.master_index, self.slave_index, 0x08, &mut stats,
)
};
if rc == 1 {
stats.into()
} else {
crate::abstractions::MailboxStatistics::empty()
}
}
fn reset_statistics(&self) {
unsafe {
ffi::mbx_reset_stats_by_master(self.master_index, self.slave_index, 0x08);
}
}
}