101 uint16_t sdo_index, /**< SDO-Index */ |
101 uint16_t sdo_index, /**< SDO-Index */ |
102 uint8_t sdo_subindex, /**< SDO-Subindex */ |
102 uint8_t sdo_subindex, /**< SDO-Subindex */ |
103 uint32_t *value /**< Speicher für gel. Wert */ |
103 uint32_t *value /**< Speicher für gel. Wert */ |
104 ) |
104 ) |
105 { |
105 { |
106 uint8_t data[0x0A]; |
106 uint8_t data[0x20]; |
107 size_t rec_size; |
107 size_t rec_size; |
108 |
108 |
109 EC_WRITE_U16(data, 0x2000); // Number (0), Service (SDO request) |
109 EC_WRITE_U16(data, 0x2000); // Number (0), Service = SDO request |
110 EC_WRITE_U8 (data + 2, 0x1 << 1 | 0x2 << 5); // Exp., Upload request |
110 EC_WRITE_U8 (data + 2, 0x1 << 1 | 0x2 << 5); // Expedited upload request |
111 EC_WRITE_U16(data + 3, sdo_index); |
111 EC_WRITE_U16(data + 3, sdo_index); |
112 EC_WRITE_U8 (data + 5, sdo_subindex); |
112 EC_WRITE_U8 (data + 5, sdo_subindex); |
113 |
113 |
114 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
114 if (ec_slave_mailbox_send(slave, 0x03, data, 6)) return -1; |
115 |
115 |
116 rec_size = 6; |
116 rec_size = 0x20; |
117 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
117 if (ec_slave_mailbox_receive(slave, 0x03, data, &rec_size)) return -1; |
118 |
118 |
119 if (EC_READ_U16(data ) >> 12 == 0x02 && // SDO request |
119 if (EC_READ_U16(data) >> 12 == 0x02 && // SDO request |
120 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transf. req. |
120 EC_READ_U8 (data + 2) >> 5 == 0x04) { // Abort SDO transfer request |
121 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
121 EC_ERR("SDO upload of 0x%04X:%X aborted on slave %i.\n", |
122 sdo_index, sdo_subindex, slave->ring_position); |
122 sdo_index, sdo_subindex, slave->ring_position); |
123 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
123 ec_canopen_abort_msg(EC_READ_U32(data + 6)); |
124 return -1; |
124 return -1; |
125 } |
125 } |