提交缺失文件

yincheng.zhong

2023-08-22 c8f2474c44c861811c9665352a9a2bcfc81d3cb9

提交缺失文件

 Src/application/dw_sync.c

¶Ô±ÈÐÂÎļþ
@@ -0,0 +1,93 @@
#include "dw_app.h"
int16_t target_offsettime = 850;
extern u32 synctimer;
extern u16 tagslotpos;
u32 target_time=100;
u8 pd_i;
u32 temptime,delaytime,expect_anctime;
int32_t error_time;
u16 delaycount;
extern u16 bigslot_num;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos_intoatl, slotpos;
void NextPollDelay(u32 anchor_time)
{
    //tagslotpos = 0;
    error_time = anchor_time-expect_anctime;
    if(error_time<2000&&error_time>0)
        clockoffset += (target_offsettime-error_time)*0.1;
    for(pd_i=0; pd_i<tag_frequency; pd_i++)
    {
        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
        if(anchor_time<temptime-5000)
        {
            current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
            expect_anctime = temptime;
            return ;
        }
    }
    temptime = (tagslotpos*SLOTTIME_MS+100+10000)*100;
    expect_anctime = temptime-1000000;
    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
    return ;
}
u16 lpcount,poll_startcount,last_lpcount;
u16 waketopolltimeus = 3200;
#define BASENUM_COMTIME 244
u32 last_anchor_time;
static u16 current_count;
void SetLPTimer(u32 anchor_time,u8 nearbase_num)
{

    NextPollDelay(anchor_time);
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}

void NextSlotDelayMs(int16_t delayms)
{
    current_slotnum++;
    if(current_slotnum>=tag_frequency)
    {
        current_slotnum = 0;
        lpcount = last_lpcount+31457/tag_frequency+1311+32.767*delayms;//(float)(delayms+40)*32.767 ;//(200/tag_frequency+delayms)*4.8*1000/LPTIMER_LSB
    } else {
        lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}
void WaitUntilPollStart(void)
{
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<poll_startcount||current_count>poll_startcount+16384)
    {
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        if(current_count<poll_startcount-300)
        {
            break;
        }
    }
}

 Src/application/ss_dw_tag_core.c

¶Ô±ÈÐÂÎļþ
@@ -0,0 +1,924 @@

/*! ----------------------------------------------------------------------------
 *  @file    main.c
 *  @brief   Double-sided two-way ranging (DS TWR) initiator example code
 *
 *
 *
 * @attention
 *
 * Copyright 2015 (c) Decawave Ltd, Dublin, Ireland.
 *
 * All rights reserved.
 *
 * @author Decawave
 */

#include <string.h>
#include <math.h>
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
#include "dw_driver.h"
#include "Spi.h"
#include "led.h"
#include "serial_at_cmd_app.h"
#include "Usart.h"
#include "global_param.h"
#include "filters.h"
#include <stdio.h>
#include "beep.h"
#include "modbus.h"
#include "BMP390.h"
/*------------------------------------ Marcos ------------------------------------------*/

/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
 * 1 uus = 512 / 499.2 ç¥ and 1 ç¥ = 499.2 * 128 dtu. */
#define UUS_TO_DWT_TIME 65536

/* Delay between frames, in UWB microseconds. See NOTE 4 below. */
/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
#define POLL_TX_TO_RESP_RX_DLY_UUS 10
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
 * frame length of approximately 2.66 ms with above configuration. */
#define RESP_RX_TO_FINAL_TX_DLY_UUS 1000

/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 4000

#define DELAY_BETWEEN_TWO_FRAME_UUS 240

#define POLL_RX_TO_RESP_TX_DLY_UUS 470
/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
#define RESP_TX_TO_FINAL_RX_DLY_UUS 200
/* Receive final timeout. See NOTE 5 below. */
#define FINAL_RX_TIMEOUT_UUS 4300


#define SPEED_OF_LIGHT 299702547

/* Indexes to access some of the fields in the frames defined above. */
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4

#define STARTPOLL  DISCPOLL//#define SWITCHBASE_DIST
#define SWITCHBASE_ZHUANDIAN
#define SWITCHBASE_DIST
enum enumtagstate
{

    DISCPOLL,
    REGPOLL,
    GETNEARMSG,
    NEARPOLL,
    SINGLEPOLL,
} tag_state=STARTPOLL;
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {0};
static uint8_t tx_final_msg[60] = {0};
static uint8_t tx_resp_msg[22] = {0};
uint8_t tx_near_msg[180] = {0};

static uint32_t frame_seq_nb = 0;
static uint32_t status_reg = 0;
static uint8_t rx_buffer[100];
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint64_t final_tx_ts;
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];    ;
uint32_t tag_id = 0;
uint32_t tag_id_recv = 0;
uint32_t anc_id_recv = 0;
uint8_t random_delay_tim = 0;
double distance, dist_no_bias, dist_cm;
uint32_t g_UWB_com_interval = 0;
float dis_after_filter;                //当前距离值
LPFilter_Frac* p_Dis_Filter;        //测距用的低通滤波器

void NextSlotDelayMs(int16_t delayms);
void GetNearMsg(void);
void Registor_Poll(void);
extern void WaitUntilPollStart(void);
extern void SetLPTimer(u32 anchor_time,u8 nearbase_num);
static uint64_t get_tx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readtxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}

static uint64_t get_rx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readrxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}

static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
{
    int i;
    for (i = 0; i < FINAL_MSG_TS_LEN; i++)
    {
        ts_field[i] = (uint8_t) ts;
        ts >>= 8;
    }
}

static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts)
{
    int i;
    *ts = 0;
    for (i = 0; i < FINAL_MSG_TS_LEN; i++)
    {
        *ts += ts_field[i] << (i * 8);
    }
}


void Dw1000_App_Init(void)
{
//g_com_map[DEV_ID] = 0x0b;
    //tag_state=DISCPOLL;
    tx_poll_msg[MESSAGE_TYPE_IDX]=POLL;
    tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
    tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
    tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;

    memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
    memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
    memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);

    memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 2);
    memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
    memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
    memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
    memcpy(&tx_near_msg[ANCHOR_ID_IDX], &dev_id, 2);
    memcpy(&tx_near_msg[TAG_ID_IDX], &dev_id, 2);
}
u16 tag_time_recv[TAG_NUM_IN_SYS];
u8 usart_send[40];
u8 battary,button;
extern uint8_t g_pairstart;
extern uint8_t g_start_send_flag;
uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
int32_t hex_dist;
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
u16 tmp_time;
int32_t temp_dist;
u16 tagslotpos;

u16 anclist_num=0,anclist_pos; //list æ€»æ•°é‡å’Œå½“前位置
u16 ancid_list[TAG_NUM_IN_SYS];
u8 nearbase_num;
u16 mainbase_id;
int32_t mainbase_dist,base_mindist;
uint8_t trygetnearmsg_times,try_reg_times;
u16 mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],nearbaseid_list0[MAX_NEARBASE_NUM],nearbaseid_list[MAX_NEARBASE_NUM],nearbaseid_list2[MAX_NEARBASE_NUM];
u8 FindNearBasePos(u16 baseid)
{
    u8 i;
    for(i=0; i<nearbase_num; i++)
    {
        if(baseid==nearbaseid_list[i])
            return i;
    }
}
u8 recbase_num=0;
#define CHANGE_BASE_THRESHOLD  5
uint8_t GetRandomValue(void)
{
    uint8_t random_value=0,temp_adc,i;
    for(i=0; i<8; i++)
    {
        temp_adc=Get_ADC_Value();
        random_value=random_value|((temp_adc&0x01)<<i);
    }
    return random_value;
}
int32_t mainbase_dist,nearbase_switchdistlist[MAX_NEARBASE_NUM],nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
u8 anclost_times=0 , mainbase_lost_count=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u8 flag_finalsend,flag_getresponse,flag_rxon;
uint16_t current_count,start_count,end_count,lastsync_timer;
extern u16 bigslot_num;
u8 nearbase_num=0,last_nearbase_num,next_nearbase_num,last_slotnum, para_update,para_len,stationary_flag=0;
u32 rec_tagpos_binary;
int16_t offset=4700,temptimer;
u8 motor_state,rec_remotepara_state,rec_remotepara[80];


#define SINGLEPOLL_BASENUM 5
uint16_t singlepoll_baseid[20]= {0x8166,0x9804,0x9689,0x9686,0x8279,0x9815,0x9803,0x9814,0x9687,0x9774,0x8278,0x9743,0x9688,0x9771,0x8277};
int32_t singlepoll_basedist[20];

extern u8 userkey_state;
extern float motor_keeptime;
uint8_t changemainbase_count=0,gotosleep_flag,singlepoll_i;
uint32_t frame_len;
int32_t salvebase_mindist;
extern int16_t intheight;
static uint8_t send_buffer[200];

//#define TAGET_OFFSETTIME 940
#define MAX_NEARBASE_ANCNUM 11
u16 rec_maxrangelen,rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
u8 outrange_times;


u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
void GetNearMsg(void)
{
    u32 start_poll,frame_len;
    u8 nearmsg_i=0;
    NextSlotDelayMs(0);
    for(nearmsg_i=0; nearmsg_i<MAX_NEARBASE_NUM; nearmsg_i++)
    {
        nearbase_distlist[nearmsg_i] = 0x1ffff;
    }
//mainbase_id = 0x1;
    memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
    memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2);
    tx_near_msg[MESSAGE_TYPE_IDX] = POS_MSG;


    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);            //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
    dwt_writetxdata(12, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
    dwt_writetxfctrl(12, 0);//设置超宽带发送数据长度
    WaitUntilPollStart();
    dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);
    NextSlotDelayMs(0);
    /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
    {
    };
    if(status_reg==0xffffffff)
    {
        NVIC_SystemReset();
    }
    if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
    {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
        frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;    //获得接收到的数据长度
        dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
        if (rx_buffer[MESSAGE_TYPE_IDX] == POS_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
        {
            nearbase_num=rx_buffer[NEARBASENUM_INDEX];
            memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
            //    tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
            //tagslotpos=rx_buffer[TAGSLOTPOS];
            memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
            memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
            //slottime=ceil((nearbase_num+2)*0.3)+1;
            //tyncpoll_time=tagslotpos*slottime;
            //tyncpoll_time=(g_com_map[DEV_ID]%max_slotpos)*slottime;
            tag_state=NEARPOLL;
        }
    } else {
//            tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
    }
}
void SendHuiZongData(void)
{
    usart_send[2] = 0x0c;//正常模式
    usart_send[3] = 15+8*(nearbase_num+1);//数据段长度
    memcpy(&usart_send[4],&dev_id,2);
    usart_send[6] = frame_seq_nb;
    usart_send[7] = frame_seq_nb>>8;
    usart_send[8] = bat_percent;
    usart_send[9] = tx_near_msg[BUTTON_IDX];

    memcpy(&usart_send[10],&intheight,2);
    usart_send[12] = tag_frequency;
    usart_send[13] = tagslotpos;
    usart_send[14] = 0;
    usart_send[15] = 0;
    usart_send[16] = nearbase_num+1;
    memcpy(&usart_send[17],&mainbase_id,2);
    memcpy(&usart_send[19],&nearbaseid_list,2*nearbase_num);
    memcpy(&usart_send[19+nearbase_num*2],&nearbase_distlist,4*nearbase_num+4);
    memcpy(&usart_send[19+nearbase_num*6+4],&rec_anc_signalpower,2*nearbase_num+2);

    checksum = Checksum_u16(&usart_send[2],23+8*nearbase_num);
    memcpy(&usart_send[25+8*nearbase_num],&checksum,2);
    USART_puts(usart_send,27+8*nearbase_num);
    //HexToAsciiSendUDP(usart_send,27+8*nearbase_num);
}
#define FREQ_OFFSET_MULTIPLIER          (998.4e6/2.0/1024.0/131072.0)
#define FREQ_OFFSET_MULTIPLIER_110KB    (998.4e6/2.0/8192.0/131072.0)

// Multiplication factors to convert frequency offset in Hertz to PPM crystal offset
// NB: also changes sign so a positive value means the local RX clock is running slower than the remote TX device.

#define HERTZ_TO_PPM_MULTIPLIER_CHAN_1     (-1.0e6/3494.4e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_2     (-1.0e6/3993.6e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_3     (-1.0e6/4492.8e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_5     (-1.0e6/6489.6e6)
int32_t test2;
uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
uint8_t motor_flag;
uint32_t testtimer[10],testtimer2,anc_pollrx[11],anc_resptx[11],tag_resprx[11];
int32_t anc_clockoffset[11];
int16_t anc_distoffset[11];
extern uint8_t Pah_HRD_flag_fangchai;
uint8_t get_newdist,notenoughdist_count;
void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
{
    memcpy(&anc_pollrx[i],pollrx,4);
    memcpy(&anc_resptx[i],resptx,4);
    memcpy(&tag_resprx[i],&resprx,4);
    memcpy(&anc_distoffset[i],distoffset,2);
    anc_clockoffset[i] = test2;
}
float clockOffsetRatio;
double rtd_init, rtd_resp;
double tof,distance;
extern int32_t dwt_readcarrierintegrator(void) ;
void CalculateDists(void)
{
    for(int i=0; i<11; i++)
    {
        rec_anc_signalpower[i] = exsistbase_list[i];
        if(exsistbase_list[i]==KEEP_TIMES)
        {

            exsistbase_list[i]--;
#ifdef _UWB_4G
            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
#else
            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
#endif
            rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff;
            rtd_resp = anc_resptx[i] - anc_pollrx[i];
            tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
            distance = tof * SPEED_OF_LIGHT;
            if(distance>-10&&distance<1000)
            {
                nearbase_distlist[i] = distance*100+anc_distoffset[i];
            } else {
                nearbase_distlist[i] = 0x1ffff;
            }
        } else {
            nearbase_distlist[i] = 0x1ffff;
        }

    }
}
void DiscPoll2(void)
{
    uint32_t temp1,temp2,dw_systime,mindist_slavebaseid,mindist;
    static u8 notenoughdist_count=0;
    uint32_t final_tx_time;
    u32 start_poll;
    u8 i,j,getsync_flag=0,timeout,get_newdist;

    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);            //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
    tag_succ_times = 0;
    salvebase_mindist=999999;
    get_newdist = 0;
    if(next_nearbase_num>=MAX_NEARBASE_NUM)
    {
        next_nearbase_num = MAX_NEARBASE_NUM-1;
    }
    nearbase_num = 10;
    recbase_num=0;

    tx_near_msg[BATTARY_IDX] = bat_percent;
    tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
    tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
    tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
    tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
    memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);
    if(intheight!=0)
        intheight+=g_com_map[HEIGHTOFFEST_INDEX];
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
    tx_near_msg[MESSAGE_TYPE_IDX] = DISCOVERPOLL2;
    tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;
    tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;
    memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
    dwt_writetxdata(29+8*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
    dwt_writetxfctrl(29+8*nearbase_num, 0);//设置超宽带发送数据长度
    WaitUntilPollStart();
    dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
    tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
    para_update = 0;
    flag_finalsend=0;
    flag_rxon=1;
    flag_getresponse=0;
    start_count=HAL_LPTIM_ReadCounter(&hlptim1);
    recbase_num=0;
    timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
    end_count=start_count+(timeout<<5);
    if(end_count>=32768)
    {
        end_count-=32768;
    }
    mainbase_dist=100000;
    mainbase_lost_count++;
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<end_count||current_count>end_count+15000)
    {
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
        {
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
            if(current_count>=end_count&&current_count<end_count+15000)
                break;
        };
        if(status_reg==0xffffffff)
        {
            NVIC_SystemReset();
        }
        if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
        {
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;    //获得接收到的数据长度
            dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
            test2 = dwt_readcarrierintegrator();
            dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
            dwt_rxenable(0);//打开接收
            if (rx_buffer[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
            {   u16 rec_nearbaseid,rec_nearbasepos;
                poll_tx_ts = get_tx_timestamp_u64();                                        //获得POLL发送时间T1
                resp_rx_ts = get_rx_timestamp_u64();                                        //获得RESPONSE接收时间T4
                // recbase_num++;
                memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                rec_nearbasepos = recbase_num;
                nearbaseid_list[recbase_num] = rec_nearbaseid;
                {
                    //rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                    SetANCTimestap(rec_nearbasepos,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
                    exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
                    //memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                    temp_dist = nearbase_distlist[rec_nearbasepos];
                    if(frame_len==38)
                        memcpy(&rec_anc_signalpower[rec_nearbasepos],&rx_buffer[ANC_SIGNALPOWER],2);

                    nearbase_distlist[rec_nearbasepos]=temp_dist;    //    nearbase_distlist[1]对应    rec_nearbaseid[0]的距离
                }

            }
            else {
                dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
                if(recbase_num!=nearbase_num+1)
                {
                    dwt_rxenable(0);
                }
            }
        }
    }
    dwt_forcetrxoff();
    CalculateDists();
    mindist = 0x1ffff;
    for(i=0; i<11; i++)
    {
        if(mindist>nearbase_distlist[i])
        {
            mindist = nearbase_distlist[i];
            mindist_slavebaseid = nearbaseid_list[i];

        }
    }
        mainbase_id = mindist_slavebaseid;
        try_reg_times = 0;
        tag_state = REGPOLL;
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
}
void NearPoll(void)
{

    uint32_t temp1,temp2,dw_systime,mindist_slavebaseid;
    static u8 notenoughdist_count=0;
    uint32_t final_tx_time;
    u32 start_poll;
    u8 i,j,getsync_flag=0,timeout,get_newdist;
    // USART_putc(current_slotnum);
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);            //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
    tag_succ_times = 0;
    salvebase_mindist=999999;
    get_newdist = 0;
    if(next_nearbase_num>=MAX_NEARBASE_NUM)
    {
        next_nearbase_num = MAX_NEARBASE_NUM-1;
    }
    if(nearbase_num>10)
    {
        nearbase_num = 10;
    }
    recbase_num=0;
//    motor_state=0;
    if(motor_state!=0&&motor_state!=3)
    {
        motor_flag = 1;
    } else {
        motor_flag = 0;
    }

    tx_near_msg[BATTARY_IDX] = bat_percent;
    tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
    tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
    tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
    tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
    memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);


    if(intheight!=0)
        intheight+=g_com_map[HEIGHTOFFEST_INDEX];

    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
    tx_near_msg[MESSAGE_TYPE_IDX] = POS_POLL;

    tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;
    tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;

    memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
    dwt_writetxdata(29+8*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
    dwt_writetxfctrl(29+8*nearbase_num, 0);//设置超宽带发送数据长度
    WaitUntilPollStart();
    dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
    tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
    para_update = 0;
    flag_finalsend=0;
    flag_rxon=1;
    flag_getresponse=0;
    start_count=HAL_LPTIM_ReadCounter(&hlptim1);
    recbase_num=0;
    timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
    end_count=start_count+(timeout<<5);
    if(end_count>=32768)
    {
        end_count-=32768;
    }
    mainbase_dist=100000;
    mainbase_lost_count++;
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<end_count||current_count>end_count+15000)
    {
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
        {
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
            if(current_count>=end_count&&current_count<end_count+15000)
                break;
        };
        if(status_reg==0xffffffff)
        {
            NVIC_SystemReset();
        }
        if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
        {
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;    //获得接收到的数据长度
            dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
            test2 = dwt_readcarrierintegrator();
            dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
            dwt_rxenable(0);//打开接收
            //HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
            if (rx_buffer[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
            {   u16 rec_nearbaseid,rec_nearbasepos;
                poll_tx_ts = get_tx_timestamp_u64();                                        //获得POLL发送时间T1
                resp_rx_ts = get_rx_timestamp_u64();                                        //获得RESPONSE接收时间T4
                recbase_num++;
                memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                if(rec_nearbaseid==mainbase_id)
                {
                    exsistbase_list[0]=KEEP_TIMES;
                    SetANCTimestap(0,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

                    //////////////////////////////////时间同步
                    memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
                    memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
                    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
                    dwt_forcetrxoff();
                    rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;
                    if(rec_remotepara_state&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2))
                    {
                        para_update = 1;
                        tx_near_msg[TAGCONFIGSUCCESS_INDEX] = 1;
                        para_len = frame_len-22;
                        memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],para_len);
                    }
                    userkey_state = !GET_USERKEY;
                    temp_dist = nearbase_distlist[0];
                    if(temp_dist!=nearbase_distlist[0])
                    {
                        get_newdist++;
                    }
                    mainbase_dist=temp_dist;
                    nearbase_distlist[0] = temp_dist;
                    base_mindist = temp_dist;
                    if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
                        motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;
                    if(frame_len==38)
                    {
                        memcpy(&rec_maxrangelen,&rx_buffer[MAXRANGE_DISTANCE],2);
                        memcpy(&rec_anc_signalpower[0],&rx_buffer[ANC_SIGNALPOWER],2);
                        if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)
                        {
                            if(outrange_times++>OUTRANGE_RESTARTTIMES)
                            {
                                tag_state = STARTPOLL;
                            }
                        } else {
                            outrange_times = 0;
                        }
                    }
                    mainbase_lost_count=0;
                    flag_finalsend=1;
                    flag_getresponse=1;
                    //时间同步
                    tmp_time=tmp_time+450;
                    if(tmp_time>999)
                    {
                        tmp_time-=999;
                        sync_timer++;
                        if(sync_timer>=1000)
                        {
                            sync_timer=0;
                        }
                    }
                    ancsync_time=((sync_timer)*1000+tmp_time);
                    SetLPTimer(ancsync_time,nearbase_num);
                    break;
                } else {

                    rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                    SetANCTimestap(rec_nearbasepos+1,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

                    exsistbase_list[rec_nearbasepos+1]=KEEP_TIMES;
                    temp_dist = nearbase_distlist[rec_nearbasepos+1];
                    if(frame_len==38)
                        memcpy(&rec_anc_signalpower[rec_nearbasepos+1],&rx_buffer[ANC_SIGNALPOWER],2);
                    memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buffer[NR_NEARSWITCH_DISTANCE],2);
                    if(temp_dist!=nearbase_distlist[rec_nearbasepos+1])
                    {
                        get_newdist++;
                    }
                    nearbase_distlist[rec_nearbasepos+1]=temp_dist;    //    nearbase_distlist[1]对应    rec_nearbaseid[0]的距离
                    final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
                    if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]!=0&&(nearbase_switchdistlist[rec_nearbasepos]==1||temp_dist<nearbase_switchdistlist[rec_nearbasepos]))
                    {
                        salvebase_mindist = temp_dist;
                        mindist_slavebaseid = rec_nearbaseid;
                    }
                }
            }
        } else {
            dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
            if(recbase_num!=nearbase_num+1)
            {
                dwt_rxenable(0);
            }
        }
    }
    if(flag_finalsend!=1)
    {
        flag_finalsend = 2;
    }
    dwt_forcetrxoff();
    dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
//            HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
    if(mainbase_lost_count==0)
    {
#ifdef SWITCHBASE_DIST
        if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST )
        {
            changemainbase_count++;
            if(changemainbase_count>2)
            {
                changemainbase_count = 0;
                mainbase_id = mindist_slavebaseid;
                tag_state = GETNEARMSG;
                trygetnearmsg_times = 0;
                GetNearMsg();
            }
        } else {
            changemainbase_count = 0;
        }
#endif
    } else if(mainbase_lost_count>10)
    {
        mainbase_lost_count = 0;
        tag_state = DISCPOLL;
        DiscPoll2();
    }
    if(mainbase_lost_count!=0)
    {
        if(mainbase_lost_count<=5)
        {   NextSlotDelayMs(0);
        } else {
            NextSlotDelayMs(0);
        }
    }
    CalculateDists();
    if(para_update)
    {

        pack_msgtype = rec_remotepara[0];
        pack_index = rec_remotepara[1];
        pack_length = rec_remotepara[2];
        if(pack_msgtype==2)
        {
            if( pack_index == MOTOR_ONTIME_INDEX)
            {
                if(motor_keeptime==0)
                    motor_keeptime = rec_remotepara[3];
            } else if( pack_index == 2*COM_INTERVAL)
            {
                if(memcmp(&g_com_map[COM_INTERVAL],&rec_remotepara[3],2)!=0)
                {
                    memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
                    save_com_map_to_flash();
                    tag_frequency = 1000/g_com_map[COM_INTERVAL];
                    bigslot_num = TOTAL_SLOTNUM/tag_frequency;
                }
            }
            else {
                if(pack_index<200)
                {
                    memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
                    //返回一个error状态
                    //SendComMap(pack_datalen,pack_index);
                    save_com_map_to_flash();
                    delay_ms(100);
                    NVIC_SystemReset();
                }
            }
        }
    }
//            if(get_newdist>=2)
//            {
//                notenoughdist_count = 0;
//            }else{
//                if(notenoughdist_count++>10)
//                {
//                    notenoughdist_count = 0;
//                    tag_state = DISCPOLL;
//                }
//            }
    for(i=0; i<nearbase_num; i++)
    {
        if(nearbaseid_list[i]!=nearbaseid_list2[i])
        {
            tag_state = GETNEARMSG;
            GetNearMsg();
            trygetnearmsg_times = 0;
            nearbaseid_list0[i]=1;
        }
    }
    //    SendHuiZongData();
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
//HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);

}
extern uint8_t module_power,imu_enable,motor_enable;
void Registor_Poll(void)
{
    static u8 regpoll_count=0;
    mainbase_lost_count = 0;
    tag_frequency = REGISTER_FREQUENCY;
    bigslot_num = TOTAL_SLOTNUM/tag_frequency;
    regpoll_count++;
    if(regpoll_count%2)
    {
        if(tagslotpos--<2)
            tagslotpos=TOTAL_SLOTNUM;
    }
    //SetNextPollTime(tagslotpos);
    NextSlotDelayMs(-2);

    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);            //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);

    tx_near_msg[REGP_TAGSTATE_INDEX] = !GET_USERKEY<<2|imu_enable<<1|motor_enable;
    tx_near_msg[MESSAGE_TYPE_IDX] = REG_POLL2;
    tx_near_msg[REGP_FREQUENCY_INDEX] = 1000/g_com_map[COM_INTERVAL];
    tx_near_msg[REGP_POWER_INDEX] = module_power;
    memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);
    memcpy(&tx_near_msg[REGP_VERSION_INDEX],&g_com_map[VERSION],2);
    memcpy(&tx_near_msg[REGP_IMUTHRES_INDEX],&g_com_map[IMU_THRES],2);
    memcpy(&tx_near_msg[REGP_NOMOVESLEEPTIME_INDEX],&g_com_map[NOMOVESLEEP_TIME],2);
    send_buffer[REGP_BATTARY_INDEX] = bat_percent;
//    memcpy(&tx_near_msg[REGP_HEIGHTOFFSET_INDEX],&g_com_map[HEIGHTOFFEST_INDEX],2);
    dwt_writetxdata(23, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
    dwt_writetxfctrl(23, 0);//设置超宽带发送数据长度
    dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
    {
        IdleTask();

    };
    if(status_reg==0xffffffff)
    {
        NVIC_SystemReset();
    }
    if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
    {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
        frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;    //获得接收到的数据长度
        dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
        if (rx_buffer[MESSAGE_TYPE_IDX] == REG_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
        {   u16 rec_nearbaseid,rec_nearbasepos;

            memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
            tag_frequency = rx_buffer[REGR_TAGFREQ_INDEX];
            bigslot_num = TOTAL_SLOTNUM/tag_frequency;
            tagslotpos = rx_buffer[REGR_TAGSLOTPOS_INDEX];
            //////////////////////////////////时间同步
            memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
            memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//                                    memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);
            tmp_time=tmp_time+450;
            if(tmp_time>999)
            {
                tmp_time-=999;
                sync_timer++;
                if(sync_timer>=1010)
                {
                    sync_timer=0;
                }
            }
            ancsync_time=((sync_timer)*1000+tmp_time);
            SetLPTimer(ancsync_time,0);
            mainbase_id=rec_nearbaseid;
            nearbase_num=rx_buffer[NEARBASENUM_INDEX];
            memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
            memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
            if(tagslotpos!=255)
            {
                trygetnearmsg_times = 0;
                tag_state = GETNEARMSG;
                GetNearMsg();
            }
        }
    }
}
void SwitchTagState(void)
{
    switch(tag_state)
    {
    case REGPOLL:
        LED_LG_ON;
        Registor_Poll();
        if(try_reg_times++>5)
        {
            tag_state = STARTPOLL;
        }
        LED_LG_OFF;
//        GetPressAndHeight();
        break;
    case DISCPOLL:
        LED_LG_ON;
        DiscPoll2();
        LED_LG_OFF;
        break;
    case GETNEARMSG:

        LED_LG_ON;
        GetNearMsg();
        LED_LG_OFF;
        if(trygetnearmsg_times++>5)
        {
            tag_state = STARTPOLL;
        }
        break;
    case NEARPOLL:
        NearPoll();
        //    GetPressAndHeight();
        //    intheight =Altitude*100;
        break;
    case SINGLEPOLL:
        //  Poll();
        break;
    }
}