18 You should have received a copy of the GNU Lesser General Public |
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 |
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 |
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
21 */ |
21 */ |
22 |
22 |
|
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 */ |
23 |
32 |
24 #include "data.h" |
33 #include "data.h" |
25 #include "sync.h" |
34 #include "sync.h" |
26 #include "canfestival.h" |
35 #include "canfestival.h" |
27 |
36 |
28 /* Prototypes for internals functions */ |
37 /* Prototypes for internals functions */ |
|
38 |
|
39 /*! |
|
40 ** |
|
41 ** |
|
42 ** @param d |
|
43 ** @param id |
|
44 **/ |
29 void SyncAlarm(CO_Data* d, UNS32 id); |
45 void SyncAlarm(CO_Data* d, UNS32 id); |
30 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, |
46 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, |
31 UNS8 unsused_bSubindex); |
47 UNS8 unsused_bSubindex); |
32 |
48 |
33 |
49 /*! |
34 |
50 ** |
35 /*****************************************************************************/ |
51 ** |
|
52 ** @param d |
|
53 ** @param id |
|
54 **/ |
36 void SyncAlarm(CO_Data* d, UNS32 id) |
55 void SyncAlarm(CO_Data* d, UNS32 id) |
37 { |
56 { |
38 sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ; |
57 sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ; |
39 } |
58 } |
40 |
59 |
41 /*****************************************************************************/ |
60 /*! |
42 /* This is called when Index 0x1005 is updated.*/ |
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 **/ |
43 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex) |
69 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex) |
44 { |
70 { |
45 startSYNC(d); |
71 startSYNC(d); |
46 return 0; |
72 return 0; |
47 } |
73 } |
48 |
74 |
49 /*****************************************************************************/ |
75 /*! |
|
76 ** |
|
77 ** |
|
78 ** @param d |
|
79 **/ |
50 void startSYNC(CO_Data* d) |
80 void startSYNC(CO_Data* d) |
51 { |
81 { |
52 RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate); |
82 RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate); |
53 RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate); |
83 RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate); |
54 |
84 |
87 resultat = canSend(d->canHandle,&m) ; |
128 resultat = canSend(d->canHandle,&m) ; |
88 proceedSYNC(d, &m) ; |
129 proceedSYNC(d, &m) ; |
89 return resultat ; |
130 return resultat ; |
90 } |
131 } |
91 |
132 |
92 /*****************************************************************************/ |
133 /*! |
|
134 ** |
|
135 ** |
|
136 ** @param d |
|
137 ** @param m |
|
138 ** |
|
139 ** @return |
|
140 **/ |
93 UNS8 proceedSYNC(CO_Data* d, Message *m) |
141 UNS8 proceedSYNC(CO_Data* d, Message *m) |
94 { |
142 { |
95 |
143 |
96 UNS8 pdoNum, /* number of the actual processed pdo-nr. */ |
144 UNS8 pdoNum, /* number of the actual processed pdo-nr. */ |
97 prp_j; |
145 prp_j; |
98 |
146 |
99 const UNS8 * pMappingCount = NULL; /* count of mapped objects...*/ |
147 const UNS8 * pMappingCount = NULL; /* count of mapped objects...*/ |
100 /* pointer to the var which is mapped to a pdo */ |
148 /** pointer to the var which is mapped to a pdo */ |
101 /* void * pMappedAppObject = NULL; */ |
149 /* void * pMappedAppObject = NULL; */ |
102 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
150 /** pointer fo the var which holds the mapping parameter of an mapping entry */ |
103 UNS32 * pMappingParameter = NULL; |
151 UNS32 * pMappingParameter = NULL; |
104 /* pointer to the transmissiontype...*/ |
152 /** pointer to the transmissiontype...*/ |
105 UNS8 * pTransmissionType = NULL; |
153 UNS8 * pTransmissionType = NULL; |
106 UNS32 * pwCobId = NULL; |
154 UNS32 * pwCobId = NULL; |
107 |
155 |
108 UNS8 dataType; |
156 UNS8 dataType; |
109 UNS16 index; |
157 UNS16 index; |
123 |
171 |
124 MSG_WAR(0x3002, "SYNC received. Proceed. ", 0); |
172 MSG_WAR(0x3002, "SYNC received. Proceed. ", 0); |
125 |
173 |
126 (*d->post_sync)(); |
174 (*d->post_sync)(); |
127 |
175 |
128 /* only operational state allows PDO transmission */ |
176 /** only operational state allows PDO transmission */ |
129 if( d->nodeState != Operational ) |
177 if( d->nodeState != Operational ) |
130 return 0; |
178 return 0; |
131 |
179 |
132 /* So, the node is in operational state */ |
180 /** So, the node is in operational state */ |
133 /* study all PDO stored in the objects dictionary */ |
181 /** study all PDO stored in the objects dictionary */ |
134 |
182 |
135 offsetObjdict = d->firstIndex->PDO_TRS; |
183 offsetObjdict = d->firstIndex->PDO_TRS; |
136 lastIndex = d->lastIndex->PDO_TRS; |
184 lastIndex = d->lastIndex->PDO_TRS; |
137 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
185 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
138 |
186 |
139 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
187 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
140 switch( status ) { |
188 switch( status ) { |
141 |
189 |
142 case state3: /* get the PDO transmission type */ |
190 case state3: /** get the PDO transmission type */ |
143 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
191 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
144 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
192 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
145 return 0xFF; |
193 return 0xFF; |
146 } |
194 } |
147 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
195 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
148 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
196 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
149 status = state4; |
197 status = state4; |
150 break; |
198 break; |
151 case state4: /* check if transmission type is after (this) SYNC */ |
199 case state4: /** check if transmission type is after (this) SYNC */ |
152 /* The message may not be transmited every SYNC but every n SYNC */ |
200 /** The message may not be transmited every SYNC but every n SYNC */ |
153 if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) && |
201 if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) && |
154 (++d->count_sync[pdoNum] == *pTransmissionType) ) { |
202 (++d->count_sync[pdoNum] == *pTransmissionType) ) { |
155 d->count_sync[pdoNum] = 0; |
203 d->count_sync[pdoNum] = 0; |
156 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
204 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
157 status = state5; |
205 status = state5; |
164 offsetObjdict++; |
212 offsetObjdict++; |
165 offsetObjdictMap++; |
213 offsetObjdictMap++; |
166 status = state11; |
214 status = state11; |
167 break; |
215 break; |
168 } |
216 } |
169 case state5: /* get PDO CobId */ |
217 case state5: /** get PDO CobId */ |
170 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
218 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
171 MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId); |
219 MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId); |
172 status = state7; |
220 status = state7; |
173 break; |
221 break; |
174 case state7: /* get mapped objects number to transmit with this PDO */ |
222 case state7: /** get mapped objects number to transmit with this PDO */ |
175 pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject; |
223 pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject; |
176 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
224 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
177 status = state8; |
225 status = state8; |
178 case state8: /* get mapping parameters */ |
226 case state8: /** get mapping parameters */ |
179 pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject; |
227 pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject; |
180 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
228 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
181 MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum); |
229 MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum); |
182 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
230 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
183 status = state9; |
231 status = state9; |
184 |
232 |
185 case state9: /* get data to transmit */ |
233 case state9: /** get data to transmit */ |
186 { |
234 { |
187 UNS8 ByteSize; |
235 UNS8 ByteSize; |
188 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
236 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
189 index = (UNS16)((*pMappingParameter) >> 16); |
237 index = (UNS16)((*pMappingParameter) >> 16); |
190 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
238 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
191 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
239 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
192 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
240 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
193 objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ); |
241 objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ); |
194 /* copy bit per bit in little endian*/ |
242 /** copy bit per bit in little endian*/ |
195 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0); |
243 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0); |
196 } |
244 } |
197 if( objDict != OD_SUCCESSFUL ){ |
245 if( objDict != OD_SUCCESSFUL ){ |
198 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
246 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
199 return 0xFF; |
247 return 0xFF; |