1
2use crate::data::error::{DarraError, Result};
3use crate::utils::ffi;
4use std::fmt;
5use std::os::raw::c_int;
6
7#[derive(Debug, Clone, Copy, PartialEq, Eq)]
8pub enum MdpModuleClass {
9
10 DigitalInput = 0x0001,
11
12 DigitalOutput = 0x0002,
13
14 AnalogInput = 0x0003,
15
16 AnalogOutput = 0x0004,
17
18 CounterInput = 0x0005,
19
20 DriveAxis = 0x0006,
21
22 FunctionalSafety = 0x0007,
23
24 EncoderInterface = 0x0008,
25
26 CommunicationBridge = 0x0009,
27
28 Unknown = 0xFFFF,
29}
30
31impl MdpModuleClass {
32 fn from_raw(val: u16) -> Self {
33 match val {
34 0x0001 => Self::DigitalInput,
35 0x0002 => Self::DigitalOutput,
36 0x0003 => Self::AnalogInput,
37 0x0004 => Self::AnalogOutput,
38 0x0005 => Self::CounterInput,
39 0x0006 => Self::DriveAxis,
40 0x0007 => Self::FunctionalSafety,
41 0x0008 => Self::EncoderInterface,
42 0x0009 => Self::CommunicationBridge,
43 _ => Self::Unknown,
44 }
45 }
46}
47
48impl fmt::Display for MdpModuleClass {
49 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
50 let s = match self {
51 Self::DigitalInput => "数字量输入",
52 Self::DigitalOutput => "数字量输出",
53 Self::AnalogInput => "模拟量输入",
54 Self::AnalogOutput => "模拟量输出",
55 Self::CounterInput => "计数器输入",
56 Self::DriveAxis => "驱动器轴",
57 Self::FunctionalSafety => "功能安全",
58 Self::EncoderInterface => "编码器接口",
59 Self::CommunicationBridge => "通信桥接",
60 Self::Unknown => "未知",
61 };
62 write!(f, "{}", s)
63 }
64}
65
66#[derive(Debug, Clone)]
67pub struct MdpModule {
68
69 pub module_number: u8,
70
71 pub vendor_id: u32,
72
73 pub product_code: u32,
74
75 pub revision_no: u32,
76
77 pub module_class: MdpModuleClass,
78
79 pub raw_ident: u32,
80}
81
82impl MdpModule {
83
84 pub fn is_safety(&self) -> bool {
85 self.module_class == MdpModuleClass::FunctionalSafety
86 }
87
88 pub fn is_drive(&self) -> bool {
89 self.module_class == MdpModuleClass::DriveAxis
90 }
91}
92
93impl fmt::Display for MdpModule {
94 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
95 write!(f,
96 "模块[{:02}] 分类:{:<12} VID:0x{:08X} PC:0x{:08X} Rev:0x{:08X}",
97 self.module_number, self.module_class.to_string(),
98 self.vendor_id, self.product_code, self.revision_no
99 )
100 }
101}
102
103pub fn mdp_discover(master_index: u16, slave_index: u16) -> Result<Vec<MdpModule>> {
104
105 let module_count = read_u8_sdo(master_index, slave_index, 0xF050, 0)?;
106 if module_count == 0 {
107 return Ok(Vec::new());
108 }
109
110 let mut modules = Vec::with_capacity(module_count as usize);
111
112 for sub in 1..=module_count {
113
114 let raw_ident = match read_u32_sdo(master_index, slave_index, 0xF050, sub) {
115 Ok(v) => v,
116 Err(_) => continue,
117 };
118
119 let class_raw = ((raw_ident >> 16) & 0xFFFF) as u16;
120 let module_class = MdpModuleClass::from_raw(class_raw);
121
122 let vendor_id = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x01)
123 .unwrap_or(0);
124 let product_code = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x02)
125 .unwrap_or(0);
126 let revision_no = read_u32_sdo(master_index, slave_index, 0xF000 + (sub as u16 - 1) * 0x0800, 0x03)
127 .unwrap_or(0);
128
129 modules.push(MdpModule {
130 module_number: sub,
131 vendor_id,
132 product_code,
133 revision_no,
134 module_class,
135 raw_ident,
136 });
137 }
138
139 Ok(modules)
140}
141
142pub fn mdp_discover_safety(master_index: u16, slave_index: u16) -> Result<Vec<MdpModule>> {
143 let all = mdp_discover(master_index, slave_index)?;
144 Ok(all.into_iter().filter(|m| m.is_safety()).collect())
145}
146
147pub fn mdp_is_mdp_device(master_index: u16, slave_index: u16) -> bool {
148 read_u8_sdo(master_index, slave_index, 0xF000, 0).is_ok()
149}
150
151#[derive(Debug, Clone)]
152pub struct MdpModuleProfile {
153
154 pub slot_index: u8,
155
156 pub profile_number: u32,
157
158 pub module_ident: u32,
159}
160
161impl fmt::Display for MdpModuleProfile {
162 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
163 write!(f, "Slot[{:02}] Profile:0x{:08X} Ident:0x{:08X}",
164 self.slot_index, self.profile_number, self.module_ident)
165 }
166}
167
168pub fn read_configured_address_list(master_index: u16, slave_index: u16) -> Result<Vec<u32>> {
169 Ok(native_get_module_list(master_index, slave_index, true))
170}
171
172pub fn read_detected_address_list(master_index: u16, slave_index: u16) -> Result<Vec<u32>> {
173 Ok(native_get_module_list(master_index, slave_index, false))
174}
175
176fn native_get_module_list(mi: u16, si: u16, configured: bool) -> Vec<u32> {
177 let mut buf = [0u32; 32];
178 let n = unsafe {
179 if configured {
180 ffi::MDPGetConfigModuleList(mi, si, buf.as_mut_ptr(), buf.len() as c_int)
181 } else {
182 ffi::MDPGetDetectedModuleList(mi, si, buf.as_mut_ptr(), buf.len() as c_int)
183 }
184 };
185 if n <= 0 { return Vec::new(); }
186 buf[..(n as usize)].to_vec()
187}
188
189pub fn read_module_profile_list(master_index: u16, slave_index: u16) -> Result<Vec<MdpModuleProfile>> {
190
191 let count = read_u8_sdo(master_index, slave_index, 0xF010, 0)?;
192 if count == 0 {
193 return Ok(Vec::new());
194 }
195
196 let mut profiles = Vec::with_capacity(count as usize);
197
198 for sub in 1..=count {
199
200 let profile_number = match read_u32_sdo(master_index, slave_index, 0xF010, sub) {
201 Ok(v) => v,
202 Err(_) => continue,
203 };
204
205 let module_ident = read_u32_sdo(master_index, slave_index, 0xF050, sub)
206 .unwrap_or(0);
207
208 profiles.push(MdpModuleProfile {
209 slot_index: sub,
210 profile_number,
211 module_ident,
212 });
213 }
214
215 Ok(profiles)
216}
217
218pub fn is_module_config_consistent(master_index: u16, slave_index: u16) -> bool {
219 let mut first_mismatch: c_int = -1;
220 let rc = unsafe { ffi::MDPCheckModuleMatch(master_index, slave_index, &mut first_mismatch) };
221 rc == 1
222}
223
224pub fn check_module_match(master_index: u16, slave_index: u16) -> (bool, i32) {
225 let mut first_mismatch: c_int = -1;
226 let rc = unsafe { ffi::MDPCheckModuleMatch(master_index, slave_index, &mut first_mismatch) };
227 (rc == 1, first_mismatch as i32)
228}
229
230pub fn auto_enumerate(master_index: u16) -> i32 {
231 unsafe { ffi::MDPAutoEnumerate(master_index) }
232}
233
234pub fn auto_configure_from_detected_modules(master_index: u16, slave_index: u16) -> Result<u8> {
235
236 let detected = read_detected_address_list(master_index, slave_index)?;
237 if detected.is_empty() {
238 return Ok(0);
239 }
240
241 let count = detected.len() as u8;
242
243 write_u8_sdo(master_index, slave_index, 0xF030, 0, count)?;
244
245 let mut written: u8 = 0;
246 for (i, ident) in detected.iter().enumerate() {
247 let sub = (i + 1) as u8;
248 if write_u32_sdo(master_index, slave_index, 0xF030, sub, *ident).is_ok() {
249 written += 1;
250 }
251 }
252
253 Ok(written)
254}
255
256#[derive(Debug, Clone)]
257pub struct MdpModulePdoInfo {
258
259 pub slot_index: u8,
260
261 pub input_offset: u32,
262
263 pub input_size: u16,
264
265 pub output_offset: u32,
266
267 pub output_size: u16,
268}
269
270pub fn get_module_pdo_layout(master_index: u16, slave_index: u16) -> Result<Vec<MdpModulePdoInfo>> {
271 let detected = read_detected_address_list(master_index, slave_index)?;
272 if detected.is_empty() {
273 return Ok(Vec::new());
274 }
275
276 let input_pdo_sizes = read_pdo_assignment_sizes(master_index, slave_index, 0x1C13)?;
277 let output_pdo_sizes = read_pdo_assignment_sizes(master_index, slave_index, 0x1C12)?;
278
279 if input_pdo_sizes.is_empty() && output_pdo_sizes.is_empty() {
280 return Ok(Vec::new());
281 }
282
283 let mut result = Vec::with_capacity(detected.len());
284 let mut input_cursor: u32 = 0;
285 let mut output_cursor: u32 = 0;
286
287 let input_per_module = if !input_pdo_sizes.is_empty() {
288 std::cmp::max(1, input_pdo_sizes.len() / detected.len())
289 } else { 0 };
290 let output_per_module = if !output_pdo_sizes.is_empty() {
291 std::cmp::max(1, output_pdo_sizes.len() / detected.len())
292 } else { 0 };
293
294 for (i, _ident) in detected.iter().enumerate() {
295 let mut input_bytes: u16 = 0;
296 let mut output_bytes: u16 = 0;
297
298 if input_per_module > 0 {
299 let start = i * input_per_module;
300 for j in start..std::cmp::min(start + input_per_module, input_pdo_sizes.len()) {
301 input_bytes += input_pdo_sizes[j];
302 }
303 }
304
305 if output_per_module > 0 {
306 let start = i * output_per_module;
307 for j in start..std::cmp::min(start + output_per_module, output_pdo_sizes.len()) {
308 output_bytes += output_pdo_sizes[j];
309 }
310 }
311
312 result.push(MdpModulePdoInfo {
313 slot_index: (i + 1) as u8,
314 input_offset: input_cursor,
315 input_size: input_bytes,
316 output_offset: output_cursor,
317 output_size: output_bytes,
318 });
319
320 input_cursor += input_bytes as u32;
321 output_cursor += output_bytes as u32;
322 }
323
324 Ok(result)
325}
326
327fn read_pdo_assignment_sizes(master: u16, slave: u16, assignment_index: u16) -> Result<Vec<u16>> {
328 let mut sizes = Vec::new();
329 let count = match read_u8_sdo(master, slave, assignment_index, 0) {
330 Ok(c) => c,
331 Err(_) => return Ok(sizes),
332 };
333
334 for i in 1..=count {
335 let pdo_data = read_u16_sdo(master, slave, assignment_index, i);
336 let pdo_mapping_index = match pdo_data {
337 Ok(v) if v != 0 => v,
338 _ => continue,
339 };
340
341 let pdo_bytes = read_pdo_mapping_size(master, slave, pdo_mapping_index);
342 sizes.push(pdo_bytes);
343 }
344
345 Ok(sizes)
346}
347
348fn read_pdo_mapping_size(master: u16, slave: u16, pdo_mapping_index: u16) -> u16 {
349 let mut total_bits: u32 = 0;
350 let entry_count = match read_u8_sdo(master, slave, pdo_mapping_index, 0) {
351 Ok(c) => c,
352 Err(_) => return 0,
353 };
354
355 for i in 1..=entry_count {
356 if let Ok(entry_val) = read_u32_sdo(master, slave, pdo_mapping_index, i) {
357
358 total_bits += (entry_val & 0xFF) as u32;
359 }
360 }
361
362 ((total_bits + 7) / 8) as u16
363}
364
365fn read_u16_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u16> {
366 let mut size: c_int = 0;
367 let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
368 if ptr.is_null() || size < 2 {
369 if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
370 return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
371 }
372 let val = unsafe {
373 u16::from_le_bytes([*ptr, *ptr.add(1)])
374 };
375 unsafe { ffi::FreeMemory(ptr as *mut _) };
376 Ok(val)
377}
378
379#[allow(dead_code)]
380fn read_u32_object_list(master: u16, slave: u16, od_index: u16) -> Result<Vec<u32>> {
381 let count = read_u8_sdo(master, slave, od_index, 0)?;
382 if count == 0 {
383 return Ok(Vec::new());
384 }
385
386 let mut list = Vec::with_capacity(count as usize);
387 for sub in 1..=count {
388 match read_u32_sdo(master, slave, od_index, sub) {
389 Ok(v) => list.push(v),
390 Err(_) => continue,
391 }
392 }
393 Ok(list)
394}
395
396fn read_u8_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u8> {
397 let mut size: c_int = 0;
398 let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
399 if ptr.is_null() || size < 1 {
400 if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
401 return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
402 }
403 let val = unsafe { *ptr };
404 unsafe { ffi::FreeMemory(ptr as *mut _) };
405 Ok(val)
406}
407
408fn read_u32_sdo(master: u16, slave: u16, index: u16, sub: u8) -> Result<u32> {
409 let mut size: c_int = 0;
410 let ptr = unsafe { ffi::SDOread(master, slave, index, sub, 0, &mut size) };
411 if ptr.is_null() || size < 4 {
412 if !ptr.is_null() { unsafe { ffi::FreeMemory(ptr as *mut _) }; }
413 return Err(DarraError::SdoReadFailed { index, subindex: sub, abort_code: None });
414 }
415 let val = unsafe {
416 u32::from_le_bytes([*ptr, *ptr.add(1), *ptr.add(2), *ptr.add(3)])
417 };
418 unsafe { ffi::FreeMemory(ptr as *mut _) };
419 Ok(val)
420}
421
422fn write_u8_sdo(master: u16, slave: u16, index: u16, sub: u8, value: u8) -> Result<()> {
423 let data = [value];
424 let ret = unsafe {
425 ffi::SDOwrite_raw(master, slave, index, sub, 0, data.as_ptr(), 1)
426 };
427 if ret != 0 {
428 Err(DarraError::SdoWriteFailed { index, subindex: sub, abort_code: None })
429 } else {
430 Ok(())
431 }
432}
433
434fn write_u32_sdo(master: u16, slave: u16, index: u16, sub: u8, value: u32) -> Result<()> {
435 let data = value.to_le_bytes();
436 let ret = unsafe {
437 ffi::SDOwrite_raw(master, slave, index, sub, 0, data.as_ptr(), 4)
438 };
439 if ret != 0 {
440 Err(DarraError::SdoWriteFailed { index, subindex: sub, abort_code: None })
441 } else {
442 Ok(())
443 }
444}