2 This file is part of CanFestival, a library implementing CanOpen
5 Copyright (C): Edouard TISSERANT and Francis DUPIN
7 See COPYING file for copyrights details.
9 This library is free software; you can redistribute it and/or
10 modify it under the terms of the GNU Lesser General Public
11 License as published by the Free Software Foundation; either
12 version 2.1 of the License, or (at your option) any later version.
14 This library is distributed in the hope that it will be useful,
15 but WITHOUT ANY WARRANTY; without even the implied warranty of
16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 Lesser General Public License for more details.
19 You should have received a copy of the GNU Lesser General Public
20 License along with this library; if not, write to the Free Software
21 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
26 #include "canfestival.h"
30 ** @author Edouard TISSERANT and Francis DUPIN
31 ** @date Tue Jun 5 09:32:32 2007
42 ** @param TPDO_com TPDO communication parameters OD entry
43 ** @param TPDO_map TPDO mapping parameters OD entry
48 UNS8 buildPDO(CO_Data* d, UNS8 numPdo, Message *pdo)
50 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo;
51 const indextable* TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo;
55 const UNS8* pMappingCount = (UNS8*) TPDO_map->pSubindex[0].pObject;
57 pdo->cob_id.w = *(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF;
58 pdo->rtr = NOT_A_REQUEST;
60 MSG_WAR(0x3009, " PDO CobId is : ", *(UNS32*)TPDO_com->pSubindex[1].pObject);
61 MSG_WAR(0x300D, " Number of objects mapped : ",*pMappingCount );
64 UNS8 dataType; /* Unused */
65 UNS8 tmp[]= {0,0,0,0,0,0,0,0}; /* temporary space to hold bits */
67 /* pointer fo the var which holds the mapping parameter of an mapping entry */
68 UNS32* pMappingParameter = (UNS32*) TPDO_map->pSubindex[prp_j + 1].pObject;
69 UNS16 index = (UNS16)((*pMappingParameter) >> 16);
70 UNS8 Size = (UNS8)(*pMappingParameter); /* Size in bits */
71 UNS8 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
72 UNS8 subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
74 MSG_WAR(0x300F, " got mapping parameter : ", *pMappingParameter);
75 MSG_WAR(0x3050, " at index : ", TPDO_map->index);
76 MSG_WAR(0x3051, " sub-index : ", prp_j + 1);
78 if( getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 ) != OD_SUCCESSFUL ){
79 MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
82 /* copy bit per bit in little endian*/
83 CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&pdo->data[offset>>3], offset%8, 0);
87 }while( prp_j < *pMappingCount );
89 pdo->len = 1 + ((offset - 1) >> 3);
91 MSG_WAR(0x3015, " End scan mapped variable", 0);
104 UNS8 sendPDOrequest( CO_Data* d, UNS16 RPDOIndex )
107 UNS16 offset = d->firstIndex->PDO_RCV;
108 UNS16 lastIndex = d->lastIndex->PDO_RCV;
110 /* Sending the request only if the cobid have been found on the PDO
112 /* part dictionary */
114 MSG_WAR(0x3930, "sendPDOrequest RPDO Index : ",RPDOIndex);
116 if (offset && RPDOIndex >= 0x1400){
117 offset += RPDOIndex - 0x1400;
118 if (offset <= lastIndex) {
120 pwCobId = (UNS32*) d->objdict[offset].pSubindex[1].pObject;
122 MSG_WAR(0x3930, "sendPDOrequest cobId is : ",*pwCobId);
125 pdo.cob_id.w = *pwCobId;
128 return canSend(d->canHandle,&pdo);
132 MSG_ERR(0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex);
145 UNS8 proceedPDO(CO_Data* d, Message *m)
148 UNS8 numMap; /* Number of the mapped varable */
150 UNS8 * pMappingCount = NULL; /* count of mapped objects... */
151 /* pointer to the var which is mapped to a pdo... */
152 /* void * pMappedAppObject = NULL; */
153 /* pointer fo the var which holds the mapping parameter of an
155 UNS32 * pMappingParameter = NULL;
156 UNS8 * pTransmissionType = NULL; /* pointer to the transmission
158 UNS32 * pwCobId = NULL;
169 MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
173 if((*m).rtr == NOT_A_REQUEST ) { /* The PDO received is not a
176 offsetObjdict = d->firstIndex->PDO_RCV;
177 lastIndex = d->lastIndex->PDO_RCV;
179 /* study of all the PDO stored in the dictionary */
181 while (offsetObjdict <= lastIndex) {
186 /* get CobId of the dictionary correspondant to the received
188 pwCobId = (UNS32*) d->objdict[offsetObjdict].pSubindex[1].pObject;
189 /* check the CobId coherance */
190 /*pwCobId is the cobId read in the dictionary at the state 3
192 if ( *pwCobId == (*m).cob_id.w ){
193 /* The cobId is recognized */
195 MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
199 /* cobId received does not match with those write in the
207 case state4:/* Get Mapped Objects Number */
208 /* The cobId of the message received has been found in the
210 offsetObjdict = d->firstIndex->PDO_RCV_MAP;
211 lastIndex = d->lastIndex->PDO_RCV_MAP;
212 pMappingCount = (UNS8*) (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
214 while (numMap < *pMappingCount) {
215 UNS8 tmp[]= {0,0,0,0,0,0,0,0};
217 pMappingParameter = (UNS32*) (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
218 if (pMappingParameter == NULL) {
219 MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
222 /* Get the addresse of the mapped variable. */
223 /* detail of *pMappingParameter : */
224 /* The 16 hight bits contains the index, the medium 8 bits
225 contains the subindex, */
226 /* and the lower 8 bits contains the size of the mapped
229 Size = (UNS8)(*pMappingParameter);
231 /* copy bit per bit in little endian */
232 CopyBits(Size, (UNS8*)&m->data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
234 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
237 objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
238 (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
241 if(objDict != OD_SUCCESSFUL) {
242 MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
243 MSG_WAR(0x2939, " Mapped at index : ", (*pMappingParameter) >> 16);
244 MSG_WAR(0x2940, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
248 MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
249 MSG_WAR(0x3943, " Mapped at index : ", (*pMappingParameter) >> 16);
250 MSG_WAR(0x3944, " subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
251 /* MSG_WAR(0x3945, " data : ",*((UNS32*)pMappedAppObject)); */
254 /*TODO : check that offset is not not greater that message size (in bit) */
255 } /* end loop while on mapped variables */
261 }/* end switch status*/
263 }/* end if Donnees */
264 else if ((*m).rtr == REQUEST ){
265 MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
267 offsetObjdict = d->firstIndex->PDO_TRS;
268 lastIndex = d->lastIndex->PDO_TRS;
269 if(offsetObjdict) while( offsetObjdict <= lastIndex ){
270 /* study of all PDO stored in the objects dictionary */
274 case state1:/* check the CobId */
275 /* get CobId of the dictionary which match to the received PDO
277 pwCobId = (UNS32*) (d->objdict + offsetObjdict)->pSubindex[1].pObject;
278 if ( *pwCobId == (*m).cob_id.w ) {
290 case state4:/* check transmission type */
291 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
292 /* If PDO is to be sampled and send on RTR, do it*/
293 if ( (*pTransmissionType == TRANS_RTR)) {
296 /* RTR_SYNC mean data is prepared at SYNC, and transmitted on RTR */
297 }else if ((*pTransmissionType == TRANS_RTR_SYNC )) {
298 if(d->PDO_status[numPdo].transmit_type_parameter & PDO_RTR_SYNC_READY){
299 /*Data ready, just send*/
300 canSend(d->canHandle,&d->PDO_status[numPdo].last_message);
303 /* if SYNC did never occur, force emission with current data */
304 /* DS301 do not tell what to do in such a case...*/
305 MSG_ERR(0x1947, "Not ready RTR_SYNC TPDO send current data : ", m->cob_id.w);
310 (*pTransmissionType == TRANS_EVENT_PROFILE) ||
311 (*pTransmissionType == TRANS_EVENT_SPECIFIC) ) {
312 /* Zap all timers and inhibit flag */
313 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
314 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
315 d->PDO_status[numPdo].transmit_type_parameter &= ~PDO_INHIBITED;
316 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
317 PDOEventTimerAlarm(d, numPdo);
320 /* The requested PDO is not to send on request. So, does
322 MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
326 case state5:/* build and send requested PDO */
329 if( buildPDO(d, numPdo, &pdo))
331 MSG_ERR(0x1948, " Couldn't build TPDO n°", numPdo);
334 canSend(d->canHandle,&pdo);
337 }/* end switch status */
339 }/* end if Requete */
348 ** @param SrcByteIndex
349 ** @param SrcBitIndex
350 ** @param SrcBigEndian
351 ** @param DestByteIndex
352 ** @param DestBitIndex
353 ** @param DestBigEndian
355 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
357 /* This loop copy as many bits that it can each time, crossing*/
358 /* successively bytes*/
359 // boundaries from LSB to MSB.
362 /* Bit missalignement between src and dest*/
363 INTEGER8 Vect = DestBitIndex - SrcBitIndex;
365 /* We can now get src and align it to dest*/
366 UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
368 /* Compute the nb of bit we will be able to copy*/
369 UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex : 8 - SrcBitIndex );
370 UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
372 /* Create a mask that will serve in:*/
373 UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
376 UNS8 Filtered = Aligned & ~Mask;
378 /* - and erase bits where we write, preserve where we don't*/
379 *DestByteIndex &= Mask;
382 *DestByteIndex |= Filtered ;
384 /*Compute next time cursors for src*/
385 if((SrcBitIndex += BitsToCopy)>7)/* cross boundary ?*/
387 SrcBitIndex = 0;/* First bit*/
388 SrcByteIndex += (SrcBigEndian ? -1 : 1);/* Next byte*/
392 /*Compute next time cursors for dest*/
393 if((DestBitIndex += BitsToCopy)>7)
395 DestBitIndex = 0;/* First bit*/
396 DestByteIndex += (DestBigEndian ? -1 : 1);/* Next byte*/
399 /*And decrement counter.*/
400 NbBits -= BitsToCopy;
412 UNS8 sendPDOevent( CO_Data* d)
414 /* Calls _sendPDOevent specifying it is not a sync event */
415 return _sendPDOevent(d, 0);
419 void PDOEventTimerAlarm(CO_Data* d, UNS32 pdoNum)
421 /* This is needed to avoid deletion of re-attribuated timer */
422 d->PDO_status[pdoNum].event_timer = TIMER_NONE;
423 /* force emission of PDO by artificially changing last emitted*/
424 d->PDO_status[pdoNum].last_message.cob_id.w = 0;
425 _sendPDOevent( d, 0 ); /* not a Sync Event*/
428 void PDOInhibitTimerAlarm(CO_Data* d, UNS32 pdoNum)
430 /* This is needed to avoid deletion of re-attribuated timer */
431 d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE;
432 /* Remove inhibit flag */
433 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED;
434 _sendPDOevent( d, 0 ); /* not a Sync Event*/
441 ** @param isSyncEvent
446 UNS8 _sendPDOevent( CO_Data* d, UNS8 isSyncEvent )
448 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
449 UNS8* pTransmissionType = NULL;
450 UNS8 status = state3;
451 UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
452 UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
453 UNS16 lastIndex = d->lastIndex->PDO_TRS;
455 /* study all PDO stored in the objects dictionary */
457 Message pdo = Message_Initializer;
458 while( offsetObjdict <= lastIndex) {
461 if (/*d->objdict[offsetObjdict].bSubCount < 5 || not necessary with objdictedit (always 5)*/
462 /* check if TPDO is not valid */
463 *(UNS32*)d->objdict[offsetObjdict].pSubindex[0].pObject & 0x8000) {
464 MSG_WAR(0x3960, "Not a valid PDO ", 0x1800 + pdoNum);
469 /* get the PDO transmission type */
470 pTransmissionType = (UNS8*) d->objdict[offsetObjdict].pSubindex[2].pObject;
471 MSG_WAR(0x3962, "Reading PDO at index : ", 0x1800 + pdoNum);
473 /* check if transmission type is SYNCRONOUS */
474 /* The message is transmited every n SYNC with n=TransmissionType */
476 (*pTransmissionType >= TRANS_SYNC_MIN) &&
477 (*pTransmissionType <= TRANS_SYNC_MAX) &&
478 (++d->PDO_status[pdoNum].transmit_type_parameter == *pTransmissionType) ) {
479 /*Reset count of SYNC*/
480 d->PDO_status[pdoNum].transmit_type_parameter = 0;
481 MSG_WAR(0x3964, " PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
483 Message msg_init = Message_Initializer;
486 if(buildPDO(d, pdoNum, &pdo))
488 MSG_ERR(0x1906, " Couldn't build TPDO number : ", pdoNum);
493 /* If transmission RTR, with data sampled on SYNC */
494 }else if( isSyncEvent &&
495 (*pTransmissionType == TRANS_RTR_SYNC)) {
496 if(buildPDO(d, pdoNum, &d->PDO_status[pdoNum].last_message))
498 MSG_ERR(0x1966, " Couldn't build TPDO number : ", pdoNum);
499 d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_RTR_SYNC_READY;
501 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_RTR_SYNC_READY;
505 /* If transmission on Event and not inhibited, check for changes */
506 }else if((*pTransmissionType == TRANS_EVENT_PROFILE ||
507 *pTransmissionType == TRANS_EVENT_SPECIFIC )&&
508 !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) {
509 MSG_WAR(0x3968, " PDO is on EVENT. Trans type : ", *pTransmissionType);
511 Message msg_init = Message_Initializer;
514 if(buildPDO(d, pdoNum, &pdo))
516 MSG_ERR(0x3907, " Couldn't build TPDO number : ", pdoNum);
521 /*Compare new and old PDO*/
522 if(d->PDO_status[pdoNum].last_message.cob_id.w == pdo.cob_id.w &&
523 d->PDO_status[pdoNum].last_message.len == pdo.len &&
524 *(UNS64*)(&d->PDO_status[pdoNum].last_message.data[0]) == *(UNS64*)(&pdo.data[0])){
525 /* No changes -> go to next pdo*/
528 MSG_WAR(0x306A, "Changes TPDO number : ", pdoNum);
529 /* Changes detected -> transmit message */
530 UNS16 EventTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[5].pObject;
531 UNS16 InhibitTimerDuration = *(UNS16*)d->objdict[offsetObjdict].pSubindex[3].pObject;
535 /* Start both event_timer and inhibit_timer*/
536 if(EventTimerDuration){
537 DelAlarm(d->PDO_status[pdoNum].event_timer);
538 d->PDO_status[pdoNum].event_timer = SetAlarm(d, pdoNum, &PDOEventTimerAlarm, MS_TO_TIMEVAL(EventTimerDuration), 0);
541 if(InhibitTimerDuration){
542 DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
543 d->PDO_status[pdoNum].inhibit_timer = SetAlarm(d, pdoNum, &PDOInhibitTimerAlarm, US_TO_TIMEVAL(InhibitTimerDuration * 100), 0);
544 /* and inhibit TPDO */
545 d->PDO_status[pdoNum].transmit_type_parameter |= PDO_INHIBITED;
550 MSG_WAR(0x306C, " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", *pTransmissionType);
554 case state5: /*Send the pdo*/
555 /*store_as_last_message*/
556 d->PDO_status[pdoNum].last_message = pdo;
557 MSG_WAR(0x396D, "sendPDO cobId :", pdo.cob_id.w);
558 MSG_WAR(0x396E, " Nb octets : ", pdo.len);
560 canSend(d->canHandle,&pdo);
563 case state11: /*Go to next TPDO*/
567 MSG_WAR(0x3970, "next pdo index : ", pdoNum);
572 MSG_ERR(0x1972,"Unknown state has been reached : %d",status);
574 }/* end switch case */
590 UNS32 TPDO_Communication_Parameter_Callback(CO_Data* d, const indextable * OD_entry, UNS8 bSubindex)
592 /* If PDO are actives */
593 if(d->CurrentCommunicationState.csPDO) switch(bSubindex)
595 case 2: /* Changed transmition type */
596 case 3: /* Changed inhibit time */
597 case 5: /* Changed event time */
599 UNS8 pTransmissionType = *(UNS8*) OD_entry->pSubindex[2].pObject;
600 const indextable* TPDO_com = d->objdict + d->firstIndex->PDO_TRS;
601 UNS8 numPdo = OD_entry - TPDO_com; /* number of the actual processed pdo-nr. */
603 /* Zap all timers and inhibit flag */
604 d->PDO_status[numPdo].event_timer = DelAlarm(d->PDO_status[numPdo].event_timer);
605 d->PDO_status[numPdo].inhibit_timer = DelAlarm(d->PDO_status[numPdo].inhibit_timer);
606 d->PDO_status[numPdo].transmit_type_parameter = 0;
607 /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */
608 PDOEventTimerAlarm(d, numPdo);
612 default: /* other subindex are ignored*/
618 void PDOInit(CO_Data* d)
620 /* For each TPDO mapping parameters */
621 UNS16 pdoIndex = 0x1800; /* OD index of TDPO */
623 UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
624 UNS16 lastIndex = d->lastIndex->PDO_TRS;
625 if(offsetObjdict) while( offsetObjdict <= lastIndex) {
626 /* Assign callbacks to sensible TPDO mapping subindexes */
628 ODCallback_t *CallbackList;
629 /* Find callback list */
630 scanIndexOD (d, pdoIndex, &errorCode, &CallbackList);
631 if(errorCode == OD_SUCCESSFUL && CallbackList)
633 /*Assign callbacks to corresponding subindex*/
634 /* Transmission type */
635 CallbackList[2] = &TPDO_Communication_Parameter_Callback;
637 CallbackList[3] = &TPDO_Communication_Parameter_Callback;
639 CallbackList[5] = &TPDO_Communication_Parameter_Callback;
645 /* Trigger a non-sync event */
646 _sendPDOevent( d, 0 );
649 void PDOStop(CO_Data* d)
651 /* For each TPDO mapping parameters */
652 UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */
653 UNS16 offsetObjdict = d->firstIndex->PDO_TRS;
654 UNS16 lastIndex = d->lastIndex->PDO_TRS;
655 if(offsetObjdict) while( offsetObjdict <= lastIndex) {
656 /* Delete TPDO timers */
657 d->PDO_status[pdoNum].event_timer = DelAlarm(d->PDO_status[pdoNum].event_timer);
658 d->PDO_status[pdoNum].inhibit_timer = DelAlarm(d->PDO_status[pdoNum].inhibit_timer);
659 /* Reset transmit type parameter */
660 d->PDO_status[pdoNum].transmit_type_parameter = 0;
661 d->PDO_status[pdoNum].last_message.cob_id.w = 0;