20 |
21 |
21 /** |
22 /** |
22 Schreibt ein CANopen-SDO (service data object). |
23 Schreibt ein CANopen-SDO (service data object). |
23 */ |
24 */ |
24 |
25 |
25 int EtherCAT_rt_canopen_sdo_write( |
26 int EtherCAT_rt_canopen_sdo_write(ec_slave_t *slave, /**< EtherCAT-Slave */ |
26 ec_slave_t *slave, /**< EtherCAT-Slave */ |
27 uint16_t sdo_index, /**< SDO-Index */ |
27 unsigned int sdo_index, /**< SDO-Index */ |
28 uint8_t sdo_subindex, /**< SDO-Subindex */ |
28 unsigned char sdo_subindex, /**< SDO-Subindex */ |
29 uint32_t value, /**< Neuer Wert */ |
29 unsigned int value, /**< Neuer Wert */ |
30 size_t size /**< Größe des Datenfeldes */ |
30 unsigned int size /**< Größe des Datenfeldes */ |
31 ) |
31 ) |
|
32 { |
32 { |
33 unsigned char data[0xF6]; |
33 unsigned char data[0xF6]; |
34 ec_frame_t frame; |
34 ec_frame_t frame; |
35 unsigned int tries_left, i; |
35 unsigned int tries_left, i; |
36 ec_master_t *master; |
36 ec_master_t *master; |
42 if (size == 0 || size > 4) { |
42 if (size == 0 || size > 4) { |
43 printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size); |
43 printk(KERN_ERR "EtherCAT: Illegal SDO data size: %i!\n", size); |
44 return -1; |
44 return -1; |
45 } |
45 } |
46 |
46 |
47 data[0] = 0x0A; // Length of the Mailbox service data |
47 EC_WRITE_U16(data, 0x000A); // Length of the Mailbox service data |
48 data[1] = 0x00; |
48 EC_WRITE_U16(data + 2, slave->station_address); // Station address |
49 data[2] = slave->station_address & 0xFF; // Station address |
49 EC_WRITE_U8 (data + 4, 0x00); // Channel & priority |
50 data[3] = (slave->station_address >> 8) & 0xFF; |
50 EC_WRITE_U8 (data + 5, 0x03); // CANopen over EtherCAT |
51 data[4] = 0x00; // Channel & priority |
51 EC_WRITE_U16(data + 6, 0x2000); // Number & Service |
52 data[5] = 0x03; // CANopen over EtherCAT |
52 EC_WRITE_U8 (data + 8, 0x13 | ((4 - size) << 2)); // Spec., exp., init. |
53 data[6] = 0x00; // Number(7-0) |
53 EC_WRITE_U16(data + 9, sdo_index); |
54 data[7] = 0x2 << 4; // Number(8) & Service = SDO Request (0x02) |
54 EC_WRITE_U8 (data + 11, sdo_subindex); |
55 data[8] = 0x01 // Size specified |
|
56 | (0x1 << 1) // Transfer type = Expedited |
|
57 | ((4 - size) << 2) // Data Set Size |
|
58 | (0x1 << 5); // Command specifier = Initiate download request (0x01) |
|
59 data[9] = sdo_index & 0xFF; |
|
60 data[10] = (sdo_index >> 8) & 0xFF; |
|
61 data[11] = sdo_subindex; |
|
62 |
55 |
63 for (i = 0; i < size; i++) { |
56 for (i = 0; i < size; i++) { |
64 data[12 + i] = value & 0xFF; |
57 EC_WRITE_U8(data + 12 + i, value & 0xFF); |
65 value >>= 8; |
58 value >>= 8; |
66 } |
59 } |
67 |
60 |
68 ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
61 ec_frame_init_npwr(&frame, master, slave->station_address, 0x1800, 0xF6, |
69 data); |
62 data); |
113 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
106 printk(KERN_ERR "EtherCAT: Mailbox receive - Slave %i did not" |
114 " respond!\n", slave->ring_position); |
107 " respond!\n", slave->ring_position); |
115 return -1; |
108 return -1; |
116 } |
109 } |
117 |
110 |
118 if (frame.data[5] != 0x03 // COE |
111 if (EC_READ_U8 (frame.data + 5) != 0x03 || // COE |
119 || (frame.data[7] >> 4) != 0x03 // SDO response |
112 EC_READ_U16(frame.data + 6) != 0x3000 || // SDO response |
120 || (frame.data[8] >> 5) != 0x03 // Initiate download response |
113 EC_READ_U8 (frame.data + 8) >> 5 != 0x03 || // Download response |
121 || (frame.data[9] != (sdo_index & 0xFF)) // Index |
114 EC_READ_U16(frame.data + 9) != sdo_index || // Index |
122 || (frame.data[10] != ((sdo_index >> 8) & 0xFF)) |
115 EC_READ_U8 (frame.data + 11) != sdo_subindex) // Subindex |
123 || (frame.data[11] != sdo_subindex)) // Subindex |
|
124 { |
116 { |
125 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
117 printk(KERN_ERR "EtherCAT: Illegal mailbox response at slave %i!\n", |
126 slave->ring_position); |
118 slave->ring_position); |
127 return -1; |
119 return -1; |
128 } |
120 } |