260 /* and the lower 8 bits contains the size of the mapped |
260 /* and the lower 8 bits contains the size of the mapped |
261 variable. */ |
261 variable. */ |
262 |
262 |
263 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF); |
263 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF); |
264 |
264 |
265 /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
265 /* set variable only if Size != 0 and |
|
266 * Size is lower than remaining bits in the PDO */ |
266 if (Size && ((offset + Size) <= (m->len << 3))) |
267 if (Size && ((offset + Size) <= (m->len << 3))) |
267 { |
268 { |
268 /* copy bit per bit in little endian */ |
269 /* copy bit per bit in little endian */ |
269 CopyBits (Size, (UNS8 *) & m->data[offset >> 3], |
270 CopyBits (Size, (UNS8 *) & m->data[offset >> 3], |
270 offset % 8, 0, ((UNS8 *) tmp), 0, 0); |
271 offset % 8, 0, ((UNS8 *) tmp), 0, 0); |
271 |
272 /*1->8 => 1 ; 9->16 =>2, ... */ |
272 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
273 ByteSize = 1 + ((Size - 1) >> 3); |
273 2, ... */ |
|
274 |
274 |
275 objDict = |
275 objDict = |
276 setODentry (d, (UNS16) ((*pMappingParameter) >> 16), |
276 setODentry (d, (UNS16) ((*pMappingParameter) >> 16), |
277 (UNS8) (((*pMappingParameter) >> 8) & |
277 (UNS8) (((*pMappingParameter) >> 8) & |
278 0xFF), tmp, &ByteSize, 0); |
278 0xFF), tmp, &ByteSize, 0); |
288 ((*pMappingParameter) >> 8) & 0xFF); |
288 ((*pMappingParameter) >> 8) & 0xFF); |
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 by PDO cobid : ", |
294 UNS16_LE(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)); */ |
|
300 offset += Size; |
299 offset += Size; |
301 } |
300 } |
302 numMap++; |
301 numMap++; |
303 } /* end loop while on mapped variables */ |
302 } /* end loop while on mapped variables */ |
304 |
303 |
347 /* If PDO is to be sampled and send on RTR, do it */ |
346 /* If PDO is to be sampled and send on RTR, do it */ |
348 if ((*pTransmissionType == TRANS_RTR)) |
347 if ((*pTransmissionType == TRANS_RTR)) |
349 { |
348 { |
350 status = state5; |
349 status = state5; |
351 break; |
350 break; |
352 /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */ |
|
353 } |
351 } |
|
352 /* RTR_SYNC means data prepared at SYNC, transmitted on RTR */ |
354 else if ((*pTransmissionType == TRANS_RTR_SYNC)) |
353 else if ((*pTransmissionType == TRANS_RTR_SYNC)) |
355 { |
354 { |
356 if (d->PDO_status[numPdo]. |
355 if (d->PDO_status[numPdo]. |
357 transmit_type_parameter & PDO_RTR_SYNC_READY) |
356 transmit_type_parameter & PDO_RTR_SYNC_READY) |
358 { |
357 { |
361 &d->PDO_status[numPdo].last_message); |
360 &d->PDO_status[numPdo].last_message); |
362 return 0; |
361 return 0; |
363 } |
362 } |
364 else |
363 else |
365 { |
364 { |
366 /* if SYNC did never occur, force emission with current data */ |
365 /* if SYNC did never occur, transmit current data */ |
367 /* DS301 do not tell what to do in such a case... */ |
366 /* DS301 do not tell what to do in such a case... */ |
368 MSG_ERR (0x1947, |
367 MSG_ERR (0x1947, |
369 "Not ready RTR_SYNC TPDO send current data : ", |
368 "Not ready RTR_SYNC TPDO send current data : ", |
370 UNS16_LE(m->cob_id)); |
369 UNS16_LE(m->cob_id)); |
371 status = state5; |
370 status = state5; |
380 DelAlarm (d->PDO_status[numPdo].event_timer); |
379 DelAlarm (d->PDO_status[numPdo].event_timer); |
381 d->PDO_status[numPdo].inhibit_timer = |
380 d->PDO_status[numPdo].inhibit_timer = |
382 DelAlarm (d->PDO_status[numPdo].inhibit_timer); |
381 DelAlarm (d->PDO_status[numPdo].inhibit_timer); |
383 d->PDO_status[numPdo].transmit_type_parameter &= |
382 d->PDO_status[numPdo].transmit_type_parameter &= |
384 ~PDO_INHIBITED; |
383 ~PDO_INHIBITED; |
385 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
384 /* Call PDOEventTimerAlarm for this TPDO, |
|
385 * this will trigger emission et reset timers */ |
386 PDOEventTimerAlarm (d, numPdo); |
386 PDOEventTimerAlarm (d, numPdo); |
387 return 0; |
387 return 0; |
388 } |
388 } |
389 else |
389 else |
390 { |
390 { |
536 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
536 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
537 |
537 |
538 /* study all PDO stored in the objects dictionary */ |
538 /* study all PDO stored in the objects dictionary */ |
539 if (offsetObjdict) |
539 if (offsetObjdict) |
540 { |
540 { |
541 Message pdo = Message_Initializer; |
541 Message pdo;/* = Message_Initializer;*/ |
|
542 memset(&pdo, 0, sizeof(pdo)); |
542 while (offsetObjdict <= lastIndex) |
543 while (offsetObjdict <= lastIndex) |
543 { |
544 { |
544 switch (status) |
545 switch (status) |
545 { |
546 { |
546 case state3: |
547 case state3: |
547 if ( /*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5) */ |
548 if ( /* bSubCount always 5 with objdictedit -> check disabled */ |
|
549 /*d->objdict[offsetObjdict].bSubCount < 5 ||*/ |
548 /* check if TPDO is not valid */ |
550 /* check if TPDO is not valid */ |
549 *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1]. |
551 *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1]. |
550 pObject & 0x80000000) |
552 pObject & 0x80000000) |
551 { |
553 { |
552 MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum); |
554 MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum); |
558 pTransmissionType = |
560 pTransmissionType = |
559 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject; |
561 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject; |
560 MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); |
562 MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); |
561 |
563 |
562 /* check if transmission type is SYNCRONOUS */ |
564 /* check if transmission type is SYNCRONOUS */ |
563 /* The message is transmited every n SYNC with n=TransmissionType */ |
565 /* message transmited every n SYNC with n=TransmissionType */ |
564 if (isSyncEvent && |
566 if (isSyncEvent && |
565 (*pTransmissionType >= TRANS_SYNC_MIN) && |
567 (*pTransmissionType >= TRANS_SYNC_MIN) && |
566 (*pTransmissionType <= TRANS_SYNC_MAX) && |
568 (*pTransmissionType <= TRANS_SYNC_MAX) && |
567 (++d->PDO_status[pdoNum].transmit_type_parameter == |
569 (++d->PDO_status[pdoNum].transmit_type_parameter == |
568 *pTransmissionType)) |
570 *pTransmissionType)) |
569 { |
571 { |
570 /*Reset count of SYNC */ |
572 /*Reset count of SYNC */ |
571 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
573 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
572 MSG_WAR (0x3964, " PDO is on SYNCHRO. Trans type : ", |
574 MSG_WAR (0x3964, " PDO is on SYNCHRO. Trans type : ", |
573 *pTransmissionType); |
575 *pTransmissionType); |
574 { |
576 memset(&pdo, 0, sizeof(pdo)); |
|
577 /*{ |
575 Message msg_init = Message_Initializer; |
578 Message msg_init = Message_Initializer; |
576 pdo = msg_init; |
579 pdo = msg_init; |
577 } |
580 }*/ |
578 if (buildPDO (d, pdoNum, &pdo)) |
581 if (buildPDO (d, pdoNum, &pdo)) |
579 { |
582 { |
580 MSG_ERR (0x1906, " Couldn't build TPDO number : ", |
583 MSG_ERR (0x1906, " Couldn't build TPDO number : ", |
581 pdoNum); |
584 pdoNum); |
582 status = state11; |
585 status = state11; |
613 && !(d->PDO_status[pdoNum]. |
616 && !(d->PDO_status[pdoNum]. |
614 transmit_type_parameter & PDO_INHIBITED))) |
617 transmit_type_parameter & PDO_INHIBITED))) |
615 { |
618 { |
616 MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ", |
619 MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ", |
617 *pTransmissionType); |
620 *pTransmissionType); |
618 { |
621 memset(&pdo, 0, sizeof(pdo)); |
|
622 /*{ |
619 Message msg_init = Message_Initializer; |
623 Message msg_init = Message_Initializer; |
620 pdo = msg_init; |
624 pdo = msg_init; |
621 } |
625 }*/ |
622 if (buildPDO (d, pdoNum, &pdo)) |
626 if (buildPDO (d, pdoNum, &pdo)) |
623 { |
627 { |
624 MSG_ERR (0x3907, " Couldn't build TPDO number : ", |
628 MSG_ERR (0x3907, " Couldn't build TPDO number : ", |
625 pdoNum); |
629 pdoNum); |
626 status = state11; |
630 status = state11; |
627 break; |
631 break; |
628 } |
632 } |
629 |
633 |
630 /*Compare new and old PDO */ |
634 /*Compare new and old PDO */ |
631 if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id |
635 if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id |
632 && d->PDO_status[pdoNum].last_message.len == pdo.len && |
636 && d->PDO_status[pdoNum].last_message.len == pdo.len |
633 memcmp(d->PDO_status[pdoNum].last_message.data, |
637 && memcmp(d->PDO_status[pdoNum].last_message.data, |
634 pdo.data, 8) == 0 |
638 pdo.data, 8) == 0 |
635 ) |
639 ) |
636 { |
640 { |
637 /* No changes -> go to next pdo */ |
641 /* No changes -> go to next pdo */ |
638 status = state11; |
642 status = state11; |