use crate::data::error::{DarraError, Result};
use crate::utils::ffi;
use std::fmt;
use std::os::raw::c_int;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum MdpModuleClass {
DigitalInput = 0x0001,
DigitalOutput = 0x0002,
AnalogInput = 0x0003,
AnalogOutput = 0x0004,
CounterInput = 0x0005,
DriveAxis = 0x0006,
FunctionalSafety = 0x0007,
EncoderInterface = 0x0008,
CommunicationBridge = 0x0009,
Unknown = 0xFFFF,
}
impl MdpModuleClass {
fn from_raw(val: u16) -> Self {
match val {
0x0001 => Self::DigitalInput,
0x0002 => Self::DigitalOutput,
0x0003 => Self::AnalogInput,
0x0004 => Self::AnalogOutput,
0x0005 => Self::CounterInput,
0x0006 => Self::DriveAxis,
0x0007 => Self::FunctionalSafety,
0x0008 => Self::EncoderInterface,
0x0009 => Self::CommunicationBridge,
_ => Self::Unknown,
}
}
}
impl fmt::Display for MdpModuleClass {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
let s = match self {
Self::DigitalInput => "数字量输入",
Self::DigitalOutput => "数字量输出",
Self::AnalogInput => "模拟量输入",
Self::AnalogOutput => "模拟量输出",
Self::CounterInput => "计数器输入",
Self::DriveAxis => "驱动器轴",
Self::FunctionalSafety => "功能安全",
Self::EncoderInterface => "编码器接口",
Self::CommunicationBridge => "通信桥接",
Self::Unknown => "未知",
};
write!(f, "{}", s)
}
}
#[derive(Debug, Clone)]
pub struct MdpModule {
pub module_number: u8,
pub vendor_id: u32,
pub product_code: u32,
pub revision_no: u32,
pub module_class: MdpModuleClass,
pub raw_ident: u32,
}
impl MdpModule {
pub fn is_safety(&self) -> bool {
self.module_class == MdpModuleClass::FunctionalSafety
}
pub fn is_drive(&self) -> bool {
self.module_class == MdpModuleClass::DriveAxis
}
}
impl fmt::Display for MdpModule {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f,
"模块[{:02}] 分类:{:<12} VID:0x{:08X} PC:0x{:08X} Rev:0x{:08X}",
self.module_number, self.module_class.to_string(),
self.vendor_id, self.product_code, self.revision_no
)
}
}
pub fn mdp_discover(master_index: u16, slave_index: u16) -> Result<Vec<MdpModule>> {
let module_count = read_u8_sdo(master_index, slave_index, 0xF050, 0)?;
if module_count == 0 {
return Ok(Vec::new());
}
let mut modules = Vec::with_capacity(module_count as usize);
for sub in 1..=module_count {
let raw_ident = match read_u32_sdo(master_index, slave_index, 0xF050, sub) {
Ok(v) => v,
Err(_) => continue,
};
let class_raw = ((raw_ident >> 16) & 0xFFFF) as u16;
let module_class = MdpModuleClass::from_raw(class_raw);
let vendor_id = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x01)
.unwrap_or(0);
let product_code = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x02)
.unwrap_or(0);
let revision_no = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x03)
.unwrap_or(0);
modules.push(MdpModule {
module_number: sub,
vendor_id,
product_code,
revision_no,
module_class,
raw_ident,
});
}
Ok(modules)
}
pub fn mdp_discover_safety(master_index: u16, slave_index: u16) -> Result<Vec<MdpModule>> {
let all = mdp_discover(master_index, slave_index)?;
Ok(all.into_iter().filter(|m| m.is_safety()).collect())
}
pub fn mdp_is_mdp_device(master_index: u16, slave_index: u16) -> bool {
read_u8_sdo(master_index, slave_index, 0xF000, 0).is_ok()
}
#[derive(Debug, Clone)]
pub struct MdpModuleProfile {
pub slot_index: u8,
pub profile_number: u32,
pub module_ident: u32,
}
impl fmt::Display for MdpModuleProfile {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f, "Slot[{:02}] Profile:0x{:08X} Ident:0x{:08X}",
self.slot_index, self.profile_number, self.module_ident)
}
}
pub fn read_configured_address_list(master_index: u16, slave_index: u16) -> Result<Vec<u32>> {
Ok(native_get_module_list(master_index, slave_index, true))
}
pub fn read_detected_address_list(master_index: u16, slave_index: u16) -> Result<Vec<u32>> {
Ok(native_get_module_list(master_index, slave_index, false))
}
fn native_get_module_list(mi: u16, si: u16, configured: bool) -> Vec<u32> {
let mut buf = [0u32; 32];
let n = unsafe {
if configured {
ffi::MDPGetConfigModuleList(mi, si, buf.as_mut_ptr(), buf.len() as c_int)
} else {
ffi::MDPGetDetectedModuleList(mi, si, buf.as_mut_ptr(), buf.len() as c_int)
}
};
if n <= 0 { return Vec::new(); }
buf[..(n as usize)].to_vec()
}
pub fn read_module_profile_list(master_index: u16, slave_index: u16) -> Result<Vec<MdpModuleProfile>> {
let count = read_u8_sdo(master_index, slave_index, 0xF010, 0)?;
if count == 0 {
return Ok(Vec::new());
}
let mut profiles = Vec::with_capacity(count as usize);
for sub in 1..=count {
let profile_number = match read_u32_sdo(master_index, slave_index, 0xF010, sub) {
Ok(v) => v,
Err(_) => continue,
};
let module_ident = read_u32_sdo(master_index, slave_index, 0xF050, sub)
.unwrap_or(0);
profiles.push(MdpModuleProfile {
slot_index: sub,
profile_number,
module_ident,
});
}
Ok(profiles)
}
pub fn is_module_config_consistent(master_index: u16, slave_index: u16) -> bool {
let mut first_mismatch: c_int = -1;
let rc = unsafe { ffi::MDPCheckModuleMatch(master_index, slave_index, &mut first_mismatch) };
rc == 1
}
pub fn check_module_match(master_index: u16, slave_index: u16) -> (bool, i32) {
let mut first_mismatch: c_int = -1;
let rc = unsafe { ffi::MDPCheckModuleMatch(master_index, slave_index, &mut first_mismatch) };
(rc == 1, first_mismatch as i32)
}
pub fn auto_enumerate(master_index: u16) -> i32 {
unsafe { ffi::MDPAutoEnumerate(master_index) }
}
pub fn auto_configure_from_detected_modules(master_index: u16, slave_index: u16) -> Result<u8> {
let detected = read_detected_address_list(master_index, slave_index)?;
if detected.is_empty() {
return Ok(0);
}
let count = detected.len() as u8;
write_u8_sdo(master_index, slave_index, 0xF030, 0, count)?;
let mut written: u8 = 0;
for (i, ident) in detected.iter().enumerate() {
let sub = (i + 1) as u8;
if write_u32_sdo(master_index, slave_index, 0xF030, sub, *ident).is_ok() {
written += 1;
}
}
Ok(written)
}
#[derive(Debug, Clone)]
pub struct MdpModulePdoInfo {
pub slot_index: u8,
pub input_offset: u32,
pub input_size: u16,
pub output_offset: u32,
pub output_size: u16,
}
pub fn get_module_pdo_layout(master_index: u16, slave_index: u16) -> Result<Vec<MdpModulePdoInfo>> {
let detected = read_detected_address_list(master_index, slave_index)?;
if detected.is_empty() {
return Ok(Vec::new());
}
let input_pdo_sizes = read_pdo_assignment_sizes(master_index, slave_index, 0x1C13)?;
let output_pdo_sizes = read_pdo_assignment_sizes(master_index, slave_index, 0x1C12)?;
if input_pdo_sizes.is_empty() && output_pdo_sizes.is_empty() {
return Ok(Vec::new());
}
let mut result = Vec::with_capacity(detected.len());
let mut input_cursor: u32 = 0;
let mut output_cursor: u32 = 0;
let input_per_module = if !input_pdo_sizes.is_empty() {
std::cmp::max(1, input_pdo_sizes.len() / detected.len())
} else { 0 };
let output_per_module = if !output_pdo_sizes.is_empty() {
std::cmp::max(1, output_pdo_sizes.len() / detected.len())
} else { 0 };
for (i, _ident) in detected.iter().enumerate() {
let mut input_bytes: u16 = 0;
let mut output_bytes: u16 = 0;
if input_per_module > 0 {
let start = i * input_per_module;
for j in start..std::cmp::min(start + input_per_module, input_pdo_sizes.len()) {
input_bytes += input_pdo_sizes[j];
}
}
if output_per_module > 0 {
let start = i * output_per_module;
for j in start..std::cmp::min(start + output_per_module, output_pdo_sizes.len()) {
output_bytes += output_pdo_sizes[j];
}
}
result.push(MdpModulePdoInfo {
slot_index: (i + 1) as u8,
input_offset: input_cursor,
input_size: input_bytes,
output_offset: output_cursor,
output_size: output_bytes,
});
input_cursor += input_bytes as u32;
output_cursor += output_bytes as u32;
}
Ok(result)
}
fn read_pdo_assignment_sizes(master: u16, slave: u16, assignment_index: u16) -> Result<Vec<u16>> {
let mut sizes = Vec::new();
let count = match read_u8_sdo(master, slave, assignment_index, 0) {
Ok(c) => c,
Err(_) => return Ok(sizes),
};
for i in 1..=count {
let pdo_data = read_u16_sdo(master, slave, assignment_index, i);
let pdo_mapping_index = match pdo_data {
Ok(v) if v != 0 => v,
_ => continue,
};
let pdo_bytes = read_pdo_mapping_size(master, slave, pdo_mapping_index);
sizes.push(pdo_bytes);
}
Ok(sizes)
}
fn read_pdo_mapping_size(master: u16, slave: u16, pdo_mapping_index: u16) -> u16 {
let mut total_bits: u32 = 0;
let entry_count = match read_u8_sdo(master, slave, pdo_mapping_index, 0) {
Ok(c) => c,
Err(_) => return 0,
};
for i in 1..=entry_count {
if let Ok(entry_val) = read_u32_sdo(master, slave, pdo_mapping_index, i) {
total_bits += (entry_val & 0xFF) as u32;
}
}
((total_bits + 7) / 8) as u16
}
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)
}
#[allow(dead_code)]
fn read_u32_object_list(master: u16, slave: u16, od_index: u16) -> Result<Vec<u32>> {
let count = read_u8_sdo(master, slave, od_index, 0)?;
if count == 0 {
return Ok(Vec::new());
}
let mut list = Vec::with_capacity(count as usize);
for sub in 1..=count {
match read_u32_sdo(master, slave, od_index, sub) {
Ok(v) => list.push(v),
Err(_) => continue,
}
}
Ok(list)
}
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_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)
}
fn write_u8_sdo(master: u16, slave: u16, index: u16, sub: u8, value: u8) -> Result<()> {
let data = [value];
let ret = unsafe {
ffi::SDOwrite_raw(master, slave, index, sub, 0, data.as_ptr(), 1)
};
if ret != 0 {
Err(DarraError::SdoWriteFailed { index, subindex: sub, abort_code: None })
} else {
Ok(())
}
}
fn write_u32_sdo(master: u16, slave: u16, index: u16, sub: u8, value: u32) -> Result<()> {
let data = value.to_le_bytes();
let ret = unsafe {
ffi::SDOwrite_raw(master, slave, index, sub, 0, data.as_ptr(), 4)
};
if ret != 0 {
Err(DarraError::SdoWriteFailed { index, subindex: sub, abort_code: None })
} else {
Ok(())
}
}