diff -r bcf346f558bd -r 02a2b5dee5e3 etherlab/plc_cia402node.c
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/etherlab/plc_cia402node.c	Sat Jun 23 09:08:13 2018 +0200
@@ -0,0 +1,193 @@
+/*
+
+Template C code used to produce target Ethercat C CIA402 code
+
+Copyright (C) 2011-2014: Laurent BESSARD, Edouard TISSERANT
+
+Distributed under the terms of the GNU Lesser General Public License as
+published by the Free Software Foundation; either version 2 of the License, or
+(at your option) any later version.
+
+See COPYING file for copyrights details.
+
+*/
+
+#include "ecrt.h"
+
+#include "beremiz.h"
+#include "iec_types_all.h"
+
+#include "accessor.h"
+#include "POUS.h"
+
+/* From CiA402, page 27
+
+        Table 30 - State coding
+    Statusword      |      PDS FSA state
+xxxx xxxx x0xx 0000 | Not ready to switch on
+xxxx xxxx x1xx 0000 | Switch on disabled
+xxxx xxxx x01x 0001 | Ready to switch on
+xxxx xxxx x01x 0011 | Switched on
+xxxx xxxx x01x 0111 | Operation enabled
+xxxx xxxx x00x 0111 | Quick stop active
+xxxx xxxx x0xx 1111 | Fault reaction active
+xxxx xxxx x0xx 1000 | Fault
+*/
+#define FSAFromStatusWord(SW) (SW & 0x006f)
+#define NotReadyToSwitchOn  0b00000000 FSA_sep 0b00100000
+#define SwitchOnDisabled    0b01000000 FSA_sep 0b01100000
+#define ReadyToSwitchOn     0b00100001
+#define SwitchedOn          0b00100011
+#define OperationEnabled    0b00100111
+#define QuickStopActive     0b00000111
+#define FaultReactionActive 0b00001111 FSA_sep 0b00101111
+#define Fault               0b00001000 FSA_sep 0b00101000
+
+// SatusWord bits :
+#define SW_ReadyToSwitchOn     0x0001
+#define SW_SwitchedOn          0x0002
+#define SW_OperationEnabled    0x0004
+#define SW_Fault               0x0008
+#define SW_VoltageEnabled      0x0010
+#define SW_QuickStop           0x0020
+#define SW_SwitchOnDisabled    0x0040
+#define SW_Warning             0x0080
+#define SW_Remote              0x0200
+#define SW_TargetReached       0x0400
+#define SW_InternalLimitActive 0x0800
+
+// ControlWord bits :
+#define SwitchOn        0x0001
+#define EnableVoltage   0x0002
+#define QuickStop       0x0004
+#define EnableOperation 0x0008
+#define FaultReset      0x0080
+#define Halt            0x0100
+
+
+IEC_INT beremiz__IW%(location_str)s = %(slave_pos)s;
+IEC_INT *__IW%(location_str)s = &beremiz__IW%(location_str)s;
+IEC_INT beremiz__IW%(location_str)s_402;
+IEC_INT *__IW%(location_str)s_402 = &beremiz__IW%(location_str)s_402;
+
+%(MCL_headers)s
+
+static IEC_BOOL __FirstTick = 1;
+
+typedef struct {
+%(entry_variables)s
+    axis_s* axis;
+} __CIA402Node;
+
+#define AxsPub __CIA402Node_%(location_str)s
+
+static __CIA402Node AxsPub;
+
+%(extern_located_variables_declaration)s
+
+%(fieldbus_interface_declaration)s
+
+int __init_%(location_str)s()
+{
+    __FirstTick = 1;
+%(init_entry_variables)s
+	*(AxsPub.ModesOfOperation) = 0x08;
+    return 0;
+}
+
+void __cleanup_%(location_str)s()
+{
+}
+
+void __retrieve_%(location_str)s()
+{
+	if (__FirstTick) {
+		*__IW%(location_str)s_402 = __MK_Alloc_AXIS_REF();
+		AxsPub.axis = 
+            __MK_GetPublic_AXIS_REF(*__IW%(location_str)s_402);
+		AxsPub.axis->NetworkPosition = beremiz__IW%(location_str)s;
+%(init_axis_params)s
+%(fieldbus_interface_definition)s
+		__FirstTick = 0;
+	}
+
+	// Default variables retrieve
+	AxsPub.axis->CommunicationReady = 
+        *(AxsPub.StatusWord) != 0;
+#define FSA_sep || FSA ==
+    {
+        uint16_t FSA = FSAFromStatusWord(*(AxsPub.StatusWord));
+        AxsPub.axis->ReadyForPowerOn = FSA == ReadyToSwitchOn;
+        AxsPub.axis->PowerFeedback = FSA == OperationEnabled;
+    }
+#undef FSA_sep 
+	AxsPub.axis->ActualRawPosition = *(AxsPub.ActualPosition);
+	AxsPub.axis->ActualRawVelocity = *(AxsPub.ActualVelocity);
+	AxsPub.axis->ActualRawTorque = *(AxsPub.ActualTorque);
+
+	// Extra variables retrieve
+%(extra_variables_retrieve)s
+}
+
+void __publish_%(location_str)s()
+{
+	IEC_BOOL power = 
+        ((*(AxsPub.StatusWord) & SW_VoltageEnabled) != 0) 
+        && AxsPub.axis->Power;
+    uint16_t CW = *(AxsPub.ControlWord);
+
+#define FSA_sep : case
+	// CIA402 node state transition computation
+	switch (FSAFromStatusWord(*(AxsPub.StatusWord))) {
+	    case SwitchOnDisabled :
+            CW &= ~(SwitchOn | FaultReset);
+            CW |= EnableVoltage | QuickStop;
+	    	break;
+	    case ReadyToSwitchOn :
+	    case OperationEnabled :
+	    	if (!power) {
+                CW &= ~(FaultReset | EnableOperation);
+                CW |= SwitchOn | EnableVoltage | QuickStop;
+	    		break;
+	    	}
+	    case SwitchedOn :
+	    	if (power) {
+                CW &= ~(FaultReset);
+                CW |= SwitchOn | EnableVoltage | QuickStop | EnableOperation;
+	    	}
+	    	break;
+	    case Fault :
+            /* TODO reset fault only when MC_Reset */
+            CW &= ~(SwitchOn | EnableVoltage | QuickStop | EnableOperation);
+            CW |= FaultReset;
+	    	break;
+	    default:
+	    	break;
+	}
+#undef FSA_sep 
+    *(AxsPub.ControlWord) = CW;
+
+	// CIA402 node modes of operation computation according to axis motion mode
+	switch (AxsPub.axis->AxisMotionMode) {
+		case mc_mode_cst:
+			*(AxsPub.ModesOfOperation) = 0x0a;
+			break;
+		case mc_mode_csv:
+			*(AxsPub.ModesOfOperation) = 0x09;
+			break;
+		default:
+			*(AxsPub.ModesOfOperation) = 0x08;
+			break;
+	}
+
+	// Default variables publish
+	*(AxsPub.TargetPosition) = 
+            AxsPub.axis->RawPositionSetPoint;
+	*(AxsPub.TargetVelocity) = 
+            AxsPub.axis->RawVelocitySetPoint;
+	*(AxsPub.TargetTorque) = 
+            AxsPub.axis->RawTorqueSetPoint;
+
+	// Extra variables publish
+%(extra_variables_publish)s
+}