V1.69

zhyinch

2022-03-28 34b038b96a86f70e96ea29c537c87fe8a168ea9b

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

对比新文件
@@ -0,0 +1,43 @@
#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
extern uint16_t vel_factor,pos_factor;
#define time_correction 0.01
float vel_predict[255],pos_predict[255];
void NewTrackingDiffUpdate(u8 channel, float target)
{
   float error;
   if(g_Tagdist[channel]==0x1ffff)
   {
   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;
}