author | lbessard |
Mon, 20 Apr 2009 17:42:36 +0200 | |
changeset 546 | 13701680330c |
parent 517 | 003679edc437 |
child 668 | ba53613e94e4 |
permissions | -rw-r--r-- |
0 | 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 |
*/ |
|
208 | 22 |
/*! |
23 |
** @file states.c |
|
24 |
** @author Edouard TISSERANT and Francis DUPIN |
|
25 |
** @date Tue Jun 5 09:32:32 2007 |
|
26 |
** |
|
27 |
** @brief |
|
28 |
** |
|
29 |
** |
|
30 |
*/ |
|
0 | 31 |
|
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
32 |
#include "data.h" |
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
33 |
#include "sysdep.h" |
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
34 |
|
208 | 35 |
/** Prototypes for internals functions */ |
36 |
/*! |
|
37 |
** |
|
38 |
** |
|
39 |
** @param d |
|
40 |
** @param newCommunicationState |
|
41 |
**/ |
|
53 | 42 |
void switchCommunicationState(CO_Data* d, |
43 |
s_state_communication *newCommunicationState); |
|
44 |
||
208 | 45 |
/*! |
46 |
** |
|
47 |
** |
|
48 |
** @param d |
|
49 |
** |
|
50 |
** @return |
|
51 |
**/ |
|
0 | 52 |
e_nodeState getState(CO_Data* d) |
53 |
{ |
|
54 |
return d->nodeState; |
|
55 |
} |
|
56 |
||
208 | 57 |
/*! |
58 |
** |
|
59 |
** |
|
60 |
** @param d |
|
61 |
** @param m |
|
62 |
**/ |
|
0 | 63 |
void canDispatch(CO_Data* d, Message *m) |
64 |
{ |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
65 |
UNS16 cob_id = UNS16_LE(m->cob_id); |
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
66 |
switch(cob_id >> 7) |
0 | 67 |
{ |
284 | 68 |
case SYNC: /* can be a SYNC or a EMCY message */ |
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
69 |
if(cob_id == 0x080) /* SYNC */ |
284 | 70 |
{ |
71 |
if(d->CurrentCommunicationState.csSYNC) |
|
72 |
proceedSYNC(d); |
|
73 |
} else /* EMCY */ |
|
74 |
if(d->CurrentCommunicationState.csEmergency) |
|
75 |
proceedEMCY(d,m); |
|
0 | 76 |
break; |
215 | 77 |
/* case TIME_STAMP: */ |
0 | 78 |
case PDO1tx: |
79 |
case PDO1rx: |
|
80 |
case PDO2tx: |
|
81 |
case PDO2rx: |
|
82 |
case PDO3tx: |
|
83 |
case PDO3rx: |
|
84 |
case PDO4tx: |
|
85 |
case PDO4rx: |
|
86 |
if (d->CurrentCommunicationState.csPDO) |
|
87 |
proceedPDO(d,m); |
|
88 |
break; |
|
89 |
case SDOtx: |
|
90 |
case SDOrx: |
|
91 |
if (d->CurrentCommunicationState.csSDO) |
|
92 |
proceedSDO(d,m); |
|
93 |
break; |
|
94 |
case NODE_GUARD: |
|
95 |
if (d->CurrentCommunicationState.csHeartbeat) |
|
96 |
proceedNODE_GUARD(d,m); |
|
97 |
break; |
|
98 |
case NMT: |
|
88 | 99 |
if (*(d->iam_a_slave)) |
0 | 100 |
{ |
101 |
proceedNMTstateChange(d,m); |
|
102 |
} |
|
343 | 103 |
#ifdef CO_ENABLE_LSS |
104 |
case LSS: |
|
105 |
if (!d->CurrentCommunicationState.csLSS)break; |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
106 |
if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS) |
343 | 107 |
{ |
108 |
proceedLSS_Slave(d,m); |
|
109 |
} |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
110 |
else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS) |
343 | 111 |
{ |
112 |
proceedLSS_Master(d,m); |
|
113 |
} |
|
114 |
break; |
|
115 |
#endif |
|
0 | 116 |
} |
117 |
} |
|
118 |
||
119 |
#define StartOrStop(CommType, FuncStart, FuncStop) \ |
|
291
6165554cbfe9
Some tricks in communication services status struct (states.c) and in sync.c to fit with Beremiz generated node code.
etisserant
parents:
284
diff
changeset
|
120 |
if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\ |
195
1510dd61ead0
Added debug level opt in configure and re-enabled debug macros.
etisserant
parents:
183
diff
changeset
|
121 |
MSG_WAR(0x9999,#FuncStart, 9999);\ |
0 | 122 |
d->CurrentCommunicationState.CommType = 1;\ |
123 |
FuncStart;\ |
|
291
6165554cbfe9
Some tricks in communication services status struct (states.c) and in sync.c to fit with Beremiz generated node code.
etisserant
parents:
284
diff
changeset
|
124 |
}else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\ |
195
1510dd61ead0
Added debug level opt in configure and re-enabled debug macros.
etisserant
parents:
183
diff
changeset
|
125 |
MSG_WAR(0x9999,#FuncStop, 9999);\ |
0 | 126 |
d->CurrentCommunicationState.CommType = 0;\ |
127 |
FuncStop;\ |
|
128 |
} |
|
129 |
#define None |
|
208 | 130 |
|
131 |
/*! |
|
132 |
** |
|
133 |
** |
|
134 |
** @param d |
|
135 |
** @param newCommunicationState |
|
136 |
**/ |
|
0 | 137 |
void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
138 |
{ |
|
381 | 139 |
#ifdef CO_ENABLE_LSS |
140 |
StartOrStop(csLSS, startLSS(d), stopLSS(d)) |
|
141 |
#endif |
|
0 | 142 |
StartOrStop(csSDO, None, resetSDO(d)) |
143 |
StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
|
144 |
StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
|
284 | 145 |
StartOrStop(csEmergency, emergencyInit(d), emergencyStop(d)) |
235
f812bf6b7237
Preliminary implementation of Event Timer and Inhibit Timer driven TPDO
etisserant
parents:
215
diff
changeset
|
146 |
StartOrStop(csPDO, PDOInit(d), PDOStop(d)) |
14 | 147 |
StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
0 | 148 |
} |
149 |
||
208 | 150 |
/*! |
151 |
** |
|
152 |
** |
|
153 |
** @param d |
|
154 |
** @param newState |
|
155 |
** |
|
156 |
** @return |
|
157 |
**/ |
|
0 | 158 |
UNS8 setState(CO_Data* d, e_nodeState newState) |
159 |
{ |
|
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
160 |
if(newState != d->nodeState){ |
0 | 161 |
switch( newState ){ |
162 |
case Initialisation: |
|
163 |
{ |
|
343 | 164 |
s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0, 0}; |
0 | 165 |
d->nodeState = Initialisation; |
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
166 |
switchCommunicationState(d, &newCommunicationState); |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
167 |
/* call user app init callback now. */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
168 |
/* d->initialisation MUST NOT CALL SetState */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
169 |
(*d->initialisation)(d); |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
170 |
} |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
171 |
|
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
172 |
/* Automatic transition - No break statement ! */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
173 |
/* Transition from Initialisation to Pre_operational */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
174 |
/* is automatic as defined in DS301. */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
175 |
/* App don't have to call SetState(d, Pre_operational) */ |
0 | 176 |
|
177 |
case Pre_operational: |
|
178 |
{ |
|
178 | 179 |
|
343 | 180 |
s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0, 1}; |
0 | 181 |
d->nodeState = Pre_operational; |
182 |
switchCommunicationState(d, &newCommunicationState); |
|
178 | 183 |
if (!(*(d->iam_a_slave))) |
184 |
{ |
|
349 | 185 |
masterSendNMTstateChange (d, 0, NMT_Reset_Node); |
178 | 186 |
} |
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
187 |
(*d->preOperational)(d); |
0 | 188 |
} |
189 |
break; |
|
190 |
||
191 |
case Operational: |
|
192 |
if(d->nodeState == Initialisation) return 0xFF; |
|
193 |
{ |
|
343 | 194 |
s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1, 0}; |
0 | 195 |
d->nodeState = Operational; |
196 |
newState = Operational; |
|
197 |
switchCommunicationState(d, &newCommunicationState); |
|
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
198 |
(*d->operational)(d); |
0 | 199 |
} |
200 |
break; |
|
201 |
||
202 |
case Stopped: |
|
203 |
if(d->nodeState == Initialisation) return 0xFF; |
|
204 |
{ |
|
343 | 205 |
s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0, 1}; |
0 | 206 |
d->nodeState = Stopped; |
207 |
newState = Stopped; |
|
208 |
switchCommunicationState(d, &newCommunicationState); |
|
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
209 |
(*d->stopped)(d); |
0 | 210 |
} |
211 |
break; |
|
381 | 212 |
default: |
213 |
return 0xFF; |
|
214 |
||
215 | 215 |
}/* end switch case */ |
94
bdf4c86be6b2
Removed all non-supported and uncontrolled source code. Please refer to CVS version "Before_..." to see old code.
etisserant
parents:
88
diff
changeset
|
216 |
|
0 | 217 |
} |
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
218 |
/* d->nodeState contains the final state */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
219 |
/* may not be the requested state */ |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
220 |
return d->nodeState; |
0 | 221 |
} |
222 |
||
208 | 223 |
/*! |
224 |
** |
|
225 |
** |
|
226 |
** @param d |
|
227 |
** |
|
228 |
** @return |
|
229 |
**/ |
|
0 | 230 |
UNS8 getNodeId(CO_Data* d) |
231 |
{ |
|
232 |
return *d->bDeviceNodeId; |
|
233 |
} |
|
234 |
||
208 | 235 |
/*! |
236 |
** |
|
237 |
** |
|
238 |
** @param d |
|
239 |
** @param nodeId |
|
240 |
**/ |
|
0 | 241 |
void setNodeId(CO_Data* d, UNS8 nodeId) |
242 |
{ |
|
243 |
UNS16 offset = d->firstIndex->SDO_SVR; |
|
343 | 244 |
|
245 |
#ifdef CO_ENABLE_LSS |
|
381 | 246 |
d->lss_transfer.nodeID=nodeId; |
517 | 247 |
if(nodeId==0xFF){ |
343 | 248 |
*d->bDeviceNodeId = nodeId; |
249 |
return; |
|
250 |
} |
|
517 | 251 |
else |
343 | 252 |
#endif |
517 | 253 |
if(!(nodeId>0 && nodeId<=127)){ |
254 |
MSG_WAR(0x2D01, "Invalid NodeID",nodeId); |
|
255 |
return; |
|
256 |
} |
|
343 | 257 |
|
0 | 258 |
if(offset){ |
349 | 259 |
/* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/ |
260 |
if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){ |
|
215 | 261 |
/* cob_id_client = 0x600 + nodeId; */ |
447
c9d01296d6d9
Endianess patch from Edward Karpicz, Lithuania :-)
etisserant
parents:
446
diff
changeset
|
262 |
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
303
340348f0193f
Now, SetNodeId will change 1200h SDO server params to new defaul values only if already set to appropriate default values.
etisserant
parents:
291
diff
changeset
|
263 |
} |
349 | 264 |
/* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/ |
265 |
if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){ |
|
215 | 266 |
/* cob_id_server = 0x580 + nodeId; */ |
447
c9d01296d6d9
Endianess patch from Edward Karpicz, Lithuania :-)
etisserant
parents:
446
diff
changeset
|
267 |
*(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
303
340348f0193f
Now, SetNodeId will change 1200h SDO server params to new defaul values only if already set to appropriate default values.
etisserant
parents:
291
diff
changeset
|
268 |
} |
0 | 269 |
} |
270 |
||
215 | 271 |
/* |
208 | 272 |
Initialize the server(s) SDO parameters |
273 |
Remember that only one SDO server is allowed, defined at index 0x1200 |
|
274 |
||
275 |
Initialize the client(s) SDO parameters |
|
276 |
Nothing to initialize (no default values required by the DS 401) |
|
277 |
Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 |
|
278 |
*/ |
|
0 | 279 |
{ |
280 |
UNS8 i = 0; |
|
281 |
UNS16 offset = d->firstIndex->PDO_RCV; |
|
282 |
UNS16 lastIndex = d->lastIndex->PDO_RCV; |
|
283 |
UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
|
284 |
if( offset ) while( (offset <= lastIndex) && (i < 4)) { |
|
349 | 285 |
if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)) |
0 | 286 |
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
287 |
i ++; |
|
288 |
offset ++; |
|
289 |
} |
|
290 |
} |
|
71 | 291 |
/* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */ |
0 | 292 |
{ |
293 |
UNS8 i = 0; |
|
294 |
UNS16 offset = d->firstIndex->PDO_TRS; |
|
295 |
UNS16 lastIndex = d->lastIndex->PDO_TRS; |
|
296 |
UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480}; |
|
297 |
i = 0; |
|
298 |
if( offset ) while ((offset <= lastIndex) && (i < 4)) { |
|
349 | 299 |
if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)) |
0 | 300 |
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
301 |
i ++; |
|
302 |
offset ++; |
|
303 |
} |
|
304 |
} |
|
314
68e83c3ffbb5
Better EMCY support. Now EMCY COB-ID depend on OD 1014h entry, as told in DS-301.
etisserant
parents:
303
diff
changeset
|
305 |
|
68e83c3ffbb5
Better EMCY support. Now EMCY COB-ID depend on OD 1014h entry, as told in DS-301.
etisserant
parents:
303
diff
changeset
|
306 |
/* Update EMCY COB-ID if already set to default*/ |
349 | 307 |
if((*d->error_cobid == *d->bDeviceNodeId + 0x80)||(*d->bDeviceNodeId==0xFF)) |
314
68e83c3ffbb5
Better EMCY support. Now EMCY COB-ID depend on OD 1014h entry, as told in DS-301.
etisserant
parents:
303
diff
changeset
|
308 |
*d->error_cobid = nodeId + 0x80; |
68e83c3ffbb5
Better EMCY support. Now EMCY COB-ID depend on OD 1014h entry, as told in DS-301.
etisserant
parents:
303
diff
changeset
|
309 |
|
215 | 310 |
/* bDeviceNodeId is defined in the object dictionary. */ |
0 | 311 |
*d->bDeviceNodeId = nodeId; |
312 |
} |
|
149 | 313 |
|
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
314 |
void _initialisation(CO_Data* d){} |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
315 |
void _preOperational(CO_Data* d){} |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
316 |
void _operational(CO_Data* d){} |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
317 |
void _stopped(CO_Data* d){} |