# HG changeset patch
# User Laurent Bessard
# Date 1352456850 -3600
# Node ID d964dbc2c7b0bb7c12bb2eafaabbc8a08d2df5d2
# Parent  52b398c75ba9c0ee13e20ecab0294e5ea4f6f0de
Adding support for extracting ActualVelocity from CIA402 nodes PDOs

diff -r 52b398c75ba9 -r d964dbc2c7b0 etherlab/etherlab.py
--- a/etherlab/etherlab.py	Fri Nov 09 11:25:44 2012 +0100
+++ b/etherlab/etherlab.py	Fri Nov 09 11:27:30 2012 +0100
@@ -195,6 +195,7 @@
         ("StatusWord", 0x6041, 0x00, "UINT", "I"),
         ("ModesOfOperationDisplay", 0x06061, 0x00, "SINT", "I"),
         ("ActualPosition", 0x6064, 0x00, "DINT", "I"),
+        ("ActualVelocity", 0x606C, 0x00, "DINT", "I"),
     ]
     
     DEFAULT_RETRIEVE = "    __CIA402Node_%(location)s.axis->%(name)s = *(__CIA402Node_%(location)s.%(name)s);"
diff -r 52b398c75ba9 -r d964dbc2c7b0 etherlab/plc_cia402node.c
--- a/etherlab/plc_cia402node.c	Fri Nov 09 11:25:44 2012 +0100
+++ b/etherlab/plc_cia402node.c	Fri Nov 09 11:27:30 2012 +0100
@@ -39,6 +39,11 @@
     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)
+
 static __CIA402Node __CIA402Node_%(location)s;
 
 %(extern_located_variables_declaration)s
@@ -107,7 +112,8 @@
 
 	// Default variables retrieve
 	__CIA402Node_%(location)s.axis->PowerFeedback = __CIA402Node_%(location)s.state == __OperationEnabled;
-	__CIA402Node_%(location)s.axis->ActualPosition = (IEC_LREAL)(*(__CIA402Node_%(location)s.ActualPosition)) * __CIA402Node_%(location)s.axis->RatioDenominator / __CIA402Node_%(location)s.axis->RatioNumerator;
+	__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));
 
 	// Extra variables retrieve
 %(extra_variables_retrieve)s
@@ -142,7 +148,7 @@
 
 	// Default variables publish
 	if (__CIA402Node_%(location)s.axis->CSP && *(__CIA402Node_%(location)s.ModesOfOperationDisplay) == 0x08) {
-		*(__CIA402Node_%(location)s.TargetPosition) = (IEC_DINT)(__CIA402Node_%(location)s.axis->PositionSetPoint * __CIA402Node_%(location)s.axis->RatioNumerator / __CIA402Node_%(location)s.axis->RatioDenominator);
+		*(__CIA402Node_%(location)s.TargetPosition) = USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
 	}
 	else {
 		*(__CIA402Node_%(location)s.TargetPosition) = *(__CIA402Node_%(location)s.ActualPosition);