61 ::close(fd); |
60 ::close(fd); |
62 } |
61 } |
63 |
62 |
64 /****************************************************************************/ |
63 /****************************************************************************/ |
65 |
64 |
66 unsigned int Master::slaveCount() |
|
67 { |
|
68 unsigned int numSlaves; |
|
69 |
|
70 if (ioctl(fd, EC_IOCTL_SLAVE_COUNT, &numSlaves)) { |
|
71 stringstream err; |
|
72 err << "Failed to get number of slaves: " << strerror(errno); |
|
73 throw MasterException(err.str()); |
|
74 } |
|
75 |
|
76 return numSlaves; |
|
77 } |
|
78 |
|
79 /****************************************************************************/ |
|
80 |
|
81 void Master::listSlaves() |
65 void Master::listSlaves() |
82 { |
66 { |
83 unsigned int numSlaves = slaveCount(), i; |
67 unsigned int numSlaves = slaveCount(), i; |
84 struct ec_ioctl_slave_info *infos, *info; |
68 ec_ioctl_slave_t slave; |
85 uint16_t lastAlias, aliasIndex; |
69 uint16_t lastAlias, aliasIndex; |
86 |
70 |
87 if (!numSlaves) |
|
88 return; |
|
89 |
|
90 infos = new struct ec_ioctl_slave_info[numSlaves]; |
|
91 |
|
92 if (ioctl(fd, EC_IOCTL_SLAVE_INFO, infos)) { |
|
93 stringstream err; |
|
94 err << "Failed to get slave information: " << strerror(errno); |
|
95 throw MasterException(err.str()); |
|
96 } |
|
97 |
|
98 lastAlias = 0; |
71 lastAlias = 0; |
99 aliasIndex = 0; |
72 aliasIndex = 0; |
100 for (i = 0; i < numSlaves; i++) { |
73 for (i = 0; i < numSlaves; i++) { |
101 info = &infos[i]; |
74 getSlave(&slave, i); |
102 cout << setw(2) << info->ring_position << " "; |
75 cout << setw(2) << i << " "; |
103 |
76 |
104 if (info->alias) { |
77 if (slave.alias) { |
105 lastAlias = info->alias; |
78 lastAlias = slave.alias; |
106 aliasIndex = 0; |
79 aliasIndex = 0; |
107 } |
80 } |
108 if (lastAlias) { |
81 if (lastAlias) { |
109 cout << setw(10) << "#" << lastAlias << ":" << aliasIndex; |
82 cout << setw(10) << "#" << lastAlias << ":" << aliasIndex; |
110 } |
83 } |
111 |
84 |
112 cout << " " << slaveState(info->state) << " "; |
85 cout << " " << slaveState(slave.state) << " "; |
113 |
86 |
114 if (strlen(info->description)) { |
87 if (strlen(slave.name)) { |
115 cout << info->description; |
88 cout << slave.name; |
116 } else { |
89 } else { |
117 cout << "0x" << hex << setfill('0') << info->vendor_id |
90 cout << "0x" << hex << setfill('0') << slave.vendor_id |
118 << ":0x" << info->product_code; |
91 << ":0x" << slave.product_code; |
119 } |
92 } |
120 |
93 |
121 cout << endl; |
94 cout << endl; |
122 } |
95 } |
123 |
96 } |
124 delete [] infos; |
97 |
125 } |
98 /****************************************************************************/ |
126 |
99 |
127 /****************************************************************************/ |
100 void Master::listPdos(int slavePosition) |
128 |
101 { |
129 string Master::slaveState(uint8_t state) const |
102 ec_ioctl_slave_t slave; |
|
103 ec_ioctl_sync_t sync; |
|
104 ec_ioctl_pdo_t pdo; |
|
105 ec_ioctl_pdo_entry_t entry; |
|
106 unsigned int i, j, k; |
|
107 |
|
108 getSlave(&slave, slavePosition); |
|
109 |
|
110 for (i = 0; i < slave.sync_count; i++) { |
|
111 getSync(&sync, slavePosition, i); |
|
112 |
|
113 cout << "SM" << i << ":" |
|
114 << " PhysAddr 0x" |
|
115 << hex << setfill('0') << setw(4) << sync.physical_start_address |
|
116 << ", DefaultSize " |
|
117 << dec << setfill(' ') << setw(4) << sync.default_size |
|
118 << ", ControlRegister 0x" |
|
119 << hex << setfill('0') << setw(2) |
|
120 << (unsigned int) sync.control_register |
|
121 << ", Enable " << dec << (unsigned int) sync.enable |
|
122 << endl; |
|
123 |
|
124 for (j = 0; j < sync.pdo_count; j++) { |
|
125 getPdo(&pdo, slavePosition, i, j); |
|
126 |
|
127 cout << " " << (pdo.dir ? "T" : "R") << "xPdo 0x" |
|
128 << hex << setfill('0') << setw(4) << pdo.index |
|
129 << " \"" << pdo.name << "\"" << endl; |
|
130 |
|
131 for (k = 0; k < pdo.entry_count; k++) { |
|
132 getPdoEntry(&entry, slavePosition, i, j, k); |
|
133 |
|
134 cout << " Pdo entry 0x" |
|
135 << hex << setfill('0') << setw(4) << entry.index |
|
136 << ":" << hex << setfill('0') << setw(2) |
|
137 << (unsigned int) entry.subindex |
|
138 << ", " << dec << (unsigned int) entry.bit_length |
|
139 << " bit, \"" << entry.name << "\"" << endl; |
|
140 } |
|
141 } |
|
142 } |
|
143 } |
|
144 |
|
145 /****************************************************************************/ |
|
146 |
|
147 unsigned int Master::slaveCount() |
|
148 { |
|
149 int ret; |
|
150 |
|
151 if ((ret = ioctl(fd, EC_IOCTL_SLAVE_COUNT, 0)) < 0) { |
|
152 stringstream err; |
|
153 err << "Failed to get slave: " << strerror(errno); |
|
154 throw MasterException(err.str()); |
|
155 } |
|
156 |
|
157 return ret; |
|
158 } |
|
159 |
|
160 /****************************************************************************/ |
|
161 |
|
162 void Master::getSlave(ec_ioctl_slave_t *slave, uint16_t slaveIndex) |
|
163 { |
|
164 slave->position = slaveIndex; |
|
165 |
|
166 if (ioctl(fd, EC_IOCTL_SLAVE, slave)) { |
|
167 stringstream err; |
|
168 err << "Failed to get slave: "; |
|
169 if (errno == EINVAL) |
|
170 err << "Slave " << slaveIndex << " does not exist!"; |
|
171 else |
|
172 err << strerror(errno); |
|
173 throw MasterException(err.str()); |
|
174 } |
|
175 } |
|
176 |
|
177 /****************************************************************************/ |
|
178 |
|
179 void Master::getSync( |
|
180 ec_ioctl_sync_t *sync, |
|
181 uint16_t slaveIndex, |
|
182 uint8_t syncIndex |
|
183 ) |
|
184 { |
|
185 sync->slave_position = slaveIndex; |
|
186 sync->sync_index = syncIndex; |
|
187 |
|
188 if (ioctl(fd, EC_IOCTL_SYNC, sync)) { |
|
189 stringstream err; |
|
190 err << "Failed to get sync manager: "; |
|
191 if (errno == EINVAL) |
|
192 err << "Either slave " << slaveIndex << " does not exist, " |
|
193 << "or contains less than " << (unsigned int) syncIndex + 1 |
|
194 << " sync managers!"; |
|
195 else |
|
196 err << strerror(errno); |
|
197 throw MasterException(err.str()); |
|
198 } |
|
199 } |
|
200 |
|
201 /****************************************************************************/ |
|
202 |
|
203 void Master::getPdo( |
|
204 ec_ioctl_pdo_t *pdo, |
|
205 uint16_t slaveIndex, |
|
206 uint8_t syncIndex, |
|
207 uint8_t pdoPos |
|
208 ) |
|
209 { |
|
210 pdo->slave_position = slaveIndex; |
|
211 pdo->sync_index = syncIndex; |
|
212 pdo->pdo_pos = pdoPos; |
|
213 |
|
214 if (ioctl(fd, EC_IOCTL_PDO, pdo)) { |
|
215 stringstream err; |
|
216 err << "Failed to get Pdo: "; |
|
217 if (errno == EINVAL) |
|
218 err << "Either slave " << slaveIndex << " does not exist, " |
|
219 << "or contains less than " << (unsigned int) syncIndex + 1 |
|
220 << " sync managers, or sync manager " |
|
221 << (unsigned int) syncIndex << " contains less than " |
|
222 << pdoPos + 1 << " Pdos!" << endl; |
|
223 else |
|
224 err << strerror(errno); |
|
225 throw MasterException(err.str()); |
|
226 } |
|
227 } |
|
228 |
|
229 /****************************************************************************/ |
|
230 |
|
231 void Master::getPdoEntry( |
|
232 ec_ioctl_pdo_entry_t *entry, |
|
233 uint16_t slaveIndex, |
|
234 uint8_t syncIndex, |
|
235 uint8_t pdoPos, |
|
236 uint8_t entryPos |
|
237 ) |
|
238 { |
|
239 entry->slave_position = slaveIndex; |
|
240 entry->sync_index = syncIndex; |
|
241 entry->pdo_pos = pdoPos; |
|
242 entry->entry_pos = entryPos; |
|
243 |
|
244 if (ioctl(fd, EC_IOCTL_PDO_ENTRY, entry)) { |
|
245 stringstream err; |
|
246 err << "Failed to get Pdo entry: "; |
|
247 if (errno == EINVAL) |
|
248 err << "Either slave " << slaveIndex << " does not exist, " |
|
249 << "or contains less than " << (unsigned int) syncIndex + 1 |
|
250 << " sync managers, or sync manager " |
|
251 << (unsigned int) syncIndex << " contains less than " |
|
252 << pdoPos + 1 << " Pdos, or the Pdo at position " << pdoPos |
|
253 << " contains less than " << (unsigned int) entryPos + 1 |
|
254 << " entries!" << endl; |
|
255 else |
|
256 err << strerror(errno); |
|
257 throw MasterException(err.str()); |
|
258 } |
|
259 } |
|
260 |
|
261 /****************************************************************************/ |
|
262 |
|
263 string Master::slaveState(uint8_t state) |
130 { |
264 { |
131 switch (state) { |
265 switch (state) { |
132 case 1: return "INIT"; |
266 case 1: return "INIT"; |
133 case 2: return "PREOP"; |
267 case 2: return "PREOP"; |
134 case 4: return "SAFEOP"; |
268 case 4: return "SAFEOP"; |