|
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 #include "states.h" |
|
24 #include "def.h" |
|
25 |
|
26 #include "nmtSlave.h" |
|
27 |
|
28 #ifdef LED_ENABLE |
|
29 #include "led.h" |
|
30 #else |
|
31 #define led_set_state(a,b) |
|
32 #endif |
|
33 |
|
34 |
|
35 e_nodeState getState(CO_Data* d) |
|
36 { |
|
37 return d->nodeState; |
|
38 } |
|
39 |
|
40 void canDispatch(CO_Data* d, Message *m) |
|
41 { |
|
42 switch(m->cob_id.w >> 7) |
|
43 { |
|
44 case SYNC: |
|
45 if(d->CurrentCommunicationState.csSYNC) |
|
46 proceedSYNC(d,m); |
|
47 break; |
|
48 //case TIME_STAMP: |
|
49 case PDO1tx: |
|
50 case PDO1rx: |
|
51 case PDO2tx: |
|
52 case PDO2rx: |
|
53 case PDO3tx: |
|
54 case PDO3rx: |
|
55 case PDO4tx: |
|
56 case PDO4rx: |
|
57 if (d->CurrentCommunicationState.csPDO) |
|
58 proceedPDO(d,m); |
|
59 break; |
|
60 case SDOtx: |
|
61 case SDOrx: |
|
62 if (d->CurrentCommunicationState.csSDO) |
|
63 proceedSDO(d,m); |
|
64 break; |
|
65 case NODE_GUARD: |
|
66 if (d->CurrentCommunicationState.csHeartbeat) |
|
67 proceedNODE_GUARD(d,m); |
|
68 break; |
|
69 case NMT: |
|
70 if (d->iam_a_slave) |
|
71 { |
|
72 proceedNMTstateChange(d,m); |
|
73 } |
|
74 #ifdef CANOPEN_LSS_ENABLE |
|
75 default: |
|
76 if (m->cob_id.w == 0x7E4 || m->cob_id.w == 0x705) |
|
77 { |
|
78 proceedLSS(d,m); |
|
79 } |
|
80 #endif |
|
81 } |
|
82 } |
|
83 |
|
84 #define StartOrStop(CommType, FuncStart, FuncStop) \ |
|
85 if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\ |
|
86 MSG_ERR(0x9999,#FuncStart, 9999);\ |
|
87 d->CurrentCommunicationState.CommType = 1;\ |
|
88 FuncStart;\ |
|
89 }else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType){\ |
|
90 MSG_ERR(0x9999,#FuncStop, 9999);\ |
|
91 d->CurrentCommunicationState.CommType = 0;\ |
|
92 FuncStop;\ |
|
93 } |
|
94 #define None |
|
95 |
|
96 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
|
97 { |
|
98 StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
|
99 StartOrStop(csSDO, None, resetSDO(d)) |
|
100 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
|
101 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
|
102 // StartOrStop(Emergency,,) |
|
103 StartOrStop(csPDO, None, None) |
|
104 } |
|
105 |
|
106 UNS8 setState(CO_Data* d, e_nodeState newState) |
|
107 { |
|
108 while(newState != d->nodeState){ |
|
109 switch( newState ){ |
|
110 case Initialisation: |
|
111 { |
|
112 s_state_communication newCommunicationState = { |
|
113 csBoot_Up: 1, |
|
114 csSDO: 0, |
|
115 csEmergency: 0, |
|
116 csSYNC: 0, |
|
117 csHeartbeat: 0, |
|
118 csPDO: 0}; |
|
119 // This will force a second loop for the state switch |
|
120 d->nodeState = Initialisation; |
|
121 newState = Pre_operational; |
|
122 switchCommunicationState(d, &newCommunicationState); |
|
123 // call user app related state func. |
|
124 (*d->initialisation)(); |
|
125 } |
|
126 break; |
|
127 |
|
128 case Pre_operational: |
|
129 { |
|
130 s_state_communication newCommunicationState = { |
|
131 csBoot_Up: 0, |
|
132 csSDO: 1, |
|
133 csEmergency: 1, |
|
134 csSYNC: 1, |
|
135 csHeartbeat: 1, |
|
136 csPDO: 0}; |
|
137 d->nodeState = Pre_operational; |
|
138 newState = Pre_operational; |
|
139 switchCommunicationState(d, &newCommunicationState); |
|
140 (*d->preOperational)(); |
|
141 } |
|
142 break; |
|
143 |
|
144 case Operational: |
|
145 if(d->nodeState == Initialisation) return 0xFF; |
|
146 { |
|
147 s_state_communication newCommunicationState = { |
|
148 csBoot_Up: 0, |
|
149 csSDO: 1, |
|
150 csEmergency: 1, |
|
151 csSYNC: 1, |
|
152 csHeartbeat: 1, |
|
153 csPDO: 1}; |
|
154 d->nodeState = Operational; |
|
155 newState = Operational; |
|
156 switchCommunicationState(d, &newCommunicationState); |
|
157 (*d->operational)(); |
|
158 } |
|
159 break; |
|
160 |
|
161 case Stopped: |
|
162 if(d->nodeState == Initialisation) return 0xFF; |
|
163 { |
|
164 s_state_communication newCommunicationState = { |
|
165 csBoot_Up: 0, |
|
166 csSDO: 0, |
|
167 csEmergency: 0, |
|
168 csSYNC: 0, |
|
169 csHeartbeat: 1, |
|
170 csPDO: 0}; |
|
171 d->nodeState = Stopped; |
|
172 newState = Stopped; |
|
173 switchCommunicationState(d, &newCommunicationState); |
|
174 (*d->stopped)(); |
|
175 } |
|
176 break; |
|
177 |
|
178 default: |
|
179 return 0xFF; |
|
180 }//end switch case |
|
181 |
|
182 led_set_state(d, newState); |
|
183 } |
|
184 return 0; |
|
185 } |
|
186 |
|
187 UNS8 getNodeId(CO_Data* d) |
|
188 { |
|
189 return *d->bDeviceNodeId; |
|
190 } |
|
191 |
|
192 void setNodeId(CO_Data* d, UNS8 nodeId) |
|
193 { |
|
194 UNS16 offset = d->firstIndex->SDO_SVR; |
|
195 if(offset){ |
|
196 //cob_id_client = 0x600 + nodeId; |
|
197 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
|
198 //cob_id_server = 0x580 + nodeId; |
|
199 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
|
200 // node Id client. As we do not know the value, we put the node Id Server |
|
201 //*(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; |
|
202 } |
|
203 |
|
204 // ** Initialize the server(s) SDO parameters |
|
205 // Remember that only one SDO server is allowed, defined at index 0x1200 |
|
206 |
|
207 // ** Initialize the client(s) SDO parameters |
|
208 // Nothing to initialize (no default values required by the DS 401) |
|
209 // ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 |
|
210 { |
|
211 UNS8 i = 0; |
|
212 UNS16 offset = d->firstIndex->PDO_RCV; |
|
213 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
|
214 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
|
215 if( offset ) while( (offset <= lastIndex) && (i < 4)) { |
|
216 if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) |
|
217 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
|
218 i ++; |
|
219 offset ++; |
|
220 } |
|
221 } |
|
222 // ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 |
|
223 { |
|
224 UNS8 i = 0; |
|
225 UNS16 offset = d->firstIndex->PDO_TRS; |
|
226 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
|
227 UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480}; |
|
228 i = 0; |
|
229 if( offset ) while ((offset <= lastIndex) && (i < 4)) { |
|
230 if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) |
|
231 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
|
232 i ++; |
|
233 offset ++; |
|
234 } |
|
235 } |
|
236 // bDeviceNodeId is defined in the object dictionary. |
|
237 *d->bDeviceNodeId = nodeId; |
|
238 } |