From 32fa4a5edfbd06a3c0be40a2f3890aae2c1a4d23 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期五, 20 十二月 2019 10:50:00 +0800
Subject: [PATCH] ver1.15
---
源码/核心板/Src/algorithm/TrackingDiff.c | 55 +++++++++++++++++++++++++++++++++++--------------------
1 files changed, 35 insertions(+), 20 deletions(-)
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/algorithm/TrackingDiff.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/algorithm/TrackingDiff.c"
index 2b3f358..67a1922 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/algorithm/TrackingDiff.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/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;
}
--
Gitblit v1.9.3