ethercat_controller/
watchdog.rs

1use crate::SlaveOffsets;
2use std::ops::Range;
3
4/// watchdog is added to the manufcturer specific data of the statusword
5/// bits 8, 14 and 15
6/// parse the 3bit watchdog counter from the statusword
7///
8/// # Arguments
9///
10/// * `statusword` - The statusword of the slave
11///
12/// # Returns
13///
14/// * `u8` - The watchdog counter
15fn parse_watchdog_from_status(statusword: Vec<u8>) -> u8 {
16    // bit 8
17    let mut watchdog_counter = (statusword[1] & 0b0000_0001);
18    // bits 14 and 15
19    watchdog_counter = watchdog_counter | ((statusword[1] & 0b1100_0000) >> 5);
20    // return the counter
21    watchdog_counter
22}
23
24/// write the watchdog counter to the controlword
25/// to the bits 11-15 which are manufacturer specific
26///
27/// # Arguments
28///
29/// * `control_word` - The controlword of the slave
30/// * `watchdog_counter` - The watchdog counter to be written
31///
32/// # Returns
33///
34/// * `Vec<u8>` - The controlword with the watchdog counter written
35///
36fn write_watchdog_to_control(control_word: Vec<u8>, watchdog_counter: u8) -> Vec<u8> {
37    let mut control_word = control_word;
38    // clear the bits 11-15
39    control_word[1] &= 0b0000_0111;
40    // write the watchdog counter to the controlword
41    control_word[1] |= watchdog_counter << 3;
42    // return the controlword
43    control_word
44}
45
46/// verify the watchdog of the slaves
47/// verify that the slaves are still writing
48/// checking if the watchdog counter is the same as the previous cycle
49/// - if the counter is the same, check for how long has it been the same
50///      - if it is the same for more than 1s the slave is considered not responding
51/// - if the counter is different, update the timestamp
52/// - write the watchdog counter to the controlword
53///
54/// # Arguments
55///
56/// * `slave_number` - The number of slaves
57/// * `data` - The domain data
58/// * `watchdog_timeout_ms` - The timeout for the watchdog in milliseconds
59/// * `watchdog_counter` - The watchdog counter to be written
60/// * `slave_watchdog_control_offsets` - The controlword offsets for each slave
61/// * `slave_watchdog_status_offsets` - The statusword offsets for each slave
62/// * `slave_watchdog_timestamps` - The timestamps of the last watchdog update for each slave
63/// * `slave_is_watchdog_responding` - The flag to check if each slave is responding
64/// * `slave_previous_watchdog_counter` - The buffer to store the previous watchdog counter for each slave
65/// * `slave_name_from_id` - A function to get the slave name from the id
66///
67/// # Returns
68///
69/// * `bool` - The flag to check if all the slaves are responding
70pub fn verify_watchdog(
71    slave_number: u32,
72    data: &mut [u8],
73    watchdog_timeout_ms: u32,
74    watchdog_counter: u8,
75    slave_watchdog_control_offsets: &Vec<Vec<Range<usize>>>,
76    slave_watchdog_status_offsets: &Vec<Vec<Range<usize>>>,
77    slave_watchdog_timestamps: &mut Vec<std::time::Instant>,
78    slave_is_watchdog_responding: &mut Vec<bool>,
79    slave_previous_watchdog_counter: &mut Vec<u8>,
80    slave_name_from_id: &impl Fn(u16) -> String,
81) -> bool {
82    // return if all slaves responding
83    let mut all_slaves_responding = true;
84    // check each slave
85
86    for i in 0..slave_number {
87        // get slave watchdog control offset
88        let status_offset = slave_watchdog_status_offsets[i as usize].clone();
89        // get the watchdog control data
90        let status_data = status_offset
91            .iter()
92            .map(|range| data[range.clone()].to_vec())
93            .collect::<Vec<_>>();
94
95        // doutput the watchdog status in binary
96        let counter = parse_watchdog_from_status(status_data[0].clone());
97        log::debug!(
98            "Slave {} ({})| Watchdog counter received : {} ({:08b}), sent: {} ({:08b})",
99            i,
100            slave_name_from_id(i as u16),
101            counter,
102            counter,
103            watchdog_counter,
104            watchdog_counter
105        );
106
107        // check if the watchdog counter is the same as the one in the previous cycle
108        // if it is the same check for how long has it been the same
109        // if it is the same for more than 1s the slave is considered not responding
110        if slave_previous_watchdog_counter[i as usize] == counter {
111            if slave_watchdog_timestamps[i as usize].elapsed().as_millis() as u32
112                > watchdog_timeout_ms
113            {
114                all_slaves_responding &= false;
115                slave_is_watchdog_responding[i as usize] = false;
116            }
117        } else {
118            // if the watchdog counter is different
119            // update the timestamp
120            slave_watchdog_timestamps[i as usize] = std::time::Instant::now();
121            slave_is_watchdog_responding[i as usize] = true;
122            slave_previous_watchdog_counter[i as usize] = counter;
123        }
124
125        // write the counter to the controlword
126        let control_offset = slave_watchdog_control_offsets[i as usize].clone();
127        for (j, range) in control_offset.iter().enumerate() {
128            let control_word = data[range.clone()].to_vec();
129            data[range.clone()]
130                .copy_from_slice(&write_watchdog_to_control(control_word, watchdog_counter));
131        }
132    }
133    all_slaves_responding
134}
135
136/// initialize the watchdog settings
137/// find the offsets of the controlword and statusword data in the domain data
138/// initialize the timestamp, flag and buffer for the watchdog data
139///
140/// # Arguments
141///
142/// * `slave_number` - The number of slaves
143/// * `offsets` - The slave offsets
144/// * `get_reg_addr_ranges` - A function to get the register address ranges
145///
146/// # Returns
147///
148/// * `Vec<Vec<Range<usize>>>` - The controlword offsets
149/// * `Vec<Vec<Range<usize>>>` - The statusword offsets
150/// * `Vec<std::time::Instant>` - The timestamps
151/// * `Vec<bool>` - The flag to check if the slave is responding
152/// * `Vec<u8>` - The buffer to store the watchdog data
153///
154pub fn init_watchdog_settings(
155    slave_number: u32,
156    offsets: &SlaveOffsets,
157    get_reg_addr_ranges: &impl Fn(&SlaveOffsets, u16, &String) -> Vec<Range<usize>>,
158) -> (
159    Vec<Vec<Range<usize>>>,
160    Vec<Vec<Range<usize>>>,
161    Vec<std::time::Instant>,
162    Vec<bool>,
163    Vec<u8>,
164) {
165    // initialize the watchdog variables
166    // offsets of the statusword data in the domain data
167    let mut slave_watchdog_status_offsets = vec![];
168    // offsets of the controlword data in the domain data
169    let mut slave_watchdog_control_offsets = vec![];
170    // last read timestamp of the watchdog data
171    let slave_watchdog_timestamps = vec![std::time::Instant::now(); slave_number as usize];
172    // flag to check if the slave is responding
173    let slave_is_watchdog_responding = vec![true; slave_number as usize];
174    // buffer to store the watchdog data (that are read asynchronusly from the slaves)
175    let slave_previous_watchdog_counter = vec![0u8; slave_number as usize];
176
177    // find the watchdog offsets for each slave
178    for i in 0..slave_number {
179        let mut watchdog_offsets = vec![];
180        watchdog_offsets.append(&mut get_reg_addr_ranges(
181            &offsets,
182            i as u16,
183            &"controlword".to_string(),
184        ));
185        slave_watchdog_control_offsets.push(watchdog_offsets);
186    }
187    for i in 0..slave_number {
188        let mut watchdog_offsets = vec![];
189        watchdog_offsets.append(&mut get_reg_addr_ranges(
190            &offsets,
191            i as u16,
192            &"statusword".to_string(),
193        ));
194        slave_watchdog_status_offsets.push(watchdog_offsets);
195    }
196
197    (
198        slave_watchdog_control_offsets,
199        slave_watchdog_status_offsets,
200        slave_watchdog_timestamps,
201        slave_is_watchdog_responding,
202        slave_previous_watchdog_counter,
203    )
204}