139 } |
139 } |
140 |
140 |
141 /*****************************************************************************/ |
141 /*****************************************************************************/ |
142 |
142 |
143 /** |
143 /** |
144 Schweibt ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse). |
144 Liest ein CANopen-SDO (service data object). |
145 |
145 */ |
146 Siehe ecrt_slave_sdo_write() |
146 |
|
147 int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT-Slave */ |
|
148 uint16_t sdo_index, /**< SDO-Index */ |
|
149 uint8_t sdo_subindex, /**< SDO-Subindex */ |
|
150 uint8_t *target, /**< Speicher für gel. Wert */ |
|
151 size_t *size /**< Größe des Speichers */ |
|
152 ) |
|
153 { |
|
154 uint8_t data[0x20]; |
|
155 size_t rec_size, data_size; |
|
156 uint32_t complete_size; |
|
157 |
|
158 EC_WRITE_U16(data, 0x2000); // Number (0), Service = SDO request |
|
159 EC_WRITE_U8 (data + 2, 0x2 << 5); // Initiate upload request |
|
160 EC_WRITE_U16(data + 3, sdo_index); |
|
161 EC_WRITE_U8 (data + 5, sdo_subindex); |
|
162 |
|
163 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
|
164 |
|
165 rec_size = 0x20; |
|
166 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
|
167 |
|
168 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
|
169 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request |
|
170 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
|
171 sdo_index, sdo_subindex, slave->ring_position); |
|
172 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
|
173 return -1; |
|
174 } |
|
175 |
|
176 if (EC_READ_U16(data) >> 12 != 0x03 || // SDO response |
|
177 EC_READ_U8 (data + 2) >> 5 != 0x02 || // Initiate upload response |
|
178 EC_READ_U16(data + 3) != sdo_index || // Index |
|
179 EC_READ_U8 (data + 5) != sdo_subindex) { // Subindex |
|
180 EC_ERR("Invalid SDO upload response at slave %i!\n", |
|
181 slave->ring_position); |
|
182 return -1; |
|
183 } |
|
184 |
|
185 if (rec_size < 10) { |
|
186 EC_ERR("Received currupted SDO upload response!\n"); |
|
187 return -1; |
|
188 } |
|
189 |
|
190 if ((complete_size = EC_READ_U32(data + 6)) > *size) { |
|
191 EC_ERR("SDO data does not fit into buffer (%i / %i)!\n", |
|
192 complete_size, *size); |
|
193 return -1; |
|
194 } |
|
195 |
|
196 data_size = rec_size - 10; |
|
197 |
|
198 if (data_size == complete_size) { |
|
199 memcpy(target, data + 10, data_size); |
|
200 } |
|
201 else { |
|
202 EC_ERR("SDO data incomplete.\n"); |
|
203 return -1; |
|
204 } |
|
205 |
|
206 return 0; |
|
207 } |
|
208 |
|
209 /*****************************************************************************/ |
|
210 |
|
211 /** |
|
212 Schreibt ein CANopen-SDO (Angabe des Masters und der Adresse), expedited. |
|
213 |
|
214 Siehe ecrt_slave_sdo_exp_write() |
147 |
215 |
148 \return 0 wenn alles ok, < 0 bei Fehler |
216 \return 0 wenn alles ok, < 0 bei Fehler |
149 */ |
217 */ |
150 |
218 |
151 int ecrt_master_sdo_write(ec_master_t *master, |
219 int ecrt_master_sdo_exp_write(ec_master_t *master, |
152 /**< EtherCAT-Master */ |
220 /**< EtherCAT-Master */ |
153 const char *addr, |
221 const char *addr, |
154 /**< Addresse, siehe ec_master_slave_address() */ |
222 /**< Addresse, siehe ec_master_slave_address() */ |
155 uint16_t index, |
223 uint16_t index, |
156 /**< SDO-Index */ |
224 /**< SDO-Index */ |
157 uint8_t subindex, |
225 uint8_t subindex, |
158 /**< SDO-Subindex */ |
226 /**< SDO-Subindex */ |
159 uint32_t value, |
227 uint32_t value, |
160 /**< Neuer Wert */ |
228 /**< Neuer Wert */ |
161 size_t size |
229 size_t size |
162 /**< Größe des Datenfeldes */ |
230 /**< Größe des Datenfeldes */ |
163 ) |
231 ) |
164 { |
232 { |
165 ec_slave_t *slave; |
233 ec_slave_t *slave; |
166 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
234 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
167 return ecrt_slave_sdo_write(slave, index, subindex, value, size); |
235 return ecrt_slave_sdo_exp_write(slave, index, subindex, value, size); |
168 } |
236 } |
169 |
237 |
170 /*****************************************************************************/ |
238 /*****************************************************************************/ |
171 |
239 |
172 /** |
240 /** |
173 Liest ein CANopen-SDO (Variante mit Angabe des Masters und der Adresse). |
241 Liest ein CANopen-SDO (Angabe des Masters und der Adresse), expedited. |
174 |
242 |
175 Siehe ecrt_slave_sdo_read() |
243 Siehe ecrt_slave_sdo_exp_read() |
176 |
244 |
177 \return 0 wenn alles ok, < 0 bei Fehler |
245 \return 0 wenn alles ok, < 0 bei Fehler |
178 */ |
246 */ |
179 |
247 |
180 int ecrt_master_sdo_read(ec_master_t *master, |
248 int ecrt_master_sdo_exp_read(ec_master_t *master, |
181 /**< EtherCAT-Slave */ |
249 /**< EtherCAT-Slave */ |
182 const char *addr, |
250 const char *addr, |
183 /**< Addresse, siehe ec_master_slave_address() */ |
251 /**< Addresse, siehe ec_master_slave_address() */ |
184 uint16_t index, |
252 uint16_t index, |
185 /**< SDO-Index */ |
253 /**< SDO-Index */ |
186 uint8_t subindex, |
254 uint8_t subindex, |
187 /**< SDO-Subindex */ |
255 /**< SDO-Subindex */ |
188 uint32_t *value |
256 uint32_t *value |
189 /**< Speicher für gel. Wert */ |
257 /**< Speicher für gel. Wert */ |
190 ) |
258 ) |
191 { |
259 { |
192 ec_slave_t *slave; |
260 ec_slave_t *slave; |
193 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
261 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
194 return ecrt_slave_sdo_read(slave, index, subindex, value); |
262 return ecrt_slave_sdo_exp_read(slave, index, subindex, value); |
|
263 } |
|
264 |
|
265 /*****************************************************************************/ |
|
266 |
|
267 /** |
|
268 Liest ein CANopen-SDO (Angabe des Masters und der Adresse), expedited. |
|
269 |
|
270 Siehe ecrt_slave_sdo_exp_read() |
|
271 |
|
272 \return 0 wenn alles ok, < 0 bei Fehler |
|
273 */ |
|
274 |
|
275 int ecrt_master_sdo_read(ec_master_t *master, /**< EtherCAT-Master */ |
|
276 const char *addr, /**< Addresse, siehe |
|
277 ec_master_slave_address() */ |
|
278 uint16_t sdo_index, /**< SDO-Index */ |
|
279 uint8_t sdo_subindex, /**< SDO-Subindex */ |
|
280 uint8_t *target, /**< Speicher für gel. Wert */ |
|
281 size_t *size /**< Größe des Speichers */ |
|
282 ) |
|
283 { |
|
284 ec_slave_t *slave; |
|
285 if (!(slave = ec_master_slave_address(master, addr))) return -1; |
|
286 return ecrt_slave_sdo_read(slave, sdo_index, sdo_subindex, target, size); |
195 } |
287 } |
196 |
288 |
197 /*****************************************************************************/ |
289 /*****************************************************************************/ |
198 |
290 |
199 /** |
291 /** |