ethercat_controller/
mailboxes.rs1use std::{io, ops::Range};
2
3use ethercat::{Master, SdoData, SdoIdx};
4
5use crate::{MailboxPdoEntries, SlaveOffsets, SlavePos};
6
7pub fn init_mailbox_pdo_verification(
15 slave_number: u32,
16 mailbox_pdo_entries: &MailboxPdoEntries,
17 offsets: &SlaveOffsets,
18 get_reg_addr_ranges: &impl Fn(&SlaveOffsets, u16, &String) -> Vec<Range<usize>>,
19) -> (
20 Vec<Vec<Range<usize>>>,
21 Vec<std::time::Instant>,
22 Vec<bool>,
23 Vec<Vec<Vec<u8>>>,
24) {
25 let mut slave_mailbox_pdo_offsets = vec![];
28 let slave_mailbox_pdo_timestamps = vec![std::time::Instant::now(); slave_number as usize];
30 let slave_is_mailbox_pdo_responding = vec![true; slave_number as usize];
32 let slave_mailbox_pdo_data_buffer = vec![vec![]; slave_number as usize];
34
35 for i in 0..slave_number {
37 let mut mailbox_offsets = vec![];
38 for m in mailbox_pdo_entries.get(&SlavePos::from(i as u16)).unwrap() {
39 mailbox_offsets.append(&mut get_reg_addr_ranges(&offsets, i as u16, m));
40 }
41 slave_mailbox_pdo_offsets.push(mailbox_offsets);
42 }
43
44 (
45 slave_mailbox_pdo_offsets,
46 slave_mailbox_pdo_timestamps,
47 slave_is_mailbox_pdo_responding,
48 slave_mailbox_pdo_data_buffer,
49 )
50}
51
52pub fn verify_mailbox_pdos(
66 slave_number: u32,
67 data: &mut [u8],
68 slave_mailbox_pdo_offsets: &mut Vec<Vec<Range<usize>>>,
69 slave_mailbox_pdo_timestamps: &mut Vec<std::time::Instant>,
70 slave_is_mailbox_pdo_responding: &mut Vec<bool>,
71 slave_mailbox_pdo_data_buffer: &mut Vec<Vec<Vec<u8>>>,
72 mailbox_wait_time_ms: u32,
73) -> bool {
74 let mut all_slaves_responding = true;
76
77 for i in 0..slave_number {
79 let offset = slave_mailbox_pdo_offsets[i as usize].clone();
81
82 if offset.is_empty() {
83 continue;
85 }
86 let mailbox_data = offset
88 .iter()
89 .map(|range| data[range.clone()].to_vec())
90 .collect::<Vec<_>>();
91 log::debug!("{:?}", mailbox_data);
92 let is_all_zeros = mailbox_data.iter().all(|d| d.iter().all(|&x| x == 0));
94
95 slave_is_mailbox_pdo_responding[i as usize] = true;
97 if is_all_zeros {
99 if slave_mailbox_pdo_timestamps[i as usize]
100 .elapsed()
101 .as_millis() as u32
102 > mailbox_wait_time_ms
103 {
104 all_slaves_responding &= false; slave_is_mailbox_pdo_responding[i as usize] = false;
106 }
107 } else {
108 slave_mailbox_pdo_timestamps[i as usize] = std::time::Instant::now();
110 slave_is_mailbox_pdo_responding[i as usize] = true;
111 slave_mailbox_pdo_data_buffer[i as usize] = mailbox_data;
112 }
113
114 if slave_is_mailbox_pdo_responding[i as usize] {
115 for (j, range) in offset.iter().enumerate() {
116 if slave_mailbox_pdo_data_buffer[i as usize].len() > j {
117 data[range.clone()]
118 .copy_from_slice(&slave_mailbox_pdo_data_buffer[i as usize][j]);
119 }
120 }
121 }
122 }
123
124 return all_slaves_responding;
125}
126
127pub fn mailbox_sdo_write<T: SdoData>(
131 master: &mut Master,
132 slave_id: u16,
133 idx: u16,
134 sub_idx: u8,
135 data: &T,
136) -> Result<(), io::Error> {
137 let sdo_idx = SdoIdx::new(idx, sub_idx);
138 let sdo_pos = SlavePos::from(slave_id);
139 master.sdo_download(sdo_pos, sdo_idx, false, data)?;
140 Ok(())
141}
142
143pub fn mailbox_sdo_read(
147 master: &Master,
148 slave_id: u16,
149 idx: u16,
150 sub_idx: u8,
151 data: &mut Vec<u8>,
152) -> Result<(), io::Error> {
153 let sdo_idx = SdoIdx::new(idx, sub_idx);
154 let sdo_pos = SlavePos::from(slave_id);
155 master.sdo_upload(sdo_pos, sdo_idx, false, data)?;
156 Ok(())
157}