|
1 /* |
|
2 This file is part of CanFestival, a library implementing CanOpen Stack. |
|
3 |
|
4 Copyright (C): Edouard TISSERANT and Francis DUPIN |
|
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 #if defined(WIN32) && !defined(__CYGWIN__) |
|
24 #include <windows.h> |
|
25 #include "getopt.h" |
|
26 void pause(void) |
|
27 { |
|
28 system("PAUSE"); |
|
29 } |
|
30 #else |
|
31 #include <stdio.h> |
|
32 #include <string.h> |
|
33 #include <stdlib.h> |
|
34 #include <signal.h> |
|
35 #endif |
|
36 |
|
37 #include "canfestival.h" |
|
38 #include "TestMasterMicroMod.h" |
|
39 #include "TestMaster.h" |
|
40 UNS8 slavenodeid; |
|
41 |
|
42 |
|
43 /*****************************************************************************/ |
|
44 void TestMaster_heartbeatError(UNS8 heartbeatID) |
|
45 { |
|
46 eprintf("TestMaster_heartbeatError %d\n", heartbeatID); |
|
47 } |
|
48 |
|
49 /*****************************************************************************/ |
|
50 void TestMaster_SDOtimeoutError (UNS8 line) |
|
51 { |
|
52 eprintf("TestMaster_SDOtimeoutError %d\n", line); |
|
53 } |
|
54 |
|
55 /******************************************************** |
|
56 * ConfigureSlaveNode is responsible to |
|
57 * - setup master RPDO 1 to receive TPDO 1 from id 0x40 |
|
58 * - setup master TPDO 1 to send RPDO 1 to id 0x40 |
|
59 ********************************************************/ |
|
60 void TestMaster_initialisation() |
|
61 { |
|
62 UNS32 PDO1_COBID = 0x0180 + slavenodeid; |
|
63 UNS32 PDO2_COBID = 0x0200 + slavenodeid; |
|
64 UNS8 size = sizeof(UNS32); |
|
65 |
|
66 eprintf("TestMaster_initialisation\n"); |
|
67 |
|
68 /***************************************** |
|
69 * Define RPDOs to match slave ID=0x40 TPDOs* |
|
70 *****************************************/ |
|
71 setODentry( &TestMaster_Data, /*CO_Data* d*/ |
|
72 0x1400, /*UNS16 index*/ |
|
73 0x01, /*UNS8 subind*/ |
|
74 &PDO1_COBID, /*void * pSourceData,*/ |
|
75 &size, /* UNS8 * pExpectedSize*/ |
|
76 RW); /* UNS8 checkAccess */ |
|
77 |
|
78 |
|
79 /***************************************** |
|
80 * Define TPDOs to match slave ID=0x40 RPDOs* |
|
81 *****************************************/ |
|
82 setODentry( &TestMaster_Data, /*CO_Data* d*/ |
|
83 0x1800, /*UNS16 index*/ |
|
84 0x01, /*UNS8 subind*/ |
|
85 &PDO2_COBID, /*void * pSourceData,*/ |
|
86 &size, /* UNS8 * pExpectedSize*/ |
|
87 RW); /* UNS8 checkAccess */ |
|
88 } |
|
89 |
|
90 /******************************************************** |
|
91 * ConfigureSlaveNode is responsible to |
|
92 * - setup slave TPDO 1 transmit time |
|
93 * - setup slave TPDO 2 transmit time |
|
94 * - setup slave Heartbeat Producer time |
|
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_preOperational |
|
103 * then it called again each time a SDO exchange is |
|
104 * finished. |
|
105 ********************************************************/ |
|
106 static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId) |
|
107 { |
|
108 // Step counts number of times ConfigureSlaveNode is called |
|
109 static step = 1; |
|
110 |
|
111 UNS8 Transmission_Type = 0x01; |
|
112 UNS16 Heartbeat_Producer_Time = 0x03E8; |
|
113 UNS32 abortCode; |
|
114 UNS8 res; |
|
115 eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId); |
|
116 switch(step++){ |
|
117 case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/ |
|
118 eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId); |
|
119 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
120 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
121 nodeId, /*UNS8 nodeId*/ |
|
122 0x1800, /*UNS16 index*/ |
|
123 0x02, /*UNS8 subindex*/ |
|
124 1, /*UNS8 count*/ |
|
125 0, /*UNS8 dataType*/ |
|
126 &Transmission_Type,/*void *data*/ |
|
127 ConfigureSlaveNode); /*SDOCallback_t Callback*/ |
|
128 break; |
|
129 |
|
130 |
|
131 case 2: /*Second step*/ |
|
132 if(getWriteResultNetworkDict (d, slavenodeid, &abortCode) != SDO_FINISHED) |
|
133 eprintf("Master : Couldn't set slave %2.2x TPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode); |
|
134 |
|
135 /* Finalise last SDO transfer with this node */ |
|
136 closeSDOtransfer(&TestMaster_Data, |
|
137 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
138 slavenodeid, /*UNS8 nodeId*/ |
|
139 SDO_CLIENT); |
|
140 |
|
141 eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId); |
|
142 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
143 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
144 slavenodeid, /*UNS8 nodeId*/ |
|
145 0x1400, /*UNS16 index*/ |
|
146 0x02, /*UNS8 subindex*/ |
|
147 1, /*UNS8 count*/ |
|
148 0, /*UNS8 dataType*/ |
|
149 &Transmission_Type,/*void *data*/ |
|
150 ConfigureSlaveNode); /*SDOCallback_t Callback*/ |
|
151 break; |
|
152 |
|
153 case 3: /*Second step*/ |
|
154 if(getWriteResultNetworkDict (d, slavenodeid, &abortCode) != SDO_FINISHED) |
|
155 eprintf("Master : Couldn't set slave %2.2x RPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode); |
|
156 |
|
157 /* Finalise last SDO transfer with this node */ |
|
158 closeSDOtransfer(&TestMaster_Data, |
|
159 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
160 slavenodeid, /*UNS8 nodeId*/ |
|
161 SDO_CLIENT); |
|
162 |
|
163 eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId); |
|
164 res = writeNetworkDictCallBack (d, /*CO_Data* d*/ |
|
165 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
166 slavenodeid, /*UNS8 nodeId*/ |
|
167 0x1017, /*UNS16 index*/ |
|
168 0x00, /*UNS8 subindex*/ |
|
169 2, /*UNS8 count*/ |
|
170 0, /*UNS8 dataType*/ |
|
171 &Heartbeat_Producer_Time,/*void *data*/ |
|
172 ConfigureSlaveNode); /*SDOCallback_t Callback*/ |
|
173 break; |
|
174 |
|
175 case 4: /*Second step*/ |
|
176 |
|
177 if(getWriteResultNetworkDict (d, slavenodeid, &abortCode) != SDO_FINISHED) |
|
178 eprintf("Master : Couldn't set slave %2.2x Heartbeat_Producer_Time. AbortCode :%4.4x \n", nodeId, abortCode); |
|
179 |
|
180 /* Finalise last SDO transfer with this node */ |
|
181 closeSDOtransfer(&TestMaster_Data, |
|
182 /**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/ |
|
183 slavenodeid, /*UNS8 nodeId*/ |
|
184 SDO_CLIENT); |
|
185 |
|
186 /* Put the master in operational mode */ |
|
187 setState(d, Operational); |
|
188 |
|
189 /* Ask slave node to go in operational mode */ |
|
190 masterSendNMTstateChange (d, slavenodeid, NMT_Start_Node); |
|
191 } |
|
192 |
|
193 } |
|
194 |
|
195 void TestMaster_preOperational() |
|
196 { |
|
197 |
|
198 eprintf("TestMaster_preOperational\n"); |
|
199 ConfigureSlaveNode(&TestMaster_Data, slavenodeid); |
|
200 |
|
201 } |
|
202 |
|
203 void TestMaster_operational() |
|
204 { |
|
205 eprintf("TestMaster_operational\n"); |
|
206 } |
|
207 |
|
208 void TestMaster_stopped() |
|
209 { |
|
210 eprintf("TestMaster_stopped\n"); |
|
211 } |
|
212 |
|
213 void TestMaster_post_sync() |
|
214 { |
|
215 DO++; |
|
216 eprintf("MicroMod Digital Out: %2.2x In: %2.2d\n",DO,DI); |
|
217 } |
|
218 |
|
219 void TestMaster_post_TPDO() |
|
220 { |
|
221 // eprintf("TestMaster_post_TPDO\n"); |
|
222 } |
|
223 |
|
224 //s_BOARD SlaveBoard = {"0", "500K"}; |
|
225 s_BOARD MasterBoard = {"32", "125K"}; |
|
226 |
|
227 #if !defined(WIN32) || defined(__CYGWIN__) |
|
228 void catch_signal(int sig) |
|
229 { |
|
230 signal(SIGTERM, catch_signal); |
|
231 signal(SIGINT, catch_signal); |
|
232 |
|
233 eprintf("Got Signal %d\n",sig); |
|
234 } |
|
235 #endif |
|
236 |
|
237 void help() |
|
238 { |
|
239 printf("**************************************************************\n"); |
|
240 printf("* TestMasterMicroMod *\n"); |
|
241 printf("* *\n"); |
|
242 printf("* A simple example for PC. *\n"); |
|
243 printf("* A CanOpen master that control a MicroMod module: *\n"); |
|
244 printf("* - setup module TPDO 1 transmit type (ignored ???) *\n"); |
|
245 printf("* - setup module RPDO 1 transmit type (ignored ???) *\n"); |
|
246 printf("* - setup module hearbeatbeat period *\n"); |
|
247 printf("* - set state to operational *\n"); |
|
248 printf("* - send periodic SYNC (ignored ???) *\n"); |
|
249 printf("* - send periodic RPDO 1 to Micromod (digital output) *\n"); |
|
250 printf("* - listen Micromod's TPDO 1 (digital input) *\n"); |
|
251 printf("* *\n"); |
|
252 printf("* Usage: *\n"); |
|
253 printf("* ./TestMasterMicroMod [OPTIONS] *\n"); |
|
254 printf("* *\n"); |
|
255 printf("* OPTIONS: *\n"); |
|
256 printf("* -l : Can library [\"libcanfestival_can_virtual.so\"] *\n"); |
|
257 printf("* *\n"); |
|
258 printf("* Slave: *\n"); |
|
259 printf("* -i : Slave Node id format [0x01 , 0x7F] *\n"); |
|
260 printf("* *\n"); |
|
261 printf("* Master: *\n"); |
|
262 printf("* -m : bus name [\"1\"] *\n"); |
|
263 printf("* -M : 1M,500K,250K,125K,100K,50K,20K,10K,none(disable) *\n"); |
|
264 printf("* *\n"); |
|
265 printf("**************************************************************\n"); |
|
266 } |
|
267 |
|
268 /*************************** INIT *****************************************/ |
|
269 void InitNodes(CO_Data* d, UNS32 id) |
|
270 { |
|
271 /****************************** INITIALISATION MASTER *******************************/ |
|
272 if(MasterBoard.baudrate){ |
|
273 /* Defining the node Id */ |
|
274 setNodeId(&TestMaster_Data, 0x01); |
|
275 |
|
276 /* init */ |
|
277 setState(&TestMaster_Data, Initialisation); |
|
278 } |
|
279 } |
|
280 |
|
281 /****************************************************************************/ |
|
282 /*************************** MAIN *****************************************/ |
|
283 /****************************************************************************/ |
|
284 int main(int argc,char **argv) |
|
285 { |
|
286 |
|
287 char c; |
|
288 extern char *optarg; |
|
289 char* LibraryPath="libcanfestival_can_virtual.so"; |
|
290 char *snodeid; |
|
291 while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF) |
|
292 { |
|
293 switch(c) |
|
294 { |
|
295 case 'm' : |
|
296 if (optarg[0] == 0) |
|
297 { |
|
298 help(); |
|
299 exit(1); |
|
300 } |
|
301 MasterBoard.busname = optarg; |
|
302 break; |
|
303 case 'M' : |
|
304 if (optarg[0] == 0) |
|
305 { |
|
306 help(); |
|
307 exit(1); |
|
308 } |
|
309 MasterBoard.baudrate = optarg; |
|
310 break; |
|
311 case 'l' : |
|
312 if (optarg[0] == 0) |
|
313 { |
|
314 help(); |
|
315 exit(1); |
|
316 } |
|
317 LibraryPath = optarg; |
|
318 break; |
|
319 case 'i' : |
|
320 if (optarg[0] == 0) |
|
321 { |
|
322 help(); |
|
323 exit(1); |
|
324 } |
|
325 snodeid = optarg; |
|
326 sscanf(snodeid,"%x",&slavenodeid); |
|
327 break; |
|
328 default: |
|
329 help(); |
|
330 exit(1); |
|
331 } |
|
332 } |
|
333 |
|
334 #if !defined(WIN32) || defined(__CYGWIN__) |
|
335 /* install signal handler for manual break */ |
|
336 signal(SIGTERM, catch_signal); |
|
337 signal(SIGINT, catch_signal); |
|
338 #endif |
|
339 |
|
340 #ifndef NOT_USE_DYNAMIC_LOADING |
|
341 LoadCanDriver(LibraryPath); |
|
342 #endif |
|
343 |
|
344 if(MasterBoard.baudrate){ |
|
345 |
|
346 TestMaster_Data.heartbeatError = TestMaster_heartbeatError; |
|
347 TestMaster_Data.SDOtimeoutError = TestMaster_SDOtimeoutError; |
|
348 TestMaster_Data.initialisation = TestMaster_initialisation; |
|
349 TestMaster_Data.preOperational = TestMaster_preOperational; |
|
350 TestMaster_Data.operational = TestMaster_operational; |
|
351 TestMaster_Data.stopped = TestMaster_stopped; |
|
352 TestMaster_Data.post_sync = TestMaster_post_sync; |
|
353 TestMaster_Data.post_TPDO = TestMaster_post_TPDO; |
|
354 |
|
355 if(!canOpen(&MasterBoard,&TestMaster_Data)){ |
|
356 eprintf("Cannot open Master Board\n"); |
|
357 goto fail_master; |
|
358 } |
|
359 } |
|
360 |
|
361 // Start timer thread |
|
362 StartTimerLoop(&InitNodes); |
|
363 |
|
364 // wait Ctrl-C |
|
365 pause(); |
|
366 eprintf("Finishing.\n"); |
|
367 |
|
368 // Reset the slave node for next use (will stop emitting heartbeat) |
|
369 masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node); |
|
370 |
|
371 // Stop master |
|
372 setState(&TestMaster_Data, Stopped); |
|
373 |
|
374 // Stop timer thread |
|
375 StopTimerLoop(); |
|
376 |
|
377 fail_master: |
|
378 if(MasterBoard.baudrate) canClose(&TestMaster_Data); |
|
379 |
|
380 return 0; |
|
381 } |
|
382 |
|
383 |