sync.c

Go to the documentation of this file.
00001 /*
00002 This file is part of CanFestival, a library implementing CanOpen Stack. 
00003 
00004 Copyright (C): Edouard TISSERANT and Francis DUPIN
00005 
00006 See COPYING file for copyrights details.
00007 
00008 This library is free software; you can redistribute it and/or
00009 modify it under the terms of the GNU Lesser General Public
00010 License as published by the Free Software Foundation; either
00011 version 2.1 of the License, or (at your option) any later version.
00012 
00013 This library is distributed in the hope that it will be useful,
00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 Lesser General Public License for more details.
00017 
00018 You should have received a copy of the GNU Lesser General Public
00019 License along with this library; if not, write to the Free Software
00020 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021 */
00022 
00033 #include "data.h"
00034 #include "sync.h"
00035 #include "canfestival.h"
00036 
00037 /* Prototypes for internals functions */
00038 
00045 void SyncAlarm(CO_Data* d, UNS32 id);
00046 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 
00047         UNS8 unsused_bSubindex);
00048 
00055 void SyncAlarm(CO_Data* d, UNS32 id)
00056 {
00057         sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
00058 }
00059 
00069 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
00070 {
00071         startSYNC(d);
00072         return 0;
00073 }
00074 
00080 void startSYNC(CO_Data* d)
00081 {
00082         RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
00083         RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
00084 
00085         if(d->syncTimer != TIMER_NONE){
00086                 stopSYNC(d);
00087         }
00088         
00089         if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)
00090         {
00091                 d->syncTimer = SetAlarm(
00092                                 d,
00093                                 0 /*No id needed*/,
00094                                 &SyncAlarm,
00095                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period), 
00096                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period));
00097         }
00098 }
00099 
00105 void stopSYNC(CO_Data* d)
00106 {
00107         d->syncTimer = DelAlarm(d->syncTimer);
00108 }
00109 
00118 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
00119 {
00120   Message m;
00121   UNS8 resultat ;
00122   
00123   MSG_WAR(0x3001, "sendSYNC ", 0);
00124   
00125   m.cob_id.w = cob_id ;
00126   m.rtr = NOT_A_REQUEST;
00127   m.len = 0;
00128   resultat = canSend(d->canHandle,&m) ;
00129   proceedSYNC(d, &m) ; 
00130   return resultat ;
00131 }
00132 
00141 UNS8 proceedSYNC(CO_Data* d, Message *m)
00142 {
00143 
00144   UNS8  pdoNum,       /* number of the actual processed pdo-nr. */
00145         prp_j;
00146 
00147   const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/
00148   /* pointer to the var which is mapped to a pdo */
00149   /* void *     pMappedAppObject = NULL; */
00150   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
00151   UNS32 *    pMappingParameter = NULL;  
00152   /* pointer to the transmissiontype...*/
00153   UNS8 *     pTransmissionType = NULL;  
00154   UNS32 *    pwCobId = NULL;    
00155 
00156   UNS8 dataType;
00157   UNS16 index;
00158   UNS8 subIndex;
00159   UNS8 offset;
00160   UNS8 status;
00161   UNS8 Size;
00162   UNS32 objDict;        
00163   UNS16 offsetObjdict;
00164   UNS16 offsetObjdictMap;
00165   UNS16 lastIndex;
00166   
00167   status = state3;
00168   pdoNum = 0x00;
00169   prp_j = 0x00;
00170   offset = 0x00;
00171   
00172   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
00173   
00174   (*d->post_sync)();
00175 
00176   /* only operational state allows PDO transmission */
00177   if( d->nodeState != Operational ) 
00178     return 0;
00179   
00180   /* So, the node is in operational state */
00181   /* study all PDO stored in the objects dictionary */  
00182  
00183   offsetObjdict = d->firstIndex->PDO_TRS;
00184   lastIndex = d->lastIndex->PDO_TRS;
00185   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
00186   
00187   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  
00188     switch( status ) {
00189                     
00190     case state3:    /* get the PDO transmission type */
00191       if (d->objdict[offsetObjdict].bSubCount <= 2) {
00192           MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);
00193           return 0xFF;
00194         }
00195       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    
00196       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
00197       status = state4; 
00198       break;     
00199     case state4:        /* check if transmission type is after (this) SYNC */
00200                         /* The message may not be transmited every SYNC but every n SYNC */      
00201       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
00202           (++d->count_sync[pdoNum] == *pTransmissionType) ) {   
00203         d->count_sync[pdoNum] = 0;
00204         MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
00205         status = state5;
00206         break;
00207       }
00208       else {
00209         MSG_WAR(0x3008, "  Not on synchro or not at this SYNC. Trans type : ", 
00210                 *pTransmissionType);
00211         pdoNum++;
00212         offsetObjdict++;
00213         offsetObjdictMap++;
00214         status = state11;
00215         break;
00216       }      
00217     case state5:        /* get PDO CobId */
00218         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     
00219         MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);
00220         status = state7;
00221         break;     
00222     case state7:  /* get mapped objects number to transmit with this PDO */
00223       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
00224         MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
00225         status = state8;
00226     case state8:        /* get mapping parameters */
00227       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
00228         MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
00229         MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);
00230         MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
00231         status = state9;
00232     
00233     case state9:        /* get data to transmit */ 
00234         {
00235           UNS8 ByteSize;
00236           UNS8 tmp[]= {0,0,0,0,0,0,0,0};
00237           index = (UNS16)((*pMappingParameter) >> 16);
00238           subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
00239           Size = (UNS8)(*pMappingParameter); /* Size in bits */
00240           ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
00241           objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );
00242           /* copy bit per bit in little endian*/
00243           CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);
00244         }   
00245         if( objDict != OD_SUCCESSFUL ){
00246           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
00247           return 0xFF;
00248         }
00249         
00250         offset += Size ;
00251         d->process_var.count = 1 + ((offset - 1) >> 3);
00252         prp_j++;
00253         status = state10;        
00254         break;                                  
00255       
00256     case state10:       /* loop to get all the data to transmit */
00257       if( prp_j < *pMappingCount ){
00258         MSG_WAR(0x3014, "  next variable mapped : ", prp_j);
00259         status = state8;
00260         break;
00261       }
00262       else {
00263         MSG_WAR(0x3015, "  End scan mapped variable", 0);
00264         PDOmGR( d, *pwCobId );  
00265         MSG_WAR(0x3016, "  End of this pdo. Should have been sent", 0);
00266         pdoNum++;
00267         offsetObjdict++;
00268         offsetObjdictMap++;
00269         offset = 0x00;
00270         prp_j = 0x00;
00271         status = state11;
00272         break;
00273       }
00274       
00275     case state11:     
00276       MSG_WAR(0x3017, "next pdo index : ", pdoNum);
00277       status = state3;
00278       break;
00279       
00280     default:
00281       MSG_ERR(0x1019,"Unknown state has been reached : %d",status);
00282       return 0xFF;
00283     }/* end switch case */
00284     
00285   }/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */
00286    
00287   (*d->post_TPDO)();
00288 
00289   return 0;
00290 }
00291 
00292 
00293 void _post_sync(){}
00294 void _post_TPDO(){}

Generated on Fri Jun 8 08:51:39 2007 for CanFestival by  doxygen 1.5.1