143 |
143 |
144 UNS8 pdoNum, /* number of the actual processed pdo-nr. */ |
144 UNS8 pdoNum, /* number of the actual processed pdo-nr. */ |
145 prp_j; |
145 prp_j; |
146 |
146 |
147 const UNS8 * pMappingCount = NULL; /* count of mapped objects...*/ |
147 const UNS8 * pMappingCount = NULL; /* count of mapped objects...*/ |
148 /** pointer to the var which is mapped to a pdo */ |
148 /* pointer to the var which is mapped to a pdo */ |
149 /* void * pMappedAppObject = NULL; */ |
149 /* void * pMappedAppObject = NULL; */ |
150 /** 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 */ |
151 UNS32 * pMappingParameter = NULL; |
151 UNS32 * pMappingParameter = NULL; |
152 /** pointer to the transmissiontype...*/ |
152 /* pointer to the transmissiontype...*/ |
153 UNS8 * pTransmissionType = NULL; |
153 UNS8 * pTransmissionType = NULL; |
154 UNS32 * pwCobId = NULL; |
154 UNS32 * pwCobId = NULL; |
155 |
155 |
156 UNS8 dataType; |
156 UNS8 dataType; |
157 UNS16 index; |
157 UNS16 index; |
171 |
171 |
172 MSG_WAR(0x3002, "SYNC received. Proceed. ", 0); |
172 MSG_WAR(0x3002, "SYNC received. Proceed. ", 0); |
173 |
173 |
174 (*d->post_sync)(); |
174 (*d->post_sync)(); |
175 |
175 |
176 /** only operational state allows PDO transmission */ |
176 /* only operational state allows PDO transmission */ |
177 if( d->nodeState != Operational ) |
177 if( d->nodeState != Operational ) |
178 return 0; |
178 return 0; |
179 |
179 |
180 /** So, the node is in operational state */ |
180 /* So, the node is in operational state */ |
181 /** study all PDO stored in the objects dictionary */ |
181 /* study all PDO stored in the objects dictionary */ |
182 |
182 |
183 offsetObjdict = d->firstIndex->PDO_TRS; |
183 offsetObjdict = d->firstIndex->PDO_TRS; |
184 lastIndex = d->lastIndex->PDO_TRS; |
184 lastIndex = d->lastIndex->PDO_TRS; |
185 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
185 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
186 |
186 |
187 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
187 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
188 switch( status ) { |
188 switch( status ) { |
189 |
189 |
190 case state3: /** get the PDO transmission type */ |
190 case state3: /* get the PDO transmission type */ |
191 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
191 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
192 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
192 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
193 return 0xFF; |
193 return 0xFF; |
194 } |
194 } |
195 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
195 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
196 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
196 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
197 status = state4; |
197 status = state4; |
198 break; |
198 break; |
199 case state4: /** check if transmission type is after (this) SYNC */ |
199 case state4: /* check if transmission type is after (this) SYNC */ |
200 /** 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 */ |
201 if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) && |
201 if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) && |
202 (++d->count_sync[pdoNum] == *pTransmissionType) ) { |
202 (++d->count_sync[pdoNum] == *pTransmissionType) ) { |
203 d->count_sync[pdoNum] = 0; |
203 d->count_sync[pdoNum] = 0; |
204 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
204 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
205 status = state5; |
205 status = state5; |
212 offsetObjdict++; |
212 offsetObjdict++; |
213 offsetObjdictMap++; |
213 offsetObjdictMap++; |
214 status = state11; |
214 status = state11; |
215 break; |
215 break; |
216 } |
216 } |
217 case state5: /** get PDO CobId */ |
217 case state5: /* get PDO CobId */ |
218 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
218 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
219 MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId); |
219 MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId); |
220 status = state7; |
220 status = state7; |
221 break; |
221 break; |
222 case state7: /** get mapped objects number to transmit with this PDO */ |
222 case state7: /* get mapped objects number to transmit with this PDO */ |
223 pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject; |
223 pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject; |
224 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
224 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
225 status = state8; |
225 status = state8; |
226 case state8: /** get mapping parameters */ |
226 case state8: /* get mapping parameters */ |
227 pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject; |
227 pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject; |
228 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
228 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
229 MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum); |
229 MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum); |
230 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
230 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
231 status = state9; |
231 status = state9; |
232 |
232 |
233 case state9: /** get data to transmit */ |
233 case state9: /* get data to transmit */ |
234 { |
234 { |
235 UNS8 ByteSize; |
235 UNS8 ByteSize; |
236 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
236 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
237 index = (UNS16)((*pMappingParameter) >> 16); |
237 index = (UNS16)((*pMappingParameter) >> 16); |
238 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
238 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
239 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
239 Size = (UNS8)(*pMappingParameter); /* Size in bits */ |
240 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
240 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
241 objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ); |
241 objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ); |
242 /** copy bit per bit in little endian*/ |
242 /* copy bit per bit in little endian*/ |
243 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); |
244 } |
244 } |
245 if( objDict != OD_SUCCESSFUL ){ |
245 if( objDict != OD_SUCCESSFUL ){ |
246 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)); |
247 return 0xFF; |
247 return 0xFF; |