17 |
17 |
18 You should have received a copy of the GNU Lesser General Public |
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 |
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 |
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
21 */ |
21 */ |
|
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 */ |
22 |
31 |
23 #include "states.h" |
32 #include "states.h" |
24 #include "def.h" |
33 #include "def.h" |
25 #include "dcf.h" |
34 #include "dcf.h" |
26 #include "nmtSlave.h" |
35 #include "nmtSlave.h" |
27 |
36 |
28 /* Prototypes for internals functions */ |
37 /** Prototypes for internals functions */ |
|
38 /*! |
|
39 ** |
|
40 ** |
|
41 ** @param d |
|
42 ** @param newCommunicationState |
|
43 **/ |
29 void switchCommunicationState(CO_Data* d, |
44 void switchCommunicationState(CO_Data* d, |
30 s_state_communication *newCommunicationState); |
45 s_state_communication *newCommunicationState); |
31 |
46 |
32 /*****************************************************************************/ |
47 /*! |
|
48 ** |
|
49 ** |
|
50 ** @param d |
|
51 ** |
|
52 ** @return |
|
53 **/ |
33 e_nodeState getState(CO_Data* d) |
54 e_nodeState getState(CO_Data* d) |
34 { |
55 { |
35 return d->nodeState; |
56 return d->nodeState; |
36 } |
57 } |
37 |
58 |
38 /*****************************************************************************/ |
59 /*! |
|
60 ** |
|
61 ** |
|
62 ** @param d |
|
63 ** @param m |
|
64 **/ |
39 void canDispatch(CO_Data* d, Message *m) |
65 void canDispatch(CO_Data* d, Message *m) |
40 { |
66 { |
41 switch(m->cob_id.w >> 7) |
67 switch(m->cob_id.w >> 7) |
42 { |
68 { |
43 case SYNC: |
69 case SYNC: |
44 if(d->CurrentCommunicationState.csSYNC) |
70 if(d->CurrentCommunicationState.csSYNC) |
45 proceedSYNC(d,m); |
71 proceedSYNC(d,m); |
46 break; |
72 break; |
47 /* case TIME_STAMP: */ |
73 /** case TIME_STAMP: */ |
48 case PDO1tx: |
74 case PDO1tx: |
49 case PDO1rx: |
75 case PDO1rx: |
50 case PDO2tx: |
76 case PDO2tx: |
51 case PDO2rx: |
77 case PDO2rx: |
52 case PDO3tx: |
78 case PDO3tx: |
82 MSG_WAR(0x9999,#FuncStop, 9999);\ |
108 MSG_WAR(0x9999,#FuncStop, 9999);\ |
83 d->CurrentCommunicationState.CommType = 0;\ |
109 d->CurrentCommunicationState.CommType = 0;\ |
84 FuncStop;\ |
110 FuncStop;\ |
85 } |
111 } |
86 #define None |
112 #define None |
87 |
113 |
88 /*****************************************************************************/ |
114 /*! |
|
115 ** |
|
116 ** |
|
117 ** @param d |
|
118 ** @param newCommunicationState |
|
119 **/ |
89 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
120 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
90 { |
121 { |
91 StartOrStop(csSDO, None, resetSDO(d)) |
122 StartOrStop(csSDO, None, resetSDO(d)) |
92 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
123 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
93 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
124 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
94 /* StartOrStop(Emergency,,) */ |
125 /* StartOrStop(Emergency,,) */ |
95 StartOrStop(csPDO, None, None) |
126 StartOrStop(csPDO, None, None) |
96 StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
127 StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
97 } |
128 } |
98 |
129 |
99 /*****************************************************************************/ |
130 /*! |
|
131 ** |
|
132 ** |
|
133 ** @param d |
|
134 ** @param newState |
|
135 ** |
|
136 ** @return |
|
137 **/ |
100 UNS8 setState(CO_Data* d, e_nodeState newState) |
138 UNS8 setState(CO_Data* d, e_nodeState newState) |
101 { |
139 { |
102 UNS16 wIndex = 0x1F22; |
140 UNS16 wIndex = 0x1F22; |
103 const indextable *ptrTable; |
141 const indextable *ptrTable; |
104 ODCallback_t *Callback; |
142 ODCallback_t *Callback; |
106 while(newState != d->nodeState){ |
144 while(newState != d->nodeState){ |
107 switch( newState ){ |
145 switch( newState ){ |
108 case Initialisation: |
146 case Initialisation: |
109 { |
147 { |
110 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0}; |
148 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0}; |
111 /* This will force a second loop for the state switch */ |
149 /** This will force a second loop for the state switch */ |
112 d->nodeState = Initialisation; |
150 d->nodeState = Initialisation; |
113 newState = Pre_operational; |
151 newState = Pre_operational; |
114 switchCommunicationState(d, &newCommunicationState); |
152 switchCommunicationState(d, &newCommunicationState); |
115 /* call user app related state func. */ |
153 /** call user app related state func. */ |
116 (*d->initialisation)(); |
154 (*d->initialisation)(); |
117 |
155 |
118 } |
156 } |
119 break; |
157 break; |
120 |
158 |
168 } |
206 } |
169 break; |
207 break; |
170 |
208 |
171 default: |
209 default: |
172 return 0xFF; |
210 return 0xFF; |
173 }/* end switch case */ |
211 }/** end switch case */ |
174 |
212 |
175 } |
213 } |
176 return 0; |
214 return 0; |
177 } |
215 } |
178 |
216 |
179 /*****************************************************************************/ |
217 /*! |
|
218 ** |
|
219 ** |
|
220 ** @param d |
|
221 ** |
|
222 ** @return |
|
223 **/ |
180 UNS8 getNodeId(CO_Data* d) |
224 UNS8 getNodeId(CO_Data* d) |
181 { |
225 { |
182 return *d->bDeviceNodeId; |
226 return *d->bDeviceNodeId; |
183 } |
227 } |
184 |
228 |
185 /*****************************************************************************/ |
229 /*! |
|
230 ** |
|
231 ** |
|
232 ** @param d |
|
233 ** @param nodeId |
|
234 **/ |
186 void setNodeId(CO_Data* d, UNS8 nodeId) |
235 void setNodeId(CO_Data* d, UNS8 nodeId) |
187 { |
236 { |
188 UNS16 offset = d->firstIndex->SDO_SVR; |
237 UNS16 offset = d->firstIndex->SDO_SVR; |
189 if(offset){ |
238 if(offset){ |
190 /* cob_id_client = 0x600 + nodeId; */ |
239 /** cob_id_client = 0x600 + nodeId; */ |
191 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
240 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
192 /* cob_id_server = 0x580 + nodeId; */ |
241 /** cob_id_server = 0x580 + nodeId; */ |
193 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
242 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
194 /* node Id client. As we do not know the value, we put the node Id Server */ |
243 /** node Id client. As we do not know the value, we put the node Id Server */ |
195 /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */ |
244 /** *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */ |
196 } |
245 } |
197 |
246 |
198 /* ** Initialize the server(s) SDO parameters */ |
247 /** |
199 /* Remember that only one SDO server is allowed, defined at index 0x1200 */ |
248 Initialize the server(s) SDO parameters |
200 |
249 Remember that only one SDO server is allowed, defined at index 0x1200 |
201 /* ** Initialize the client(s) SDO parameters */ |
250 |
202 /* Nothing to initialize (no default values required by the DS 401) */ |
251 Initialize the client(s) SDO parameters |
203 /* ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 */ |
252 Nothing to initialize (no default values required by the DS 401) |
|
253 Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 |
|
254 */ |
204 { |
255 { |
205 UNS8 i = 0; |
256 UNS8 i = 0; |
206 UNS16 offset = d->firstIndex->PDO_RCV; |
257 UNS16 offset = d->firstIndex->PDO_RCV; |
207 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
258 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
208 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
259 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
225 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
276 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
226 i ++; |
277 i ++; |
227 offset ++; |
278 offset ++; |
228 } |
279 } |
229 } |
280 } |
230 /* bDeviceNodeId is defined in the object dictionary. */ |
281 /** bDeviceNodeId is defined in the object dictionary. */ |
231 *d->bDeviceNodeId = nodeId; |
282 *d->bDeviceNodeId = nodeId; |
232 } |
283 } |
233 |
284 |
234 void _initialisation(){} |
285 void _initialisation(){} |
235 void _preOperational(){} |
286 void _preOperational(){} |