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