laurent@2032: /*
laurent@2043:  * Ethercat CIA402 node execution code
laurent@2032:  *
laurent@2032:  * */
laurent@2032: 
laurent@2032: #include "ecrt.h"
laurent@2032: 
laurent@2032: #ifdef _WINDOWS_H
laurent@2032:   #include "iec_types.h"
laurent@2032: #else
laurent@2032:   #include "iec_std_lib.h"
laurent@2032: #endif
laurent@2032: 
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@2032: 	__FaultReactionActive,
laurent@2032: 	__Fault,
laurent@2043: } __CIA402NodeState;
laurent@2032: 
laurent@2032: typedef struct {
laurent@2032: %(entry_variables)s
laurent@2043: 	__CIA402NodeState state;
laurent@2034: 	axis_s* axis;
laurent@2043: } __CIA402Node;
laurent@2032: 
laurent@2043: static __CIA402Node __CIA402Node_%(location)s;
laurent@2032: 
laurent@2034: %(extern_located_variables_declaration)s
laurent@2032: 
laurent@2032: int __init_%(location)s()
laurent@2032: {
laurent@2041: %(init_entry_variables)s
laurent@2043: 	*__IW%(location)s_0 = __MK_AllocAxis(&(__CIA402Node_%(location)s.axis));
laurent@2032: 	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@2041: %(init_axis_params)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@2043: 	__CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled;
laurent@2043: 	__CIA402Node_%(location)s.axis->ActualPosition = (IEC_REAL)(*(__CIA402Node_%(location)s.ActualPosition)) * __CIA402Node_%(location)s.axis->RatioDenominator / __CIA402Node_%(location)s.axis->RatioNumerator;
laurent@2034: 
laurent@2034: 	__MK_UpdateAxis(*__IW%(location)s_0);
laurent@2032: }
laurent@2032: 
laurent@2032: void __publish_%(location)s()
laurent@2032: {
laurent@2034: 	__MK_ComputeAxis(*__IW%(location)s_0);
laurent@2034: 
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@2043: 	if (__CIA402Node_%(location)s.axis->CSP && *(__CIA402Node_%(location)s.ModesOfOperationDisplay) == 0x08) {
laurent@2043: 		*(__CIA402Node_%(location)s.TargetPosition) = (IEC_DINT)(__CIA402Node_%(location)s.axis->PositionSetPoint * __CIA402Node_%(location)s.axis->RatioNumerator / __CIA402Node_%(location)s.axis->RatioDenominator);
laurent@2036: 	}
laurent@2036: 	else {
laurent@2043: 		*(__CIA402Node_%(location)s.TargetPosition) = *(__CIA402Node_%(location)s.ActualPosition);
laurent@2036: 	}
laurent@2032: }