src/states.c
changeset 370 6fecf36df407
parent 365 9b76e0881beb
child 378 d2abf6c8c27b
equal deleted inserted replaced
369:8b67289d50b5 370:6fecf36df407
    35 #include "nmtSlave.h"
    35 #include "nmtSlave.h"
    36 #include "emcy.h"
    36 #include "emcy.h"
    37 #ifdef CO_ENABLE_LSS
    37 #ifdef CO_ENABLE_LSS
    38 #include "lss.h"
    38 #include "lss.h"
    39 #endif
    39 #endif
       
    40 #include "sysdep.h"
       
    41 
    40 /** Prototypes for internals functions */
    42 /** Prototypes for internals functions */
    41 /*!                                                                                                
    43 /*!                                                                                                
    42 **                                                                                                 
    44 **                                                                                                 
    43 **                                                                                                 
    45 **                                                                                                 
    44 ** @param d                                                                                        
    46 ** @param d                                                                                        
    65 ** @param d                                                                                        
    67 ** @param d                                                                                        
    66 ** @param m                                                                                        
    68 ** @param m                                                                                        
    67 **/  
    69 **/  
    68 void canDispatch(CO_Data* d, Message *m)
    70 void canDispatch(CO_Data* d, Message *m)
    69 {
    71 {
    70 	 switch(m->cob_id >> 7)
    72 	UNS16 cob_id = UNS16_LE(m->cob_id);
       
    73 	 switch(cob_id >> 7)
    71 	{
    74 	{
    72 		case SYNC:		/* can be a SYNC or a EMCY message */
    75 		case SYNC:		/* can be a SYNC or a EMCY message */
    73 			if(m->cob_id == 0x080)	/* SYNC */
    76 			if(cob_id == 0x080)	/* SYNC */
    74 			{
    77 			{
    75 				if(d->CurrentCommunicationState.csSYNC)
    78 				if(d->CurrentCommunicationState.csSYNC)
    76 					proceedSYNC(d);
    79 					proceedSYNC(d);
    77 			} else 		/* EMCY */
    80 			} else 		/* EMCY */
    78 				if(d->CurrentCommunicationState.csEmergency)
    81 				if(d->CurrentCommunicationState.csEmergency)
   105 				proceedNMTstateChange(d,m);
   108 				proceedNMTstateChange(d,m);
   106 			}
   109 			}
   107 #ifdef CO_ENABLE_LSS
   110 #ifdef CO_ENABLE_LSS
   108 		case LSS:
   111 		case LSS:
   109 			if (!d->CurrentCommunicationState.csLSS)break;
   112 			if (!d->CurrentCommunicationState.csLSS)break;
   110 			if ((*(d->iam_a_slave)) && m->cob_id==MLSS_ADRESS)
   113 			if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS)
   111 			{
   114 			{
   112 				proceedLSS_Slave(d,m);
   115 				proceedLSS_Slave(d,m);
   113 			}
   116 			}
   114 			else if(!(*(d->iam_a_slave)) && m->cob_id==SLSS_ADRESS)
   117 			else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS)
   115 			{
   118 			{
   116 				proceedLSS_Master(d,m);
   119 				proceedLSS_Master(d,m);
   117 			}
   120 			}
   118 			break;
   121 			break;
   119 #endif
   122 #endif
   268 
   271 
   269   if(offset){
   272   if(offset){
   270     /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/
   273     /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/
   271     if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
   274     if((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
   272       /* cob_id_client = 0x600 + nodeId; */
   275       /* cob_id_client = 0x600 + nodeId; */
   273       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
   276       UNS32 tmp = 0x600 + nodeId;
       
   277       *(UNS32*)d->objdict[offset].pSubindex[1].pObject = UNS32_LE(tmp);
   274     }
   278     }
   275     /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/
   279     /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/
   276     if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
   280     if((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId)||(*d->bDeviceNodeId==0xFF)){
   277       /* cob_id_server = 0x580 + nodeId; */
   281       /* cob_id_server = 0x580 + nodeId; */
   278       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
   282         UNS32 tmp = 0x580 + nodeId;
       
   283       *(UNS32*)d->objdict[offset].pSubindex[2].pObject = UNS32_LE(tmp);
   279     }
   284     }
   280   }
   285   }
   281 
   286 
   282   /* 
   287   /* 
   283    	Initialize the server(s) SDO parameters
   288    	Initialize the server(s) SDO parameters