增加跟踪微分器

zhyinch

2019-12-15 d56da5b9006bfb55a4c7ae2dbb00c026fa3148ae

增加跟踪微分器

 Ô´Âë/ºËÐÄ°å/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;
}

 Ô´Âë/ºËÐÄ°å/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

 Ô´Âë/ºËÐÄ°å/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;

 Ô´Âë/ºËÐÄ°å/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;

 Ô´Âë/ºËÐÄ°å/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()

 Ô´Âë/ºËÐÄ°å/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;
@@ -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;