CanFestival: /home/epimerde/documents/tc11/CanFestival-3/src/sync.c Source 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 
+00023 
+00024 #include "data.h"
+00025 #include "sync.h"
+00026 #include "canfestival.h"
+00027 
+00028 /* Prototypes for internals functions */
+00029 void SyncAlarm(CO_Data* d, UNS32 id);
+00030 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 
+00031         UNS8 unsused_bSubindex);
+00032 
+00033 
+00034 
+00035 /*****************************************************************************/
+00036 void SyncAlarm(CO_Data* d, UNS32 id)
+00037 {
+00038         sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
+00039 }
+00040 
+00041 /*****************************************************************************/
+00042 /* This is called when Index 0x1005 is updated.*/
+00043 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
+00044 {
+00045         startSYNC(d);
+00046         return 0;
+00047 }
+00048 
+00049 /*****************************************************************************/
+00050 void startSYNC(CO_Data* d)
+00051 {
+00052         RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
+00053         RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
+00054 
+00055         if(d->syncTimer != TIMER_NONE){
+00056                 stopSYNC(d);
+00057         }
+00058         
+00059         if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)
+00060         {
+00061                 d->syncTimer = SetAlarm(
+00062                                 d,
+00063                                 0 /*No id needed*/,
+00064                                 &SyncAlarm,
+00065                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period), 
+00066                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period));
+00067         }
+00068 }
+00069 
+00070 /*****************************************************************************/
+00071 void stopSYNC(CO_Data* d)
+00072 {
+00073         d->syncTimer = DelAlarm(d->syncTimer);
+00074 }
+00075 
+00076 /*********************************************************************/
+00077 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
+00078 {
+00079   Message m;
+00080   UNS8 resultat ;
+00081   
+00082   MSG_WAR(0x3001, "sendSYNC ", 0);
+00083   
+00084   m.cob_id.w = cob_id ;
+00085   m.rtr = NOT_A_REQUEST;
+00086   m.len = 0;
+00087   resultat = canSend(d->canHandle,&m) ;
+00088   proceedSYNC(d, &m) ; 
+00089   return resultat ;
+00090 }
+00091 
+00092 /*****************************************************************************/
+00093 UNS8 proceedSYNC(CO_Data* d, Message *m)
+00094 {
+00095 
+00096   UNS8  pdoNum,       /* number of the actual processed pdo-nr. */
+00097         prp_j;
+00098 
+00099   const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/
+00100   /* pointer to the var which is mapped to a pdo */
+00101   /* void *     pMappedAppObject = NULL; */
+00102   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
+00103   UNS32 *    pMappingParameter = NULL;  
+00104   /* pointer to the transmissiontype...*/
+00105   UNS8 *     pTransmissionType = NULL;  
+00106   UNS32 *    pwCobId = NULL;    
+00107 
+00108   UNS8 dataType;
+00109   UNS16 index;
+00110   UNS8 subIndex;
+00111   UNS8 offset;
+00112   UNS8 status;
+00113   UNS8 Size;
+00114   UNS32 objDict;        
+00115   UNS16 offsetObjdict;
+00116   UNS16 offsetObjdictMap;
+00117   UNS16 lastIndex;
+00118   
+00119   status = state3;
+00120   pdoNum = 0x00;
+00121   prp_j = 0x00;
+00122   offset = 0x00;
+00123   
+00124   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
+00125   
+00126   (*d->post_sync)();
+00127 
+00128   /* only operational state allows PDO transmission */
+00129   if( d->nodeState != Operational ) 
+00130     return 0;
+00131   
+00132   /* So, the node is in operational state */
+00133   /* study all PDO stored in the objects dictionary */  
+00134  
+00135   offsetObjdict = d->firstIndex->PDO_TRS;
+00136   lastIndex = d->lastIndex->PDO_TRS;
+00137   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
+00138   
+00139   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  
+00140     switch( status ) {
+00141                     
+00142     case state3:    /* get the PDO transmission type */
+00143       if (d->objdict[offsetObjdict].bSubCount <= 2) {
+00144           MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);
+00145           return 0xFF;
+00146         }
+00147       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    
+00148       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
+00149       status = state4; 
+00150       break;     
+00151     case state4:        /* check if transmission type is after (this) SYNC */
+00152                         /* The message may not be transmited every SYNC but every n SYNC */      
+00153       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
+00154           (++d->count_sync[pdoNum] == *pTransmissionType) ) {   
+00155         d->count_sync[pdoNum] = 0;
+00156         MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
+00157         status = state5;
+00158         break;
+00159       }
+00160       else {
+00161         MSG_WAR(0x3008, "  Not on synchro or not at this SYNC. Trans type : ", 
+00162                 *pTransmissionType);
+00163         pdoNum++;
+00164         offsetObjdict++;
+00165         offsetObjdictMap++;
+00166         status = state11;
+00167         break;
+00168       }      
+00169     case state5:        /* get PDO CobId */
+00170         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     
+00171         MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);
+00172         status = state7;
+00173         break;     
+00174     case state7:  /* get mapped objects number to transmit with this PDO */
+00175       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
+00176         MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
+00177         status = state8;
+00178     case state8:        /* get mapping parameters */
+00179       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
+00180         MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
+00181         MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);
+00182         MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
+00183         status = state9;
+00184     
+00185     case state9:        /* get data to transmit */ 
+00186         {
+00187           UNS8 ByteSize;
+00188           UNS8 tmp[]= {0,0,0,0,0,0,0,0};
+00189           index = (UNS16)((*pMappingParameter) >> 16);
+00190           subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
+00191           Size = (UNS8)(*pMappingParameter); /* Size in bits */
+00192           ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
+00193           objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );
+00194           /* copy bit per bit in little endian*/
+00195           CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);
+00196         }   
+00197         if( objDict != OD_SUCCESSFUL ){
+00198           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
+00199           return 0xFF;
+00200         }
+00201         
+00202         offset += Size ;
+00203         d->process_var.count = 1 + ((offset - 1) >> 3);
+00204         prp_j++;
+00205         status = state10;        
+00206         break;                                  
+00207       
+00208     case state10:       /* loop to get all the data to transmit */
+00209       if( prp_j < *pMappingCount ){
+00210         MSG_WAR(0x3014, "  next variable mapped : ", prp_j);
+00211         status = state8;
+00212         break;
+00213       }
+00214       else {
+00215         MSG_WAR(0x3015, "  End scan mapped variable", 0);
+00216         PDOmGR( d, *pwCobId );  
+00217         MSG_WAR(0x3016, "  End of this pdo. Should have been sent", 0);
+00218         pdoNum++;
+00219         offsetObjdict++;
+00220         offsetObjdictMap++;
+00221         offset = 0x00;
+00222         prp_j = 0x00;
+00223         status = state11;
+00224         break;
+00225       }
+00226       
+00227     case state11:     
+00228       MSG_WAR(0x3017, "next pdo index : ", pdoNum);
+00229       status = state3;
+00230       break;
+00231       
+00232     default:
+00233       MSG_ERR(0x1019,"Unknown state has been reached : %d",status);
+00234       return 0xFF;
+00235     }/* end switch case */
+00236     
+00237   }/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */
+00238    
+00239   (*d->post_TPDO)();
+00240 
+00241   return 0;
+00242 }
+00243 
+00244 
+00245 void _post_sync(){}
+00246 void _post_TPDO(){}
+