|
1 /* |
|
2 This file is part of CanFestival, a library implementing CanOpen Stack. |
|
3 |
|
4 Copyright (C): Edouard TISSERANT and Francis DUPIN |
|
5 |
|
6 See COPYING file for copyrights details. |
|
7 |
|
8 This library is free software; you can redistribute it and/or |
|
9 modify it under the terms of the GNU Lesser General Public |
|
10 License as published by the Free Software Foundation; either |
|
11 version 2.1 of the License, or (at your option) any later version. |
|
12 |
|
13 This library is distributed in the hope that it will be useful, |
|
14 but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
16 Lesser General Public License for more details. |
|
17 |
|
18 You should have received a copy of the GNU Lesser General Public |
|
19 License along with this library; if not, write to the Free Software |
|
20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
|
21 */ |
|
22 |
|
23 #include "objacces.h" |
|
24 #include "sdo.h" |
|
25 #include "dcf.h" |
|
26 |
|
27 const indextable *ptrTable; |
|
28 |
|
29 static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId) |
|
30 { |
|
31 UNS32 res; |
|
32 UNS8 line; |
|
33 UNS8 err; |
|
34 UNS16 Index; |
|
35 UNS8 SubIndex; |
|
36 UNS32 abortCode; |
|
37 |
|
38 if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED) |
|
39 printf("Master : Failed in initializing slave %2.2x, AbortCode :%4.4x \n", nodeId, abortCode); |
|
40 |
|
41 closeSDOtransfer(d, nodeId, SDO_CLIENT); |
|
42 res = decompo_dcf(d,nodeId); |
|
43 } |
|
44 |
|
45 UNS32 decompo_dcf(CO_Data* d,UNS8 nodeId) |
|
46 { |
|
47 UNS32 errorCode; |
|
48 UNS16 target_Index; |
|
49 UNS8 target_Subindex; |
|
50 UNS32 target_Size; |
|
51 void* target_data = NULL; |
|
52 UNS32 res; |
|
53 ODCallback_t *Callback; |
|
54 |
|
55 ptrTable = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback); |
|
56 if (errorCode != OD_SUCCESSFUL) |
|
57 { |
|
58 return errorCode; |
|
59 } |
|
60 |
|
61 /*Loop on all Nodes supported in DCF subindexes*/ |
|
62 while (nodeId < ptrTable->bSubCount){ |
|
63 UNS32 nb_targets; |
|
64 |
|
65 UNS8 szData = ptrTable->pSubindex[nodeId].size; |
|
66 void* dcfend; |
|
67 |
|
68 { |
|
69 void* dcf = *((void**)ptrTable->pSubindex[nodeId].pObject); |
|
70 dcfend = dcf + szData; |
|
71 if (!d->dcf_cursor) { |
|
72 d->dcf_cursor = dcf + 4; |
|
73 d->dcf_count_targets = 0; |
|
74 } |
|
75 #ifdef CANOPEN_BIG_ENDIAN |
|
76 nb_targets = ((UNS8*)d->dcf++) | ((UNS8*)d->dcf++) << 8 | ((UNS8*)d->dcf++) << 16 | ((UNS8*)d->dcf++) << 24; |
|
77 #else |
|
78 nb_targets = *((UNS32*)dcf); |
|
79 #endif |
|
80 } |
|
81 |
|
82 // condition on consise DCF string for NodeID, if big enough |
|
83 if(d->dcf_cursor + 7 < dcfend && d->dcf_count_targets < nb_targets) |
|
84 { |
|
85 // pointer to the DCF string for NodeID |
|
86 #ifdef CANOPEN_BIG_ENDIAN |
|
87 target_Index = ((UNS8*)d->dcf_cursor++) | ((UNS8*)d->dcf_cursor++) << 8; |
|
88 target_Subindex = ((UNS8*)d->dcf_cursor++); |
|
89 target_Size = ((UNS8*)d->dcf_cursor++) | ((UNS8*)d->dcf_cursor++) << 8 | ((UNS8*)d->dcf_cursor++) << 16 | ((UNS8*)d->dcf_cursor++) << 24; |
|
90 #else |
|
91 target_Index = *((UNS16*)(d->dcf_cursor)); d->dcf_cursor += 2; |
|
92 target_Subindex = *((UNS8*)(d->dcf_cursor++)); |
|
93 target_Size = *((UNS32*)(d->dcf_cursor)); d->dcf_cursor += 4; |
|
94 #endif |
|
95 |
|
96 printf("Master : ConfigureSlaveNode %2.2x (Concise DCF)\n",nodeId); |
|
97 res = writeNetworkDictCallBack(d, /*CO_Data* d*/ |
|
98 nodeId, /*UNS8 nodeId*/ |
|
99 target_Index, /*UNS16 index*/ |
|
100 target_Subindex, /*UNS8 subindex*/ |
|
101 target_Size, /*UNS8 count*/ |
|
102 0, /*UNS8 dataType*/ |
|
103 d->dcf_cursor,/*void *data*/ |
|
104 CheckSDOAndContinue); /*SDOCallback_t Callback*/ |
|
105 /*Push d->dcf_cursor to the end of data*/ |
|
106 |
|
107 d->dcf_cursor += target_Size; |
|
108 d->dcf_count_targets++; |
|
109 |
|
110 return ; |
|
111 } |
|
112 nodeId++; |
|
113 d->dcf_cursor = NULL; |
|
114 } |
|
115 /* Switch Master to preOperational state */ |
|
116 (*d->preOperational)(); |
|
117 |
|
118 } |