|
381
|
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 <unistd.h>
|
|
|
34 |
#include <stdlib.h>
|
|
|
35 |
#include <signal.h>
|
|
|
36 |
#endif
|
|
|
37 |
|
|
|
38 |
#include "canfestival.h"
|
|
|
39 |
//#include <can_driver.h>
|
|
|
40 |
//#include <timers_driver.h>
|
|
|
41 |
|
|
|
42 |
#include "Master.h"
|
|
|
43 |
#include "SlaveA.h"
|
|
|
44 |
#include "SlaveB.h"
|
|
|
45 |
#include "TestMasterSlaveLSS.h"
|
|
|
46 |
|
|
|
47 |
s_BOARD SlaveBoardA = {"0", "125K"};
|
|
|
48 |
s_BOARD SlaveBoardB = {"1", "125K"};
|
|
|
49 |
s_BOARD MasterBoard = {"2", "125K"};
|
|
|
50 |
|
|
|
51 |
#if !defined(WIN32) || defined(__CYGWIN__)
|
|
|
52 |
void catch_signal(int sig)
|
|
|
53 |
{
|
|
|
54 |
signal(SIGTERM, catch_signal);
|
|
|
55 |
signal(SIGINT, catch_signal);
|
|
|
56 |
eprintf("Got Signal %d\n",sig);
|
|
|
57 |
}
|
|
|
58 |
#endif
|
|
|
59 |
|
|
|
60 |
void help()
|
|
|
61 |
{
|
|
|
62 |
printf("**************************************************************\n");
|
|
|
63 |
printf("* TestMasterSlaveLSS *\n");
|
|
|
64 |
printf("* *\n");
|
|
|
65 |
printf("* A LSS example for PC. It does implement 3 CanOpen *\n");
|
|
|
66 |
printf("* nodes in the same process. A master and 2 slaves. Both *\n");
|
|
|
67 |
printf("* communicate together, exchanging periodically NMT, SYNC, *\n");
|
|
|
68 |
printf("* SDO and PDO. Master configure heartbeat producer time *\n");
|
|
|
69 |
printf("* at 1000 ms for slave node-id 0x02 by concise DCF. *\n");
|
|
|
70 |
printf("* *\n");
|
|
|
71 |
printf("* Usage: *\n");
|
|
|
72 |
printf("* ./TestMasterSlaveLSS [OPTIONS] *\n");
|
|
|
73 |
printf("* *\n");
|
|
|
74 |
printf("* OPTIONS: *\n");
|
|
|
75 |
printf("* -l : Can library [\"libcanfestival_can_virtual.so\"] *\n");
|
|
|
76 |
printf("* *\n");
|
|
|
77 |
printf("* SlaveA: *\n");
|
|
|
78 |
printf("* -s : bus name [\"0\"] *\n");
|
|
|
79 |
printf("* -S : 1M,500K,250K,125K,100K,50K,20K,10K,none(disable) *\n");
|
|
|
80 |
printf("* *\n");
|
|
|
81 |
printf("* Master: *\n");
|
|
|
82 |
printf("* -m : bus name [\"1\"] *\n");
|
|
|
83 |
printf("* -M : 1M,500K,250K,125K,100K,50K,20K,10K,none(disable) *\n");
|
|
|
84 |
printf("* *\n");
|
|
|
85 |
printf("**************************************************************\n");
|
|
|
86 |
}
|
|
|
87 |
|
|
|
88 |
/*************************** INIT *****************************************/
|
|
|
89 |
void InitNodes(CO_Data* d, UNS32 id)
|
|
|
90 |
{
|
|
|
91 |
/****************************** INITIALISATION SLAVE_A *******************************/
|
|
|
92 |
if(strcmp(SlaveBoardA.baudrate, "none")) {
|
|
|
93 |
/* Set an invalid nodeID */
|
|
|
94 |
setNodeId(&TestSlaveA_Data, 0xFF);
|
|
|
95 |
|
|
|
96 |
/* init */
|
|
|
97 |
setState(&TestSlaveA_Data, Initialisation);
|
|
|
98 |
}
|
|
|
99 |
|
|
|
100 |
/****************************** INITIALISATION SLAVE_B *******************************/
|
|
|
101 |
if(strcmp(SlaveBoardB.baudrate, "none")) {
|
|
|
102 |
|
|
|
103 |
/* Set an invalid nodeID */
|
|
|
104 |
setNodeId(&TestSlaveB_Data, 0xFF);
|
|
|
105 |
|
|
|
106 |
/* init */
|
|
|
107 |
setState(&TestSlaveB_Data, Initialisation);
|
|
|
108 |
}
|
|
|
109 |
|
|
|
110 |
/****************************** INITIALISATION MASTER *******************************/
|
|
|
111 |
if(strcmp(MasterBoard.baudrate, "none")){
|
|
|
112 |
|
|
|
113 |
/* Defining the node Id */
|
|
|
114 |
setNodeId(&TestMaster_Data, 0x01);
|
|
|
115 |
|
|
|
116 |
/* init */
|
|
|
117 |
setState(&TestMaster_Data, Initialisation);
|
|
|
118 |
}
|
|
|
119 |
}
|
|
|
120 |
|
|
|
121 |
/****************************************************************************/
|
|
|
122 |
/*************************** MAIN *****************************************/
|
|
|
123 |
/****************************************************************************/
|
|
|
124 |
int main(int argc,char **argv)
|
|
|
125 |
{
|
|
|
126 |
|
|
|
127 |
int c;
|
|
|
128 |
extern char *optarg;
|
|
|
129 |
char* LibraryPath="../../drivers/can_virtual/libcanfestival_can_virtual.so";
|
|
|
130 |
|
|
|
131 |
while ((c = getopt(argc, argv, "-m:s:M:S:l:")) != EOF)
|
|
|
132 |
{
|
|
|
133 |
switch(c)
|
|
|
134 |
{
|
|
|
135 |
case 's' :
|
|
|
136 |
if (optarg[0] == 0)
|
|
|
137 |
{
|
|
|
138 |
help();
|
|
|
139 |
exit(1);
|
|
|
140 |
}
|
|
|
141 |
SlaveBoardA.busname = optarg;
|
|
|
142 |
break;
|
|
|
143 |
case 'm' :
|
|
|
144 |
if (optarg[0] == 0)
|
|
|
145 |
{
|
|
|
146 |
help();
|
|
|
147 |
exit(1);
|
|
|
148 |
}
|
|
|
149 |
MasterBoard.busname = optarg;
|
|
|
150 |
break;
|
|
|
151 |
case 'S' :
|
|
|
152 |
if (optarg[0] == 0)
|
|
|
153 |
{
|
|
|
154 |
help();
|
|
|
155 |
exit(1);
|
|
|
156 |
}
|
|
|
157 |
SlaveBoardA.baudrate = optarg;
|
|
|
158 |
break;
|
|
|
159 |
case 'M' :
|
|
|
160 |
if (optarg[0] == 0)
|
|
|
161 |
{
|
|
|
162 |
help();
|
|
|
163 |
exit(1);
|
|
|
164 |
}
|
|
|
165 |
MasterBoard.baudrate = optarg;
|
|
|
166 |
break;
|
|
|
167 |
case 'l' :
|
|
|
168 |
if (optarg[0] == 0)
|
|
|
169 |
{
|
|
|
170 |
help();
|
|
|
171 |
exit(1);
|
|
|
172 |
}
|
|
|
173 |
LibraryPath = optarg;
|
|
|
174 |
break;
|
|
|
175 |
default:
|
|
|
176 |
help();
|
|
|
177 |
exit(1);
|
|
|
178 |
}
|
|
|
179 |
}
|
|
|
180 |
|
|
|
181 |
#if !defined(WIN32) || defined(__CYGWIN__)
|
|
|
182 |
/* install signal handler for manual break */
|
|
|
183 |
signal(SIGTERM, catch_signal);
|
|
|
184 |
signal(SIGINT, catch_signal);
|
|
|
185 |
#endif
|
|
|
186 |
|
|
|
187 |
#ifndef NOT_USE_DYNAMIC_LOADING
|
|
|
188 |
if (LoadCanDriver(LibraryPath) == NULL)
|
|
|
189 |
printf("Unable to load library: %s\n",LibraryPath);
|
|
|
190 |
#endif
|
|
|
191 |
// Open CAN devices
|
|
|
192 |
|
|
|
193 |
if(strcmp(SlaveBoardA.baudrate, "none")){
|
|
|
194 |
|
|
|
195 |
TestSlaveA_Data.heartbeatError = TestSlaveA_heartbeatError;
|
|
|
196 |
TestSlaveA_Data.initialisation = TestSlaveA_initialisation;
|
|
|
197 |
TestSlaveA_Data.preOperational = TestSlaveA_preOperational;
|
|
|
198 |
TestSlaveA_Data.operational = TestSlaveA_operational;
|
|
|
199 |
TestSlaveA_Data.stopped = TestSlaveA_stopped;
|
|
|
200 |
TestSlaveA_Data.post_sync = TestSlaveA_post_sync;
|
|
|
201 |
TestSlaveA_Data.post_TPDO = TestSlaveA_post_TPDO;
|
|
|
202 |
TestSlaveA_Data.storeODSubIndex = TestSlaveA_storeODSubIndex;
|
|
|
203 |
TestSlaveA_Data.post_emcy = TestSlaveA_post_emcy;
|
|
|
204 |
/* in this example the slave doesn't support Store configuration*/
|
|
|
205 |
TestSlaveA_Data.lss_StoreConfiguration = TestSlaveA_StoreConfiguration;
|
|
|
206 |
|
|
|
207 |
if(!canOpen(&SlaveBoardA,&TestSlaveA_Data)){
|
|
|
208 |
eprintf("Cannot open SlaveA Board (%s,%s)\n",SlaveBoardA.busname, SlaveBoardA.baudrate);
|
|
|
209 |
goto fail_slaveA;
|
|
|
210 |
}
|
|
|
211 |
}
|
|
|
212 |
|
|
|
213 |
if(strcmp(SlaveBoardB.baudrate, "none")){
|
|
|
214 |
|
|
|
215 |
TestSlaveB_Data.heartbeatError = TestSlaveB_heartbeatError;
|
|
|
216 |
TestSlaveB_Data.initialisation = TestSlaveB_initialisation;
|
|
|
217 |
TestSlaveB_Data.preOperational = TestSlaveB_preOperational;
|
|
|
218 |
TestSlaveB_Data.operational = TestSlaveB_operational;
|
|
|
219 |
TestSlaveB_Data.stopped = TestSlaveB_stopped;
|
|
|
220 |
TestSlaveB_Data.post_sync = TestSlaveB_post_sync;
|
|
|
221 |
TestSlaveB_Data.post_TPDO = TestSlaveB_post_TPDO;
|
|
|
222 |
TestSlaveB_Data.storeODSubIndex = TestSlaveB_storeODSubIndex;
|
|
|
223 |
TestSlaveB_Data.post_emcy = TestSlaveB_post_emcy;
|
|
|
224 |
TestSlaveB_Data.lss_StoreConfiguration = TestSlaveB_StoreConfiguration;
|
|
|
225 |
|
|
|
226 |
if(!canOpen(&SlaveBoardB,&TestSlaveB_Data)){
|
|
|
227 |
eprintf("Cannot open SlaveB Board (%s,%s)\n",SlaveBoardB.busname, SlaveBoardB.baudrate);
|
|
|
228 |
goto fail_slaveB;
|
|
|
229 |
}
|
|
|
230 |
}
|
|
|
231 |
|
|
|
232 |
if(strcmp(MasterBoard.baudrate, "none")){
|
|
|
233 |
|
|
|
234 |
TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
|
|
|
235 |
TestMaster_Data.initialisation = TestMaster_initialisation;
|
|
|
236 |
TestMaster_Data.preOperational = TestMaster_preOperational;
|
|
|
237 |
TestMaster_Data.operational = TestMaster_operational;
|
|
|
238 |
TestMaster_Data.stopped = TestMaster_stopped;
|
|
|
239 |
TestMaster_Data.post_sync = TestMaster_post_sync;
|
|
|
240 |
TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
|
|
|
241 |
TestMaster_Data.post_emcy = TestMaster_post_emcy;
|
|
|
242 |
TestMaster_Data.post_SlaveBootup=TestMaster_post_SlaveBootup;
|
|
|
243 |
|
|
|
244 |
if(!canOpen(&MasterBoard,&TestMaster_Data)){
|
|
|
245 |
eprintf("Cannot open Master Board (%s,%s)\n",MasterBoard.busname, MasterBoard.baudrate);
|
|
|
246 |
goto fail_master;
|
|
|
247 |
}
|
|
|
248 |
}
|
|
|
249 |
|
|
|
250 |
// Start timer thread
|
|
|
251 |
StartTimerLoop(&InitNodes);
|
|
|
252 |
|
|
|
253 |
// wait Ctrl-C
|
|
|
254 |
|
|
|
255 |
pause();
|
|
|
256 |
|
|
|
257 |
eprintf("Finishing.\n");
|
|
|
258 |
EnterMutex();
|
|
|
259 |
masterSendNMTstateChange (&TestMaster_Data, 0x00, NMT_Stop_Node);
|
|
|
260 |
LeaveMutex();
|
|
|
261 |
|
|
|
262 |
eprintf("reset\n");
|
|
|
263 |
// Stop master
|
|
|
264 |
EnterMutex();
|
|
|
265 |
setState(&TestMaster_Data, Stopped);
|
|
|
266 |
LeaveMutex();
|
|
|
267 |
|
|
|
268 |
// Stop timer thread
|
|
|
269 |
StopTimerLoop();
|
|
|
270 |
|
|
|
271 |
// Close CAN devices (and can threads)
|
|
|
272 |
if(strcmp(MasterBoard.baudrate, "none")) canClose(&TestMaster_Data);
|
|
|
273 |
fail_master:
|
|
|
274 |
if(strcmp(SlaveBoardB.baudrate, "none")) canClose(&TestSlaveB_Data);
|
|
|
275 |
fail_slaveB:
|
|
|
276 |
if(strcmp(SlaveBoardA.baudrate, "none")) canClose(&TestSlaveA_Data);
|
|
|
277 |
fail_slaveA:
|
|
|
278 |
return 0;
|
|
|
279 |
}
|