diff core/UdpServer.cpp @ 149:134bff10e561 ClockSync

Added simple one-variable one-measurement Kalman filter, Pid controller(which output is not used). Virtual clock is now much more precise and reactive for period. Still it is lagging behind a bit on the overall offset.

--- a/core/UdpServer.cpp	Mon Sep 21 03:11:32 2015 +0100
+++ b/core/UdpServer.cpp	Mon Sep 21 03:12:21 2015 +0100
@@ -63,15 +63,15 @@
 		return select(inSocket+1, &stReadFDS, NULL, NULL, NULL); //calling this with a NULL timeout will block indefinitely
 	FD_ZERO(&stReadFDS);
 	FD_SET(inSocket, &stReadFDS);
-	if(timeoutMsecs>=1000){
-		float timeOutSecs=timeoutMsecs*0.001;
-		stTimeOut.tv_sec=(long int)timeOutSecs;
-		timeOutSecs-=(int)timeOutSecs;
-		stTimeOut.tv_usec=(long int)(timeOutSecs*1000000);
-	} else //faster!
-		stTimeOut.tv_usec=(long int)timeoutMsecs*1000;
+	float timeOutSecs=timeoutMsecs*0.001;
+	stTimeOut.tv_sec=(long int)timeOutSecs;
+	timeOutSecs-=(int)timeOutSecs;
+	long int timeOutUsecs=timeOutSecs*1000000;
+	stTimeOut.tv_usec=timeOutUsecs;
 	int descriptorReady= select(inSocket+1, &stReadFDS, NULL, NULL, &stTimeOut);
-	return descriptorReady>0? 1 : descriptorReady;
+//	printf("stTimeOut.tv_sec=%ld, stTimeOut.tv_usec=%ld, descriptorReady: \n",stTimeOut.tv_sec,stTimeOut.tv_usec, descriptorReady);
+//	return descriptorReady>0 ? (timeOutUsecs-stTimeOut.tv_usec) : descriptorReady;
+	return descriptorReady>0 ? 1 : descriptorReady;
 }
 
 int UdpServer::read(//Returns the number of bytes read, or -1 if there was an error.