# HG changeset patch
# User Laurent Bessard
# Date 1366923666 -7200
# Node ID 71fdfd4a12a366b82277de4c91ec537e4e04f5bd
# Parent  6f4b08348a603dfe98e96454c3b4362acec9c0e1
Added support for using CIA402 motion mode csp, csv and cst

diff -r 6f4b08348a60 -r 71fdfd4a12a3 etherlab/EthercatCIA402Slave.py
--- a/etherlab/EthercatCIA402Slave.py	Wed Apr 24 23:27:35 2013 +0200
+++ b/etherlab/EthercatCIA402Slave.py	Thu Apr 25 23:01:06 2013 +0200
@@ -11,11 +11,14 @@
 NODE_VARIABLES = [
     ("ControlWord", 0x6040, 0x00, "UINT", "Q"),
     ("TargetPosition", 0x607a, 0x00, "DINT", "Q"),
+    ("TargetVelocity", 0x60ff, 0x00, "DINT", "Q"),
+    ("TargetTorque", 0x6071, 0x00, "INT", "Q"),
     ("ModesOfOperation", 0x06060, 0x00, "SINT", "Q"),
     ("StatusWord", 0x6041, 0x00, "UINT", "I"),
     ("ModesOfOperationDisplay", 0x06061, 0x00, "SINT", "I"),
     ("ActualPosition", 0x6064, 0x00, "DINT", "I"),
-    ("ActualVelocity", 0x606C, 0x00, "DINT", "I"),
+    ("ActualVelocity", 0x606c, 0x00, "DINT", "I"),
+    ("ActualTorque", 0x6077, 0x00, "INT", "I"),
 ]
 
 DEFAULT_RETRIEVE = "    __CIA402Node_%(location)s.axis->%(name)s = *(__CIA402Node_%(location)s.%(name)s);"
diff -r 6f4b08348a60 -r 71fdfd4a12a3 etherlab/plc_cia402node.c
--- a/etherlab/plc_cia402node.c	Wed Apr 24 23:27:35 2013 +0200
+++ b/etherlab/plc_cia402node.c	Thu Apr 25 23:01:06 2013 +0200
@@ -39,10 +39,15 @@
     axis_s* axis;
 } __CIA402Node;
 
-#define AXIS_UNIT_TO_USER_UNIT(param)\
-(IEC_LREAL)(param) * __CIA402Node_%(location)s.axis->RatioDenominator / __CIA402Node_%(location)s.axis->RatioNumerator
-#define USER_UNIT_TO_AXIS_UNIT(param)\
-(IEC_DINT)(param * __CIA402Node_%(location)s.axis->RatioNumerator / __CIA402Node_%(location)s.axis->RatioDenominator)
+#define AXIS_UNIT_TO_USER_UNIT(param, type, name)\
+(IEC_##type)(param) * __CIA402Node_%(location)s.axis->name##RatioDenominator / __CIA402Node_%(location)s.axis->name##RatioNumerator
+#define USER_UNIT_TO_AXIS_UNIT(param, type, name)\
+(IEC_##type)(param * __CIA402Node_%(location)s.axis->name##RatioNumerator / __CIA402Node_%(location)s.axis->name##RatioDenominator)
+
+#define DEFAULT_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL,)
+#define DEFAULT_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, DINT,)
+#define TORQUE_AXIS_UNIT_TO_USER_UNIT(param) AXIS_UNIT_TO_USER_UNIT(param, LREAL, Torque)
+#define TORQUE_USER_UNIT_TO_AXIS_UNIT(param) USER_UNIT_TO_AXIS_UNIT(param, INT, Torque)
 
 static __CIA402Node __CIA402Node_%(location)s;
 
@@ -118,8 +123,9 @@
 	__CIA402Node_%(location)s.axis->CommunicationReady = *(__CIA402Node_%(location)s.StatusWord) != 0;
 	__CIA402Node_%(location)s.axis->ReadyForPowerOn = __CIA402Node_%(location)s.state == __SwitchedOn || __OperationEnabled;
 	__CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled;
-	__CIA402Node_%(location)s.axis->ActualPosition = AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition));
-	__CIA402Node_%(location)s.axis->ActualVelocity = AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity));
+	__CIA402Node_%(location)s.axis->ActualPosition = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualPosition));
+	__CIA402Node_%(location)s.axis->ActualVelocity = DEFAULT_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualVelocity));
+	__CIA402Node_%(location)s.axis->ActualTorque = TORQUE_AXIS_UNIT_TO_USER_UNIT(*(__CIA402Node_%(location)s.ActualTorque));
 
 	// Extra variables retrieve
 %(extra_variables_retrieve)s
@@ -152,8 +158,23 @@
 	    	break;
 	}
 
+	// CIA402 node modes of operation computation according to axis motion mode
+	switch (__CIA402Node_%(location)s.axis->AxisMotionMode) {
+		case mc_mode_cst:
+			*(__CIA402Node_%(location)s.ModesOfOperation) = 0x0a;
+			break;
+		case mc_mode_csv:
+			*(__CIA402Node_%(location)s.ModesOfOperation) = 0x09;
+			break;
+		default:
+			*(__CIA402Node_%(location)s.ModesOfOperation) = 0x08;
+			break;
+	}
+
 	// Default variables publish
-	*(__CIA402Node_%(location)s.TargetPosition) = USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
+	*(__CIA402Node_%(location)s.TargetPosition) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
+	*(__CIA402Node_%(location)s.TargetVelocity) = DEFAULT_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->VelocitySetPoint);
+	*(__CIA402Node_%(location)s.TargetTorque) = TORQUE_USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->TorqueSetPoint);
 
 	// Extra variables publish
 %(extra_variables_publish)s