/home/epimerde/documents/tc11/CanFestival-3/src/pdo.c

Go to the documentation of this file.
00001 /*
00002   This file is part of CanFestival, a library implementing CanOpen
00003   Stack.
00004 
00005   Copyright (C): Edouard TISSERANT and Francis DUPIN
00006 
00007   See COPYING file for copyrights details.
00008 
00009   This library is free software; you can redistribute it and/or
00010   modify it under the terms of the GNU Lesser General Public
00011   License as published by the Free Software Foundation; either
00012   version 2.1 of the License, or (at your option) any later version.
00013 
00014   This library is distributed in the hope that it will be useful,
00015   but WITHOUT ANY WARRANTY; without even the implied warranty of
00016   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017   Lesser General Public License for more details.
00018 
00019   You should have received a copy of the GNU Lesser General Public
00020   License along with this library; if not, write to the Free Software
00021   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
00022   USA
00023 */
00024 #include "pdo.h"
00025 #include "objacces.h"
00026 #include "canfestival.h"
00027 
00037 UNS8 sendPDO(CO_Data* d, s_PDO pdo, UNS8 req)
00038 {
00039   UNS8 i;
00040   if( d->nodeState == Operational ) {
00041     Message m;
00042 
00044     m.cob_id.w = pdo.cobId & 0x7FF; 
00046     if ( req == NOT_A_REQUEST ) {
00047       UNS8 i;
00048       m.rtr = NOT_A_REQUEST;
00049       m.len = pdo.len;
00052       for (i = 0 ; i < pdo.len ; i++)
00053         m.data[i] = pdo.data[i];
00054     }
00055     else {
00056       m.rtr = REQUEST;
00057       m.len = 0;
00058     }
00059 
00060     MSG_WAR(0x3901, "sendPDO cobId :", m.cob_id.w);
00061     MSG_WAR(0x3902,  "     Nb octets  : ",  m.len);
00062     for (i = 0 ; i < m.len ; i++) {
00063       MSG_WAR(0x3903,"           data : ", m.data[i]);
00064     }
00065 
00066     return canSend(d->canHandle,&m);
00067   } 
00068   return 0xFF;
00069 }
00070 
00079 UNS8 PDOmGR(CO_Data* d, UNS32 cobId)
00080 {
00081   UNS8 res;
00082   UNS8 i;
00083   s_PDO pdo;
00084 
00085   MSG_WAR(0x3905, "PDOmGR",0);
00086 
00089   pdo.cobId = cobId;
00090   pdo.len =  d->process_var.count;
00092      /* Ce memcpy devrait tre portable */
00093     for ( i = 0 ; i < pdo.len ; i++) 
00094       pdo.data[i] = d->process_var.data[i];
00095 
00096     res = sendPDO(d, pdo, NOT_A_REQUEST);
00097 
00098     return res;
00099 }
00100 
00101 #if 0
00102 /*********************************************************************/
00103 /* TODO : implement bit mapping                                                  */
00104 /*********************************************************************/
00105 
00106 UNS8 buildPDO(CO_Data* d, UNS16 index)
00107 { /* DO NOT USE MSG_ERR because the macro may send a PDO -> infinite loop if it fails. */       
00108   UNS16 ind;
00109   UNS8      subInd;
00110 
00111   UNS8 *     pMappingCount = NULL;      /* count of mapped objects... */
00112   /* pointer to the var which is mapped to a pdo */
00113 /*  void *     pMappedAppObject = NULL;  */
00114   /* pointer fo the var which holds the mapping parameter of an mapping entry  */ 
00115  
00116   UNS32 *    pMappingParameter = NULL;
00117 
00118   UNS8      Size;
00119   UNS8      dataType;
00120   UNS8      offset;
00121   UNS16     offsetObjdict;
00122   UNS16     offsetObjdictPrm;
00123   UNS32     objDict;
00124 
00125   subInd=(UNS8)0x00;
00126   offset = 0x00;
00127   ind = index - 0x1800;
00128 
00129   MSG_WAR(0x3910,"Prepare PDO to send index :", index);
00130 
00132   if( d->nodeState != Operational ) {
00133     MSG_WAR(0x2911, "Unable to send the PDO (node not in OPERATIONAL mode). Node : ", index);
00134     return 0xFF;
00135   }
00136   offsetObjdictPrm = d->firstIndex->PDO_TRS;
00137   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
00138 
00139   if (offsetObjdictPrm && offsetObjdict)
00140     {
00142       pMappingCount = (d->objdict + offsetObjdict + ind)->pSubindex[0].pObject;
00143       MSG_WAR(0x3912, "Nb maped objects : ",* pMappingCount);
00144       MSG_WAR(0x3913, "        at index : ", 0x1A00 + ind);
00145       while (subInd < *pMappingCount) { 
00147         pMappingParameter = (d->objdict + offsetObjdict + ind)->pSubindex[subInd + 1].pObject;
00148         MSG_WAR(0x3914, "Get the mapping      at index : ", (UNS16)0x1A00 + ind);
00149         MSG_WAR(0x3915, "                     subIndex : ", subInd + 1);
00150         MSG_WAR(0x3916, "                     value    : ", *(UNS32 *)pMappingParameter);
00152         Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
00153         objDict = getODentry(d, (UNS16)((*pMappingParameter) >> 16),
00154                              (UNS8)(((*pMappingParameter) >> 8 ) & 0x000000FF),
00155                              (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
00156 
00157         if (objDict != OD_SUCCESSFUL) {
00158           MSG_WAR(0x2919, "error accessing to the mapped var : ", subInd + 1);
00159           MSG_WAR(0x2920, "         Mapped at index : ", (*pMappingParameter) >> 16);
00160           MSG_WAR(0x2921, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00161           return 0xFF;
00162         }
00163 
00164         offset += Size;
00165         d->process_var.count = offset;
00166         subInd++;
00167       }
00168     }
00169   return 0;
00170 }
00171 #endif
00172 
00181 UNS8 sendPDOrequest( CO_Data* d, UNS32 cobId )
00182 {
00183   UNS32 * pwCobId;
00184   UNS16          offset;
00185   UNS16          lastIndex;
00186   UNS8           err;
00187 
00188   MSG_WAR(0x3930, "sendPDOrequest ",0);
00192   offset = d->firstIndex->PDO_RCV;
00193   lastIndex = d->lastIndex->PDO_RCV;
00194   if (offset)
00195     while (offset <= lastIndex) {
00197       pwCobId = d->objdict[offset].pSubindex[1].pObject;
00198 
00199       if ( *pwCobId  == cobId ) {
00200         s_PDO pdo;
00201         pdo.cobId = *pwCobId;
00202         pdo.len = 0;
00203         err  = sendPDO(d, pdo, REQUEST);
00204         return err;
00205       }
00206       offset++;
00207     }
00208   MSG_WAR(0x1931, "sendPDOrequest : COBID not found : ", cobId);
00209   return 0xFF;
00210 }
00211 
00212 
00221 UNS8 proceedPDO(CO_Data* d, Message *m)
00222 {
00223   UNS8   numPdo;
00224   UNS8   numMap;  
00225   UNS8 i;
00226   UNS8 *     pMappingCount = NULL;    
00231   UNS32 *    pMappingParameter = NULL;
00232   UNS8  *    pTransmissionType = NULL; 
00234   UNS32 *    pwCobId = NULL;
00235   UNS8       Size;
00236   UNS8       dataType;
00237   UNS8       offset;
00238   UNS8       status;
00239   UNS32      objDict;
00240   UNS16      offsetObjdict;
00241   UNS16      lastIndex;
00242   status = state1;
00243 
00244   MSG_WAR(0x3935, "proceedPDO, cobID : ", ((*m).cob_id.w & 0x7ff));
00245   offset = 0x00;
00246   numPdo = 0;
00247   numMap = 0;
00248   if((*m).rtr == NOT_A_REQUEST ) { 
00250     offsetObjdict = d->firstIndex->PDO_RCV;
00251     lastIndex = d->lastIndex->PDO_RCV;
00252 
00254     if(offsetObjdict)
00255       while (offsetObjdict <= lastIndex) {
00256 
00257         switch( status ) {
00258 
00259         case state1:
00262           for ( i = 0 ; i < m->len ; i++)
00263             d->process_var.data[i] = m->data[i];
00264           d->process_var.count = (*m).len;
00265 
00266           status = state2;
00267           break;
00268 
00269         case state2:
00272           pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;
00276           if ( *pwCobId == (*m).cob_id.w ){
00278             status = state4;
00279             MSG_WAR(0x3936, "cobId found at index ", 0x1400 + numPdo);
00280             break;
00281           }
00282           else {
00285             numPdo++;
00286             offsetObjdict++;
00287             status = state2;
00288             break;
00289           }
00290 
00291             case state4:
00294                offsetObjdict = d->firstIndex->PDO_RCV_MAP;
00295              lastIndex = d->lastIndex->PDO_RCV_MAP;
00296              pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
00297              numMap = 0;
00298              while (numMap < *pMappingCount) {
00299                UNS8 tmp[]= {0,0,0,0,0,0,0,0};
00300                UNS8 ByteSize;
00301                pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
00302                if (pMappingParameter == NULL) {
00303                  MSG_ERR(0x1937, "Couldn't get mapping parameter : ", numMap + 1);
00304                  return 0xFF;
00305                }
00313                Size = (UNS8)(*pMappingParameter);
00314 
00316                CopyBits(Size, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0, ((UNS8*)tmp), 0, 0);
00317 
00318                ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 =>
00319                                                    2, ... */
00320 
00321                objDict = setODentry(d, (UNS16)((*pMappingParameter) >> 16),
00322                                     (UNS8)(((*pMappingParameter) >> 8 ) & 0xFF),
00323                                  tmp, &ByteSize, 0 );
00324 
00325                if(objDict != OD_SUCCESSFUL) {
00326                  MSG_ERR(0x1938, "error accessing to the mapped var : ", numMap + 1);
00327                  MSG_WAR(0x2939, "         Mapped at index : ", (*pMappingParameter) >> 16);
00328                  MSG_WAR(0x2940, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00329                  return 0xFF;
00330                }
00331 
00332                MSG_WAR(0x3942, "Variable updated with value received by PDO cobid : ", m->cob_id.w);
00333                MSG_WAR(0x3943, "         Mapped at index : ", (*pMappingParameter) >> 16);
00334                MSG_WAR(0x3944, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00336                offset += Size;
00337                numMap++;
00338              } 
00340              offset=0x00;
00341              numMap = 0;
00342              return 0;
00343 
00344         }
00345       }
00346   }
00347   else if ((*m).rtr == REQUEST ){
00348     MSG_WAR(0x3946, "Receive a PDO request cobId : ", m->cob_id.w);
00349     status = state1;
00350     offsetObjdict = d->firstIndex->PDO_TRS;
00351     lastIndex = d->lastIndex->PDO_TRS;
00352     if(offsetObjdict) while( offsetObjdict  <= lastIndex ){
00355       switch( status ){
00356 
00357       case state1:
00360         pwCobId = (d->objdict + offsetObjdict)->pSubindex[1].pObject;
00361         if ( *pwCobId == (*m).cob_id.w ) {
00362           status = state4;
00363           break;
00364         }
00365         else {
00366           numPdo++;
00367           offsetObjdict++;
00368         }
00369         status = state1;
00370         break;
00371 
00372 
00373       case state4:
00374         pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;
00375         if ( (*pTransmissionType == TRANS_RTR) || (*pTransmissionType == TRANS_RTR_SYNC ) || (*pTransmissionType == TRANS_EVENT) ) {
00376           status = state5;
00377           break;
00378         }
00379         else {
00382           MSG_WAR(0x2947, "PDO is not to send on request : ", m->cob_id.w);
00383           return 0xFF;
00384         }
00385 
00386       case state5:
00387         offsetObjdict = d->firstIndex->PDO_TRS_MAP;
00388         lastIndex = d->lastIndex->PDO_TRS_MAP;
00389         pMappingCount = (d->objdict + offsetObjdict + numPdo)->pSubindex[0].pObject;
00390         numMap = 0;
00391         while (numMap < *pMappingCount) {
00392           pMappingParameter = (d->objdict + offsetObjdict + numPdo)->pSubindex[numMap + 1].pObject;
00394           Size = ((UNS8)(((*pMappingParameter) & 0xFF) >> 3));
00395           objDict = getODentry( d, (UNS16)((*pMappingParameter) >> (UNS8)16),
00396                                 (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & 0xFF),
00397                                 (void *)&d->process_var.data[offset], &Size, &dataType, 0 );
00398           if (objDict != OD_SUCCESSFUL) {
00399             MSG_ERR(0x1948, "error accessing to the mapped var : ", numMap + 1);
00400             MSG_WAR(0x2949, "         Mapped at index : ", (*pMappingParameter) >> 16);
00401             MSG_WAR(0x2950, "                subindex : ", ((*pMappingParameter) >> 8 ) & 0xFF);
00402             return 0xFF;
00403           }
00404           offset += (UNS8) (((*pMappingParameter) & 0xFF) >> 3);
00405           d->process_var.count = offset;
00406           numMap++;
00407 
00408         } 
00409         PDOmGR( d, *pwCobId ); 
00410         return 0;
00411 
00412       }
00413     }
00414   }
00416   return 0;
00417 }
00418 
00430 void CopyBits(UNS8 NbBits, UNS8* SrcByteIndex, UNS8 SrcBitIndex, UNS8 SrcBigEndian, UNS8* DestByteIndex, UNS8 DestBitIndex, UNS8 DestBigEndian)
00431 {
00434   // boundaries from LSB to MSB.
00435   while(NbBits > 0)
00436     {
00438       INTEGER8 Vect = DestBitIndex - SrcBitIndex;
00439 
00441       UNS8 Aligned = Vect>0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect;
00442 
00444       UNS8 BoudaryLimit = (Vect>0 ? 8 - DestBitIndex :  8 - SrcBitIndex );
00445       UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit;
00446 
00448       UNS8 Mask = ((0xff << (DestBitIndex + BitsToCopy)) | (0xff >> (8 - DestBitIndex)));
00449 
00451       UNS8 Filtered = Aligned & ~Mask;
00452 
00454       *DestByteIndex &= Mask;
00455 
00457       *DestByteIndex |= Filtered ;
00458 
00460       if((SrcBitIndex += BitsToCopy)>7)
00461         {
00462           SrcBitIndex = 0;
00463           SrcByteIndex += (SrcBigEndian ? -1 : 1);
00464         }
00465 
00466 
00468       if((DestBitIndex += BitsToCopy)>7)
00469         {
00470           DestBitIndex = 0;
00471           DestByteIndex += (DestBigEndian ? -1 : 1);
00472         }
00473 
00475       NbBits -= BitsToCopy;
00476     }
00477 
00478 }
00479 
00480 #if 0
00481 
00482 /*********************************************************************/
00483 /* TODO : reimplement this using CallBacks
00484  */
00485 /*********************************************************************/
00486 
00495 UNS8 sendPDOevent( CO_Data* d, void * variable )
00496 { 
00498   UNS32           objDict = 0;
00499   UNS8            ind, sub_ind;
00500   UNS8            status;
00501   UNS8            offset;
00502   UNS8 *     pMappingCount = NULL;
00503   UNS32 *    pMappingParameter = NULL;
00504   void *     pMappedAppObject = NULL;
00505   UNS8 *     pTransmissionType = NULL; 
00507   UNS32 *    pwCobId = NULL;
00508   UNS8 *     pSize;
00509   UNS8       size;
00510   UNS8       dataType;
00511   UNS16      offsetObjdict;
00512   UNS16      offsetObjdictPrm;
00513   UNS16      lastIndex;
00514   UNS8       numMap;
00515   ind     = 0x00;
00516   sub_ind = 1;
00517   offset  = 0x00;
00518   pSize   = &size;
00519   status  = state1;
00520 
00521 
00525   MSG_WAR (0x3960, "sendPDOevent", 0);
00526   offsetObjdictPrm = d->firstIndex->PDO_TRS;
00527 
00528   offsetObjdict = d->firstIndex->PDO_TRS_MAP;
00529   lastIndex = d->lastIndex->PDO_TRS_MAP;
00530 
00531   if (offsetObjdictPrm && offsetObjdict)
00533     while(offsetObjdict <= lastIndex){
00535       pTransmissionType = d->objdict[offsetObjdictPrm].pSubindex[2].pObject;
00536       if (*pTransmissionType != TRANS_EVENT) {
00537         ind++;
00538         offsetObjdict++;
00539         offsetObjdictPrm++;
00540         continue;
00541       }
00542       pMappingCount = d->objdict[offsetObjdict].pSubindex[0].pObject;
00543       numMap = 1; 
00544       while (numMap <= *pMappingCount) {
00545         pMappingParameter = d->objdict[offsetObjdict].pSubindex[numMap].pObject;
00547         objDict = getODentry( d,
00548                               (UNS16)((*pMappingParameter) >> 16),
00549                               (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF),
00550                               (void * *)&pMappedAppObject, pSize, &dataType, 0 );
00551         if( objDict != OD_SUCCESSFUL ) {
00552           MSG_WAR(0x2961, "Error in dict. at index : ",
00553                   (*pMappingParameter) >> (UNS8)16);
00554 
00555           MSG_WAR(0x2962, "               subindex : ",
00556                   ((*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
00557           return 0xFF;
00558         }
00559         if (pMappedAppObject == variable) { // Variable found !
00560           MSG_WAR(0x3963, "Variable to send found at index : ",
00561                   (*pMappingParameter) >> 16);
00562           MSG_WAR(0x3964, "                       subIndex : ",
00563                   ((*pMappingParameter) >> 8 ) & 0x000000FF);
00564           buildPDO(d, 0x1800 + ind);
00566           pwCobId = d->objdict[offsetObjdictPrm].pSubindex[1].pObject;
00567           PDOmGR( d, *pwCobId ); 
00568           return 0;
00569         }
00570         numMap++;
00571       } 
00572       ind++;
00573       offsetObjdict++;
00574       offsetObjdictPrm++;
00575     } 
00577   MSG_WAR(0x2965, "Variable not found in a PDO to send on event", 0);
00578   return 0xFF;
00579 
00580 }
00581 #endif
00582 
00583 
00584 

Generated on Tue Jun 5 18:32:05 2007 for CanFestival by  doxygen 1.5.1