From d56da5b9006bfb55a4c7ae2dbb00c026fa3148ae Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期日, 15 十二月 2019 11:08:21 +0800
Subject: [PATCH] 增加跟踪微分器

---
 源码/核心板/Src/algorithm/TrackingDiff.c |   26 +++++++++++++
 源码/核心板/Src/application/dw_app.h     |    3 +
 源码/核心板/Src/main.c                   |    4 +
 源码/核心板/Src/decadriver/deca_device.c |   45 ++++++++++++++++++++++
 源码/核心板/Src/algorithm/TrackingDiff.h |   26 +++++++++++++
 源码/核心板/Src/application/dw_app.c     |    5 ++
 6 files changed, 108 insertions(+), 1 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..2b3f358
--- /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,26 @@
+#include "TrackingDiff.h"
+#include <stdlib.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;
+}
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/algorithm/TrackingDiff.h" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/algorithm/TrackingDiff.h"
new file mode 100644
index 0000000..6cc52de
--- /dev/null
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/algorithm/TrackingDiff.h"
@@ -0,0 +1,26 @@
+#ifndef TRACKINGDIFF_H_
+#define TRACKINGDIFF_H_
+#include "stm32f10x.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+	
+typedef struct TrackingDiffClass_
+{
+	float error;
+	float vel_predict;
+	float pos_predict;
+	
+	float vel_factor;
+	float pos_factor;
+	float time_correction;
+} TrackingDiffClass;
+
+TrackingDiffClass* NewTrackingDiffClass(float vel_factor, float pos_factor, float time_factor);
+void TrackingDiffUpdate(TrackingDiffClass* self, float target);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
index 653b3d7..151b817 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
@@ -474,6 +474,7 @@
 }
 int8_t correction_time;
 extern uint8_t sync_seq;
+#define TDFILTER
 
 void Anchor_App(void)
 {
@@ -623,6 +624,10 @@
 						g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
 						anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
 						g_Tagdist[tag_id_recv]=	hex_dist;
+						#ifdef TDFILTER
+						TrackingDiffUpdate(pUWBDistanceTrackingDiff, (float)hex_dist);
+						hex_dist=pUWBDistanceTrackingDiff->pos_predict;
+						#endif
 					}
 					his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
 					g_flag_Taggetdist[tag_id_recv]=0;
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h"
index bda82cb..174550c 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.h"
@@ -6,6 +6,8 @@
 #include "stm32f10x.h"
 #include "filters.h"
 #include "ADC.h"
+#include "TrackingDiff.h"
+
 //#define WORK_MODE_TAG
 #define WORK_MODE_ANCHOR
 
@@ -19,6 +21,7 @@
 #define GROUP_TAG_NUM 2
 #define TAG_NUM_IN_SYS				256
 #define DFT_RAND_DLY_TIM_MS			1
+extern TrackingDiffClass *pUWBDistanceTrackingDiff;
 extern u16 poll_timer,tag_timer;
 extern int8_t tag_delaytime;
 extern u32 dev_id;
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/decadriver/deca_device.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/decadriver/deca_device.c"
index 75a99e4..fa05f24 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/decadriver/deca_device.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/decadriver/deca_device.c"
@@ -103,6 +103,50 @@
  * returns DWT_SUCCESS for success, or DWT_ERROR for error
  */
 // OTP addresses definitions
+#define CPUID_OTP_ADRESS           0x1d
+
+uint32_t ReadUniqueID(void)
+{
+	uint8_t m_UIDAdd[4];
+	uint32_t cpuID_add;
+	//获取CPU唯一ID
+//	m_CpuID.Data32[0] = *(vu32*)(0x1ffff7e8);
+//	m_CpuID.Data32[1] = *(vu32*)(0x1ffff7ec);
+//	m_CpuID.Data32[2] = *(vu32*)(0x1ffff7f0);
+	//这里是防止反汇编能看到在哪里读取的UID,将UID的地址分解存在RAM中
+	//加密算法,很简单的加密算法
+	m_UIDAdd[0] = 0xE8;                
+	m_UIDAdd[1] = m_UIDAdd[0]+0x0F;    		//f7 = e8 + 0f
+	m_UIDAdd[2] = m_UIDAdd[1]+0x08;         //ff = f7 + 08 
+	m_UIDAdd[3] = m_UIDAdd[2]-0xe0;         //1f = ff - e0
+	memcpy(&cpuID_add, (uint8_t*)m_UIDAdd, 4);
+
+	return *(uint32_t*)(cpuID_add);
+}
+
+uint8_t UID_ERROR=0;
+void CheckCPUID(void)
+{ 	uint32_t cpuID = 0;
+	uint32_t key_ID = 0;
+	uint8_t i=10;
+	cpuID = ReadUniqueID();
+  Spi_ChangePrescaler(SPI_BaudRatePrescaler_256);
+	while(i--)
+	{
+	dwt_otpread(CPUID_OTP_ADRESS,&key_ID,1);
+		if(cpuID != key_ID)
+	{
+		UID_ERROR = 1;
+	}
+	else 
+	{
+		UID_ERROR = 0;
+		break;
+	}
+}
+	
+	 Spi_ChangePrescaler(SPI_BaudRatePrescaler_8);
+}
 #define LDOTUNE_ADDRESS (0x04)
 #define PARTID_ADDRESS (0x06)
 #define LOTID_ADDRESS  (0x07)
@@ -191,6 +235,7 @@
 		dwt_write16bitoffsetreg(PMSC_ID,PMSC_TXFINESEQ_OFFSET ,PMSC_TXFINESEQ_DIS_MASK);
 	
 	}
+	CheckCPUID();	
     return DWT_SUCCESS ;
 
 } // end dwt_initialise()
diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c"
index 78c42c8..43f6f36 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/main.c"
@@ -45,6 +45,7 @@
 	memcpy(&hbsend[14],&checksum,2);
 }
 u16 tyncpoll_time;
+TrackingDiffClass *pUWBDistanceTrackingDiff;
 void Program_Init(void)
 {uint16_t i;
 	float temp;
@@ -57,7 +58,7 @@
 //		g_com_map[DEV_ID]=1;
 //	g_com_map[COM_INTERVAL]=10;
 	OUT485_ENABLE;
-	
+
 #ifdef DEBUG_MODE
 	
 	g_com_map[COM_INTERVAL]=50;
@@ -66,6 +67,7 @@
 	g_com_map[ANC_POLL]=0;
 	g_com_map[ALARM_DISTANCE1]=100;
 #endif
+		pUWBDistanceTrackingDiff = NewTrackingDiffClass(2, 4, 0.1);
 		dev_id = g_com_map[DEV_ID];
 		g_com_map[VERSION] = 0x010b;	
 	temp=(float)g_com_map[MAX_REPORT_ANC_NUM]*4/3;	

--
Gitblit v1.9.3