|
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 |
|
|
71
|
23 |
|
|
0
|
24 |
#include "data.h"
|
|
|
25 |
#include "sync.h"
|
|
|
26 |
|
|
71
|
27 |
/* Prototypes for internals functions */
|
|
53
|
28 |
void SyncAlarm(CO_Data* d, UNS32 id);
|
|
|
29 |
UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable,
|
|
|
30 |
UNS8 unsused_bSubindex);
|
|
|
31 |
|
|
|
32 |
|
|
|
33 |
|
|
|
34 |
/*****************************************************************************/
|
|
0
|
35 |
void SyncAlarm(CO_Data* d, UNS32 id)
|
|
|
36 |
{
|
|
|
37 |
sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
|
|
|
38 |
}
|
|
|
39 |
|
|
53
|
40 |
/*****************************************************************************/
|
|
71
|
41 |
/* This is called when Index 0x1005 is updated.*/
|
|
0
|
42 |
UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
|
|
|
43 |
{
|
|
|
44 |
startSYNC(d);
|
|
31
|
45 |
return 0;
|
|
0
|
46 |
}
|
|
|
47 |
|
|
53
|
48 |
/*****************************************************************************/
|
|
0
|
49 |
void startSYNC(CO_Data* d)
|
|
|
50 |
{
|
|
|
51 |
RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
|
|
|
52 |
RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
|
|
|
53 |
|
|
|
54 |
if(d->syncTimer != TIMER_NONE){
|
|
|
55 |
stopSYNC(d);
|
|
|
56 |
}
|
|
|
57 |
|
|
|
58 |
if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)
|
|
|
59 |
{
|
|
|
60 |
d->syncTimer = SetAlarm(
|
|
|
61 |
d,
|
|
71
|
62 |
0 /*No id needed*/,
|
|
0
|
63 |
&SyncAlarm,
|
|
|
64 |
US_TO_TIMEVAL(*d->Sync_Cycle_Period),
|
|
|
65 |
US_TO_TIMEVAL(*d->Sync_Cycle_Period));
|
|
|
66 |
}
|
|
|
67 |
}
|
|
|
68 |
|
|
53
|
69 |
/*****************************************************************************/
|
|
0
|
70 |
void stopSYNC(CO_Data* d)
|
|
|
71 |
{
|
|
|
72 |
d->syncTimer = DelAlarm(d->syncTimer);
|
|
|
73 |
}
|
|
|
74 |
|
|
|
75 |
/*********************************************************************/
|
|
|
76 |
UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
|
|
|
77 |
{
|
|
|
78 |
Message m;
|
|
|
79 |
UNS8 resultat ;
|
|
|
80 |
|
|
|
81 |
MSG_WAR(0x3001, "sendSYNC ", 0);
|
|
|
82 |
|
|
|
83 |
m.cob_id.w = cob_id ;
|
|
|
84 |
m.rtr = NOT_A_REQUEST;
|
|
|
85 |
m.len = 0;
|
|
|
86 |
resultat = (*d->canSend)(&m) ;
|
|
|
87 |
proceedSYNC(d, &m) ;
|
|
|
88 |
return resultat ;
|
|
|
89 |
}
|
|
|
90 |
|
|
|
91 |
/*****************************************************************************/
|
|
|
92 |
UNS8 proceedSYNC(CO_Data* d, Message *m)
|
|
|
93 |
{
|
|
|
94 |
|
|
71
|
95 |
UNS8 pdoNum, /* number of the actual processed pdo-nr. */
|
|
|
96 |
prp_j;
|
|
|
97 |
|
|
|
98 |
const UNS8 * pMappingCount = NULL; /* count of mapped objects...*/
|
|
|
99 |
/* pointer to the var which is mapped to a pdo */
|
|
|
100 |
/* void * pMappedAppObject = NULL; */
|
|
|
101 |
/* pointer fo the var which holds the mapping parameter of an mapping entry */
|
|
0
|
102 |
UNS32 * pMappingParameter = NULL;
|
|
71
|
103 |
/* pointer to the transmissiontype...*/
|
|
0
|
104 |
UNS8 * pTransmissionType = NULL;
|
|
|
105 |
UNS32 * pwCobId = NULL;
|
|
|
106 |
|
|
|
107 |
UNS8 dataType;
|
|
|
108 |
UNS16 index;
|
|
|
109 |
UNS8 subIndex;
|
|
|
110 |
UNS8 offset;
|
|
|
111 |
UNS8 status;
|
|
|
112 |
UNS8 sizeData;
|
|
|
113 |
UNS32 objDict;
|
|
|
114 |
UNS16 offsetObjdict;
|
|
|
115 |
UNS16 offsetObjdictMap;
|
|
|
116 |
UNS16 lastIndex;
|
|
71
|
117 |
|
|
|
118 |
status = state3;
|
|
|
119 |
pdoNum = 0x00;
|
|
|
120 |
prp_j = 0x00;
|
|
|
121 |
offset = 0x00;
|
|
|
122 |
|
|
|
123 |
MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
|
|
|
124 |
|
|
0
|
125 |
/* only operational state allows PDO transmission */
|
|
|
126 |
if( d->nodeState != Operational )
|
|
|
127 |
return 0;
|
|
|
128 |
|
|
|
129 |
(*d->post_sync)();
|
|
|
130 |
|
|
|
131 |
/* So, the node is in operational state */
|
|
|
132 |
/* study all PDO stored in the objects dictionary */
|
|
|
133 |
|
|
|
134 |
offsetObjdict = d->firstIndex->PDO_TRS;
|
|
|
135 |
lastIndex = d->lastIndex->PDO_TRS;
|
|
|
136 |
offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
|
|
|
137 |
|
|
|
138 |
if(offsetObjdict) while( offsetObjdict <= lastIndex) {
|
|
|
139 |
switch( status ) {
|
|
|
140 |
|
|
|
141 |
case state3: /* get the PDO transmission type */
|
|
|
142 |
if (d->objdict[offsetObjdict].bSubCount <= 2) {
|
|
|
143 |
MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum);
|
|
|
144 |
return 0xFF;
|
|
|
145 |
}
|
|
|
146 |
pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
|
|
|
147 |
MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
|
|
|
148 |
status = state4;
|
|
|
149 |
break;
|
|
|
150 |
case state4: /* check if transmission type is after (this) SYNC */
|
|
|
151 |
/* The message may not be transmited every SYNC but every n SYNC */
|
|
|
152 |
if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
|
|
|
153 |
(++d->count_sync[pdoNum] == *pTransmissionType) ) {
|
|
|
154 |
d->count_sync[pdoNum] = 0;
|
|
|
155 |
MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
|
|
|
156 |
status = state5;
|
|
|
157 |
break;
|
|
|
158 |
}
|
|
|
159 |
else {
|
|
|
160 |
MSG_WAR(0x3008, " Not on synchro or not at this SYNC. Trans type : ",
|
|
|
161 |
*pTransmissionType);
|
|
|
162 |
pdoNum++;
|
|
|
163 |
offsetObjdict++;
|
|
|
164 |
offsetObjdictMap++;
|
|
|
165 |
status = state11;
|
|
|
166 |
break;
|
|
|
167 |
}
|
|
|
168 |
case state5: /* get PDO CobId */
|
|
|
169 |
pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
|
|
|
170 |
MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId);
|
|
|
171 |
status = state7;
|
|
|
172 |
break;
|
|
|
173 |
case state7: /* get mapped objects number to transmit with this PDO */
|
|
|
174 |
pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
|
|
|
175 |
MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount );
|
|
|
176 |
status = state8;
|
|
|
177 |
case state8: /* get mapping parameters */
|
|
|
178 |
pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
|
|
|
179 |
MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter);
|
|
|
180 |
MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum);
|
|
|
181 |
MSG_WAR(0x3051, " sub-index : ", prp_j + 1);
|
|
|
182 |
status = state9;
|
|
|
183 |
|
|
|
184 |
case state9: /* get data to transmit */
|
|
|
185 |
index = (UNS16)((*pMappingParameter) >> 16);
|
|
|
186 |
subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
|
|
71
|
187 |
/* <<3 because in *pMappingParameter the size is in bits */
|
|
0
|
188 |
sizeData = (UNS8) ((*pMappingParameter & (UNS32)0x000000FF) >> 3) ;
|
|
|
189 |
|
|
|
190 |
objDict = getODentry(d, index, subIndex, (void *)&d->process_var.data[offset], &sizeData, &dataType, 0 );
|
|
|
191 |
|
|
|
192 |
if( objDict != OD_SUCCESSFUL ){
|
|
|
193 |
MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
|
|
|
194 |
return 0xFF;
|
|
|
195 |
}
|
|
|
196 |
|
|
|
197 |
offset += sizeData ;
|
|
|
198 |
d->process_var.count = offset;
|
|
|
199 |
prp_j++;
|
|
|
200 |
status = state10;
|
|
|
201 |
break;
|
|
|
202 |
|
|
|
203 |
case state10: /* loop to get all the data to transmit */
|
|
|
204 |
if( prp_j < *pMappingCount ){
|
|
|
205 |
MSG_WAR(0x3014, " next variable mapped : ", prp_j);
|
|
|
206 |
status = state8;
|
|
|
207 |
break;
|
|
|
208 |
}
|
|
|
209 |
else {
|
|
|
210 |
MSG_WAR(0x3015, " End scan mapped variable", 0);
|
|
|
211 |
PDOmGR( d, *pwCobId );
|
|
|
212 |
MSG_WAR(0x3016, " End of this pdo. Should have been sent", 0);
|
|
|
213 |
pdoNum++;
|
|
|
214 |
offsetObjdict++;
|
|
|
215 |
offsetObjdictMap++;
|
|
|
216 |
offset = 0x00;
|
|
|
217 |
prp_j = 0x00;
|
|
|
218 |
status = state11;
|
|
|
219 |
break;
|
|
|
220 |
}
|
|
|
221 |
|
|
|
222 |
case state11:
|
|
|
223 |
MSG_WAR(0x3017, "next pdo index : ", pdoNum);
|
|
|
224 |
status = state3;
|
|
|
225 |
break;
|
|
|
226 |
|
|
|
227 |
default:
|
|
|
228 |
MSG_ERR(0x1019,"Unknown state has been reached : %d",status);
|
|
|
229 |
return 0xFF;
|
|
71
|
230 |
}/* end switch case */
|
|
0
|
231 |
|
|
71
|
232 |
}/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */
|
|
0
|
233 |
|
|
|
234 |
(*d->post_TPDO)();
|
|
|
235 |
|
|
|
236 |
return 0;
|
|
|
237 |
}
|
|
|
238 |
|
|
|
239 |
|