Tried to debug position control... no success though so far

gui.h

@@ -49,23 +49,20 @@ public:
 	    }
 
 	double cosalpha(position_f_2d &pos) {
-		return (x * pos.x + y * pos.y) / (lenght() * pos.lenght());
+		return (x * pos.x + y * pos.y) / (length() * pos.length());
 	}
 
 	double sinalpha(position_f_2d &pos) {
-		return (x * pos.y - y * pos.x) / (lenght() * pos.lenght());
+		return (x * pos.y - y * pos.x) / (length() * pos.length());
 	}
 
 	double perpendicular(position_f_2d &pos, position_f_2d &base) {
-		double l;
 		position_f_2d p = pos - base;
-		l = sinalpha(p) * p.lenght();
-
-		if (isnan(l)) l = 0.0;
-		return l;
+		double l = p.length();
+		return l == 0.0 ? 0.0 : l = sinalpha(p);
 	}
 
-	double lenght() { return hypot(x, y); }
+	double length() { return hypot(x, y); }
 
 	double x;
 	double y;

posctl.cc

@@ -57,6 +57,8 @@ void axis_history_add(int axis, double diff, double out) {
 
 
 #define CALIB_MAXSPEED 2.0
+#define CALIB_DURATION_DELTA 10.0
+#define CALIB_DURATION_AXIS (CALIB_DURATION_DELTA / CALIB_MAXSPEED)
 
 void posctl_gui_update();
 
@@ -632,7 +634,7 @@ void PosCtl::CalibModeDelta(int x, int y) {
 	gettimeofday (&tv, NULL);
 	timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
 
-	if (timediff > 10.0) {
+	if (timediff > CALIB_DURATION_DELTA) {
 		position_f_2d fp;
 
 		printf ("%s:%d %s\n", __FILE__, __LINE__, __FUNCTION__);
@@ -666,11 +668,11 @@ void PosCtl::CalibModeAxis(int x, int y) {
 	gettimeofday (&tv, NULL);
 	timediff = (float)(tv.tv_sec - calib_timestamp.tv_sec) + ((tv.tv_usec - calib_timestamp.tv_usec) / 1000000.0);
 
-	if (timediff > 5.0) {
+	if (timediff > CALIB_DURATION_AXIS) {
 		printf ("%s:%d %s calib_mode: %d\n", __FILE__, __LINE__, __FUNCTION__, calib_mode);
 
-		fp.x = (x - calib_pos.x) / (float)timediff;
-		fp.y = -(y - calib_pos.y) / (float)timediff;
+		fp.x = +(x - calib_pos.x) / timediff;
+		fp.y = -(y - calib_pos.y) / timediff;
 
 		if (calib_mode ==  POSCTL_CALIB_MODE_AXIS1)	{
 			calib_axis1_v = fp - calib_rot_v;
@@ -753,18 +755,18 @@ void PosCtl::Loop (int posx, int posy) {
 		dist_axis1 = calib_axis1_v.perpendicular(p, target_pos);
 		dist_axis2 = calib_axis2_v.perpendicular(p, target_pos);
 
-		out1 = pid_axis[0].Update(0.0, dist_axis2);
-		out2 = pid_axis[1].Update(0.0, dist_axis1);
+		out1 = pid_axis[0].Update(0.0, dist_axis1);
+		out2 = pid_axis[1].Update(0.0, dist_axis2);
 
 		printf ("%s:%d %s", __FILE__, __LINE__, __FUNCTION__);
-		printf ("Axis 1 dist: %5.3f out 2: %5.3f   ", dist_axis2, out1);
-		printf ("Axis 2 dist: %5.3f out 1: %5.3f\n", dist_axis1, out2);
+		printf (" Axis1 [dist:%+6.3f out1:%+6.3f]  ", dist_axis1, out1);
+		printf (" Axis2 [dist:%+6.3f out2:%+6.3f]\n", dist_axis2, out2);
 
 		OutputWriteValue(0, out1);
 		OutputWriteValue(1, out2);
 
-		axis_history_add(0, dist_axis2, out1);
-		axis_history_add(1, dist_axis1, out2);
+		axis_history_add(0, dist_axis1, out1);
+		axis_history_add(1, dist_axis2, out2);
 
 		UnLockMutex();
 		gdk_threads_add_idle(cb_thread_posctl, NULL);
@@ -868,7 +870,6 @@ int PosCtl::OutputWriteValue (int axis, double value) {
 	char outbuf[255];
 	// printf ("%s:%d %s   Axis %d   Value:%f\n", __FILE__, __LINE__, __FUNCTION__, axis, value);
 
-	if (isnan(value)) return -1;
 	if (device_type == POSCTL_DEVTYPE_SIM) {
 		simulation.AxisSetValue(axis, value);
 		return 0;