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}