电池标签L051工程
blame | 历史 | 补丁 | 提交 | 提交对比 | 忽略空白
yincheng.zhong
2023-05-27 a5e16e925923fa444276ca51f0e7c0cbbe07a96c 
 Src/application/dw_app.c


@@ -97,7 +97,6 @@


static uint64_t poll_rx_ts;


static uint64_t resp_tx_ts;


static uint64_t final_rx_ts;


static double tof;


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;


@@ -304,7 +303,56 @@


        freqlost_count = 0;


    }


}


#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)




#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)


u16 rec_maxrangelen;//rec_anc_signalpower[MAX_NEARBASE_ANCNUM];


float clockOffsetRatio;


double rtd_init, rtd_resp;


double tof,distance;


extern int32_t dwt_readcarrierintegrator(void) ;


uint32_t testtimer2,anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];


int32_t anc_clockoffset[MAX_NEARBASE_NUM];


int16_t anc_distoffset[MAX_NEARBASE_NUM];


int32_t test2;


uint16_t u16_nearbase_distlist[MAX_NEARBASE_NUM];


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;


}


void CalculateDists(void)


{


    for(int i=0;i<MAX_NEARBASE_NUM;i++)


    {


        //rec_anc_signalpower[i] = exsistbase_list[i];


      if(exsistbase_list[i]==KEEP_TIMES)


      {




         // exsistbase_list[i]--;


          clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;


          rtd_init = tag_resprx[i] - poll_tx_ts;


          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;


      }


    


    }


}


float range_lost_time = 0;


u16 rec_nearbaseid,rec_nearbasepos;


void NearPoll(void)


{


   


@@ -314,39 +362,45 @@


   u32 start_poll;


   u8 i,j,getsync_flag=0,timeout,flag_ancreadpara=0;


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


    dwt_setrxtimeout(10000);		


    dwt_setrxtimeout(0);		


   tag_succ_times = 0;


      //GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);


   if(next_nearbase_num>=MAX_NEARBASE_NUM)


   if(nearbase_num>=MAX_NEARBASE_NUM)


   {


      next_nearbase_num = MAX_NEARBASE_NUM-1;


      nearbase_num = MAX_NEARBASE_NUM-1;


   }


   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);


   last_nearbase_num=next_nearbase_num;


   nearbase_num=next_nearbase_num;


//   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);


//   last_nearbase_num=next_nearbase_num;


//   nearbase_num=next_nearbase_num;


   recbase_num=0;


//   motor_state=0;


    for(uint8_t i=0;i<nearbase_num;i++)


    {


        u16_nearbase_distlist[i] = nearbase_distlist[i];


    }


   tx_near_msg[GROUP_ID_IDX] = group_id;


   tx_near_msg[BATTARY_IDX] = bat_percent;


   tx_near_msg[BUTTON_IDX] = !GET_USERKEY|stationary_flag<<1;


   tx_near_msg[SEQUENCE_IDX] = frame_seq_nb++;


   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],&u16_nearbase_distlist,nearbase_num*2);


   tx_near_msg[MESSAGE_TYPE_IDX] = MBX_POLL;   


   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);   


   dwt_writetxdata(13+2*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去


   dwt_writetxfctrl(13+2*nearbase_num, 0);//设置超宽带发送数据长度


   dwt_writetxdata(13+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去


   dwt_writetxfctrl(13+4*nearbase_num, 0);//设置超宽带发送数据长度


   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置


   


   tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;


   para_update = 0;


    get_newbase = 0;


   flag_finalsend=0;


   flag_rxon=1;


   flag_getresponse=0;


   start_count=HAL_LPTIM_ReadCounter(&hlptim1);


   recbase_num=0;


   timeout=nearbase_num*SLOT_SCALE+4;


   end_count=start_count+(timeout<<5);


   timeout=nearbase_num*SLOT_SCALE+10;


   end_count=start_count+(timeout<<2);


   if(end_count>=32768)


   {end_count-=32768;}


   mainbase_dist=100000;


@@ -357,7 +411,7 @@


            current_count=HAL_LPTIM_ReadCounter(&hlptim1);


               while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误


               {


                  if(flag_finalsend)


                  if(flag_finalsend&&flag_ancreadpara)


                  {


                     dw_systime=dwt_readsystimestamphi32();


                     if(dw_systime>temp1&&dw_systime<temp2)


@@ -374,9 +428,10 @@


                                  memcpy(&tx_near_msg[MESSAGE_PARAREPLY_IDX+SENDANCPARA_LEN],&calCRC,2);


                                  dwt_writetxdata(14+SENDANCPARA_LEN, tx_near_msg, 0);//将发送数据写入DW1000


                                  dwt_writetxfctrl(14+SENDANCPARA_LEN, 0);//设定发送数据长度


                                }


                                


                        dwt_setdelayedtrxtime(final_tx_time);


                        result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送   


                                }


                        flag_finalsend=0;


                        break;


                     }


@@ -394,10 +449,11 @@


                     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);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//设定接收超时时间，0位没有超时时间


                     dwt_rxenable(0);//打开接收


                     if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是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++;


@@ -422,6 +478,9 @@


                        }


                        if(rec_nearbaseid==nearbaseid_list[0])


                        {


                                     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);


@@ -442,8 +501,7 @@


                           ////////////////////////////


                           rec_nearbasepos=0;


                           exsistbase_list[rec_nearbasepos]=KEEP_TIMES;


                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);


                           nearbase_distlist[rec_nearbasepos]=temp_dist;




                           if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)


                              motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;


                           


@@ -469,13 +527,13 @@


                           


                        


                           //时间同步


                           ancsync_time=((sync_timer)*1000+tmp_time);	


                           last_slotnum=current_slotnum;


                           current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;


                           if(current_slotnum==last_slotnum-1)


                           {flag_getresponse=1;}


                           lastsync_timer=sync_timer;


                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;


//                           ancsync_time=((sync_timer)*1000+tmp_time);	


//                           last_slotnum=current_slotnum;


//                           current_slotnum=((float)sync_timer/g_com_map[COM_INTERVAL])+1;


//                           if(current_slotnum==last_slotnum-1)


//                           {flag_getresponse=1;}


//                           lastsync_timer=sync_timer;


//                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;


//                           SetNextPollTime(tyncpoll_time);


//                           if(rec_remotepara_state==1)


//                           {


@@ -490,8 +548,9 @@


//                           } 


                        }else{


                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);


									


                              if(rec_nearbasepos>=last_nearbase_num)          //发现新的基站


                           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]);




                           if(rec_nearbasepos>=last_nearbase_num)          //发现新的基站


                           {


                              get_newbase=1;


                              nearbase_num++;


@@ -500,8 +559,7 @@


                           }


                           


                           exsistbase_list[rec_nearbasepos]=KEEP_TIMES;


                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);


                           nearbase_distlist[rec_nearbasepos]=temp_dist;




                           


                           if(motor_state<(rx_buffer[MOTORSTATE_INDEX]&0xf)&&(rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)


                           {


@@ -526,24 +584,24 @@


                  }         


               //   dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);


         }


         if(mainbase_lost_count>tag_frequency*BASELOST_STOPMOTOR_TIME)


         {motor_state=0; }


            dwt_forcetrxoff();


            dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);


            nearbase_num=recbase_num;


      //   tyncpoll_time=0;


         next_nearbase_num=0;


            CalculateDists();


            j = 0;


         for(i=0;i<last_nearbase_num+get_newbase;i++)


            next_nearbase_num = 0;


         for(i=0;i<nearbase_num;i++)


         {


            if(exsistbase_list[i]>0)


            {		


            {	


                    exsistbase_list[i]--;	


               next_nearbase_num++;


               true_exsistbase_list[j]=exsistbase_list[i];


               true_nearbase_idlist[j]=nearbaseid_list[i];         


               true_nearbase_distlist[j++]=nearbase_distlist[i];


               exsistbase_list[i]--;


					


            }


         }


            nearbase_num = next_nearbase_num;


            if(mainbase_lost_count>1&&exsistbase_list[0] != 0)


         {


                last_lpcount+= HAL_LPTIM_ReadCounter(&hlptim1)%(g_com_map[COM_INTERVAL]*16);


@@ -571,33 +629,29 @@


         }


         


         report_num=0;


//            if(last_nearbase_num>4)


//            {


//                last_nearbase_num = 5;


//            }


   for (i=0;i<last_nearbase_num;i++)


         {


            nearbaseid_list[i]=true_nearbase_idlist[i];


            nearbase_distlist[i]=true_nearbase_distlist[i];


            if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==KEEP_TIMES)


            {


               memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);


               memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);


               report_num++;


            }


                exsistbase_list[i] = true_exsistbase_list[i];


         }


            #ifdef USART_INTEGRATE_OUTPUT	


                usart_send[0] = 0x55;     


                usart_send[1] = 0xAA;              


            usart_send[2] = 4;//正常模式


            usart_send[3] = report_num*6+2;//正常模式


            checksum = Checksum_u16(&usart_send[2],report_num*6+2);


            memcpy(&usart_send[4+report_num*6],&checksum,2);


            USART_puts(usart_send,6+report_num*6);


         #endif


//            #ifdef USART_INTEGRATE_OUTPUT	


//                usart_send[0] = 0x55;     


//                usart_send[1] = 0xAA;              


//            usart_send[2] = 4;//正常模式


//            usart_send[3] = report_num*6+2;//正常模式


//            checksum = Checksum_u16(&usart_send[2],report_num*6+2);


//            memcpy(&usart_send[4+report_num*6],&checksum,2);


//            USART_puts(usart_send,6+report_num*6);


//         #endif


            


         for(i=0;i<MAX_NEARBASE_NUM;i++)


         {


            nearbase_distlist[i]=0x1ffff;


         }


   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);


            


if(para_update==1)


    {   


        uint16_t calCRC;


@@ -605,7 +659,7 @@


        if(memcmp(&calCRC,&rec_remotepara[5],2)==0)


        {   uint8_t pack_msgtype,pack_index,pack_length;


            pack_msgtype = rec_remotepara[0];


            pack_index = rec_remotepara[1]*2;


            pack_index = rec_remotepara[1];


            pack_length = rec_remotepara[2];


            if(pack_msgtype==2)


            {