| author | luis |
| Wed, 26 Sep 2007 16:02:00 +0200 | |
| changeset 284 | 24bf3d692993 |
| parent 263 | d221d387ad2f |
| child 291 | 6165554cbfe9 |
| 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 |
|
32 |
#include "states.h" |
|
33 |
#include "def.h" |
|
|
204
44ce74232ccb
Some fixes for visual studio C compiler compatiblity.
etisserant
parents:
195
diff
changeset
|
34 |
#include "dcf.h" |
| 0 | 35 |
#include "nmtSlave.h" |
| 284 | 36 |
#include "emcy.h" |
| 0 | 37 |
|
| 208 | 38 |
/** Prototypes for internals functions */ |
39 |
/*! |
|
40 |
** |
|
41 |
** |
|
42 |
** @param d |
|
43 |
** @param newCommunicationState |
|
44 |
**/ |
|
| 53 | 45 |
void switchCommunicationState(CO_Data* d, |
46 |
s_state_communication *newCommunicationState); |
|
47 |
||
| 208 | 48 |
/*! |
49 |
** |
|
50 |
** |
|
51 |
** @param d |
|
52 |
** |
|
53 |
** @return |
|
54 |
**/ |
|
| 0 | 55 |
e_nodeState getState(CO_Data* d) |
56 |
{
|
|
57 |
return d->nodeState; |
|
58 |
} |
|
59 |
||
| 208 | 60 |
/*! |
61 |
** |
|
62 |
** |
|
63 |
** @param d |
|
64 |
** @param m |
|
65 |
**/ |
|
| 0 | 66 |
void canDispatch(CO_Data* d, Message *m) |
67 |
{
|
|
68 |
switch(m->cob_id.w >> 7) |
|
69 |
{
|
|
| 284 | 70 |
case SYNC: /* can be a SYNC or a EMCY message */ |
71 |
if(m->cob_id.w == 0x080) /* SYNC */ |
|
72 |
{
|
|
73 |
if(d->CurrentCommunicationState.csSYNC) |
|
74 |
proceedSYNC(d); |
|
75 |
} else /* EMCY */ |
|
76 |
if(d->CurrentCommunicationState.csEmergency) |
|
77 |
proceedEMCY(d,m); |
|
| 0 | 78 |
break; |
| 215 | 79 |
/* case TIME_STAMP: */ |
| 0 | 80 |
case PDO1tx: |
81 |
case PDO1rx: |
|
82 |
case PDO2tx: |
|
83 |
case PDO2rx: |
|
84 |
case PDO3tx: |
|
85 |
case PDO3rx: |
|
86 |
case PDO4tx: |
|
87 |
case PDO4rx: |
|
88 |
if (d->CurrentCommunicationState.csPDO) |
|
89 |
proceedPDO(d,m); |
|
90 |
break; |
|
91 |
case SDOtx: |
|
92 |
case SDOrx: |
|
93 |
if (d->CurrentCommunicationState.csSDO) |
|
94 |
proceedSDO(d,m); |
|
95 |
break; |
|
96 |
case NODE_GUARD: |
|
97 |
if (d->CurrentCommunicationState.csHeartbeat) |
|
98 |
proceedNODE_GUARD(d,m); |
|
99 |
break; |
|
100 |
case NMT: |
|
| 88 | 101 |
if (*(d->iam_a_slave)) |
| 0 | 102 |
{
|
103 |
proceedNMTstateChange(d,m); |
|
104 |
} |
|
105 |
} |
|
106 |
} |
|
107 |
||
108 |
#define StartOrStop(CommType, FuncStart, FuncStop) \ |
|
109 |
if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
|
|
|
195
1510dd61ead0
Added debug level opt in configure and re-enabled debug macros.
etisserant
parents:
183
diff
changeset
|
110 |
MSG_WAR(0x9999,#FuncStart, 9999);\ |
| 0 | 111 |
d->CurrentCommunicationState.CommType = 1;\ |
112 |
FuncStart;\ |
|
113 |
}else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\
|
|
|
195
1510dd61ead0
Added debug level opt in configure and re-enabled debug macros.
etisserant
parents:
183
diff
changeset
|
114 |
MSG_WAR(0x9999,#FuncStop, 9999);\ |
| 0 | 115 |
d->CurrentCommunicationState.CommType = 0;\ |
116 |
FuncStop;\ |
|
117 |
} |
|
118 |
#define None |
|
| 208 | 119 |
|
120 |
/*! |
|
121 |
** |
|
122 |
** |
|
123 |
** @param d |
|
124 |
** @param newCommunicationState |
|
125 |
**/ |
|
| 0 | 126 |
void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
127 |
{
|
|
128 |
StartOrStop(csSDO, None, resetSDO(d)) |
|
129 |
StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
|
130 |
StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
|
| 284 | 131 |
StartOrStop(csEmergency, emergencyInit(d), emergencyStop(d)) |
|
235
f812bf6b7237
Preliminary implementation of Event Timer and Inhibit Timer driven TPDO
etisserant
parents:
215
diff
changeset
|
132 |
StartOrStop(csPDO, PDOInit(d), PDOStop(d)) |
| 14 | 133 |
StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
| 0 | 134 |
} |
135 |
||
| 208 | 136 |
/*! |
137 |
** |
|
138 |
** |
|
139 |
** @param d |
|
140 |
** @param newState |
|
141 |
** |
|
142 |
** @return |
|
143 |
**/ |
|
| 0 | 144 |
UNS8 setState(CO_Data* d, e_nodeState newState) |
145 |
{
|
|
| 183 | 146 |
UNS16 wIndex = 0x1F22; |
147 |
const indextable *ptrTable; |
|
148 |
ODCallback_t *Callback; |
|
149 |
UNS32 errorCode; |
|
| 0 | 150 |
while(newState != d->nodeState){
|
151 |
switch( newState ){
|
|
152 |
case Initialisation: |
|
153 |
{
|
|
| 71 | 154 |
s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
|
| 215 | 155 |
/* This will force a second loop for the state switch */ |
| 0 | 156 |
d->nodeState = Initialisation; |
157 |
newState = Pre_operational; |
|
158 |
switchCommunicationState(d, &newCommunicationState); |
|
| 215 | 159 |
/* call user app related state func. */ |
| 0 | 160 |
(*d->initialisation)(); |
| 178 | 161 |
|
| 0 | 162 |
} |
163 |
break; |
|
164 |
||
165 |
case Pre_operational: |
|
166 |
{
|
|
| 178 | 167 |
|
| 71 | 168 |
s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
|
| 0 | 169 |
d->nodeState = Pre_operational; |
170 |
newState = Pre_operational; |
|
171 |
switchCommunicationState(d, &newCommunicationState); |
|
| 178 | 172 |
if (!(*(d->iam_a_slave))) |
173 |
{
|
|
| 183 | 174 |
ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback); |
175 |
||
176 |
if (errorCode != OD_SUCCESSFUL) |
|
177 |
{
|
|
178 |
(*d->preOperational)(); |
|
179 |
} |
|
180 |
else |
|
181 |
{
|
|
182 |
UNS32 res; |
|
183 |
res = decompo_dcf(d,0x01); |
|
184 |
} |
|
| 178 | 185 |
} |
186 |
else |
|
187 |
{
|
|
188 |
(*d->preOperational)(); |
|
189 |
} |
|
| 0 | 190 |
} |
191 |
break; |
|
192 |
||
193 |
case Operational: |
|
194 |
if(d->nodeState == Initialisation) return 0xFF; |
|
195 |
{
|
|
| 71 | 196 |
s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
|
| 0 | 197 |
d->nodeState = Operational; |
198 |
newState = Operational; |
|
199 |
switchCommunicationState(d, &newCommunicationState); |
|
200 |
(*d->operational)(); |
|
201 |
} |
|
202 |
break; |
|
203 |
||
204 |
case Stopped: |
|
205 |
if(d->nodeState == Initialisation) return 0xFF; |
|
206 |
{
|
|
| 71 | 207 |
s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
|
| 0 | 208 |
d->nodeState = Stopped; |
209 |
newState = Stopped; |
|
210 |
switchCommunicationState(d, &newCommunicationState); |
|
211 |
(*d->stopped)(); |
|
212 |
} |
|
213 |
break; |
|
214 |
||
215 |
default: |
|
216 |
return 0xFF; |
|
| 215 | 217 |
}/* 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
|
218 |
|
| 0 | 219 |
} |
220 |
return 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; |
|
244 |
if(offset){
|
|
| 215 | 245 |
/* cob_id_client = 0x600 + nodeId; */ |
| 0 | 246 |
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
| 215 | 247 |
/* cob_id_server = 0x580 + nodeId; */ |
| 0 | 248 |
*(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
| 215 | 249 |
/* node Id client. As we do not know the value, we put the node Id Server */ |
250 |
/* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */ |
|
| 0 | 251 |
} |
252 |
||
| 215 | 253 |
/* |
| 208 | 254 |
Initialize the server(s) SDO parameters |
255 |
Remember that only one SDO server is allowed, defined at index 0x1200 |
|
256 |
||
257 |
Initialize the client(s) SDO parameters |
|
258 |
Nothing to initialize (no default values required by the DS 401) |
|
259 |
Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 |
|
260 |
*/ |
|
| 0 | 261 |
{
|
262 |
UNS8 i = 0; |
|
263 |
UNS16 offset = d->firstIndex->PDO_RCV; |
|
264 |
UNS16 lastIndex = d->lastIndex->PDO_RCV; |
|
265 |
UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
|
|
266 |
if( offset ) while( (offset <= lastIndex) && (i < 4)) {
|
|
|
236
905677ed00f3
Full preliminary implementation of TPDO transmit type:
etisserant
parents:
235
diff
changeset
|
267 |
if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) |
| 0 | 268 |
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
269 |
i ++; |
|
270 |
offset ++; |
|
271 |
} |
|
272 |
} |
|
| 71 | 273 |
/* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */ |
| 0 | 274 |
{
|
275 |
UNS8 i = 0; |
|
276 |
UNS16 offset = d->firstIndex->PDO_TRS; |
|
277 |
UNS16 lastIndex = d->lastIndex->PDO_TRS; |
|
278 |
UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
|
|
279 |
i = 0; |
|
280 |
if( offset ) while ((offset <= lastIndex) && (i < 4)) {
|
|
|
236
905677ed00f3
Full preliminary implementation of TPDO transmit type:
etisserant
parents:
235
diff
changeset
|
281 |
if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) |
| 0 | 282 |
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
283 |
i ++; |
|
284 |
offset ++; |
|
285 |
} |
|
286 |
} |
|
| 215 | 287 |
/* bDeviceNodeId is defined in the object dictionary. */ |
| 0 | 288 |
*d->bDeviceNodeId = nodeId; |
289 |
} |
|
| 149 | 290 |
|
291 |
void _initialisation(){}
|
|
292 |
void _preOperational(){}
|
|
293 |
void _operational(){}
|
|
294 |
void _stopped(){}
|