ver1.15

zhyinch

2019-12-20 32fa4a5edfbd06a3c0be40a2f3890aae2c1a4d23

 源码/核心板/Src/algorithm/TrackingDiff.c

@@ -1,26 +1,41 @@
#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;
//}

TrackingDiffClass* NewTrackingDiffClass(float vel_factor, float pos_factor, float time_factor)
//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)
{
   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;
   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;
}