#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;
|
}
|
