--- a/etherlab/plc_cia402node.c	Sat Apr 13 12:27:49 2013 +0200
+++ b/etherlab/plc_cia402node.c	Mon Apr 15 00:37:59 2013 +0200
@@ -153,7 +153,12 @@
 	}
 
 	// Default variables publish
-	*(__CIA402Node_%(location)s.TargetPosition) = USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
+	if (__CIA402Node_%(location)s.axis->CSP && *(__CIA402Node_%(location)s.ModesOfOperationDisplay) == 0x08) {
+		*(__CIA402Node_%(location)s.TargetPosition) = USER_UNIT_TO_AXIS_UNIT(__CIA402Node_%(location)s.axis->PositionSetPoint);
+	}
+	else {
+		*(__CIA402Node_%(location)s.TargetPosition) = *(__CIA402Node_%(location)s.ActualPosition);
+	}
 
 	// Extra variables publish
 %(extra_variables_publish)s