|
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 "data.h" |
|
24 #include "sync.h" |
|
25 |
|
26 void SyncAlarm(CO_Data* d, UNS32 id) |
|
27 { |
|
28 sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ; |
|
29 } |
|
30 |
|
31 // This is called when Index 0x1005 is updated. |
|
32 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex) |
|
33 { |
|
34 startSYNC(d); |
|
35 } |
|
36 |
|
37 void startSYNC(CO_Data* d) |
|
38 { |
|
39 RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate); |
|
40 RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate); |
|
41 |
|
42 if(d->syncTimer != TIMER_NONE){ |
|
43 stopSYNC(d); |
|
44 } |
|
45 |
|
46 if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period) |
|
47 { |
|
48 d->syncTimer = SetAlarm( |
|
49 d, |
|
50 0/*No id needed*/, |
|
51 &SyncAlarm, |
|
52 US_TO_TIMEVAL(*d->Sync_Cycle_Period), |
|
53 US_TO_TIMEVAL(*d->Sync_Cycle_Period)); |
|
54 } |
|
55 } |
|
56 |
|
57 void stopSYNC(CO_Data* d) |
|
58 { |
|
59 d->syncTimer = DelAlarm(d->syncTimer); |
|
60 } |
|
61 |
|
62 /*********************************************************************/ |
|
63 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id) |
|
64 { |
|
65 Message m; |
|
66 UNS8 resultat ; |
|
67 |
|
68 MSG_WAR(0x3001, "sendSYNC ", 0); |
|
69 |
|
70 m.cob_id.w = cob_id ; |
|
71 m.rtr = NOT_A_REQUEST; |
|
72 m.len = 0; |
|
73 resultat = (*d->canSend)(&m) ; |
|
74 proceedSYNC(d, &m) ; |
|
75 return resultat ; |
|
76 } |
|
77 |
|
78 /*****************************************************************************/ |
|
79 UNS8 proceedSYNC(CO_Data* d, Message *m) |
|
80 { |
|
81 |
|
82 MSG_WAR(0x3002, "SYNC received. Proceed. ", 0); |
|
83 |
|
84 UNS8 pdoNum, // number of the actual processed pdo-nr. |
|
85 prp_j; |
|
86 |
|
87 const UNS8 * pMappingCount = NULL; // count of mapped objects... |
|
88 // pointer to the var which is mapped to a pdo |
|
89 // void * pMappedAppObject = NULL; |
|
90 // pointer fo the var which holds the mapping parameter of an mapping entry |
|
91 UNS32 * pMappingParameter = NULL; |
|
92 // pointer to the transmissiontype... |
|
93 UNS8 * pTransmissionType = NULL; |
|
94 UNS32 * pwCobId = NULL; |
|
95 |
|
96 UNS8 * pSize; |
|
97 UNS8 size; |
|
98 UNS8 dataType; |
|
99 UNS16 index; |
|
100 UNS8 subIndex; |
|
101 UNS8 offset; |
|
102 UNS8 status; |
|
103 UNS8 sizeData; |
|
104 pSize = &size; |
|
105 UNS32 objDict; |
|
106 status = state3; |
|
107 pdoNum=0x00; |
|
108 prp_j=0x00; |
|
109 offset = 0x00; |
|
110 UNS16 offsetObjdict; |
|
111 UNS16 offsetObjdictMap; |
|
112 UNS16 lastIndex; |
|
113 |
|
114 /* only operational state allows PDO transmission */ |
|
115 if( d->nodeState != Operational ) |
|
116 return 0; |
|
117 |
|
118 (*d->post_sync)(); |
|
119 |
|
120 /* So, the node is in operational state */ |
|
121 /* study all PDO stored in the objects dictionary */ |
|
122 |
|
123 offsetObjdict = d->firstIndex->PDO_TRS; |
|
124 lastIndex = d->lastIndex->PDO_TRS; |
|
125 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
|
126 |
|
127 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
|
128 switch( status ) { |
|
129 |
|
130 case state3: /* get the PDO transmission type */ |
|
131 if (d->objdict[offsetObjdict].bSubCount <= 2) { |
|
132 MSG_ERR(0x1004, "Subindex 2 not found at index ", 0x1800 + pdoNum); |
|
133 return 0xFF; |
|
134 } |
|
135 pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject; |
|
136 MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum); |
|
137 status = state4; |
|
138 break; |
|
139 case state4: /* check if transmission type is after (this) SYNC */ |
|
140 /* The message may not be transmited every SYNC but every n SYNC */ |
|
141 if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) && |
|
142 (++d->count_sync[pdoNum] == *pTransmissionType) ) { |
|
143 d->count_sync[pdoNum] = 0; |
|
144 MSG_WAR(0x3007, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
|
145 status = state5; |
|
146 break; |
|
147 } |
|
148 else { |
|
149 MSG_WAR(0x3008, " Not on synchro or not at this SYNC. Trans type : ", |
|
150 *pTransmissionType); |
|
151 pdoNum++; |
|
152 offsetObjdict++; |
|
153 offsetObjdictMap++; |
|
154 status = state11; |
|
155 break; |
|
156 } |
|
157 case state5: /* get PDO CobId */ |
|
158 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
|
159 MSG_WAR(0x3009, " PDO CobId is : ", *pwCobId); |
|
160 status = state7; |
|
161 break; |
|
162 case state7: /* get mapped objects number to transmit with this PDO */ |
|
163 pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject; |
|
164 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
|
165 status = state8; |
|
166 case state8: /* get mapping parameters */ |
|
167 pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject; |
|
168 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
|
169 MSG_WAR(0x3050, " at index : ", 0x1A00 + pdoNum); |
|
170 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
|
171 status = state9; |
|
172 |
|
173 case state9: /* get data to transmit */ |
|
174 index = (UNS16)((*pMappingParameter) >> 16); |
|
175 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
|
176 // <<3 because in *pMappingParameter the size is in bits |
|
177 sizeData = (UNS8) ((*pMappingParameter & (UNS32)0x000000FF) >> 3) ; |
|
178 |
|
179 objDict = getODentry(d, index, subIndex, (void *)&d->process_var.data[offset], &sizeData, &dataType, 0 ); |
|
180 |
|
181 if( objDict != OD_SUCCESSFUL ){ |
|
182 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
|
183 return 0xFF; |
|
184 } |
|
185 |
|
186 offset += sizeData ; |
|
187 d->process_var.count = offset; |
|
188 prp_j++; |
|
189 status = state10; |
|
190 break; |
|
191 |
|
192 case state10: /* loop to get all the data to transmit */ |
|
193 if( prp_j < *pMappingCount ){ |
|
194 MSG_WAR(0x3014, " next variable mapped : ", prp_j); |
|
195 status = state8; |
|
196 break; |
|
197 } |
|
198 else { |
|
199 MSG_WAR(0x3015, " End scan mapped variable", 0); |
|
200 PDOmGR( d, *pwCobId ); |
|
201 MSG_WAR(0x3016, " End of this pdo. Should have been sent", 0); |
|
202 pdoNum++; |
|
203 offsetObjdict++; |
|
204 offsetObjdictMap++; |
|
205 offset = 0x00; |
|
206 prp_j = 0x00; |
|
207 status = state11; |
|
208 break; |
|
209 } |
|
210 |
|
211 case state11: |
|
212 MSG_WAR(0x3017, "next pdo index : ", pdoNum); |
|
213 status = state3; |
|
214 break; |
|
215 |
|
216 default: |
|
217 MSG_ERR(0x1019,"Unknown state has been reached : %d",status); |
|
218 return 0xFF; |
|
219 }// end switch case |
|
220 |
|
221 }// end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) |
|
222 |
|
223 (*d->post_TPDO)(); |
|
224 |
|
225 return 0; |
|
226 } |
|
227 |
|
228 |