From d4e52e570a19bee6932b8ad34c5bd5966b1d3d3a Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期一, 08 十一月 2021 16:45:53 +0800
Subject: [PATCH] 菲曼模组测试完成
---
源码/核心板/Src/algorithm/TrackingDiff.c | 57 +++++++++++++++++++++++++++++++++++++--------------------
1 files changed, 37 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..17f2a5f 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,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;
+//}
-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
+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)
{
- 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