516 offset = d->firstIndex->SDO_SVR; |
516 offset = d->firstIndex->SDO_SVR; |
517 if (offset == 0) { |
517 if (offset == 0) { |
518 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); |
518 MSG_ERR(0x1A42, "SendSDO : No SDO server found", 0); |
519 return 0xFF; |
519 return 0xFF; |
520 } |
520 } |
521 pwCobId = d->objdict[offset].pSubindex[2].pObject; |
521 pwCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
522 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); |
522 MSG_WAR(0x3A41, "I am server. cobId : ", *pwCobId); |
523 } |
523 } |
524 else { /*case client*/ |
524 else { /*case client*/ |
525 /* Get the client->server cobid.*/ |
525 /* Get the client->server cobid.*/ |
526 UNS16 sdoNum = 0; |
526 UNS16 sdoNum = 0; |
535 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum); |
535 MSG_WAR(0x3A43,"Reading index : ", 0x1280 + sdoNum); |
536 if (d->objdict[offset].bSubCount <= 3) { |
536 if (d->objdict[offset].bSubCount <= 3) { |
537 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum); |
537 MSG_ERR(0x1A28, "Subindex 3 not found at index ", 0x1280 + sdoNum); |
538 return 0xFF; |
538 return 0xFF; |
539 } |
539 } |
540 pwNodeId = d->objdict[offset].pSubindex[3].pObject; |
540 pwNodeId = (UNS32*) d->objdict[offset].pSubindex[3].pObject; |
541 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId); |
541 MSG_WAR(0x3A44, "Found nodeId server = ", *pwNodeId); |
542 if(*pwNodeId == sdo.nodeId) { |
542 if(*pwNodeId == sdo.nodeId) { |
543 found = 1; |
543 found = 1; |
544 break; |
544 break; |
545 } |
545 } |
549 if (! found){ |
549 if (! found){ |
550 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); |
550 MSG_WAR (0x2A45, "No SDO client corresponds to the mesage to send to node ", sdo.nodeId); |
551 return 0xFF; |
551 return 0xFF; |
552 } |
552 } |
553 /* Second, read the cobid client->server */ |
553 /* Second, read the cobid client->server */ |
554 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
554 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
555 } |
555 } |
556 /* message copy for sending */ |
556 /* message copy for sending */ |
557 m.cob_id.w = *pwCobId; |
557 m.cob_id.w = *pwCobId; |
558 m.rtr = NOT_A_REQUEST; |
558 m.rtr = NOT_A_REQUEST; |
559 /* the length of SDO must be 8 */ |
559 /* the length of SDO must be 8 */ |
634 if(offset) while (offset <= lastIndex) { |
634 if(offset) while (offset <= lastIndex) { |
635 if (d->objdict[offset].bSubCount <= 1) { |
635 if (d->objdict[offset].bSubCount <= 1) { |
636 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
636 MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j); |
637 return 0xFF; |
637 return 0xFF; |
638 } |
638 } |
639 pCobId = d->objdict[offset].pSubindex[1].pObject; |
639 pCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject; |
640 if ( *pCobId == (*m).cob_id.w ) { |
640 if ( *pCobId == (*m).cob_id.w ) { |
641 whoami = SDO_SERVER; |
641 whoami = SDO_SERVER; |
642 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
642 MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j); |
643 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
643 /* In case of server, the node id of the client may be unknown. So we put the index minus offset */ |
644 /* 0x1200 where the cobid received is defined. */ |
644 /* 0x1200 where the cobid received is defined. */ |
657 if (d->objdict[offset].bSubCount <= 3) { |
657 if (d->objdict[offset].bSubCount <= 3) { |
658 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
658 MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j); |
659 return 0xFF; |
659 return 0xFF; |
660 } |
660 } |
661 /* a) Looking for the cobid received. */ |
661 /* a) Looking for the cobid received. */ |
662 pCobId = d->objdict[offset].pSubindex[2].pObject; |
662 pCobId = (UNS32*) d->objdict[offset].pSubindex[2].pObject; |
663 if (*pCobId == (*m).cob_id.w ) { |
663 if (*pCobId == (*m).cob_id.w ) { |
664 /* b) cobid found, so reading the node id of the server. */ |
664 /* b) cobid found, so reading the node id of the server. */ |
665 pNodeId = d->objdict[offset].pSubindex[3].pObject; |
665 pNodeId = (UNS32*) d->objdict[offset].pSubindex[3].pObject; |
666 whoami = SDO_CLIENT; |
666 whoami = SDO_CLIENT; |
667 nodeId_32 = *pNodeId; |
667 nodeId_32 = *pNodeId; |
668 nodeId = (UNS8)nodeId_32; |
668 nodeId = (UNS8)nodeId_32; |
669 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
669 MSG_WAR(0x3A64, "proceedSDO. I am server. index : ", 0x1280 + j); |
670 MSG_WAR(0x3A65, " Server nodeId : ", nodeId); |
670 MSG_WAR(0x3A65, " Server nodeId : ", nodeId); |
1286 if (d->objdict[offset].bSubCount <= 3) { |
1286 if (d->objdict[offset].bSubCount <= 3) { |
1287 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i); |
1287 MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i); |
1288 return 0xFF; |
1288 return 0xFF; |
1289 } |
1289 } |
1290 /* looking for the nodeId server */ |
1290 /* looking for the nodeId server */ |
1291 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; |
1291 pNodeIdServer = (UNS32*) d->objdict[offset].pSubindex[3].pObject; |
1292 nodeIdServer = *pNodeIdServer; |
1292 nodeIdServer = *pNodeIdServer; |
1293 MSG_WAR(0x1AD2, "index : ", 0x1280 + i); |
1293 MSG_WAR(0x1AD2, "index : ", 0x1280 + i); |
1294 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); |
1294 MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer); |
1295 |
1295 |
1296 if(nodeIdServer == (UNS32)nodeId) { |
1296 if(nodeIdServer == (UNS32)nodeId) { |
1446 if (d->objdict[offset].bSubCount <= 3) { |
1446 if (d->objdict[offset].bSubCount <= 3) { |
1447 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i); |
1447 MSG_ERR(0x1AE2, "Subindex 3 not found at index ", 0x1280 + i); |
1448 return 0xFF; |
1448 return 0xFF; |
1449 } |
1449 } |
1450 /* looking for the nodeId server */ |
1450 /* looking for the nodeId server */ |
1451 pNodeIdServer = d->objdict[offset].pSubindex[3].pObject; |
1451 pNodeIdServer = (UNS32*) d->objdict[offset].pSubindex[3].pObject; |
1452 nodeIdServer = *pNodeIdServer; |
1452 nodeIdServer = *pNodeIdServer; |
1453 |
1453 |
1454 if(nodeIdServer == (UNS32)nodeId) { |
1454 if(nodeIdServer == (UNS32)nodeId) { |
1455 SDOfound = 1; |
1455 SDOfound = 1; |
1456 break; |
1456 break; |