ethercat_controller/
mailboxes.rs

1use std::{io, ops::Range};
2
3use ethercat::{Master, SdoData, SdoIdx};
4
5use crate::{MailboxPdoEntries, SlaveOffsets, SlavePos};
6
7/// init the necessary variables for mailbox pdo verification
8/// offsets of the mailboxes data in the domain data
9/// last read timestamp of the mailbox data
10/// flag to check if the slave is responding
11/// buffer to store the mailbox data (that are read asynchronusly from the slaves)
12///
13/// **Note:**  mailbox PDOs are different from the normal buffered PDOs and they are not always present
14pub 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    // initialize the mailbox verification variables
26    // offsets of the mailboxes data in the domain data
27    let mut slave_mailbox_pdo_offsets = vec![];
28    // last read timestamp of the mailbox data
29    let slave_mailbox_pdo_timestamps = vec![std::time::Instant::now(); slave_number as usize];
30    // flag to check if the slave is responding
31    let slave_is_mailbox_pdo_responding = vec![true; slave_number as usize];
32    // buffer to store the mailbox data (that are read asynchronusly from the slaves)
33    let slave_mailbox_pdo_data_buffer = vec![vec![]; slave_number as usize];
34
35    // find the mailbox offsets for each slave
36    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
52/// verify the mailboxe pdos of the slaves (if they are available)
53/// verify that the slaves are still writing
54/// checking if all the mailbox values are zero for more than 1s
55/// - if the values are not zero, update the timestamp
56/// - if the values are zero, check if the timestamp is more than 1s
57///      - if the timestamp is more than 1s, set the slave as not responding
58/// - if the values are not zero, update the timestamp
59///
60/// NOTE:
61///  - mailbox PDOs are different from the normal buffered PDOs as they are only updated once the slave writes to them
62///    and if the slave is not writing to them, the values will be read as zero
63///  - therefore this function is used to check if the slaves are still writing to the mailbox PDOs
64///    and if they are the mailbox pdo data is buffered and copied to the domain data
65pub 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    // return if all slaves responding
75    let mut all_slaves_responding = true;
76
77    // check each slave
78    for i in 0..slave_number {
79        // get slave mailbox offset
80        let offset = slave_mailbox_pdo_offsets[i as usize].clone();
81
82        if offset.is_empty() {
83            // if there are no mailbox pdos for the slave, continue
84            continue;
85        }
86        // get the mailbox data
87        let mailbox_data = offset
88            .iter()
89            .map(|range| data[range.clone()].to_vec())
90            .collect::<Vec<_>>();
91        log::debug!("{:?}", mailbox_data);
92        // check if all the values are zero
93        let is_all_zeros = mailbox_data.iter().all(|d| d.iter().all(|&x| x == 0));
94
95        // flag to check if slave is responding
96        slave_is_mailbox_pdo_responding[i as usize] = true;
97        // if all the values are zero for more than 1s
98        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; // set the all slaves responding flag to false
105                slave_is_mailbox_pdo_responding[i as usize] = false;
106            }
107        } else {
108            // if the values are not zero
109            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
127/// write to the mailbox sdo
128/// - write the data to the mailbox sdo with the given index and subindex
129/// - the data size is determined automatically by the data type
130pub 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
143/// read from the mailbox sdo
144/// - read the data from the mailbox sdo with the given index and subindex
145/// - the data size is determined automatically from the data vector's number of elements
146pub 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}
ethercat_controller/mailboxes.rs

ethercat_controller/
mailboxes.rs
