|
1 /* |
|
2 This file is part of CanFestival, a library implementing CanOpen Stack. |
|
3 |
|
4 Copyright (C): Edouard TISSERANT , Francis DUPIN and Jorge BERZOSA |
|
5 |
|
6 See COPYING file for copyrights details. |
|
7 |
|
8 This library is free software; you can redistribute it and/or |
|
9 modify it under the terms of the GNU Lesser General Public |
|
10 License as published by the Free Software Foundation; either |
|
11 version 2.1 of the License, or (at your option) any later version. |
|
12 |
|
13 This library is distributed in the hope that it will be useful, |
|
14 but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
16 Lesser General Public License for more details. |
|
17 |
|
18 You should have received a copy of the GNU Lesser General Public |
|
19 License along with this library; if not, write to the Free Software |
|
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
|
21 */ |
|
22 |
|
23 #include "Master.h" |
|
24 #include "SlaveA.h" |
|
25 #include "SlaveB.h" |
|
26 #include "TestMasterSlaveLSS.h" |
|
27 |
|
28 extern s_BOARD MasterBoard; |
|
29 |
|
30 /*****************************************************************************/ |
|
31 void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID) |
|
32 { |
|
33 eprintf("TestMaster_heartbeatError %d\n", heartbeatID); |
|
34 } |
|
35 |
|
36 /******************************************************** |
|
37 * TestMaster_initialisation is responsible to |
|
38 * - setup master RPDO 1 to receive TPDO 1 from id 2 |
|
39 * - setup master RPDO 2 to receive TPDO 1 from id 3 |
|
40 ********************************************************/ |
|
41 void TestMaster_initialisation(CO_Data* d) |
|
42 { |
|
43 UNS32 PDO1_COBID = 0x0182; |
|
44 UNS32 PDO2_COBID = 0x0183; |
|
45 UNS8 size = sizeof(UNS32); |
|
46 |
|
47 eprintf("TestMaster_initialisation\n"); |
|
48 |
|
49 /***************************************** |
|
50 * Define RPDO to match slave ID=2 TPDO1* |
|
51 *****************************************/ |
|
52 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/ |
|
53 0x1400, /*UNS16 index*/ |
|
54 0x01, /*UNS8 subind*/ |
|
55 &PDO1_COBID, /*void * pSourceData,*/ |
|
56 &size, /* UNS8 * pExpectedSize*/ |
|
57 RW); /* UNS8 checkAccess */ |
|
58 |
|
59 /***************************************** |
|
60 * Define RPDO to match slave ID=3 TPDO1* |
|
61 *****************************************/ |
|
62 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/ |
|
63 0x1401, /*UNS16 index*/ |
|
64 0x01, /*UNS8 subind*/ |
|
65 &PDO2_COBID, /*void * pSourceData,*/ |
|
66 &size, /* UNS8 * pExpectedSize*/ |
|
67 RW); /* UNS8 checkAccess */ |
|
68 |
|
69 } |
|
70 |
|
71 // Step counts number of times ConfigureSlaveNode is called |
|
72 // There is one per each slave |
|
73 static init_step[] ={0,0}; |
|
74 |
|
75 /*Forward declaration*/ |
|
76 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId); |
|
77 |
|
78 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId) |
|
79 { |
|
80 UNS32 abortCode; |
|
81 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
|
82 eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode); |
|
83 |
|
84 /* Finalise last SDO transfer with this node */ |
|
85 closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT); |
|
86 |
|
87 ConfigureSlaveNode(d, nodeId); |
|
88 } |
|
89 |
|
90 /******************************************************** |
|
91 * ConfigureSlaveNode is responsible to |
|
92 * - setup slave 'n' TPDO 1 transmit type |
|
93 * - setup slave 'n' Producer Hertbeat Time |
|
94 * - setup the Consumer Heartbeat Time for slave 'n' |
|
95 * - switch to operational mode |
|
96 * - send NMT to slave |
|
97 ******************************************************** |
|
98 * This an example of : |
|
99 * Network Dictionary Access (SDO) with Callback |
|
100 * Slave node state change request (NMT) |
|
101 ******************************************************** |
|
102 * This is called first by TestMaster_post_SlaveBootup |
|
103 * after the LSS configuration has been done |
|
104 * then it called again each time a SDO exchange is |
|
105 * finished. |
|
106 ********************************************************/ |
|
107 |
|
108 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId) |
|
109 { |
|
110 /* Master configure heartbeat producer time at 0 ms |
|
111 * for slaves node-id 0x02 and 0x03 by DCF concise */ |
|
112 |
|
113 UNS8 Transmission_Type = 0x01; |
|
114 UNS16 Slave_Prod_Heartbeat_T=1000;//ms |
|
115 UNS32 Master_Cons_Heartbeat_Base=0x05DC; //1500ms |
|
116 UNS32 abortCode; |
|
117 UNS8 res; |
|
118 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId); |
|
119 |
|
120 switch(++init_step[nodeId-2]){ |
|
121 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/ |
|
122 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId); |
|
123 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
124 nodeId, /*UNS8 nodeId*/ |
|
125 0x1800, /*UNS16 index*/ |
|
126 0x02, /*UNS8 subindex*/ |
|
127 1, /*UNS8 count*/ |
|
128 0, /*UNS8 dataType*/ |
|
129 &Transmission_Type,/*void *data*/ |
|
130 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
131 break; |
|
132 case 2: /* Second step : Set the new heartbeat producer time in the slave */ |
|
133 { |
|
134 UNS32 Master_Cons_Heartbeat_T=Master_Cons_Heartbeat_Base + (nodeId * 0x10000); |
|
135 UNS8 size = sizeof(UNS32); |
|
136 |
|
137 eprintf("Master : set slave %2.2x Producer Heartbeat Time = %d\n", nodeId,Slave_Prod_Heartbeat_T); |
|
138 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
139 nodeId, /*UNS8 nodeId*/ |
|
140 0x1017, /*UNS16 index*/ |
|
141 0x00, /*UNS8 subindex*/ |
|
142 2, /*UNS8 count*/ |
|
143 0, /*UNS8 dataType*/ |
|
144 &Slave_Prod_Heartbeat_T,/*void *data*/ |
|
145 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
146 break; |
|
147 |
|
148 /* Set the new heartbeat consumer time in the master*/ |
|
149 eprintf("Master : set Consumer Heartbeat Time for slave %2.2x = %d\n", nodeId,Master_Cons_Heartbeat_T); |
|
150 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/ |
|
151 0x1016, /*UNS16 index*/ |
|
152 nodeId-1, /*UNS8 subind*/ |
|
153 &Master_Cons_Heartbeat_T, /*void * pSourceData,*/ |
|
154 &size, /* UNS8 * pExpectedSize*/ |
|
155 RW); /* UNS8 checkAccess */ |
|
156 } |
|
157 break; |
|
158 case 3: |
|
159 |
|
160 /****************************** START *******************************/ |
|
161 |
|
162 /* Put the master in operational mode */ |
|
163 setState(d, Operational); |
|
164 |
|
165 /* Ask slave node to go in operational mode */ |
|
166 masterSendNMTstateChange (d, nodeId, NMT_Start_Node); |
|
167 } |
|
168 } |
|
169 |
|
170 static void ConfigureLSSNode(CO_Data* d); |
|
171 // Step counts number of times ConfigureLSSNode is called |
|
172 UNS8 init_step_LSS=1; |
|
173 |
|
174 static void CheckLSSAndContinue(CO_Data* d, UNS8 command) |
|
175 { |
|
176 UNS32 dat1; |
|
177 UNS8 dat2; |
|
178 |
|
179 printf("CheckLSS->"); |
|
180 if(getConfigResultNetworkNode (d, command, &dat1, &dat2) != LSS_FINISHED){ |
|
181 eprintf("Master : Failed in LSS comand %d. Trying again\n", command); |
|
182 } |
|
183 else |
|
184 { |
|
185 init_step_LSS++; |
|
186 |
|
187 switch(command){ |
|
188 case LSS_CONF_NODE_ID: |
|
189 switch(dat1){ |
|
190 case 0: printf("Node ID change succesful\n");break; |
|
191 case 1: printf("Node ID change error:out of range\n");break; |
|
192 case 0xFF:printf("Node ID change error:specific error\n");break; |
|
193 default:break; |
|
194 } |
|
195 break; |
|
196 case LSS_CONF_BIT_TIMING: |
|
197 switch(dat1){ |
|
198 case 0: printf("Baud rate change succesful\n");break; |
|
199 case 1: printf("Baud rate change error: change baud rate not supported\n");break; |
|
200 case 0xFF:printf("Baud rate change error:specific error\n");break; |
|
201 default:break; |
|
202 } |
|
203 break; |
|
204 case LSS_CONF_STORE: |
|
205 switch(dat1){ |
|
206 case 0: printf("Store configuration succesful\n");break; |
|
207 case 1: printf("Store configuration error:not supported\n");break; |
|
208 case 0xFF:printf("Store configuration error:specific error\n");break; |
|
209 default:break; |
|
210 } |
|
211 break; |
|
212 case LSS_CONF_ACT_BIT_TIMING: |
|
213 if(dat1==0){ |
|
214 UNS8 LSS_mode=LSS_WAITING_MODE; |
|
215 UNS32 SINC_cicle=50000;// us |
|
216 UNS8 size = sizeof(UNS32); |
|
217 |
|
218 /* The slaves are now configured (nodeId and Baudrate) via the LSS services. |
|
219 * Switch the LSS state to WAITING and restart the slaves. */ |
|
220 |
|
221 printf("Master : Switch Delay period finished. Switching to LSS WAITING state\n"); |
|
222 configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0,NULL); |
|
223 |
|
224 printf("Master : Restarting all the slaves\n"); |
|
225 masterSendNMTstateChange (&TestMaster_Data, 0x00, NMT_Reset_Node); |
|
226 |
|
227 printf("Master : Starting the SYNC producer\n"); |
|
228 writeLocalDict( &TestMaster_Data, /*CO_Data* d*/ |
|
229 0x1006, /*UNS16 index*/ |
|
230 0x00, /*UNS8 subind*/ |
|
231 &SINC_cicle, /*void * pSourceData,*/ |
|
232 &size, /* UNS8 * pExpectedSize*/ |
|
233 RW); /* UNS8 checkAccess */ |
|
234 |
|
235 return; |
|
236 } |
|
237 else{ |
|
238 printf("Master : unable to activate bit timing. trying again\n"); |
|
239 init_step_LSS--; |
|
240 } |
|
241 break; |
|
242 case LSS_SM_SELECTIVE_SERIAL: |
|
243 printf("Slave in LSS CONFIGURATION state\n"); |
|
244 break; |
|
245 case LSS_IDENT_REMOTE_SERIAL_HIGH: |
|
246 printf("node identified\n"); |
|
247 break; |
|
248 case LSS_IDENT_REMOTE_NON_CONF: |
|
249 if(dat1==0) |
|
250 eprintf("There are no-configured remote slave(s) in the net\n"); |
|
251 else |
|
252 { |
|
253 UNS16 Switch_delay=1; |
|
254 UNS8 LSS_mode=LSS_CONFIGURATION_MODE; |
|
255 |
|
256 /*The configuration of the slaves' nodeId ended. |
|
257 * Start the configuration of the baud rate. */ |
|
258 eprintf("Master : There are not no-configured slaves in the net\n", command); |
|
259 eprintf("Switching all the nodes to LSS CONFIGURATION state\n"); |
|
260 configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0,NULL); |
|
261 eprintf("LSS=>Activate Bit Timing\n"); |
|
262 configNetworkNode(&TestMaster_Data,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0,CheckLSSAndContinue); |
|
263 return; |
|
264 } |
|
265 break; |
|
266 case LSS_INQ_VENDOR_ID: |
|
267 printf("Slave VendorID %x\n", dat1); |
|
268 break; |
|
269 case LSS_INQ_PRODUCT_CODE: |
|
270 printf("Slave Product Code %x\n", dat1); |
|
271 break; |
|
272 case LSS_INQ_REV_NUMBER: |
|
273 printf("Slave Revision Number %x\n", dat1); |
|
274 break; |
|
275 case LSS_INQ_SERIAL_NUMBER: |
|
276 printf("Slave Serial Number %x\n", dat1); |
|
277 break; |
|
278 case LSS_INQ_NODE_ID: |
|
279 printf("Slave nodeid %x\n", dat1); |
|
280 break; |
|
281 #ifdef CO_ENABLE_LSS_FS |
|
282 case LSS_IDENT_FASTSCAN: |
|
283 if(dat1==0) |
|
284 printf("Slave node identified with FastScan\n"); |
|
285 else |
|
286 { |
|
287 printf("There is not unconfigured node in the net\n"); |
|
288 return; |
|
289 } |
|
290 init_step_LSS++; |
|
291 break; |
|
292 #endif |
|
293 |
|
294 } |
|
295 } |
|
296 |
|
297 printf("\n"); |
|
298 ConfigureLSSNode(d); |
|
299 } |
|
300 |
|
301 /* Initial nodeID and VendorID. They are incremented by one for each slave*/ |
|
302 UNS8 NodeID=0x02; |
|
303 UNS32 Vendor_ID=0x12345678; |
|
304 |
|
305 /* Configuration of the nodeID and baudrate with LSS services: |
|
306 * --First ask if there is a node with an invalid nodeID. |
|
307 * --If FastScan is activated it is used to put the slave in the state “configuration”. |
|
308 * --If FastScan is not activated, identification services are used to identify the slave. Then |
|
309 * switch mode service is used to put it in configuration state. |
|
310 * --Next, all the inquire services are used (only for example) and a valid nodeId and a |
|
311 * new baudrate are assigned to the slave. |
|
312 * --Finally, the slave's LSS state is restored to “waiting” and all the process is repeated |
|
313 * again until there isn't any node with an invalid nodeID. |
|
314 * --After the configuration of all the slaves finished the LSS state of all of them is switched |
|
315 * again to "configuration" and the Activate Bit Timing service is requested. On sucessfull, the |
|
316 * LSS state is restored to "waiting" and NMT state is changed to reset (by means of the NMT services). |
|
317 * */ |
|
318 static void ConfigureLSSNode(CO_Data* d) |
|
319 { |
|
320 UNS32 Product_Code=0x90123456; |
|
321 UNS32 Revision_Number=0x78901234; |
|
322 UNS32 Serial_Number=0x56789012; |
|
323 UNS32 Revision_Number_high=0x78901240; |
|
324 UNS32 Revision_Number_low=0x78901230; |
|
325 UNS32 Serial_Number_high=0x56789020; |
|
326 UNS32 Serial_Number_low=0x56789010; |
|
327 UNS8 LSS_mode=LSS_WAITING_MODE; |
|
328 UNS8 Baud_Table=0; |
|
329 UNS8 Baud_BitTiming=3; |
|
330 UNS8 res; |
|
331 eprintf("ConfigureLSSNode step %d -> ",init_step_LSS); |
|
332 |
|
333 switch(init_step_LSS){ |
|
334 case 1: /* LSS=>identify non-configured remote slave */ |
|
335 eprintf("LSS=>identify no-configured remote slave(s)\n"); |
|
336 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue); |
|
337 break; |
|
338 #ifdef CO_ENABLE_LSS_FS |
|
339 case 2: /* LSS=>FastScan */ |
|
340 eprintf("LSS=>FastScan\n"); |
|
341 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_FASTSCAN,0,0,CheckLSSAndContinue); |
|
342 break; |
|
343 #else |
|
344 case 2: /* LSS=>identify node */ |
|
345 eprintf("LSS=>identify node\n"); |
|
346 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_VENDOR,&Vendor_ID,0,NULL); |
|
347 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_PRODUCT,&Product_Code,0,NULL); |
|
348 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_LOW,&Revision_Number_low,0,NULL); |
|
349 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_HIGH,&Revision_Number_high,0,NULL); |
|
350 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_LOW,&Serial_Number_low,0,NULL); |
|
351 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_HIGH,&Serial_Number_high,0,CheckLSSAndContinue); |
|
352 break; |
|
353 case 3: /*LSS=>put in configuration mode*/ |
|
354 eprintf("LSS=>put in configuration mode\n"); |
|
355 res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_VENDOR,&Vendor_ID,0,NULL); |
|
356 res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_PRODUCT,&Product_Code,0,NULL); |
|
357 res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_REVISION,&Revision_Number,0,NULL); |
|
358 res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_SERIAL,&Serial_Number,0,CheckLSSAndContinue); |
|
359 Vendor_ID++; |
|
360 break; |
|
361 #endif |
|
362 case 4: /* LSS=>inquire nodeID */ |
|
363 eprintf("LSS=>inquire nodeID\n"); |
|
364 res=configNetworkNode(&TestMaster_Data,LSS_INQ_NODE_ID,0,0,CheckLSSAndContinue); |
|
365 break; |
|
366 case 5: /* LSS=>inquire VendorID */ |
|
367 eprintf("LSS=>inquire VendorID\n"); |
|
368 res=configNetworkNode(&TestMaster_Data,LSS_INQ_VENDOR_ID,0,0,CheckLSSAndContinue); |
|
369 break; |
|
370 case 6: /* LSS=>inquire Product code */ |
|
371 eprintf("LSS=>inquire Product code\n"); |
|
372 res=configNetworkNode(&TestMaster_Data,LSS_INQ_PRODUCT_CODE,0,0,CheckLSSAndContinue); |
|
373 break; |
|
374 case 7: /* LSS=>inquire Revision Number */ |
|
375 eprintf("LSS=>inquire Revision Number\n"); |
|
376 res=configNetworkNode(&TestMaster_Data,LSS_INQ_REV_NUMBER,0,0,CheckLSSAndContinue); |
|
377 break; |
|
378 case 8: /* LSS=>inquire Serial Number */ |
|
379 eprintf("LSS=>inquire Serial Number\n"); |
|
380 res=configNetworkNode(&TestMaster_Data,LSS_INQ_SERIAL_NUMBER,0,0,CheckLSSAndContinue); |
|
381 break; |
|
382 case 9: /* LSS=>change the nodeID */ |
|
383 eprintf("LSS=>change the nodeId\n"); |
|
384 res=configNetworkNode(&TestMaster_Data,LSS_CONF_NODE_ID,&NodeID,0,CheckLSSAndContinue); |
|
385 NodeID++; |
|
386 break; |
|
387 case 10: /* LSS=>change the Baud rate */ |
|
388 eprintf("LSS=>change the Baud rate\n"); |
|
389 res=configNetworkNode(&TestMaster_Data,LSS_CONF_BIT_TIMING,&Baud_Table,&Baud_BitTiming,CheckLSSAndContinue); |
|
390 break; |
|
391 case 11: |
|
392 /*LSS=>store configuration*/ |
|
393 eprintf("LSS=>store configuration\n"); |
|
394 res=configNetworkNode(&TestMaster_Data,LSS_CONF_STORE,0,0,CheckLSSAndContinue); |
|
395 break; |
|
396 case 12: /* LSS=>put in waiting mode */ |
|
397 eprintf("LSS=>put in waiting mode\n"); |
|
398 res=configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0,NULL); |
|
399 /* Search again for no-configured slaves*/ |
|
400 eprintf("LSS=>identify no-configured remote slave(s)\n"); |
|
401 res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue); |
|
402 init_step_LSS=1; |
|
403 break; |
|
404 } |
|
405 } |
|
406 |
|
407 void TestMaster_preOperational(CO_Data* d) |
|
408 { |
|
409 eprintf("TestMaster_preOperational\n"); |
|
410 |
|
411 /* Ask slaves to go in stop mode */ |
|
412 masterSendNMTstateChange (d, 0, NMT_Stop_Node); |
|
413 ConfigureLSSNode(&TestMaster_Data); |
|
414 } |
|
415 |
|
416 void TestMaster_operational(CO_Data* d) |
|
417 { |
|
418 eprintf("TestMaster_operational\n"); |
|
419 } |
|
420 |
|
421 void TestMaster_stopped(CO_Data* d) |
|
422 { |
|
423 eprintf("TestMaster_stopped\n"); |
|
424 } |
|
425 |
|
426 void TestMaster_post_sync(CO_Data* d) |
|
427 { |
|
428 eprintf("TestMaster_post_sync\n"); |
|
429 eprintf("Master: %d %d %d\n", |
|
430 MasterMap1, |
|
431 MasterMap2, |
|
432 MasterMap3); |
|
433 } |
|
434 |
|
435 void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg) |
|
436 { |
|
437 eprintf("Master received EMCY message. Node: %2.2x ErrorCode: %4.4x ErrorRegister: %2.2x\n", nodeID, errCode, errReg); |
|
438 } |
|
439 |
|
440 void TestMaster_post_TPDO(CO_Data* d) |
|
441 { |
|
442 eprintf("TestMaster_post_TPDO\n"); |
|
443 } |
|
444 |
|
445 void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid) |
|
446 { |
|
447 eprintf("TestMaster_post_SlaveBootup %x\n", nodeid); |
|
448 /* Wait until the new baud rate is stored before configure the slaves*/ |
|
449 if(MasterBoard.baudrate=="250K") |
|
450 ConfigureSlaveNode(d, nodeid); |
|
451 } |
|
452 |
|
453 void TestMaster_ChangeBaudRate(CO_Data* d, char *baudrate) |
|
454 { |
|
455 eprintf("TestMaster_ChangeBaudRate from %s to %s\n", MasterBoard.baudrate, baudrate); |
|
456 MasterBoard.baudrate=baudrate; |
|
457 /* something to do with the new baudrate */ |
|
458 } |