CanFestival: /home/epimerde/documents/tc11/CanFestival-3/src/sync.c Source File

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