laurent@2032: /*
laurent@2043:  * Ethercat CIA402 node execution code
laurent@2032:  *
laurent@2032:  * */
laurent@2032: 
laurent@2032: #include "ecrt.h"
laurent@2032: 
Laurent@2122: #include "beremiz.h"
Laurent@2122: #include "iec_types_all.h"
laurent@2032: 
Laurent@2094: #include "accessor.h"
Laurent@2092: #include "POUS.h"
Laurent@2092: 
laurent@2034: IEC_INT beremiz__IW%(location)s_0;
laurent@2034: IEC_INT *__IW%(location)s_0 = &beremiz__IW%(location)s_0;
laurent@2034: 
laurent@2034: %(MCL_headers)s
laurent@2032: 
Edouard@2059: static IEC_UINT __InactiveMask = 0x4f;
Edouard@2059: static IEC_UINT __ActiveMask = 0x6f;
Edouard@2059: static IEC_UINT __PowerMask = 0x10;
Edouard@2059: static IEC_BOOL __FirstTick = 1;
laurent@2032: 
laurent@2032: typedef enum {
laurent@2032: 	__Unknown,
laurent@2032: 	__NotReadyToSwitchOn,
laurent@2032: 	__SwitchOnDisabled,
laurent@2032: 	__ReadyToSwitchOn,
laurent@2032: 	__SwitchedOn,
laurent@2032: 	__OperationEnabled,
laurent@2032: 	__QuickStopActive,
Laurent@2077:     __FaultReactionActive,
Laurent@2077:     __Fault,
laurent@2043: } __CIA402NodeState;
laurent@2032: 
laurent@2032: typedef struct {
laurent@2032: %(entry_variables)s
Laurent@2077:     __CIA402NodeState state;
Laurent@2077:     axis_s* axis;
laurent@2043: } __CIA402Node;
laurent@2032: 
Laurent@2091: #define AXIS_UNIT_TO_USER_UNIT(param)\
Laurent@2091: (IEC_LREAL)(param) * __CIA402Node_%(location)s.axis->RatioDenominator / __CIA402Node_%(location)s.axis->RatioNumerator
Laurent@2091: #define USER_UNIT_TO_AXIS_UNIT(param)\
Laurent@2091: (IEC_DINT)(param * __CIA402Node_%(location)s.axis->RatioNumerator / __CIA402Node_%(location)s.axis->RatioDenominator)
Laurent@2091: 
laurent@2043: static __CIA402Node __CIA402Node_%(location)s;
laurent@2032: 
laurent@2034: %(extern_located_variables_declaration)s
laurent@2032: 
Laurent@2092: %(fieldbus_interface_declaration)s
Laurent@2092: 
laurent@2032: int __init_%(location)s()
laurent@2032: {
Laurent@2118:     __FirstTick = 1;
laurent@2041: %(init_entry_variables)s
Laurent@2079: 	*(__CIA402Node_%(location)s.ModesOfOperation) = 0x08;
Laurent@2079:     return 0;
laurent@2032: }
laurent@2032: 
laurent@2032: void __cleanup_%(location)s()
laurent@2032: {
laurent@2032: }
laurent@2032: 
laurent@2032: void __retrieve_%(location)s()
laurent@2032: {
laurent@2043: 	IEC_UINT statusword_inactive = *(__CIA402Node_%(location)s.StatusWord) & __InactiveMask;
laurent@2043: 	IEC_UINT statusword_active = *(__CIA402Node_%(location)s.StatusWord) & __ActiveMask;
laurent@2032: 
laurent@2041: 	if (__FirstTick) {
Laurent@2118: 		*__IW%(location)s_0 = __MK_Alloc_AXIS_REF();
Laurent@2118: 		__CIA402Node_%(location)s.axis = __MK_GetPublic_AXIS_REF(*__IW%(location)s_0);
Laurent@2118: 		__CIA402Node_%(location)s.axis->NetworkPosition = %(slave_pos)d;
laurent@2041: %(init_axis_params)s
Laurent@2118: %(fieldbus_interface_definition)s
laurent@2045: 		__FirstTick = 0;
laurent@2041: 	}
laurent@2041: 
laurent@2043: 	// CIA402 node state computation
laurent@2043: 	__CIA402Node_%(location)s.state = __Unknown;
laurent@2032: 	switch (statusword_inactive) {
laurent@2032: 		case 0x00:
laurent@2043: 			__CIA402Node_%(location)s.state = __NotReadyToSwitchOn;
laurent@2032: 			break;
laurent@2032: 		case 0x40:
laurent@2043: 			__CIA402Node_%(location)s.state = __SwitchOnDisabled;
laurent@2032: 			break;
laurent@2032: 		case 0x0f:
laurent@2043: 			__CIA402Node_%(location)s.state = __FaultReactionActive;
laurent@2032: 			break;
laurent@2032: 		case 0x08:
laurent@2043: 			__CIA402Node_%(location)s.state = __Fault;
laurent@2032: 			break;
laurent@2032: 		default:
laurent@2032: 			break;
laurent@2032: 	}
laurent@2032: 	switch (statusword_active) {
laurent@2032: 		case 0x21:
laurent@2043: 			__CIA402Node_%(location)s.state = __ReadyToSwitchOn;
laurent@2032: 			break;
laurent@2032: 		case 0x23:
laurent@2043: 			__CIA402Node_%(location)s.state = __SwitchedOn;
laurent@2032: 			break;
laurent@2032: 		case 0x27:
laurent@2043: 			__CIA402Node_%(location)s.state = __OperationEnabled;
laurent@2032: 			break;
laurent@2032: 		case 0x07:
laurent@2043: 			__CIA402Node_%(location)s.state = __QuickStopActive;
laurent@2032: 			break;
laurent@2032: 		default:
laurent@2032: 			break;
laurent@2032: 	}
laurent@2043: 	if (__CIA402Node_%(location)s.state == __Unknown) {
laurent@2032: 		return;
laurent@2032: 	}
laurent@2032: 
Laurent@2084: 	// Default variables retrieve
Laurent@2096: 	__CIA402Node_%(location)s.axis->CommunicationReady = *(__CIA402Node_%(location)s.StatusWord) != 0;
Laurent@2096: 	__CIA402Node_%(location)s.axis->ReadyForPowerOn = __CIA402Node_%(location)s.state == __SwitchedOn || __OperationEnabled;
laurent@2043: 	__CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled;
Laurent@2091: 	__CIA402Node_%(location)s.axis->ActualPosition = AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition));
Laurent@2091: 	__CIA402Node_%(location)s.axis->ActualVelocity = AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity));
Laurent@2084: 
Laurent@2084: 	// Extra variables retrieve
Laurent@2084: %(extra_variables_retrieve)s
laurent@2032: }
laurent@2032: 
laurent@2032: void __publish_%(location)s()
laurent@2032: {
laurent@2043: 	IEC_BOOL power = ((*(__CIA402Node_%(location)s.StatusWord) & __PowerMask) > 0) && __CIA402Node_%(location)s.axis->Power;
laurent@2034: 
laurent@2043: 	// CIA402 node state transition computation
laurent@2043: 	switch (__CIA402Node_%(location)s.state) {
laurent@2032: 	    case __SwitchOnDisabled:
laurent@2043: 	    	*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x87) | 0x06;
laurent@2032: 	    	break;
laurent@2032: 	    case __ReadyToSwitchOn:
laurent@2034: 	    case __OperationEnabled:
laurent@2034: 	    	if (!power) {
laurent@2043: 	    		*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x07;
laurent@2034: 	    		break;
laurent@2034: 	    	}
laurent@2032: 	    case __SwitchedOn:
laurent@2034: 	    	if (power) {
laurent@2043: 	    	    *(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x0f;
laurent@2034: 	    	}
laurent@2032: 	    	break;
laurent@2032: 	    case __Fault:
laurent@2043: 	    	*(__CIA402Node_%(location)s.ControlWord) = (*(__CIA402Node_%(location)s.ControlWord) & ~0x8f) | 0x80;
laurent@2032: 	    	break;
laurent@2032: 	    default:
laurent@2032: 	    	break;
laurent@2032: 	}
laurent@2032: 
Laurent@2084: 	// Default variables publish
Laurent@2128: 	*(__CIA402Node_%(location)s.TargetPosition) = USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
Laurent@2084: 
Laurent@2084: 	// Extra variables publish
Laurent@2084: %(extra_variables_publish)s
laurent@2032: }