136 { |
136 { |
137 offset += RPDOIndex - 0x1400; |
137 offset += RPDOIndex - 0x1400; |
138 if (offset <= lastIndex) |
138 if (offset <= lastIndex) |
139 { |
139 { |
140 /* get the CobId */ |
140 /* get the CobId */ |
141 pwCobId = (UNS16 *) d->objdict[offset].pSubindex[1].pObject; |
141 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
142 |
142 |
143 MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId); |
143 MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId); |
144 { |
144 { |
145 Message pdo; |
145 Message pdo; |
146 pdo.cob_id = *pwCobId; |
146 pdo.cob_id = UNS16_LE(*pwCobId); |
147 pdo.rtr = REQUEST; |
147 pdo.rtr = REQUEST; |
148 pdo.len = 0; |
148 pdo.len = 0; |
149 return canSend (d->canHandle, &pdo); |
149 return canSend (d->canHandle, &pdo); |
150 } |
150 } |
151 } |
151 } |
174 /* pointer fo the var which holds the mapping parameter of an |
174 /* pointer fo the var which holds the mapping parameter of an |
175 mapping entry */ |
175 mapping entry */ |
176 UNS32 *pMappingParameter = NULL; |
176 UNS32 *pMappingParameter = NULL; |
177 UNS8 *pTransmissionType = NULL; /* pointer to the transmission |
177 UNS8 *pTransmissionType = NULL; /* pointer to the transmission |
178 type */ |
178 type */ |
179 UNS16 *pwCobId = NULL; |
179 UNS32 *pwCobId = NULL; |
180 UNS8 Size; |
180 UNS8 Size; |
181 UNS8 offset; |
181 UNS8 offset; |
182 UNS8 status; |
182 UNS8 status; |
183 UNS32 objDict; |
183 UNS32 objDict; |
184 UNS16 offsetObjdict; |
184 UNS16 offsetObjdict; |
185 UNS16 lastIndex; |
185 UNS16 lastIndex; |
186 |
186 |
187 status = state2; |
187 status = state2; |
188 |
188 |
189 MSG_WAR (0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE (0x7ff))); |
189 MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff)); |
190 offset = 0x00; |
190 offset = 0x00; |
191 numPdo = 0; |
191 numPdo = 0; |
192 numMap = 0; |
192 numMap = 0; |
193 if ((*m).rtr == NOT_A_REQUEST) |
193 if ((*m).rtr == NOT_A_REQUEST) |
194 { /* The PDO received is not a |
194 { /* The PDO received is not a |
207 |
207 |
208 case state2: |
208 case state2: |
209 /* get CobId of the dictionary correspondant to the received |
209 /* get CobId of the dictionary correspondant to the received |
210 PDO */ |
210 PDO */ |
211 pwCobId = |
211 pwCobId = |
212 (UNS16 *) d->objdict[offsetObjdict].pSubindex[1].pObject; |
212 d->objdict[offsetObjdict].pSubindex[1].pObject; |
213 /* check the CobId coherance */ |
213 /* check the CobId coherance */ |
214 /*pwCobId is the cobId read in the dictionary at the state 3 |
214 /*pwCobId is the cobId read in the dictionary at the state 3 |
215 */ |
215 */ |
216 if (*pwCobId == (*m).cob_id) |
216 if (*pwCobId == UNS16_LE(m->cob_id)) |
217 { |
217 { |
218 /* The cobId is recognized */ |
218 /* The cobId is recognized */ |
219 status = state4; |
219 status = state4; |
220 MSG_WAR (0x3936, "cobId found at index ", |
220 MSG_WAR (0x3936, "cobId found at index ", |
221 0x1400 + numPdo); |
221 0x1400 + numPdo); |
289 return 0xFF; |
289 return 0xFF; |
290 } |
290 } |
291 |
291 |
292 MSG_WAR (0x3942, |
292 MSG_WAR (0x3942, |
293 "Variable updated with value received by PDO cobid : ", |
293 "Variable updated with value received by PDO cobid : ", |
294 m->cob_id); |
294 UNS16_LE(m->cob_id)); |
295 MSG_WAR (0x3943, " Mapped at index : ", |
295 MSG_WAR (0x3943, " Mapped at index : ", |
296 (*pMappingParameter) >> 16); |
296 (*pMappingParameter) >> 16); |
297 MSG_WAR (0x3944, " subindex : ", |
297 MSG_WAR (0x3944, " subindex : ", |
298 ((*pMappingParameter) >> 8) & 0xFF); |
298 ((*pMappingParameter) >> 8) & 0xFF); |
299 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
299 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
300 offset += Size; |
300 offset += Size; |
301 } |
301 } |
302 numMap++; |
302 numMap++; |
303 } /* end loop while on mapped variables */ |
303 } /* end loop while on mapped variables */ |
304 |
304 |
305 offset = 0x00; |
|
306 numMap = 0; |
|
307 return 0; |
305 return 0; |
308 |
306 |
309 } /* end switch status */ |
307 } /* end switch status */ |
310 } /* end while */ |
308 } /* end while */ |
311 } /* end if Donnees */ |
309 } /* end if Donnees */ |
312 else if ((*m).rtr == REQUEST) |
310 else if ((*m).rtr == REQUEST) |
313 { |
311 { |
314 MSG_WAR (0x3946, "Receive a PDO request cobId : ", m->cob_id); |
312 MSG_WAR (0x3946, "Receive a PDO request cobId : ", UNS16_LE(m->cob_id)); |
315 status = state1; |
313 status = state1; |
316 offsetObjdict = d->firstIndex->PDO_TRS; |
314 offsetObjdict = d->firstIndex->PDO_TRS; |
317 lastIndex = d->lastIndex->PDO_TRS; |
315 lastIndex = d->lastIndex->PDO_TRS; |
318 if (offsetObjdict) |
316 if (offsetObjdict) |
319 while (offsetObjdict <= lastIndex) |
317 while (offsetObjdict <= lastIndex) |
367 { |
365 { |
368 /* if SYNC did never occur, force emission with current data */ |
366 /* if SYNC did never occur, force emission with current data */ |
369 /* DS301 do not tell what to do in such a case... */ |
367 /* DS301 do not tell what to do in such a case... */ |
370 MSG_ERR (0x1947, |
368 MSG_ERR (0x1947, |
371 "Not ready RTR_SYNC TPDO send current data : ", |
369 "Not ready RTR_SYNC TPDO send current data : ", |
372 m->cob_id); |
370 UNS16_LE(m->cob_id)); |
373 status = state5; |
371 status = state5; |
374 } |
372 } |
375 break; |
373 break; |
376 } |
374 } |
377 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) || |
375 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) || |
391 else |
389 else |
392 { |
390 { |
393 /* The requested PDO is not to send on request. So, does |
391 /* The requested PDO is not to send on request. So, does |
394 nothing. */ |
392 nothing. */ |
395 MSG_WAR (0x2947, "PDO is not to send on request : ", |
393 MSG_WAR (0x2947, "PDO is not to send on request : ", |
396 m->cob_id); |
394 UNS16_LE(m->cob_id)); |
397 return 0xFF; |
395 return 0xFF; |
398 } |
396 } |
399 |
397 |
400 case state5: /* build and send requested PDO */ |
398 case state5: /* build and send requested PDO */ |
401 { |
399 { |
402 Message pdo; |
400 Message pdo; |
403 if (buildPDO (d, numPdo, &pdo)) |
401 if (buildPDO (d, numPdo, &pdo)) |
404 { |
402 { |
405 MSG_ERR (0x1948, " Couldn't build TPDO n°", numPdo); |
403 MSG_ERR (0x1948, " Couldn't build TPDO n�", numPdo); |
406 return 0xFF; |
404 return 0xFF; |
407 } |
405 } |
408 canSend (d->canHandle, &pdo); |
406 canSend (d->canHandle, &pdo); |
409 return 0; |
407 return 0; |
410 } |
408 } |
695 } |
693 } |
696 break; |
694 break; |
697 case state5: /*Send the pdo */ |
695 case state5: /*Send the pdo */ |
698 /*store_as_last_message */ |
696 /*store_as_last_message */ |
699 d->PDO_status[pdoNum].last_message = pdo; |
697 d->PDO_status[pdoNum].last_message = pdo; |
700 MSG_WAR (0x396D, "sendPDO cobId :", pdo.cob_id); |
698 MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo.cob_id)); |
701 MSG_WAR (0x396E, " Nb octets : ", pdo.len); |
699 MSG_WAR (0x396E, " Nb octets : ", pdo.len); |
702 |
700 |
703 canSend (d->canHandle, &pdo); |
701 canSend (d->canHandle, &pdo); |
704 status = state11; |
702 status = state11; |
705 break; |
703 break; |