43 ** @param TPDO_com TPDO communication parameters OD entry |
43 ** @param TPDO_com TPDO communication parameters OD entry |
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 |
|
49 static void dbg(CO_Data * d, UNS8 v1, UNS8 v2) |
|
50 { |
|
51 Message pdo; |
|
52 pdo.cob_id = 0x100; |
|
53 pdo.rtr = NOT_A_REQUEST; |
|
54 pdo.len = 2; |
|
55 pdo.data[0] = v1; |
|
56 pdo.data[1] = v2; |
|
57 canSend (d->canHandle, &pdo); |
|
58 } |
|
59 |
48 |
60 |
49 UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo) |
61 UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo) |
50 { |
62 { |
51 const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; |
63 const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; |
52 const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; |
64 const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; |
144 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
156 pwCobId = d->objdict[offset].pSubindex[1].pObject; |
145 |
157 |
146 MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId); |
158 MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId); |
147 { |
159 { |
148 Message pdo; |
160 Message pdo; |
149 pdo.cob_id = (UNS16)UNS16_LE(*pwCobId); |
161 pdo.cob_id = UNS16_LE(*pwCobId); |
150 pdo.rtr = REQUEST; |
162 pdo.rtr = REQUEST; |
151 pdo.len = 0; |
163 pdo.len = 0; |
152 return canSend (d->canHandle, &pdo); |
164 return canSend (d->canHandle, &pdo); |
153 } |
165 } |
154 } |
166 } |
177 /* pointer fo the var which holds the mapping parameter of an |
189 /* pointer fo the var which holds the mapping parameter of an |
178 mapping entry */ |
190 mapping entry */ |
179 UNS32 *pMappingParameter = NULL; |
191 UNS32 *pMappingParameter = NULL; |
180 UNS8 *pTransmissionType = NULL; /* pointer to the transmission |
192 UNS8 *pTransmissionType = NULL; /* pointer to the transmission |
181 type */ |
193 type */ |
182 UNS32 *pwCobId = NULL; |
194 UNS16 *pwCobId = NULL; |
183 UNS8 Size; |
195 UNS8 Size; |
184 UNS8 offset; |
196 UNS8 offset; |
185 UNS8 status; |
197 UNS8 status; |
186 UNS32 objDict; |
198 UNS32 objDict; |
187 UNS16 offsetObjdict; |
199 UNS16 offsetObjdict; |
188 UNS16 lastIndex; |
200 UNS16 lastIndex; |
189 TIMEVAL EventTimerDuration = 0; |
|
190 |
201 |
191 status = state2; |
202 status = state2; |
192 |
203 |
193 MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff)); |
204 MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff)); |
194 offset = 0x00; |
205 offset = 0x00; |
195 numPdo = 0; |
206 numPdo = 0; |
196 numMap = 0; |
207 numMap = 0; |
197 if ((*m).rtr == NOT_A_REQUEST) |
208 if ((*m).rtr == NOT_A_REQUEST) |
198 { /* The PDO received is not a |
209 { |
199 request. */ |
|
200 |
|
201 offsetObjdict = d->firstIndex->PDO_RCV; |
210 offsetObjdict = d->firstIndex->PDO_RCV; |
202 lastIndex = d->lastIndex->PDO_RCV; |
211 lastIndex = d->lastIndex->PDO_RCV; |
203 |
212 |
204 /* study of all the PDO stored in the dictionary */ |
|
205 if (offsetObjdict) |
213 if (offsetObjdict) |
206 while (offsetObjdict <= lastIndex) |
214 while (offsetObjdict <= lastIndex) |
207 { |
215 { |
208 |
|
209 switch (status) |
216 switch (status) |
210 { |
217 { |
211 |
218 |
212 case state2: |
219 case state2: |
213 /* get CobId of the dictionary correspondant to the received |
220 pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject; |
214 PDO */ |
|
215 pwCobId = |
|
216 d->objdict[offsetObjdict].pSubindex[1].pObject; |
|
217 /* check the CobId coherance */ |
|
218 /*pwCobId is the cobId read in the dictionary at the state 3 |
|
219 */ |
|
220 if (*pwCobId == UNS16_LE(m->cob_id)) |
221 if (*pwCobId == UNS16_LE(m->cob_id)) |
221 { |
222 { |
222 /* The cobId is recognized */ |
223 /* The cobId is recognized */ |
223 status = state4; |
224 status = state4; |
224 EventTimerDuration = *(UNS16 *)d->objdict[offsetObjdict].pSubindex[5].pObject; |
|
225 MSG_WAR (0x3936, "cobId found at index ", |
225 MSG_WAR (0x3936, "cobId found at index ", |
226 0x1400 + numPdo); |
226 0x1400 + numPdo); |
227 break; |
227 break; |
228 } |
228 } |
229 else |
229 else |
230 { |
230 { |
231 /* cobId received does not match with those write in the |
231 /* received cobId does not match */ |
232 dictionnary */ |
|
233 numPdo++; |
232 numPdo++; |
234 offsetObjdict++; |
233 offsetObjdict++; |
235 status = state2; |
234 status = state2; |
236 break; |
235 break; |
237 } |
236 } |
303 ((*pMappingParameter) >> 8) & 0xFF); |
302 ((*pMappingParameter) >> 8) & 0xFF); |
304 offset += Size; |
303 offset += Size; |
305 } |
304 } |
306 numMap++; |
305 numMap++; |
307 } /* end loop while on mapped variables */ |
306 } /* end loop while on mapped variables */ |
308 if (EventTimerDuration && d->RxPDO_EventTimers) |
307 if (d->RxPDO_EventTimers) |
309 { |
308 { |
310 DelAlarm (d->RxPDO_EventTimers[numPdo]); |
309 TIMEVAL EventTimerDuration = *(UNS16 *)d->objdict[offsetObjdict].pSubindex[5].pObject; |
311 d->RxPDO_EventTimers[numPdo] = SetAlarm (d, numPdo, d->RxPDO_EventTimers_Handler, |
310 if(EventTimerDuration){ |
312 MS_TO_TIMEVAL (EventTimerDuration), 0); |
311 DelAlarm (d->RxPDO_EventTimers[numPdo]); |
|
312 d->RxPDO_EventTimers[numPdo] = SetAlarm (d, numPdo, d->RxPDO_EventTimers_Handler, |
|
313 MS_TO_TIMEVAL (EventTimerDuration), 0); |
|
314 } |
313 } |
315 } |
314 return 0; |
316 return 0; |
315 |
317 |
316 } /* end switch status */ |
318 } /* end switch status */ |
317 } /* end while */ |
319 } /* end while */ |