#include "TrackingDiff.h"
#include <stdlib.h>
#include "dw_app.h"

//TrackingDiffClass* NewTrackingDiffClass(float vel_factor, float pos_factor, float time_factor)
//{
//	TrackingDiffClass* p = (TrackingDiffClass*) malloc(sizeof(TrackingDiffClass));
//	
//	p->pos_factor = pos_factor;
//	p->vel_factor = vel_factor;
//	p->time_correction = time_factor;
//	
//	p->pos_predict = 0;
//	p->vel_predict = 0;
//	p->error = 0;
//	
//	return p;
//}

//void TrackingDiffUpdate(TrackingDiffClass* self, float target)
//{
//	self->error = target - self->pos_predict;
//	
//	self->vel_predict += (self->error * self->vel_factor) * self->time_correction;
//	self->pos_predict += (self->vel_predict + self->error * self->pos_factor) * self->time_correction;
//}
#define vel_factor 2
#define pos_factor 4
#define time_correction 0.03
float vel_predict[255],pos_predict[255];
void NewTrackingDiffUpdate(u8 channel, float target)
{
	float error;
	if(g_Tagdist[channel]==0xffff)
	{
	pos_predict[channel]=target;
	}
	error=target-pos_predict[channel];
	vel_predict[channel]+=(error*vel_factor)*time_correction;
	pos_predict[channel]+=(vel_predict[channel]+error*pos_factor)*time_correction;
}