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