|
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 |
*/
|
|
|
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 |
}
|