annotate core/VirtualClock.cpp @ 141:44d07fa9bd03 ClockSync

Ultra-basic feedback for clock sync works^CIssues: response time of the IIR filter is too slow, requires PID and better filtering algorithm.

#include "VirtualClock.h"
void VirtualClock::init(){
	firstRun=true;
	movingAverage.setLength(101); //TODO: a better filtering algorithm ( Did you say Kalman?)
	period=-1;
	elapsedPeriods=0;
	startTime=0;
	startTimeOffset=0;
	elapsedPeriodsOffset=0;
}
	
VirtualClock::VirtualClock(){
	init();
}
void VirtualClock::sync(){
	sync(1);
}
void VirtualClock::sync(double numPeriods){
	myClock_t currentTime=Clock::getTimeUs();
	static int calls=0;
	elapsedPeriods+=numPeriods;
	if(calls==50){ //TODO: this is dangerous as the clock might jump suddenly if currentTime is not precise
		calls=0;
		startTimeOffset=startTime;
		startTime=currentTime;
		elapsedPeriodsOffset=elapsedPeriods;
	}
	calls++;
	if(firstRun==true){
		firstRun=false;
		startTime=currentTime;
	} else {
		double newPeriod=(currentTime-lastSync)/numPeriods;
		double expectedPeriod=22.67;
		double maxPeriodDeviation=10;
		if(fabs(newPeriod-expectedPeriod)<maxPeriodDeviation){ // filtering outliers
			period=movingAverage.add(newPeriod); //TODO: replace with Kalman filter
		} else {
			printf("period out of range: %f\n", newPeriod);
		}
	}
	lastSync=currentTime;
//	printf("lastSync: %lld\n",lastSync-startTime);
}

double VirtualClock::getNow(){
	myClock_t currentSystemTime=Clock::getTimeUs();
	if(period<=0){
		return currentSystemTime; // TODO: this is not very meaningful.
	}
	//  double beginningOfPeriod=lastSync; // TODO: if sync() does not get called every time (but e.g. only every so often),
													 // then this line (and the class) needs editing
	myClock_t elapsed=(currentSystemTime-startTime);
	double now=elapsedPeriodsOffset + elapsed/(double)period;
//	printf("elapsed=%lld;  sincelastSync=%lld; period=%f; now=%f\n", elapsed, currentSystemTime-lastSync, period, now);
	return now;
}

void VirtualClock::addOffset(double periodOffset){
	elapsedPeriods-=periodOffset;
	if(elapsedPeriods<0){
		printf("ERROR: periodOffset adjustment of %f resulted in elapsedPeriods=%f\n", periodOffset, elapsedPeriods);
		exit(1);
	}
	movingAverage.reset();
}
double VirtualClock::getPeriod(){
	return period;
}