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