view core/VirtualClock.cpp @ 138:53e3c0a3295d ClockSync

Added xenomai version of Clock, now includes header

#include "VirtualClock.h"
void VirtualClock::init(){
	firstRun=true;
	movingAverage.setLength(31); //TODO: a better filtering algorithm ( Did you say Kalman?)
	period=-1;
}
	
VirtualClock::VirtualClock(){
	init();
}
void VirtualClock::sync(){
	sync(1);
}
void VirtualClock::sync(double count){
	myClock_t currentTime=Clock::getTimeUs();
	if(firstRun==true){
		firstRun=false;
		startTime=currentTime;
	} else {
		period=movingAverage.add((currentTime-lastSync)/count); //TODO: replace with Kalman filter
	}
	lastSync=currentTime;
}

double VirtualClock::getNow(){
	myClock_t now=Clock::getTimeUs();
	if(period<=0){
		return now;
	}
	//  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=(now-startTime);
	double frac=elapsed/(double)period;
	//  printf("now=%lld; beginningOfPeriod=%f; lastSync=%lld; period=%lld; frac=%f\n", now, beginningOfPeriod, lastSync, period, frac);
	return frac;
}

double VirtualClock::getPeriod(){
	return period;
}