59 MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode); |
58 MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode); |
60 MSG_WAR(0x2A02, "server node : ", nodeId); |
59 MSG_WAR(0x2A02, "server node : ", nodeId); |
61 } |
60 } |
62 |
61 |
63 closeSDOtransfer(d, nodeId, SDO_CLIENT); |
62 closeSDOtransfer(d, nodeId, SDO_CLIENT); |
64 decompo_dcf(d,nodeId); |
63 send_consise_dcf(d,nodeId); |
65 } |
64 } |
66 |
65 |
67 /** |
66 /** |
68 ** |
67 ** |
69 ** |
68 ** |
70 ** @param d |
69 ** @param d |
71 ** @param nodeId |
70 ** @param nodeId |
72 ** |
71 ** |
73 ** @return |
72 ** @return |
74 */ |
73 */ |
75 UNS32 decompo_dcf(CO_Data* d,UNS8 nodeId) |
74 void send_consise_dcf(CO_Data* d,UNS8 nodeId) |
76 { |
75 { |
77 UNS32 errorCode; |
76 /* Fetch DCF OD entry, if not already done */ |
78 UNS16 target_Index; |
77 if(!d->dcf_odentry) |
79 UNS8 target_Subindex; |
78 { |
80 UNS32 target_Size; |
79 UNS32 errorCode; |
81 UNS32 res; |
80 ODCallback_t *Callback; |
82 ODCallback_t *Callback; |
81 d->dcf_odentry = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
|
82 /* If DCF entry do not exist... Nothing to do.*/ |
|
83 if (errorCode != OD_SUCCESSFUL) goto DCF_finish; |
|
84 } |
83 |
85 |
84 ptrTable = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
86 /* Loop on all DCF subindexes, corresponding to nodes ID */ |
85 if (errorCode != OD_SUCCESSFUL) |
87 while (nodeId < d->dcf_odentry->bSubCount){ |
86 { |
88 UNS32 nb_entries; |
87 return errorCode; |
|
88 } |
|
89 |
89 |
90 /* Loop on all Nodes supported in DCF subindexes*/ |
90 UNS8 szData = d->dcf_odentry->pSubindex[nodeId].size; |
91 while (nodeId < ptrTable->bSubCount){ |
|
92 UNS32 nb_targets; |
|
93 |
|
94 UNS8 szData = ptrTable->pSubindex[nodeId].size; |
|
95 UNS8* dcfend; |
91 UNS8* dcfend; |
96 |
92 |
97 { |
93 { |
98 UNS8* dcf = *((UNS8**)ptrTable->pSubindex[nodeId].pObject); |
94 UNS8* dcf = *((UNS8**)d->dcf_odentry->pSubindex[nodeId].pObject); |
99 dcfend = dcf + szData; |
95 dcfend = dcf + szData; |
100 if (!d->dcf_cursor){ |
96 if (!d->dcf_cursor){ |
101 d->dcf_cursor = (UNS8*)dcf + 4; |
97 d->dcf_cursor = (UNS8*)dcf + 4; |
102 d->dcf_count_targets = 0; |
98 d->dcf_entries_count = 0; |
103 } |
99 } |
104 nb_targets = UNS32_LE(*((UNS32*)dcf)); |
100 nb_entries = UNS32_LE(*((UNS32*)dcf)); |
105 } |
101 } |
106 |
102 |
107 /* condition on consise DCF string for NodeID, if big enough */ |
103 /* condition on consise DCF string for NodeID, if big enough */ |
108 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_count_targets < nb_targets) |
104 if((UNS8*)d->dcf_cursor + 7 < (UNS8*)dcfend && d->dcf_entries_count < nb_entries) |
109 { |
105 { |
|
106 UNS16 target_Index; |
|
107 UNS8 target_Subindex; |
|
108 UNS32 target_Size; |
|
109 |
110 /* pointer to the DCF string for NodeID */ |
110 /* pointer to the DCF string for NodeID */ |
111 target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2; |
111 target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2; |
112 target_Subindex = *((UNS8*)((UNS8*)d->dcf_cursor++)); |
112 target_Subindex = *((UNS8*)((UNS8*)d->dcf_cursor++)); |
113 target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4; |
113 target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4; |
114 |
114 |
115 /* printf("Master : ConfigureSlaveNode %2.2x (Concise |
115 _writeNetworkDict(d, /* CO_Data* d*/ |
116 DCF)\n",nodeId);*/ |
|
117 res = _writeNetworkDict(d, /* CO_Data* d*/ |
|
118 nodeId, /* UNS8 nodeId*/ |
116 nodeId, /* UNS8 nodeId*/ |
119 target_Index, /* UNS16 index*/ |
117 target_Index, /* UNS16 index*/ |
120 target_Subindex, /* UNS8 subindex*/ |
118 target_Subindex, /* UNS8 subindex*/ |
121 target_Size, /* UNS8 count*/ |
119 target_Size, /* UNS8 count*/ |
122 0, /* UNS8 dataType*/ |
120 0, /* UNS8 dataType*/ |