src/sync.c
changeset 208 05d95c45b388
parent 204 44ce74232ccb
child 215 f49e5a6b7804
equal deleted inserted replaced
207:b6572d0336c3 208:05d95c45b388
    18 You should have received a copy of the GNU Lesser General Public
    18 You should have received a copy of the GNU Lesser General Public
    19 License along with this library; if not, write to the Free Software
    19 License along with this library; if not, write to the Free Software
    20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
    20 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
    21 */
    21 */
    22 
    22 
       
    23 /*!
       
    24 ** @file   sync.c
       
    25 ** @author Edouard TISSERANT and Francis DUPIN
       
    26 ** @date   Tue Jun  5 09:32:32 2007
       
    27 **
       
    28 ** @brief
       
    29 **
       
    30 **
       
    31 */
    23 
    32 
    24 #include "data.h"
    33 #include "data.h"
    25 #include "sync.h"
    34 #include "sync.h"
    26 #include "canfestival.h"
    35 #include "canfestival.h"
    27 
    36 
    28 /* Prototypes for internals functions */
    37 /* Prototypes for internals functions */
       
    38 
       
    39 /*!                                                                                                
       
    40 **                                                                                                 
       
    41 **                                                                                                 
       
    42 ** @param d                                                                                        
       
    43 ** @param id                                                                                       
       
    44 **/  
    29 void SyncAlarm(CO_Data* d, UNS32 id);
    45 void SyncAlarm(CO_Data* d, UNS32 id);
    30 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 
    46 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, 
    31 	UNS8 unsused_bSubindex);
    47 	UNS8 unsused_bSubindex);
    32 
    48 
    33 
    49 /*!                                                                                                
    34 
    50 **                                                                                                 
    35 /*****************************************************************************/
    51 **                                                                                                 
       
    52 ** @param d                                                                                        
       
    53 ** @param id                                                                                       
       
    54 **/   
    36 void SyncAlarm(CO_Data* d, UNS32 id)
    55 void SyncAlarm(CO_Data* d, UNS32 id)
    37 {
    56 {
    38 	sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
    57 	sendSYNC(d, *d->COB_ID_Sync & 0x1FFFFFFF) ;
    39 }
    58 }
    40 
    59 
    41 /*****************************************************************************/
    60 /*!                                                                                                
    42 /* This is called when Index 0x1005 is updated.*/
    61 ** This is called when Index 0x1005 is updated.                                                                                                
       
    62 **                                                                                                 
       
    63 ** @param d                                                                                        
       
    64 ** @param unsused_indextable                                                                       
       
    65 ** @param unsused_bSubindex                                                                        
       
    66 **                                                                                                 
       
    67 ** @return                                                                                         
       
    68 **/  
    43 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
    69 UNS32 OnCOB_ID_SyncUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
    44 {
    70 {
    45 	startSYNC(d);
    71 	startSYNC(d);
    46 	return 0;
    72 	return 0;
    47 }
    73 }
    48 
    74 
    49 /*****************************************************************************/
    75 /*!                                                                                                
       
    76 **                                                                                                 
       
    77 **                                                                                                 
       
    78 ** @param d                                                                                        
       
    79 **/ 
    50 void startSYNC(CO_Data* d)
    80 void startSYNC(CO_Data* d)
    51 {
    81 {
    52 	RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
    82 	RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
    53 	RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
    83 	RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
    54 
    84 
    65 				US_TO_TIMEVAL(*d->Sync_Cycle_Period), 
    95 				US_TO_TIMEVAL(*d->Sync_Cycle_Period), 
    66 				US_TO_TIMEVAL(*d->Sync_Cycle_Period));
    96 				US_TO_TIMEVAL(*d->Sync_Cycle_Period));
    67 	}
    97 	}
    68 }
    98 }
    69 
    99 
    70 /*****************************************************************************/
   100 /*!                                                                                                
       
   101 **                                                                                                 
       
   102 **                                                                                                 
       
   103 ** @param d                                                                                        
       
   104 **/   
    71 void stopSYNC(CO_Data* d)
   105 void stopSYNC(CO_Data* d)
    72 {
   106 {
    73 	d->syncTimer = DelAlarm(d->syncTimer);
   107 	d->syncTimer = DelAlarm(d->syncTimer);
    74 }
   108 }
    75 
   109 
    76 /*********************************************************************/
   110 /*!                                                                                                
       
   111 **                                                                                                 
       
   112 **                                                                                                 
       
   113 ** @param d                                                                                        
       
   114 ** @param cob_id                                                                                   
       
   115 **                                                                                                 
       
   116 ** @return                                                                                         
       
   117 **/  
    77 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
   118 UNS8 sendSYNC(CO_Data* d, UNS32 cob_id)
    78 {
   119 {
    79   Message m;
   120   Message m;
    80   UNS8 resultat ;
   121   UNS8 resultat ;
    81   
   122   
    87   resultat = canSend(d->canHandle,&m) ;
   128   resultat = canSend(d->canHandle,&m) ;
    88   proceedSYNC(d, &m) ; 
   129   proceedSYNC(d, &m) ; 
    89   return resultat ;
   130   return resultat ;
    90 }
   131 }
    91 
   132 
    92 /*****************************************************************************/
   133 /*!                                                                                                
       
   134 **                                                                                                 
       
   135 **                                                                                                 
       
   136 ** @param d                                                                                        
       
   137 ** @param m                                                                                        
       
   138 **                                                                                                 
       
   139 ** @return                                                                                         
       
   140 **/ 
    93 UNS8 proceedSYNC(CO_Data* d, Message *m)
   141 UNS8 proceedSYNC(CO_Data* d, Message *m)
    94 {
   142 {
    95 
   143 
    96   UNS8 	pdoNum,       /* number of the actual processed pdo-nr. */
   144   UNS8 	pdoNum,       /* number of the actual processed pdo-nr. */
    97         prp_j;
   145         prp_j;
    98 
   146 
    99   const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/
   147   const UNS8 *     pMappingCount = NULL;      /* count of mapped objects...*/
   100   /* pointer to the var which is mapped to a pdo */
   148   /** pointer to the var which is mapped to a pdo */
   101   /* void *     pMappedAppObject = NULL; */
   149   /* void *     pMappedAppObject = NULL; */
   102   /* pointer fo the var which holds the mapping parameter of an mapping entry  */
   150   /** pointer fo the var which holds the mapping parameter of an mapping entry  */
   103   UNS32 *    pMappingParameter = NULL;  
   151   UNS32 *    pMappingParameter = NULL;  
   104   /* pointer to the transmissiontype...*/
   152   /** pointer to the transmissiontype...*/
   105   UNS8 *     pTransmissionType = NULL;  
   153   UNS8 *     pTransmissionType = NULL;  
   106   UNS32 *    pwCobId = NULL;	
   154   UNS32 *    pwCobId = NULL;	
   107 
   155 
   108   UNS8 dataType;
   156   UNS8 dataType;
   109   UNS16 index;
   157   UNS16 index;
   123   
   171   
   124   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
   172   MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
   125   
   173   
   126   (*d->post_sync)();
   174   (*d->post_sync)();
   127 
   175 
   128   /* only operational state allows PDO transmission */
   176   /** only operational state allows PDO transmission */
   129   if( d->nodeState != Operational ) 
   177   if( d->nodeState != Operational ) 
   130     return 0;
   178     return 0;
   131   
   179   
   132   /* So, the node is in operational state */
   180   /** So, the node is in operational state */
   133   /* study all PDO stored in the objects dictionary */	
   181   /** study all PDO stored in the objects dictionary */	
   134  
   182  
   135   offsetObjdict = d->firstIndex->PDO_TRS;
   183   offsetObjdict = d->firstIndex->PDO_TRS;
   136   lastIndex = d->lastIndex->PDO_TRS;
   184   lastIndex = d->lastIndex->PDO_TRS;
   137   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
   185   offsetObjdictMap = d->firstIndex->PDO_TRS_MAP;
   138   
   186   
   139   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  
   187   if(offsetObjdict) while( offsetObjdict <= lastIndex) {  
   140     switch( status ) {
   188     switch( status ) {
   141                     
   189                     
   142     case state3:    /* get the PDO transmission type */
   190     case state3:    /** get the PDO transmission type */
   143       if (d->objdict[offsetObjdict].bSubCount <= 2) {
   191       if (d->objdict[offsetObjdict].bSubCount <= 2) {
   144 	  MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);
   192 	  MSG_ERR(0x1004, "Subindex 2  not found at index ", 0x1800 + pdoNum);
   145 	  return 0xFF;
   193 	  return 0xFF;
   146 	}
   194 	}
   147       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    
   195       pTransmissionType = d->objdict[offsetObjdict].pSubindex[2].pObject;    
   148       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
   196       MSG_WAR(0x3005, "Reading PDO at index : ", 0x1800 + pdoNum);
   149       status = state4; 
   197       status = state4; 
   150       break;     
   198       break;     
   151     case state4:	/* check if transmission type is after (this) SYNC */
   199     case state4:	/** check if transmission type is after (this) SYNC */
   152                         /* The message may not be transmited every SYNC but every n SYNC */      
   200                         /** The message may not be transmited every SYNC but every n SYNC */      
   153       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
   201       if( (*pTransmissionType >= TRANS_SYNC_MIN) && (*pTransmissionType <= TRANS_SYNC_MAX) &&
   154           (++d->count_sync[pdoNum] == *pTransmissionType) ) {	
   202           (++d->count_sync[pdoNum] == *pTransmissionType) ) {	
   155 	d->count_sync[pdoNum] = 0;
   203 	d->count_sync[pdoNum] = 0;
   156 	MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
   204 	MSG_WAR(0x3007, "  PDO is on SYNCHRO. Trans type : ", *pTransmissionType);
   157 	status = state5;
   205 	status = state5;
   164 	offsetObjdict++;
   212 	offsetObjdict++;
   165 	offsetObjdictMap++;
   213 	offsetObjdictMap++;
   166 	status = state11;
   214 	status = state11;
   167 	break;
   215 	break;
   168       }      
   216       }      
   169     case state5:	/* get PDO CobId */
   217     case state5:	/** get PDO CobId */
   170         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     
   218         pwCobId = d->objdict[offsetObjdict].pSubindex[1].pObject;     
   171 	MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);
   219 	MSG_WAR(0x3009, "  PDO CobId is : ", *pwCobId);
   172 	status = state7;
   220 	status = state7;
   173 	break;     
   221 	break;     
   174     case state7:  /* get mapped objects number to transmit with this PDO */
   222     case state7:  /** get mapped objects number to transmit with this PDO */
   175       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
   223       pMappingCount = d->objdict[offsetObjdictMap].pSubindex[0].pObject;
   176 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
   224 	MSG_WAR(0x300D, "  Number of objects mapped : ",*pMappingCount );
   177 	status = state8;
   225 	status = state8;
   178     case state8:	/* get mapping parameters */
   226     case state8:	/** get mapping parameters */
   179       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
   227       pMappingParameter = d->objdict[offsetObjdictMap].pSubindex[prp_j + 1].pObject;
   180 	MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
   228 	MSG_WAR(0x300F, "  got mapping parameter : ", *pMappingParameter);
   181 	MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);
   229 	MSG_WAR(0x3050, "    at index : ", 0x1A00 + pdoNum);
   182 	MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
   230 	MSG_WAR(0x3051, "    sub-index : ", prp_j + 1);
   183 	status = state9;
   231 	status = state9;
   184     
   232     
   185     case state9:	/* get data to transmit */ 
   233     case state9:	/** get data to transmit */ 
   186 	{
   234 	{
   187 	  UNS8 ByteSize;
   235 	  UNS8 ByteSize;
   188 	  UNS8 tmp[]= {0,0,0,0,0,0,0,0};
   236 	  UNS8 tmp[]= {0,0,0,0,0,0,0,0};
   189 	  index = (UNS16)((*pMappingParameter) >> 16);
   237 	  index = (UNS16)((*pMappingParameter) >> 16);
   190           subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
   238           subIndex = (UNS8)(( (*pMappingParameter) >> (UNS8)8 ) & (UNS32)0x000000FF);
   191 	  Size = (UNS8)(*pMappingParameter); /* Size in bits */
   239 	  Size = (UNS8)(*pMappingParameter); /* Size in bits */
   192 	  ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
   240 	  ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */
   193 	  objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );
   241 	  objDict = getODentry(d, index, subIndex, tmp, &ByteSize, &dataType, 0 );
   194 	  /* copy bit per bit in little endian*/
   242 	  /** copy bit per bit in little endian*/
   195 	  CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);
   243 	  CopyBits(Size, ((UNS8*)tmp), 0 , 0, (UNS8*)&d->process_var.data[offset>>3], offset%8, 0);
   196 	}   
   244 	}   
   197         if( objDict != OD_SUCCESSFUL ){
   245         if( objDict != OD_SUCCESSFUL ){
   198           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
   246           MSG_ERR(0x1013, " Couldn't find mapped variable at index-subindex-size : ", (UNS16)(*pMappingParameter));
   199           return 0xFF;
   247           return 0xFF;
   203 	d->process_var.count = 1 + ((offset - 1) >> 3);
   251 	d->process_var.count = 1 + ((offset - 1) >> 3);
   204 	prp_j++;
   252 	prp_j++;
   205 	status = state10;	 
   253 	status = state10;	 
   206 	break;					
   254 	break;					
   207       
   255       
   208     case state10:	/* loop to get all the data to transmit */
   256     case state10:	/** loop to get all the data to transmit */
   209       if( prp_j < *pMappingCount ){
   257       if( prp_j < *pMappingCount ){
   210 	MSG_WAR(0x3014, "  next variable mapped : ", prp_j);
   258 	MSG_WAR(0x3014, "  next variable mapped : ", prp_j);
   211 	status = state8;
   259 	status = state8;
   212 	break;
   260 	break;
   213       }
   261       }