|
178
|
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"
|
|
200
|
26 |
#include "sysdep.h"
|
|
178
|
27 |
|
|
|
28 |
const indextable *ptrTable;
|
|
|
29 |
|
|
|
30 |
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
|
|
|
31 |
{
|
|
|
32 |
UNS32 abortCode;
|
|
|
33 |
|
|
|
34 |
if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
|
|
191
|
35 |
{
|
|
|
36 |
MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
|
|
|
37 |
MSG_WAR(0x2A02, "server node : ", nodeId);
|
|
|
38 |
}
|
|
|
39 |
|
|
178
|
40 |
closeSDOtransfer(d, nodeId, SDO_CLIENT);
|
|
191
|
41 |
decompo_dcf(d,nodeId);
|
|
178
|
42 |
}
|
|
|
43 |
|
|
|
44 |
UNS32 decompo_dcf(CO_Data* d,UNS8 nodeId)
|
|
|
45 |
{
|
|
|
46 |
UNS32 errorCode;
|
|
|
47 |
UNS16 target_Index;
|
|
|
48 |
UNS8 target_Subindex;
|
|
|
49 |
UNS32 target_Size;
|
|
|
50 |
UNS32 res;
|
|
|
51 |
ODCallback_t *Callback;
|
|
|
52 |
|
|
|
53 |
ptrTable = (*d->scanIndexOD)(0x1F22, &errorCode, &Callback);
|
|
|
54 |
if (errorCode != OD_SUCCESSFUL)
|
|
|
55 |
{
|
|
|
56 |
return errorCode;
|
|
|
57 |
}
|
|
|
58 |
|
|
|
59 |
/*Loop on all Nodes supported in DCF subindexes*/
|
|
|
60 |
while (nodeId < ptrTable->bSubCount){
|
|
|
61 |
UNS32 nb_targets;
|
|
|
62 |
|
|
|
63 |
UNS8 szData = ptrTable->pSubindex[nodeId].size;
|
|
|
64 |
void* dcfend;
|
|
|
65 |
|
|
|
66 |
{
|
|
|
67 |
void* dcf = *((void**)ptrTable->pSubindex[nodeId].pObject);
|
|
|
68 |
dcfend = dcf + szData;
|
|
|
69 |
if (!d->dcf_cursor) {
|
|
|
70 |
d->dcf_cursor = dcf + 4;
|
|
|
71 |
d->dcf_count_targets = 0;
|
|
|
72 |
}
|
|
200
|
73 |
nb_targets = UNS32_LE(*((UNS32*)dcf));
|
|
178
|
74 |
}
|
|
|
75 |
|
|
|
76 |
// condition on consise DCF string for NodeID, if big enough
|
|
|
77 |
if(d->dcf_cursor + 7 < dcfend && d->dcf_count_targets < nb_targets)
|
|
|
78 |
{
|
|
|
79 |
// pointer to the DCF string for NodeID
|
|
200
|
80 |
target_Index = UNS16_LE(*((UNS16*)(d->dcf_cursor))); d->dcf_cursor += 2;
|
|
178
|
81 |
target_Subindex = *((UNS8*)(d->dcf_cursor++));
|
|
200
|
82 |
target_Size = UNS32_LE(*((UNS32*)(d->dcf_cursor))); d->dcf_cursor += 4;
|
|
178
|
83 |
|
|
191
|
84 |
/*printf("Master : ConfigureSlaveNode %2.2x (Concise DCF)\n",nodeId);*/
|
|
178
|
85 |
res = writeNetworkDictCallBack(d, /*CO_Data* d*/
|
|
|
86 |
nodeId, /*UNS8 nodeId*/
|
|
|
87 |
target_Index, /*UNS16 index*/
|
|
|
88 |
target_Subindex, /*UNS8 subindex*/
|
|
|
89 |
target_Size, /*UNS8 count*/
|
|
|
90 |
0, /*UNS8 dataType*/
|
|
|
91 |
d->dcf_cursor,/*void *data*/
|
|
|
92 |
CheckSDOAndContinue); /*SDOCallback_t Callback*/
|
|
|
93 |
/*Push d->dcf_cursor to the end of data*/
|
|
|
94 |
|
|
|
95 |
d->dcf_cursor += target_Size;
|
|
|
96 |
d->dcf_count_targets++;
|
|
|
97 |
|
|
|
98 |
return ;
|
|
|
99 |
}
|
|
|
100 |
nodeId++;
|
|
|
101 |
d->dcf_cursor = NULL;
|
|
|
102 |
}
|
|
|
103 |
/* Switch Master to preOperational state */
|
|
|
104 |
(*d->preOperational)();
|
|
|
105 |
|
|
|
106 |
}
|