98 * then it called again each time a SDO exchange is |
117 * then it called again each time a SDO exchange is |
99 * finished. |
118 * finished. |
100 ********************************************************/ |
119 ********************************************************/ |
101 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId) |
120 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId) |
102 { |
121 { |
103 // Step counts number of times ConfigureSlaveNode is called |
|
104 static step = 1; |
|
105 |
|
106 UNS8 Transmission_Type = 0x01; |
|
107 UNS16 Heartbeat_Producer_Time = 0x03E8; |
|
108 UNS32 abortCode; |
|
109 UNS8 res; |
122 UNS8 res; |
110 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId); |
123 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId); |
111 switch(step++){ |
124 switch(++init_step){ |
112 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/ |
125 case 1: |
|
126 { /*disable Slave's TPDO 1 */ |
|
127 UNS32 TPDO_COBId = 0x80000180 + nodeId; |
|
128 |
|
129 eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId); |
|
130 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
131 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
132 nodeId, /*UNS8 nodeId*/ |
|
133 0x1800, /*UNS16 index*/ |
|
134 0x01, /*UNS8 subindex*/ |
|
135 4, /*UNS8 count*/ |
|
136 0, /*UNS8 dataType*/ |
|
137 &TPDO_COBId,/*void *data*/ |
|
138 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
139 } |
|
140 break; |
|
141 |
|
142 case 2: |
|
143 { /*setup Slave's TPDO 1 to be transmitted on SYNC*/ |
|
144 UNS8 Transmission_Type = 0x01; |
|
145 |
113 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId); |
146 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId); |
114 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
147 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
115 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
148 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
116 nodeId, /*UNS8 nodeId*/ |
149 nodeId, /*UNS8 nodeId*/ |
117 0x1800, /*UNS16 index*/ |
150 0x1800, /*UNS16 index*/ |
118 0x02, /*UNS8 subindex*/ |
151 0x02, /*UNS8 subindex*/ |
119 1, /*UNS8 count*/ |
152 1, /*UNS8 count*/ |
120 0, /*UNS8 dataType*/ |
153 0, /*UNS8 dataType*/ |
121 &Transmission_Type,/*void *data*/ |
154 &Transmission_Type,/*void *data*/ |
122 ConfigureSlaveNode); /*SDOCallback_t Callback*/ |
155 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
156 } |
|
157 break; |
|
158 |
|
159 case 3: |
|
160 { /*re-enable Slave's TPDO 1 */ |
|
161 UNS32 TPDO_COBId = 0x00000180 + nodeId; |
|
162 |
|
163 eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId); |
|
164 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
165 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
166 nodeId, /*UNS8 nodeId*/ |
|
167 0x1800, /*UNS16 index*/ |
|
168 0x01, /*UNS8 subindex*/ |
|
169 4, /*UNS8 count*/ |
|
170 0, /*UNS8 dataType*/ |
|
171 &TPDO_COBId,/*void *data*/ |
|
172 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
173 } |
123 break; |
174 break; |
124 |
175 |
|
176 case 4: |
|
177 { /*disable Slave's TPDO 2 */ |
|
178 UNS32 TPDO_COBId = 0x80000200 + nodeId; |
|
179 |
|
180 eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId); |
|
181 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
182 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
183 nodeId, /*UNS8 nodeId*/ |
|
184 0x1400, /*UNS16 index*/ |
|
185 0x01, /*UNS8 subindex*/ |
|
186 4, /*UNS8 count*/ |
|
187 0, /*UNS8 dataType*/ |
|
188 &TPDO_COBId,/*void *data*/ |
|
189 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
190 } |
|
191 break; |
|
192 |
125 |
193 |
126 case 2: /*Second step*/ |
194 case 5: |
127 if(getWriteResultNetworkDict (d, slavenodeid, &abortCode) != SDO_FINISHED) |
195 { |
128 eprintf("Master : Couldn't set slave %2.2x TPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode); |
196 UNS8 Transmission_Type = 0x01; |
129 |
197 |
130 /* Finalise last SDO transfer with this node */ |
|
131 closeSDOtransfer(&TestMaster_Data, |
|
132 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
133 slavenodeid, /*UNS8 nodeId*/ |
|
134 SDO_CLIENT); |
|
135 |
|
136 eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId); |
198 eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId); |
137 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
199 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
138 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
200 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
139 slavenodeid, /*UNS8 nodeId*/ |
201 nodeId, /*UNS8 nodeId*/ |
140 0x1400, /*UNS16 index*/ |
202 0x1400, /*UNS16 index*/ |
141 0x02, /*UNS8 subindex*/ |
203 0x02, /*UNS8 subindex*/ |
142 1, /*UNS8 count*/ |
204 1, /*UNS8 count*/ |
143 0, /*UNS8 dataType*/ |
205 0, /*UNS8 dataType*/ |
144 &Transmission_Type,/*void *data*/ |
206 &Transmission_Type,/*void *data*/ |
145 ConfigureSlaveNode); /*SDOCallback_t Callback*/ |
207 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
208 } |
|
209 break; |
|
210 |
|
211 case 6: |
|
212 { /*re-enable Slave's TPDO 1 */ |
|
213 UNS32 TPDO_COBId = 0x00000200 + nodeId; |
|
214 |
|
215 eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId); |
|
216 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
217 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
218 nodeId, /*UNS8 nodeId*/ |
|
219 0x1400, /*UNS16 index*/ |
|
220 0x01, /*UNS8 subindex*/ |
|
221 4, /*UNS8 count*/ |
|
222 0, /*UNS8 dataType*/ |
|
223 &TPDO_COBId,/*void *data*/ |
|
224 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
225 } |
146 break; |
226 break; |
147 |
227 |
148 case 3: /*Second step*/ |
228 case 7: |
149 if(getWriteResultNetworkDict (d, slavenodeid, &abortCode) != SDO_FINISHED) |
229 { |
150 eprintf("Master : Couldn't set slave %2.2x RPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode); |
230 UNS16 Heartbeat_Producer_Time = 0x03E8; |
151 |
|
152 /* Finalise last SDO transfer with this node */ |
|
153 closeSDOtransfer(&TestMaster_Data, |
|
154 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
155 slavenodeid, /*UNS8 nodeId*/ |
|
156 SDO_CLIENT); |
|
157 |
|
158 eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId); |
231 eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId); |
159 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
232 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
160 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
233 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
161 slavenodeid, /*UNS8 nodeId*/ |
234 nodeId, /*UNS8 nodeId*/ |
162 0x1017, /*UNS16 index*/ |
235 0x1017, /*UNS16 index*/ |
163 0x00, /*UNS8 subindex*/ |
236 0x00, /*UNS8 subindex*/ |
164 2, /*UNS8 count*/ |
237 2, /*UNS8 count*/ |
165 0, /*UNS8 dataType*/ |
238 0, /*UNS8 dataType*/ |
166 &Heartbeat_Producer_Time,/*void *data*/ |
239 &Heartbeat_Producer_Time,/*void *data*/ |
167 ConfigureSlaveNode); /*SDOCallback_t Callback*/ |
240 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
168 break; |
241 } |
|
242 break; |
|
243 |
|
244 case 8: |
|
245 { /*disable Slave's TPDO 2 */ |
|
246 UNS32 TPDO_COBId = 0x80000280 + nodeId; |
|
247 |
|
248 eprintf("Master : disable slave %2.2x TPDO 2 \n", nodeId); |
|
249 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
250 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
251 nodeId, /*UNS8 nodeId*/ |
|
252 0x1801, /*UNS16 index*/ |
|
253 0x01, /*UNS8 subindex*/ |
|
254 4, /*UNS8 count*/ |
|
255 0, /*UNS8 dataType*/ |
|
256 &TPDO_COBId,/*void *data*/ |
|
257 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
258 } |
|
259 break; |
|
260 |
|
261 case 9: |
|
262 { /*disable Slave's TPDO 3 */ |
|
263 UNS32 TPDO_COBId = 0x80000380 + nodeId; |
|
264 |
|
265 eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId); |
|
266 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
267 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
268 nodeId, /*UNS8 nodeId*/ |
|
269 0x1802, /*UNS16 index*/ |
|
270 0x01, /*UNS8 subindex*/ |
|
271 4, /*UNS8 count*/ |
|
272 0, /*UNS8 dataType*/ |
|
273 &TPDO_COBId,/*void *data*/ |
|
274 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
275 } |
|
276 break; |
|
277 |
|
278 case 10: |
|
279 { /*disable Slave's TPDO 2 */ |
|
280 UNS32 TPDO_COBId = 0x80000480 + nodeId; |
|
281 |
|
282 eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId); |
|
283 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
284 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
285 nodeId, /*UNS8 nodeId*/ |
|
286 0x1803, /*UNS16 index*/ |
|
287 0x01, /*UNS8 subindex*/ |
|
288 4, /*UNS8 count*/ |
|
289 0, /*UNS8 dataType*/ |
|
290 &TPDO_COBId,/*void *data*/ |
|
291 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
292 } |
|
293 break; |
169 |
294 |
170 case 4: /*Second step*/ |
295 case 11: |
171 |
|
172 if(getWriteResultNetworkDict (d, slavenodeid, &abortCode) != SDO_FINISHED) |
|
173 eprintf("Master : Couldn't set slave %2.2x Heartbeat_Producer_Time. AbortCode :%4.4x \n", nodeId, abortCode); |
|
174 |
|
175 /* Finalise last SDO transfer with this node */ |
|
176 closeSDOtransfer(&TestMaster_Data, |
|
177 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
178 slavenodeid, /*UNS8 nodeId*/ |
|
179 SDO_CLIENT); |
|
180 |
|
181 /* Put the master in operational mode */ |
296 /* Put the master in operational mode */ |
182 setState(d, Operational); |
297 setState(d, Operational); |
183 |
298 |
184 /* Ask slave node to go in operational mode */ |
299 /* Ask slave node to go in operational mode */ |
185 masterSendNMTstateChange (d, slavenodeid, NMT_Start_Node); |
300 masterSendNMTstateChange (d, nodeId, NMT_Start_Node); |
186 } |
301 } |
187 |
302 |
188 } |
303 } |
189 |
304 |
190 void TestMaster_preOperational() |
305 void TestMaster_preOperational() |