29 #include "led.h" |
29 #include "led.h" |
30 #else |
30 #else |
31 #define led_set_state(a,b) |
31 #define led_set_state(a,b) |
32 #endif |
32 #endif |
33 |
33 |
34 // Prototypes for internals functions |
34 /* Prototypes for internals functions */ |
35 void switchCommunicationState(CO_Data* d, |
35 void switchCommunicationState(CO_Data* d, |
36 s_state_communication *newCommunicationState); |
36 s_state_communication *newCommunicationState); |
37 |
37 |
38 /*****************************************************************************/ |
38 /*****************************************************************************/ |
39 e_nodeState getState(CO_Data* d) |
39 e_nodeState getState(CO_Data* d) |
102 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
102 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState) |
103 { |
103 { |
104 StartOrStop(csSDO, None, resetSDO(d)) |
104 StartOrStop(csSDO, None, resetSDO(d)) |
105 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
105 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d)) |
106 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
106 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d)) |
107 // StartOrStop(Emergency,,) |
107 /* StartOrStop(Emergency,,) */ |
108 StartOrStop(csPDO, None, None) |
108 StartOrStop(csPDO, None, None) |
109 StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
109 StartOrStop(csBoot_Up, None, slaveSendBootUp(d)) |
110 } |
110 } |
111 |
111 |
112 /*****************************************************************************/ |
112 /*****************************************************************************/ |
114 { |
114 { |
115 while(newState != d->nodeState){ |
115 while(newState != d->nodeState){ |
116 switch( newState ){ |
116 switch( newState ){ |
117 case Initialisation: |
117 case Initialisation: |
118 { |
118 { |
119 s_state_communication newCommunicationState = { |
119 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0}; |
120 csBoot_Up: 1, |
120 /* This will force a second loop for the state switch */ |
121 csSDO: 0, |
|
122 csEmergency: 0, |
|
123 csSYNC: 0, |
|
124 csHeartbeat: 0, |
|
125 csPDO: 0}; |
|
126 // This will force a second loop for the state switch |
|
127 d->nodeState = Initialisation; |
121 d->nodeState = Initialisation; |
128 newState = Pre_operational; |
122 newState = Pre_operational; |
129 switchCommunicationState(d, &newCommunicationState); |
123 switchCommunicationState(d, &newCommunicationState); |
130 // call user app related state func. |
124 /* call user app related state func. */ |
131 (*d->initialisation)(); |
125 (*d->initialisation)(); |
132 } |
126 } |
133 break; |
127 break; |
134 |
128 |
135 case Pre_operational: |
129 case Pre_operational: |
136 { |
130 { |
137 s_state_communication newCommunicationState = { |
131 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0}; |
138 csBoot_Up: 0, |
|
139 csSDO: 1, |
|
140 csEmergency: 1, |
|
141 csSYNC: 1, |
|
142 csHeartbeat: 1, |
|
143 csPDO: 0}; |
|
144 d->nodeState = Pre_operational; |
132 d->nodeState = Pre_operational; |
145 newState = Pre_operational; |
133 newState = Pre_operational; |
146 switchCommunicationState(d, &newCommunicationState); |
134 switchCommunicationState(d, &newCommunicationState); |
147 (*d->preOperational)(); |
135 (*d->preOperational)(); |
148 } |
136 } |
149 break; |
137 break; |
150 |
138 |
151 case Operational: |
139 case Operational: |
152 if(d->nodeState == Initialisation) return 0xFF; |
140 if(d->nodeState == Initialisation) return 0xFF; |
153 { |
141 { |
154 s_state_communication newCommunicationState = { |
142 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1}; |
155 csBoot_Up: 0, |
|
156 csSDO: 1, |
|
157 csEmergency: 1, |
|
158 csSYNC: 1, |
|
159 csHeartbeat: 1, |
|
160 csPDO: 1}; |
|
161 d->nodeState = Operational; |
143 d->nodeState = Operational; |
162 newState = Operational; |
144 newState = Operational; |
163 switchCommunicationState(d, &newCommunicationState); |
145 switchCommunicationState(d, &newCommunicationState); |
164 (*d->operational)(); |
146 (*d->operational)(); |
165 } |
147 } |
166 break; |
148 break; |
167 |
149 |
168 case Stopped: |
150 case Stopped: |
169 if(d->nodeState == Initialisation) return 0xFF; |
151 if(d->nodeState == Initialisation) return 0xFF; |
170 { |
152 { |
171 s_state_communication newCommunicationState = { |
153 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0}; |
172 csBoot_Up: 0, |
|
173 csSDO: 0, |
|
174 csEmergency: 0, |
|
175 csSYNC: 0, |
|
176 csHeartbeat: 1, |
|
177 csPDO: 0}; |
|
178 d->nodeState = Stopped; |
154 d->nodeState = Stopped; |
179 newState = Stopped; |
155 newState = Stopped; |
180 switchCommunicationState(d, &newCommunicationState); |
156 switchCommunicationState(d, &newCommunicationState); |
181 (*d->stopped)(); |
157 (*d->stopped)(); |
182 } |
158 } |
183 break; |
159 break; |
184 |
160 |
185 default: |
161 default: |
186 return 0xFF; |
162 return 0xFF; |
187 }//end switch case |
163 }/* end switch case */ |
188 |
164 |
189 led_set_state(d, newState); |
165 led_set_state(d, newState); |
190 } |
166 } |
191 return 0; |
167 return 0; |
192 } |
168 } |
200 /*****************************************************************************/ |
176 /*****************************************************************************/ |
201 void setNodeId(CO_Data* d, UNS8 nodeId) |
177 void setNodeId(CO_Data* d, UNS8 nodeId) |
202 { |
178 { |
203 UNS16 offset = d->firstIndex->SDO_SVR; |
179 UNS16 offset = d->firstIndex->SDO_SVR; |
204 if(offset){ |
180 if(offset){ |
205 //cob_id_client = 0x600 + nodeId; |
181 /* cob_id_client = 0x600 + nodeId; */ |
206 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
182 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId; |
207 //cob_id_server = 0x580 + nodeId; |
183 /* cob_id_server = 0x580 + nodeId; */ |
208 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
184 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId; |
209 // node Id client. As we do not know the value, we put the node Id Server |
185 /* node Id client. As we do not know the value, we put the node Id Server */ |
210 //*(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; |
186 /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */ |
211 } |
187 } |
212 |
188 |
213 // ** Initialize the server(s) SDO parameters |
189 /* ** Initialize the server(s) SDO parameters */ |
214 // Remember that only one SDO server is allowed, defined at index 0x1200 |
190 /* Remember that only one SDO server is allowed, defined at index 0x1200 */ |
215 |
191 |
216 // ** Initialize the client(s) SDO parameters |
192 /* ** Initialize the client(s) SDO parameters */ |
217 // Nothing to initialize (no default values required by the DS 401) |
193 /* Nothing to initialize (no default values required by the DS 401) */ |
218 // ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 |
194 /* ** Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 */ |
219 { |
195 { |
220 UNS8 i = 0; |
196 UNS8 i = 0; |
221 UNS16 offset = d->firstIndex->PDO_RCV; |
197 UNS16 offset = d->firstIndex->PDO_RCV; |
222 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
198 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
223 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
199 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500}; |
226 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
202 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId; |
227 i ++; |
203 i ++; |
228 offset ++; |
204 offset ++; |
229 } |
205 } |
230 } |
206 } |
231 // ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 |
207 /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */ |
232 { |
208 { |
233 UNS8 i = 0; |
209 UNS8 i = 0; |
234 UNS16 offset = d->firstIndex->PDO_TRS; |
210 UNS16 offset = d->firstIndex->PDO_TRS; |
235 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
211 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
236 UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480}; |
212 UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480}; |