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