From 8aca4882a9626eaf1c75d973b91d2b09b1758b72 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期六, 07 十一月 2020 17:39:50 +0800
Subject: [PATCH] Revert "Revert "V1.34 增加滤波 防撞1对10发货""
---
源码/核心板/Src/algorithm/TrackingDiff.c | 43 +++++++++++++++++++++++++++++++++++++++++++
1 files changed, 43 insertions(+), 0 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"
new file mode 100644
index 0000000..17f2a5f
--- /dev/null
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/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]==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