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@210: 00033 #include "data.h"
nico@210: 00034 #include "sync.h"
nico@210: 00035 #include "canfestival.h"
nico@210: 00036 
nico@210: 00037 /* Prototypes for internals functions */
nico@210: 00038 
nico@210: 00045 void SyncAlarm(CO_Data* d, UNS32 id);
nico@210: 00046 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 
nico@210: 00047         UNS8 unsused_bSubindex);
nico@207: 00048 
nico@210: 00055 void SyncAlarm(CO_Data* d, UNS32 id)
nico@210: 00056 {
nico@210: 00057         sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
nico@210: 00058 }
nico@210: 00059 
nico@210: 00069 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
nico@210: 00070 {
nico@210: 00071         startSYNC(d);
nico@210: 00072         return 0;
nico@210: 00073 }
nico@210: 00074 
nico@210: 00080 void startSYNC(CO_Data* d)
nico@210: 00081 {
nico@210: 00082         RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
nico@210: 00083         RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
nico@210: 00084 
nico@210: 00085         if(d->syncTimer != TIMER_NONE){
nico@210: 00086                 stopSYNC(d);
nico@210: 00087         }
nico@210: 00088         
nico@210: 00089         if(*d->COB_ID_Sync & 0x40000000 && *d->Sync_Cycle_Period)
nico@210: 00090         {
nico@210: 00091                 d->syncTimer = SetAlarm(
nico@210: 00092                                 d,
nico@210: 00093                                 0 /*No id needed*/,
nico@210: 00094                                 &SyncAlarm,
nico@210: 00095                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period), 
nico@210: 00096                                 US_TO_TIMEVAL(*d->Sync_Cycle_Period));
nico@210: 00097         }
nico@210: 00098 }
nico@210: 00099 
nico@210: 00105 void stopSYNC(CO_Data* d)
nico@210: 00106 {
nico@210: 00107         d->syncTimer = DelAlarm(d->syncTimer);
nico@210: 00108 }
nico@210: 00109 
nico@210: 00118 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
nico@210: 00119 {
nico@210: 00120   Message m;
nico@210: 00121   UNS8 resultat ;
nico@210: 00122   
nico@210: 00123   MSG_WAR(0x3001, "sendSYNC ", 0);
nico@210: 00124   
nico@210: 00125   m.cob_id.w = cob_id ;
nico@210: 00126   m.rtr = NOT_A_REQUEST;
nico@210: 00127   m.len = 0;
nico@210: 00128   resultat = canSend(d->canHandle,&m) ;
nico@210: 00129   proceedSYNC(d, &m) ; 
nico@210: 00130   return resultat ;
nico@210: 00131 }
nico@210: 00132 
nico@210: 00141 UNS8 proceedSYNC(CO_Data* d, Message *m)
nico@210: 00142 {
nico@210: 00143 
nico@210: 00144   UNS8  pdoNum,       /* number of the actual processed pdo-nr. */
nico@210: 00145         prp_j;
nico@210: 00146 
nico@210: 00147   const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/
nico@210: 00149   /* void *     pMappedAppObject = NULL; */
nico@210: 00151   UNS32 *    pMappingParameter = NULL;  
nico@210: 00153   UNS8 *     pTransmissionType = NULL;  
nico@210: 00154   UNS32 *    pwCobId = NULL;    
nico@210: 00155 
nico@210: 00156   UNS8 dataType;
nico@210: 00157   UNS16 index;
nico@210: 00158   UNS8 subIndex;
nico@210: 00159   UNS8 offset;
nico@210: 00160   UNS8 status;
nico@210: 00161   UNS8 Size;
nico@210: 00162   UNS32 objDict;        
nico@210: 00163   UNS16 offsetObjdict;
nico@210: 00164   UNS16 offsetObjdictMap;
nico@210: 00165   UNS16 lastIndex;
nico@210: 00166   
nico@210: 00167   status = state3;
nico@210: 00168   pdoNum = 0x00;
nico@210: 00169   prp_j = 0x00;
nico@210: 00170   offset = 0x00;
nico@210: 00171   
nico@210: 00172   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
nico@210: 00173   
nico@210: 00174   (*d->post_sync)();
nico@210: 00175 
nico@210: 00177   if( d->nodeState != Operational ) 
nico@210: 00178     return 0;
nico@210: 00179   
nico@210: 00183   offsetObjdict = d->firstIndex->PDO_TRS;
nico@210: 00184   lastIndex = d->lastIndex->PDO_TRS;
nico@210: 00185   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
nico@210: 00186   
nico@210: 00187   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  
nico@210: 00188     switch( status ) {
nico@210: 00189                     
nico@210: 00190     case state3:    
nico@210: 00191       if (d->objdict[offsetObjdict].bSubCount <= 2) {
nico@210: 00192           MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);
nico@210: 00193           return 0xFF;
nico@210: 00194         }
nico@210: 00195       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    
nico@210: 00196       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
nico@210: 00197       status = state4; 
nico@210: 00198       break;     
nico@210: 00199     case state4:        
nico@210: 00201       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
nico@210: 00202           (++d->count_sync[pdoNum] == *pTransmissionType) ) {   
nico@210: 00203         d->count_sync[pdoNum] = 0;
nico@210: 00204         MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
nico@210: 00205         status = state5;
nico@210: 00206         break;
nico@210: 00207       }
nico@210: 00208       else {
nico@210: 00209         MSG_WAR(0x3008, "  Not on synchro or not at this SYNC. Trans type : ", 
nico@210: 00210                 *pTransmissionType);
nico@210: 00211         pdoNum++;
nico@210: 00212         offsetObjdict++;
nico@210: 00213         offsetObjdictMap++;
nico@210: 00214         status = state11;
nico@210: 00215         break;
nico@210: 00216       }      
nico@210: 00217     case state5:        
nico@210: 00218         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     
nico@210: 00219         MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);
nico@210: 00220         status = state7;
nico@210: 00221         break;     
nico@210: 00222     case state7:  
nico@210: 00223       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
nico@210: 00224         MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
nico@210: 00225         status = state8;
nico@210: 00226     case state8:        
nico@210: 00227       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
nico@210: 00228         MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
nico@210: 00229         MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);
nico@210: 00230         MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
nico@210: 00231         status = state9;
nico@210: 00232     
nico@210: 00233     case state9:        
nico@210: 00234         {
nico@210: 00235           UNS8 ByteSize;
nico@210: 00236           UNS8 tmp[]= {0,0,0,0,0,0,0,0};
nico@210: 00237           index = (UNS16)((*pMappingParameter) >> 16);
nico@210: 00238           subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
nico@210: 00239           Size = (UNS8)(*pMappingParameter); /* Size in bits */
nico@210: 00240           ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
nico@210: 00241           objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );
nico@210: 00243           CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);
nico@210: 00244         }   
nico@210: 00245         if( objDict != OD_SUCCESSFUL ){
nico@210: 00246           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
nico@210: 00247           return 0xFF;
nico@210: 00248         }
nico@210: 00249         
nico@210: 00250         offset += Size ;
nico@210: 00251         d->process_var.count = 1 + ((offset - 1) >> 3);
nico@210: 00252         prp_j++;
nico@210: 00253         status = state10;        
nico@210: 00254         break;                                  
nico@210: 00255       
nico@210: 00256     case state10:       
nico@210: 00257       if( prp_j < *pMappingCount ){
nico@210: 00258         MSG_WAR(0x3014, "  next variable mapped : ", prp_j);
nico@210: 00259         status = state8;
nico@210: 00260         break;
nico@210: 00261       }
nico@210: 00262       else {
nico@210: 00263         MSG_WAR(0x3015, "  End scan mapped variable", 0);
nico@210: 00264         PDOmGR( d, *pwCobId );  
nico@210: 00265         MSG_WAR(0x3016, "  End of this pdo. Should have been sent", 0);
nico@210: 00266         pdoNum++;
nico@210: 00267         offsetObjdict++;
nico@210: 00268         offsetObjdictMap++;
nico@210: 00269         offset = 0x00;
nico@210: 00270         prp_j = 0x00;
nico@210: 00271         status = state11;
nico@210: 00272         break;
nico@210: 00273       }
nico@210: 00274       
nico@210: 00275     case state11:     
nico@210: 00276       MSG_WAR(0x3017, "next pdo index : ", pdoNum);
nico@210: 00277       status = state3;
nico@210: 00278       break;
nico@210: 00279       
nico@210: 00280     default:
nico@210: 00281       MSG_ERR(0x1019,"Unknown state has been reached : %d",status);
nico@210: 00282       return 0xFF;
nico@210: 00283     }/* end switch case */
nico@210: 00284     
nico@210: 00285   }/* end while( prp_i<dict_cstes.max_count_of_PDO_transmit ) */
nico@210: 00286    
nico@210: 00287   (*d->post_TPDO)();
nico@210: 00288 
nico@210: 00289   return 0;
nico@210: 00290 }
nico@210: 00291 
nico@210: 00292 
nico@210: 00293 void _post_sync(){}
nico@210: 00294 void _post_TPDO(){}
nico@210: