44 ** @param TPDO_map TPDO mapping parameters OD entry |
44 ** @param TPDO_map TPDO mapping parameters OD entry |
45 ** |
45 ** |
46 ** @return |
46 ** @return |
47 **/ |
47 **/ |
48 |
48 |
49 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo) |
49 UNS8 |
50 { |
50 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo) |
51 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; |
51 { |
52 const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; |
52 const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; |
53 |
53 const indextable *TPDO_map = |
54 UNS8 prp_j = 0x00; |
54 d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; |
55 UNS8 offset = 0x00; |
55 |
56 const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject; |
56 UNS8 prp_j = 0x00; |
57 |
57 UNS8 offset = 0x00; |
58 pdo->cob_id = *(UNS16*)TPDO_com->pSubindex[1].pObject & UNS16_LE(0x7FF); |
58 const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject; |
59 pdo->rtr = NOT_A_REQUEST; |
59 |
60 |
60 pdo->cob_id = *(UNS16 *) TPDO_com->pSubindex[1].pObject & UNS16_LE (0x7FF); |
61 MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject); |
61 pdo->rtr = NOT_A_REQUEST; |
62 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount ); |
62 |
63 |
63 MSG_WAR (0x3009, " PDO CobId is : ", |
64 do{ |
64 *(UNS32 *) TPDO_com->pSubindex[1].pObject); |
65 UNS8 dataType; /* Unused */ |
65 MSG_WAR (0x300D, " Number of objects mapped : ", *pMappingCount); |
66 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */ |
66 |
67 |
67 do |
68 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
68 { |
69 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject; |
69 UNS8 dataType; /* Unused */ |
70 UNS16 index = (UNS16)((*pMappingParameter) >> 16); |
70 UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /* temporary space to hold bits */ |
71 UNS8 Size = (UNS8)(*pMappingParameter & (UNS32)0x000000FF); /* Size in bits */ |
71 |
72 |
72 /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
73 /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
73 UNS32 *pMappingParameter = |
74 if(Size && ((offset + Size) <= 64)) { |
74 (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject; |
75 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
75 UNS16 index = (UNS16) ((*pMappingParameter) >> 16); |
76 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF); |
76 UNS8 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF); /* Size in bits */ |
77 |
77 |
78 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter); |
78 /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
79 MSG_WAR(0x3050, " at index : ", TPDO_map->index); |
79 if (Size && ((offset + Size) <= 64)) |
80 MSG_WAR(0x3051, " sub-index : ", prp_j + 1); |
80 { |
81 |
81 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
82 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){ |
82 UNS8 subIndex = |
83 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter)); |
83 (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF); |
84 return 0xFF; |
84 |
85 } |
85 MSG_WAR (0x300F, " got mapping parameter : ", *pMappingParameter); |
86 /* copy bit per bit in little endian*/ |
86 MSG_WAR (0x3050, " at index : ", TPDO_map->index); |
87 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0); |
87 MSG_WAR (0x3051, " sub-index : ", prp_j + 1); |
88 |
88 |
89 offset += Size ; |
89 if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) != |
90 } |
90 OD_SUCCESSFUL) |
91 prp_j++; |
91 { |
92 }while( prp_j < *pMappingCount ); |
92 MSG_ERR (0x1013, |
93 |
93 " Couldn't find mapped variable at index-subindex-size : ", |
94 pdo->len = 1 + ((offset - 1) >> 3); |
94 (UNS16) (*pMappingParameter)); |
95 |
95 return 0xFF; |
96 MSG_WAR(0x3015, " End scan mapped variable", 0); |
96 } |
97 |
97 /* copy bit per bit in little endian */ |
98 return 0; |
98 CopyBits (Size, ((UNS8 *) tmp), 0, 0, |
|
99 (UNS8 *) & pdo->data[offset >> 3], offset % 8, 0); |
|
100 |
|
101 offset += Size; |
|
102 } |
|
103 prp_j++; |
|
104 } |
|
105 while (prp_j < *pMappingCount); |
|
106 |
|
107 pdo->len = 1 + ((offset - 1) >> 3); |
|
108 |
|
109 MSG_WAR (0x3015, " End scan mapped variable", 0); |
|
110 |
|
111 return 0; |
99 } |
112 } |
100 |
113 |
101 /*! |
114 /*! |
102 ** |
115 ** |
103 ** |
116 ** |
104 ** @param d |
117 ** @param d |
105 ** @param cobId |
118 ** @param cobId |
106 ** |
119 ** |
107 ** @return |
120 ** @return |
108 **/ |
121 **/ |
109 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex ) |
122 UNS8 |
110 { |
123 sendPDOrequest (CO_Data * d, UNS16 RPDOIndex) |
111 UNS16 * pwCobId; |
124 { |
|
125 UNS16 *pwCobId; |
112 UNS16 offset = d->firstIndex->PDO_RCV; |
126 UNS16 offset = d->firstIndex->PDO_RCV; |
113 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
127 UNS16 lastIndex = d->lastIndex->PDO_RCV; |
114 |
128 |
115 /* Sending the request only if the cobid have been found on the PDO |
129 /* Sending the request only if the cobid have been found on the PDO |
116 receive */ |
130 receive */ |
117 /* part dictionary */ |
131 /* part dictionary */ |
118 |
132 |
119 MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex); |
133 MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex); |
120 |
134 |
121 if (offset && RPDOIndex >= 0x1400){ |
135 if (offset && RPDOIndex >= 0x1400) |
122 offset += RPDOIndex - 0x1400; |
136 { |
123 if (offset <= lastIndex) { |
137 offset += RPDOIndex - 0x1400; |
124 /* get the CobId*/ |
138 if (offset <= lastIndex) |
125 pwCobId = (UNS16*) d->objdict[offset].pSubindex[1].pObject; |
139 { |
126 |
140 /* get the CobId */ |
127 MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId); |
141 pwCobId = (UNS16 *) d->objdict[offset].pSubindex[1].pObject; |
128 { |
142 |
129 Message pdo; |
143 MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId); |
130 pdo.cob_id = *pwCobId; |
144 { |
131 pdo.rtr = REQUEST; |
145 Message pdo; |
132 pdo.len = 0; |
146 pdo.cob_id = *pwCobId; |
133 return canSend(d->canHandle,&pdo); |
147 pdo.rtr = REQUEST; |
134 } |
148 pdo.len = 0; |
|
149 return canSend (d->canHandle, &pdo); |
|
150 } |
|
151 } |
135 } |
152 } |
136 } |
153 MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex); |
137 MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex); |
|
138 return 0xFF; |
154 return 0xFF; |
139 } |
155 } |
140 |
156 |
141 |
157 |
142 /*! |
158 /*! |
145 ** @param d |
161 ** @param d |
146 ** @param m |
162 ** @param m |
147 ** |
163 ** |
148 ** @return |
164 ** @return |
149 **/ |
165 **/ |
150 UNS8 proceedPDO(CO_Data* d, Message *m) |
166 UNS8 |
151 { |
167 proceedPDO (CO_Data * d, Message * m) |
152 UNS8 numPdo; |
168 { |
153 UNS8 numMap; /* Number of the mapped varable */ |
169 UNS8 numPdo; |
154 UNS8 * pMappingCount = NULL; /* count of mapped objects... */ |
170 UNS8 numMap; /* Number of the mapped varable */ |
|
171 UNS8 *pMappingCount = NULL; /* count of mapped objects... */ |
155 /* pointer to the var which is mapped to a pdo... */ |
172 /* pointer to the var which is mapped to a pdo... */ |
156 /* void * pMappedAppObject = NULL; */ |
173 /* void * pMappedAppObject = NULL; */ |
157 /* pointer fo the var which holds the mapping parameter of an |
174 /* pointer fo the var which holds the mapping parameter of an |
158 mapping entry */ |
175 mapping entry */ |
159 UNS32 * pMappingParameter = NULL; |
176 UNS32 *pMappingParameter = NULL; |
160 UNS8 * pTransmissionType = NULL; /* pointer to the transmission |
177 UNS8 *pTransmissionType = NULL; /* pointer to the transmission |
161 type */ |
178 type */ |
162 UNS16 * pwCobId = NULL; |
179 UNS16 *pwCobId = NULL; |
163 UNS8 Size; |
180 UNS8 Size; |
164 UNS8 offset; |
181 UNS8 offset; |
165 UNS8 status; |
182 UNS8 status; |
166 UNS32 objDict; |
183 UNS32 objDict; |
167 UNS16 offsetObjdict; |
184 UNS16 offsetObjdict; |
168 UNS16 lastIndex; |
185 UNS16 lastIndex; |
169 |
186 |
170 status = state2; |
187 status = state2; |
171 |
188 |
172 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE(0x7ff))); |
189 MSG_WAR (0x3935, "proceedPDO, cobID : ", ((*m).cob_id & UNS16_LE (0x7ff))); |
173 offset = 0x00; |
190 offset = 0x00; |
174 numPdo = 0; |
191 numPdo = 0; |
175 numMap = 0; |
192 numMap = 0; |
176 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a |
193 if ((*m).rtr == NOT_A_REQUEST) |
177 request. */ |
194 { /* The PDO received is not a |
178 |
195 request. */ |
179 offsetObjdict = d->firstIndex->PDO_RCV; |
196 |
180 lastIndex = d->lastIndex->PDO_RCV; |
197 offsetObjdict = d->firstIndex->PDO_RCV; |
181 |
198 lastIndex = d->lastIndex->PDO_RCV; |
182 /* study of all the PDO stored in the dictionary */ |
199 |
183 if(offsetObjdict) |
200 /* study of all the PDO stored in the dictionary */ |
184 while (offsetObjdict <= lastIndex) { |
201 if (offsetObjdict) |
185 |
202 while (offsetObjdict <= lastIndex) |
186 switch( status ) { |
203 { |
187 |
204 |
188 case state2: |
205 switch (status) |
189 /* get CobId of the dictionary correspondant to the received |
206 { |
190 PDO */ |
207 |
191 pwCobId = (UNS16*) d->objdict[offsetObjdict].pSubindex[1].pObject; |
208 case state2: |
192 /* check the CobId coherance */ |
209 /* get CobId of the dictionary correspondant to the received |
193 /*pwCobId is the cobId read in the dictionary at the state 3 |
210 PDO */ |
194 */ |
211 pwCobId = |
195 if ( *pwCobId == (*m).cob_id ){ |
212 (UNS16 *) d->objdict[offsetObjdict].pSubindex[1].pObject; |
196 /* The cobId is recognized */ |
213 /* check the CobId coherance */ |
197 status = state4; |
214 /*pwCobId is the cobId read in the dictionary at the state 3 |
198 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo); |
215 */ |
199 break; |
216 if (*pwCobId == (*m).cob_id) |
200 } |
217 { |
201 else { |
218 /* The cobId is recognized */ |
202 /* cobId received does not match with those write in the |
219 status = state4; |
203 dictionnary */ |
220 MSG_WAR (0x3936, "cobId found at index ", |
204 numPdo++; |
221 0x1400 + numPdo); |
205 offsetObjdict++; |
222 break; |
206 status = state2; |
223 } |
207 break; |
224 else |
208 } |
225 { |
209 |
226 /* cobId received does not match with those write in the |
210 case state4:/* Get Mapped Objects Number */ |
227 dictionnary */ |
211 /* The cobId of the message received has been found in the |
228 numPdo++; |
212 dictionnary. */ |
229 offsetObjdict++; |
213 offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
230 status = state2; |
214 lastIndex = d->lastIndex->PDO_RCV_MAP; |
231 break; |
215 pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject; |
232 } |
216 numMap = 0; |
233 |
217 while (numMap < *pMappingCount) { |
234 case state4: /* Get Mapped Objects Number */ |
218 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; |
235 /* The cobId of the message received has been found in the |
219 UNS8 ByteSize; |
236 dictionnary. */ |
220 pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject; |
237 offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
221 if (pMappingParameter == NULL) { |
238 lastIndex = d->lastIndex->PDO_RCV_MAP; |
222 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1); |
239 pMappingCount = |
223 return 0xFF; |
240 (UNS8 *) (d->objdict + offsetObjdict + |
224 } |
241 numPdo)->pSubindex[0].pObject; |
225 /* Get the addresse of the mapped variable. */ |
242 numMap = 0; |
226 /* detail of *pMappingParameter : */ |
243 while (numMap < *pMappingCount) |
227 /* The 16 hight bits contains the index, the medium 8 bits |
244 { |
228 contains the subindex, */ |
245 UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; |
229 /* and the lower 8 bits contains the size of the mapped |
246 UNS8 ByteSize; |
230 variable. */ |
247 pMappingParameter = |
231 |
248 (UNS32 *) (d->objdict + offsetObjdict + |
232 Size = (UNS8)(*pMappingParameter & (UNS32)0x000000FF); |
249 numPdo)->pSubindex[numMap + 1].pObject; |
233 |
250 if (pMappingParameter == NULL) |
234 /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
251 { |
235 if(Size && ((offset + Size) <= (m->len << 3))) { |
252 MSG_ERR (0x1937, "Couldn't get mapping parameter : ", |
236 /* copy bit per bit in little endian */ |
253 numMap + 1); |
237 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0); |
254 return 0xFF; |
238 |
255 } |
239 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
256 /* Get the addresse of the mapped variable. */ |
240 2, ... */ |
257 /* detail of *pMappingParameter : */ |
241 |
258 /* The 16 hight bits contains the index, the medium 8 bits |
242 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16), |
259 contains the subindex, */ |
243 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF), |
260 /* and the lower 8 bits contains the size of the mapped |
244 tmp, &ByteSize, 0 ); |
261 variable. */ |
245 |
262 |
246 if(objDict != OD_SUCCESSFUL) { |
263 Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF); |
247 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1); |
264 |
248 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16); |
265 /* set variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
249 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
266 if (Size && ((offset + Size) <= (m->len << 3))) |
250 return 0xFF; |
267 { |
251 } |
268 /* copy bit per bit in little endian */ |
252 |
269 CopyBits (Size, (UNS8 *) & m->data[offset >> 3], |
253 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id); |
270 offset % 8, 0, ((UNS8 *) tmp), 0, 0); |
254 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16); |
271 |
255 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF); |
272 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => |
256 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
273 2, ... */ |
257 offset += Size; |
274 |
258 } |
275 objDict = |
259 numMap++; |
276 setODentry (d, (UNS16) ((*pMappingParameter) >> 16), |
260 } /* end loop while on mapped variables */ |
277 (UNS8) (((*pMappingParameter) >> 8) & |
261 |
278 0xFF), tmp, &ByteSize, 0); |
262 offset=0x00; |
279 |
263 numMap = 0; |
280 if (objDict != OD_SUCCESSFUL) |
264 return 0; |
281 { |
265 |
282 MSG_ERR (0x1938, |
266 }/* end switch status*/ |
283 "error accessing to the mapped var : ", |
267 }/* end while*/ |
284 numMap + 1); |
268 }/* end if Donnees */ |
285 MSG_WAR (0x2939, " Mapped at index : ", |
269 else if ((*m).rtr == REQUEST ){ |
286 (*pMappingParameter) >> 16); |
270 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id); |
287 MSG_WAR (0x2940, " subindex : ", |
271 status = state1; |
288 ((*pMappingParameter) >> 8) & 0xFF); |
272 offsetObjdict = d->firstIndex->PDO_TRS; |
289 return 0xFF; |
273 lastIndex = d->lastIndex->PDO_TRS; |
290 } |
274 if(offsetObjdict) while( offsetObjdict <= lastIndex ){ |
291 |
275 /* study of all PDO stored in the objects dictionary */ |
292 MSG_WAR (0x3942, |
276 |
293 "Variable updated with value received by PDO cobid : ", |
277 switch( status ){ |
294 m->cob_id); |
278 |
295 MSG_WAR (0x3943, " Mapped at index : ", |
279 case state1:/* check the CobId */ |
296 (*pMappingParameter) >> 16); |
280 /* get CobId of the dictionary which match to the received PDO |
297 MSG_WAR (0x3944, " subindex : ", |
281 */ |
298 ((*pMappingParameter) >> 8) & 0xFF); |
282 pwCobId = (UNS16*) (d->objdict + offsetObjdict)->pSubindex[1].pObject; |
299 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */ |
283 if ( *pwCobId == (*m).cob_id ) { |
300 offset += Size; |
284 status = state4; |
301 } |
285 break; |
302 numMap++; |
286 } |
303 } /* end loop while on mapped variables */ |
287 else { |
304 |
288 numPdo++; |
305 offset = 0x00; |
289 offsetObjdict++; |
306 numMap = 0; |
290 } |
307 return 0; |
291 status = state1; |
308 |
292 break; |
309 } /* end switch status */ |
293 |
310 } /* end while */ |
294 |
311 } /* end if Donnees */ |
295 case state4:/* check transmission type */ |
312 else if ((*m).rtr == REQUEST) |
296 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
313 { |
297 /* If PDO is to be sampled and send on RTR, do it*/ |
314 MSG_WAR (0x3946, "Receive a PDO request cobId : ", m->cob_id); |
298 if ( (*pTransmissionType == TRANS_RTR)) { |
315 status = state1; |
299 status = state5; |
316 offsetObjdict = d->firstIndex->PDO_TRS; |
300 break; |
317 lastIndex = d->lastIndex->PDO_TRS; |
301 /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */ |
318 if (offsetObjdict) |
302 }else if ((*pTransmissionType == TRANS_RTR_SYNC )) { |
319 while (offsetObjdict <= lastIndex) |
303 if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){ |
320 { |
304 /*Data ready, just send*/ |
321 /* study of all PDO stored in the objects dictionary */ |
305 canSend(d->canHandle,&d->PDO_status[numPdo].last_message); |
322 |
306 return 0; |
323 switch (status) |
307 }else{ |
324 { |
308 /* if SYNC did never occur, force emission with current data */ |
325 |
309 /* DS301 do not tell what to do in such a case...*/ |
326 case state1: /* check the CobId */ |
310 MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id); |
327 /* get CobId of the dictionary which match to the received PDO |
311 status = state5; |
328 */ |
312 } |
329 pwCobId = |
313 break; |
330 (UNS16 *) (d->objdict + |
314 }else if( |
331 offsetObjdict)->pSubindex[1].pObject; |
315 (*pTransmissionType == TRANS_EVENT_PROFILE) || |
332 if (*pwCobId == (*m).cob_id) |
316 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) { |
333 { |
317 /* Zap all timers and inhibit flag */ |
334 status = state4; |
318 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer); |
335 break; |
319 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer); |
336 } |
320 d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED; |
337 else |
321 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
338 { |
322 PDOEventTimerAlarm(d, numPdo); |
339 numPdo++; |
323 return 0; |
340 offsetObjdict++; |
324 }else { |
341 } |
325 /* The requested PDO is not to send on request. So, does |
342 status = state1; |
326 nothing. */ |
343 break; |
327 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id); |
344 |
328 return 0xFF; |
345 |
329 } |
346 case state4: /* check transmission type */ |
330 |
347 pTransmissionType = |
331 case state5:/* build and send requested PDO */ |
348 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject; |
332 { |
349 /* If PDO is to be sampled and send on RTR, do it */ |
333 Message pdo; |
350 if ((*pTransmissionType == TRANS_RTR)) |
334 if( buildPDO(d, numPdo, &pdo)) |
351 { |
335 { |
352 status = state5; |
336 MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo); |
353 break; |
337 return 0xFF; |
354 /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */ |
338 } |
355 } |
339 canSend(d->canHandle,&pdo); |
356 else if ((*pTransmissionType == TRANS_RTR_SYNC)) |
340 return 0; |
357 { |
341 } |
358 if (d->PDO_status[numPdo]. |
342 }/* end switch status */ |
359 transmit_type_parameter & PDO_RTR_SYNC_READY) |
343 }/* end while */ |
360 { |
344 }/* end if Requete */ |
361 /*Data ready, just send */ |
|
362 canSend (d->canHandle, |
|
363 &d->PDO_status[numPdo].last_message); |
|
364 return 0; |
|
365 } |
|
366 else |
|
367 { |
|
368 /* if SYNC did never occur, force emission with current data */ |
|
369 /* DS301 do not tell what to do in such a case... */ |
|
370 MSG_ERR (0x1947, |
|
371 "Not ready RTR_SYNC TPDO send current data : ", |
|
372 m->cob_id); |
|
373 status = state5; |
|
374 } |
|
375 break; |
|
376 } |
|
377 else if ((*pTransmissionType == TRANS_EVENT_PROFILE) || |
|
378 (*pTransmissionType == TRANS_EVENT_SPECIFIC)) |
|
379 { |
|
380 /* Zap all timers and inhibit flag */ |
|
381 d->PDO_status[numPdo].event_timer = |
|
382 DelAlarm (d->PDO_status[numPdo].event_timer); |
|
383 d->PDO_status[numPdo].inhibit_timer = |
|
384 DelAlarm (d->PDO_status[numPdo].inhibit_timer); |
|
385 d->PDO_status[numPdo].transmit_type_parameter &= |
|
386 ~PDO_INHIBITED; |
|
387 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
|
388 PDOEventTimerAlarm (d, numPdo); |
|
389 return 0; |
|
390 } |
|
391 else |
|
392 { |
|
393 /* The requested PDO is not to send on request. So, does |
|
394 nothing. */ |
|
395 MSG_WAR (0x2947, "PDO is not to send on request : ", |
|
396 m->cob_id); |
|
397 return 0xFF; |
|
398 } |
|
399 |
|
400 case state5: /* build and send requested PDO */ |
|
401 { |
|
402 Message pdo; |
|
403 if (buildPDO (d, numPdo, &pdo)) |
|
404 { |
|
405 MSG_ERR (0x1948, " Couldn't build TPDO n°", numPdo); |
|
406 return 0xFF; |
|
407 } |
|
408 canSend (d->canHandle, &pdo); |
|
409 return 0; |
|
410 } |
|
411 } /* end switch status */ |
|
412 } /* end while */ |
|
413 } /* end if Requete */ |
345 |
414 |
346 return 0; |
415 return 0; |
347 } |
416 } |
348 |
417 |
349 /*! |
418 /*! |
355 ** @param SrcBigEndian |
424 ** @param SrcBigEndian |
356 ** @param DestByteIndex |
425 ** @param DestByteIndex |
357 ** @param DestBitIndex |
426 ** @param DestBitIndex |
358 ** @param DestBigEndian |
427 ** @param DestBigEndian |
359 **/ |
428 **/ |
360 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian) |
429 void |
361 { |
430 CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex, |
362 /* This loop copy as many bits that it can each time, crossing*/ |
431 UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex, |
363 /* successively bytes*/ |
432 UNS8 DestBigEndian) |
|
433 { |
|
434 /* This loop copy as many bits that it can each time, crossing */ |
|
435 /* successively bytes */ |
364 // boundaries from LSB to MSB. |
436 // boundaries from LSB to MSB. |
365 while(NbBits > 0) |
437 while (NbBits > 0) |
366 { |
438 { |
367 /* Bit missalignement between src and dest*/ |
439 /* Bit missalignement between src and dest */ |
368 INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
440 INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
369 |
441 |
370 /* We can now get src and align it to dest*/ |
442 /* We can now get src and align it to dest */ |
371 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
443 UNS8 Aligned = |
372 |
444 Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
373 /* Compute the nb of bit we will be able to copy*/ |
445 |
374 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex ); |
446 /* Compute the nb of bit we will be able to copy */ |
|
447 UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex); |
375 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
448 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
376 |
449 |
377 /* Create a mask that will serve in:*/ |
450 /* Create a mask that will serve in: */ |
378 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex))); |
451 UNS8 Mask = |
379 |
452 ((0xff << (DestBitIndex + BitsToCopy)) | |
380 /* - Filtering src*/ |
453 (0xff >> (8 - DestBitIndex))); |
|
454 |
|
455 /* - Filtering src */ |
381 UNS8 Filtered = Aligned & ~Mask; |
456 UNS8 Filtered = Aligned & ~Mask; |
382 |
457 |
383 /* - and erase bits where we write, preserve where we don't*/ |
458 /* - and erase bits where we write, preserve where we don't */ |
384 *DestByteIndex &= Mask; |
459 *DestByteIndex &= Mask; |
385 |
460 |
386 /* Then write.*/ |
461 /* Then write. */ |
387 *DestByteIndex |= Filtered ; |
462 *DestByteIndex |= Filtered; |
388 |
463 |
389 /*Compute next time cursors for src*/ |
464 /*Compute next time cursors for src */ |
390 if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/ |
465 if ((SrcBitIndex += BitsToCopy) > 7) /* cross boundary ? */ |
391 { |
466 { |
392 SrcBitIndex = 0;/* First bit*/ |
467 SrcBitIndex = 0; /* First bit */ |
393 SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/ |
468 SrcByteIndex += (SrcBigEndian ? -1 : 1); /* Next byte */ |
394 } |
469 } |
395 |
470 |
396 |
471 |
397 /*Compute next time cursors for dest*/ |
472 /*Compute next time cursors for dest */ |
398 if((DestBitIndex += BitsToCopy)>7) |
473 if ((DestBitIndex += BitsToCopy) > 7) |
399 { |
474 { |
400 DestBitIndex = 0;/* First bit*/ |
475 DestBitIndex = 0; /* First bit */ |
401 DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/ |
476 DestByteIndex += (DestBigEndian ? -1 : 1); /* Next byte */ |
402 } |
477 } |
403 |
478 |
404 /*And decrement counter.*/ |
479 /*And decrement counter. */ |
405 NbBits -= BitsToCopy; |
480 NbBits -= BitsToCopy; |
406 } |
481 } |
407 |
482 |
408 } |
483 } |
|
484 |
409 /*! |
485 /*! |
410 ** |
486 ** |
411 ** |
487 ** |
412 ** @param d |
488 ** @param d |
413 ** |
489 ** |
414 ** @return |
490 ** @return |
415 **/ |
491 **/ |
416 |
492 |
417 UNS8 sendPDOevent( CO_Data* d) |
493 UNS8 |
|
494 sendPDOevent (CO_Data * d) |
418 { |
495 { |
419 /* Calls _sendPDOevent specifying it is not a sync event */ |
496 /* Calls _sendPDOevent specifying it is not a sync event */ |
420 return _sendPDOevent(d, 0); |
497 return _sendPDOevent (d, 0); |
421 } |
498 } |
422 |
499 |
423 |
500 |
424 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum) |
501 void |
425 { |
502 PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum) |
426 /* This is needed to avoid deletion of re-attribuated timer */ |
503 { |
427 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
504 /* This is needed to avoid deletion of re-attribuated timer */ |
428 /* force emission of PDO by artificially changing last emitted*/ |
505 d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
429 d->PDO_status[pdoNum].last_message.cob_id = 0; |
506 /* force emission of PDO by artificially changing last emitted */ |
430 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
507 d->PDO_status[pdoNum].last_message.cob_id = 0; |
431 } |
508 _sendPDOevent (d, 0); /* not a Sync Event */ |
432 |
509 } |
433 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum) |
510 |
434 { |
511 void |
435 /* This is needed to avoid deletion of re-attribuated timer */ |
512 PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum) |
436 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
513 { |
437 /* Remove inhibit flag */ |
514 /* This is needed to avoid deletion of re-attribuated timer */ |
438 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
515 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
439 _sendPDOevent( d, 0 ); /* not a Sync Event*/ |
516 /* Remove inhibit flag */ |
|
517 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
|
518 _sendPDOevent (d, 0); /* not a Sync Event */ |
440 } |
519 } |
441 |
520 |
442 /*! |
521 /*! |
443 ** |
522 ** |
444 ** |
523 ** |
446 ** @param isSyncEvent |
525 ** @param isSyncEvent |
447 ** |
526 ** |
448 ** @return |
527 ** @return |
449 **/ |
528 **/ |
450 |
529 |
451 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent ) |
530 UNS8 |
452 { |
531 _sendPDOevent (CO_Data * d, UNS8 isSyncEvent) |
453 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
532 { |
454 UNS8* pTransmissionType = NULL; |
533 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
|
534 UNS8 *pTransmissionType = NULL; |
455 UNS8 status = state3; |
535 UNS8 status = state3; |
456 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
536 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
457 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
537 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
458 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
538 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
459 |
539 |
460 /* study all PDO stored in the objects dictionary */ |
540 /* study all PDO stored in the objects dictionary */ |
461 if(offsetObjdict){ |
541 if (offsetObjdict) |
462 Message pdo = Message_Initializer; |
542 { |
463 while( offsetObjdict <= lastIndex) { |
543 Message pdo = Message_Initializer; |
464 switch( status ) { |
544 while (offsetObjdict <= lastIndex) |
465 case state3: |
|
466 if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/ |
|
467 /* check if TPDO is not valid */ |
|
468 *(UNS32*)d->objdict[offsetObjdict].pSubindex[1].pObject & 0x80000000) { |
|
469 MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum); |
|
470 /*Go next TPDO*/ |
|
471 status = state11; |
|
472 break; |
|
473 } |
|
474 /* get the PDO transmission type */ |
|
475 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject; |
|
476 MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); |
|
477 |
|
478 /* check if transmission type is SYNCRONOUS */ |
|
479 /* The message is transmited every n SYNC with n=TransmissionType */ |
|
480 if( isSyncEvent && |
|
481 (*pTransmissionType >= TRANS_SYNC_MIN) && |
|
482 (*pTransmissionType <= TRANS_SYNC_MAX) && |
|
483 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) { |
|
484 /*Reset count of SYNC*/ |
|
485 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
|
486 MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType); |
|
487 { |
|
488 Message msg_init = Message_Initializer; |
|
489 pdo = msg_init; |
|
490 } |
|
491 if(buildPDO(d, pdoNum, &pdo)) |
|
492 { |
545 { |
493 MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum); |
546 switch (status) |
494 status = state11; |
547 { |
495 break; |
548 case state3: |
496 } |
549 if ( /*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5) */ |
497 status = state5; |
550 /* check if TPDO is not valid */ |
498 /* If transmission RTR, with data sampled on SYNC */ |
551 *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1]. |
499 }else if( isSyncEvent && |
552 pObject & 0x80000000) |
500 (*pTransmissionType == TRANS_RTR_SYNC)) { |
553 { |
501 if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message)) |
554 MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum); |
502 { |
555 /*Go next TPDO */ |
503 MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum); |
556 status = state11; |
504 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY; |
557 break; |
505 }else{ |
558 } |
506 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY; |
559 /* get the PDO transmission type */ |
507 } |
560 pTransmissionType = |
508 status = state11; |
561 (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject; |
509 break; |
562 MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); |
510 /* If transmission on Event and not inhibited, check for changes */ |
563 |
511 }else if((*pTransmissionType == TRANS_EVENT_PROFILE || |
564 /* check if transmission type is SYNCRONOUS */ |
512 *pTransmissionType == TRANS_EVENT_SPECIFIC )&& |
565 /* The message is transmited every n SYNC with n=TransmissionType */ |
513 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) { |
566 if (isSyncEvent && |
514 MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType); |
567 (*pTransmissionType >= TRANS_SYNC_MIN) && |
515 { |
568 (*pTransmissionType <= TRANS_SYNC_MAX) && |
516 Message msg_init = Message_Initializer; |
569 (++d->PDO_status[pdoNum].transmit_type_parameter == |
517 pdo = msg_init; |
570 *pTransmissionType)) |
518 } |
571 { |
519 if(buildPDO(d, pdoNum, &pdo)) |
572 /*Reset count of SYNC */ |
520 { |
573 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
521 MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum); |
574 MSG_WAR (0x3964, " PDO is on SYNCHRO. Trans type : ", |
522 status = state11; |
575 *pTransmissionType); |
523 break; |
576 { |
524 } |
577 Message msg_init = Message_Initializer; |
525 |
578 pdo = msg_init; |
526 /*Compare new and old PDO*/ |
579 } |
527 if(d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id && |
580 if (buildPDO (d, pdoNum, &pdo)) |
528 d->PDO_status[pdoNum].last_message.len == pdo.len && |
581 { |
|
582 MSG_ERR (0x1906, " Couldn't build TPDO number : ", |
|
583 pdoNum); |
|
584 status = state11; |
|
585 break; |
|
586 } |
|
587 status = state5; |
|
588 /* If transmission RTR, with data sampled on SYNC */ |
|
589 } |
|
590 else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC)) |
|
591 { |
|
592 if (buildPDO |
|
593 (d, pdoNum, &d->PDO_status[pdoNum].last_message)) |
|
594 { |
|
595 MSG_ERR (0x1966, " Couldn't build TPDO number : ", |
|
596 pdoNum); |
|
597 d->PDO_status[pdoNum].transmit_type_parameter &= |
|
598 ~PDO_RTR_SYNC_READY; |
|
599 } |
|
600 else |
|
601 { |
|
602 d->PDO_status[pdoNum].transmit_type_parameter |= |
|
603 PDO_RTR_SYNC_READY; |
|
604 } |
|
605 status = state11; |
|
606 break; |
|
607 /* If transmission on Event and not inhibited, check for changes */ |
|
608 } |
|
609 else if ((*pTransmissionType == TRANS_EVENT_PROFILE || |
|
610 *pTransmissionType == TRANS_EVENT_SPECIFIC) && |
|
611 !(d->PDO_status[pdoNum]. |
|
612 transmit_type_parameter & PDO_INHIBITED)) |
|
613 { |
|
614 MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ", |
|
615 *pTransmissionType); |
|
616 { |
|
617 Message msg_init = Message_Initializer; |
|
618 pdo = msg_init; |
|
619 } |
|
620 if (buildPDO (d, pdoNum, &pdo)) |
|
621 { |
|
622 MSG_ERR (0x3907, " Couldn't build TPDO number : ", |
|
623 pdoNum); |
|
624 status = state11; |
|
625 break; |
|
626 } |
|
627 |
|
628 /*Compare new and old PDO */ |
|
629 if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id |
|
630 && d->PDO_status[pdoNum].last_message.len == pdo.len && |
529 #ifdef UNS64 |
631 #ifdef UNS64 |
530 *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){ |
632 *(UNS64 *) (&d->PDO_status[pdoNum].last_message. |
531 #else /* don't ALLOW_64BIT_OPS*/ |
633 data[0]) == *(UNS64 *) (&pdo.data[0]) |
532 *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS32*)(&pdo.data[0]) && |
634 #else /* don't ALLOW_64BIT_OPS */ |
533 *(UNS32*)(&d->PDO_status[pdoNum].last_message.data[4]) == *(UNS32*)(&pdo.data[4])){ |
635 *(UNS32 *) (&d->PDO_status[pdoNum].last_message. |
534 #endif |
636 data[0]) == *(UNS32 *) (&pdo.data[0]) |
535 /* No changes -> go to next pdo*/ |
637 && *(UNS32 *) (&d->PDO_status[pdoNum].last_message. |
536 status = state11; |
638 data[4]) == *(UNS32 *) (&pdo.data[4]) |
537 }else{ |
639 #endif |
538 |
640 ) |
539 UNS16 EventTimerDuration; |
641 { |
540 UNS16 InhibitTimerDuration; |
642 /* No changes -> go to next pdo */ |
541 |
643 status = state11; |
542 MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum); |
644 } |
543 /* Changes detected -> transmit message */ |
645 else |
544 EventTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject; |
646 { |
545 InhibitTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject; |
647 |
546 |
648 UNS16 EventTimerDuration; |
547 status = state5; |
649 UNS16 InhibitTimerDuration; |
548 |
650 |
549 /* Start both event_timer and inhibit_timer*/ |
651 MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum); |
550 if(EventTimerDuration){ |
652 /* Changes detected -> transmit message */ |
551 DelAlarm(d->PDO_status[pdoNum].event_timer); |
653 EventTimerDuration = |
552 d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(EventTimerDuration), 0); |
654 *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5]. |
553 } |
655 pObject; |
554 |
656 InhibitTimerDuration = |
555 if(InhibitTimerDuration){ |
657 *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3]. |
556 DelAlarm(d->PDO_status[pdoNum].inhibit_timer); |
658 pObject; |
557 d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(InhibitTimerDuration * 100), 0); |
659 |
558 /* and inhibit TPDO */ |
660 status = state5; |
559 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED; |
661 |
560 } |
662 /* Start both event_timer and inhibit_timer */ |
561 |
663 if (EventTimerDuration) |
562 } |
664 { |
563 }else{ |
665 DelAlarm (d->PDO_status[pdoNum].event_timer); |
564 MSG_WAR(0x306C, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType); |
666 d->PDO_status[pdoNum].event_timer = |
565 status = state11; |
667 SetAlarm (d, pdoNum, &PDOEventTimerAlarm, |
566 } |
668 MS_TO_TIMEVAL (EventTimerDuration), 0); |
567 break; |
669 } |
568 case state5: /*Send the pdo*/ |
670 |
569 /*store_as_last_message*/ |
671 if (InhibitTimerDuration) |
570 d->PDO_status[pdoNum].last_message = pdo; |
672 { |
571 MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id); |
673 DelAlarm (d->PDO_status[pdoNum].inhibit_timer); |
572 MSG_WAR(0x396E, " Nb octets : ", pdo.len); |
674 d->PDO_status[pdoNum].inhibit_timer = |
573 |
675 SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm, |
574 canSend(d->canHandle,&pdo); |
676 US_TO_TIMEVAL (InhibitTimerDuration * |
575 status = state11; |
677 100), 0); |
576 break; |
678 /* and inhibit TPDO */ |
577 case state11: /*Go to next TPDO*/ |
679 d->PDO_status[pdoNum].transmit_type_parameter |= |
578 pdoNum++; |
680 PDO_INHIBITED; |
579 offsetObjdict++; |
681 } |
580 offsetObjdictMap++; |
682 |
581 MSG_WAR(0x3970, "next pdo index : ", pdoNum); |
683 } |
582 status = state3; |
684 } |
583 break; |
685 else |
584 |
686 { |
585 default: |
687 MSG_WAR (0x306C, |
586 MSG_ERR(0x1972,"Unknown state has been reached : %d",status); |
688 " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", |
587 return 0xFF; |
689 *pTransmissionType); |
588 }/* end switch case */ |
690 status = state11; |
589 |
691 } |
590 }/* end while */ |
692 break; |
591 } |
693 case state5: /*Send the pdo */ |
|
694 /*store_as_last_message */ |
|
695 d->PDO_status[pdoNum].last_message = pdo; |
|
696 MSG_WAR (0x396D, "sendPDO cobId :", pdo.cob_id); |
|
697 MSG_WAR (0x396E, " Nb octets : ", pdo.len); |
|
698 |
|
699 canSend (d->canHandle, &pdo); |
|
700 status = state11; |
|
701 break; |
|
702 case state11: /*Go to next TPDO */ |
|
703 pdoNum++; |
|
704 offsetObjdict++; |
|
705 offsetObjdictMap++; |
|
706 MSG_WAR (0x3970, "next pdo index : ", pdoNum); |
|
707 status = state3; |
|
708 break; |
|
709 |
|
710 default: |
|
711 MSG_ERR (0x1972, "Unknown state has been reached : %d", status); |
|
712 return 0xFF; |
|
713 } /* end switch case */ |
|
714 |
|
715 } /* end while */ |
|
716 } |
592 return 0; |
717 return 0; |
593 } |
718 } |
594 |
719 |
595 /*! |
720 /*! |
596 ** |
721 ** |
599 ** @param OD_entry |
724 ** @param OD_entry |
600 ** @param bSubindex |
725 ** @param bSubindex |
601 ** @return always 0 |
726 ** @return always 0 |
602 **/ |
727 **/ |
603 |
728 |
604 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex) |
729 UNS32 |
|
730 TPDO_Communication_Parameter_Callback (CO_Data * d, |
|
731 const indextable * OD_entry, |
|
732 UNS8 bSubindex) |
605 { |
733 { |
606 /* If PDO are actives */ |
734 /* If PDO are actives */ |
607 if(d->CurrentCommunicationState.csPDO) switch(bSubindex) |
735 if (d->CurrentCommunicationState.csPDO) |
608 { |
736 switch (bSubindex) |
609 case 2: /* Changed transmition type */ |
737 { |
610 case 3: /* Changed inhibit time */ |
738 case 2: /* Changed transmition type */ |
611 case 5: /* Changed event time */ |
739 case 3: /* Changed inhibit time */ |
612 { |
740 case 5: /* Changed event time */ |
613 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS; |
741 { |
614 UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */ |
742 const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS; |
615 |
743 UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */ |
|
744 |
616 /* Zap all timers and inhibit flag */ |
745 /* Zap all timers and inhibit flag */ |
617 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer); |
746 d->PDO_status[numPdo].event_timer = |
618 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer); |
747 DelAlarm (d->PDO_status[numPdo].event_timer); |
|
748 d->PDO_status[numPdo].inhibit_timer = |
|
749 DelAlarm (d->PDO_status[numPdo].inhibit_timer); |
619 d->PDO_status[numPdo].transmit_type_parameter = 0; |
750 d->PDO_status[numPdo].transmit_type_parameter = 0; |
620 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
751 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
621 PDOEventTimerAlarm(d, numPdo); |
752 PDOEventTimerAlarm (d, numPdo); |
622 return 0; |
753 return 0; |
623 } |
754 } |
624 |
755 |
625 default: /* other subindex are ignored*/ |
756 default: /* other subindex are ignored */ |
626 break; |
757 break; |
627 } |
758 } |
628 return 0; |
759 return 0; |
629 } |
760 } |
630 |
761 |
631 void PDOInit(CO_Data* d) |
762 void |
|
763 PDOInit (CO_Data * d) |
632 { |
764 { |
633 /* For each TPDO mapping parameters */ |
765 /* For each TPDO mapping parameters */ |
634 UNS16 pdoIndex = 0x1800; /* OD index of TDPO */ |
766 UNS16 pdoIndex = 0x1800; /* OD index of TDPO */ |
635 |
767 |
636 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
768 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
637 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
769 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
638 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
770 if (offsetObjdict) |
639 /* Assign callbacks to sensible TPDO mapping subindexes */ |
771 while (offsetObjdict <= lastIndex) |
640 UNS32 errorCode; |
772 { |
641 ODCallback_t *CallbackList; |
773 /* Assign callbacks to sensible TPDO mapping subindexes */ |
642 /* Find callback list */ |
774 UNS32 errorCode; |
643 scanIndexOD (d, pdoIndex, &errorCode, &CallbackList); |
775 ODCallback_t *CallbackList; |
644 if(errorCode == OD_SUCCESSFUL && CallbackList) |
776 /* Find callback list */ |
645 { |
777 scanIndexOD (d, pdoIndex, &errorCode, &CallbackList); |
646 /*Assign callbacks to corresponding subindex*/ |
778 if (errorCode == OD_SUCCESSFUL && CallbackList) |
647 /* Transmission type */ |
779 { |
648 CallbackList[2] = &TPDO_Communication_Parameter_Callback; |
780 /*Assign callbacks to corresponding subindex */ |
649 /* Inhibit time */ |
781 /* Transmission type */ |
650 CallbackList[3] = &TPDO_Communication_Parameter_Callback; |
782 CallbackList[2] = &TPDO_Communication_Parameter_Callback; |
651 /* Event timer */ |
783 /* Inhibit time */ |
652 CallbackList[5] = &TPDO_Communication_Parameter_Callback; |
784 CallbackList[3] = &TPDO_Communication_Parameter_Callback; |
653 } |
785 /* Event timer */ |
654 pdoIndex++; |
786 CallbackList[5] = &TPDO_Communication_Parameter_Callback; |
655 offsetObjdict++; |
787 } |
656 } |
788 pdoIndex++; |
|
789 offsetObjdict++; |
|
790 } |
657 |
791 |
658 /* Trigger a non-sync event */ |
792 /* Trigger a non-sync event */ |
659 _sendPDOevent( d, 0 ); |
793 _sendPDOevent (d, 0); |
660 } |
794 } |
661 |
795 |
662 void PDOStop(CO_Data* d) |
796 void |
|
797 PDOStop (CO_Data * d) |
663 { |
798 { |
664 /* For each TPDO mapping parameters */ |
799 /* For each TPDO mapping parameters */ |
665 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
800 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
666 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
801 UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
667 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
802 UNS16 lastIndex = d->lastIndex->PDO_TRS; |
668 if(offsetObjdict) while( offsetObjdict <= lastIndex) { |
803 if (offsetObjdict) |
669 /* Delete TPDO timers */ |
804 while (offsetObjdict <= lastIndex) |
670 d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer); |
805 { |
671 d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer); |
806 /* Delete TPDO timers */ |
672 /* Reset transmit type parameter */ |
807 d->PDO_status[pdoNum].event_timer = |
673 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
808 DelAlarm (d->PDO_status[pdoNum].event_timer); |
674 d->PDO_status[pdoNum].last_message.cob_id = 0; |
809 d->PDO_status[pdoNum].inhibit_timer = |
675 pdoNum++; |
810 DelAlarm (d->PDO_status[pdoNum].inhibit_timer); |
676 offsetObjdict++; |
811 /* Reset transmit type parameter */ |
677 } |
812 d->PDO_status[pdoNum].transmit_type_parameter = 0; |
678 } |
813 d->PDO_status[pdoNum].last_message.cob_id = 0; |
|
814 pdoNum++; |
|
815 offsetObjdict++; |
|
816 } |
|
817 } |