电池标签L051工程
blame | 历史 | 补丁 | 提交 | 提交对比 | 忽略空白
yincheng.zhong
2022-12-16 1f703ab0c4761e3260b68481bf9f8626f08e7c98 
 Src/application/dw_app.c


@@ -69,7 +69,7 @@


#define FINAL_MSG_FINAL_TX_TS_IDX 18


#define FINAL_MSG_TS_LEN 4




#define STARTPOLL  REGPOLL//#define SWITCHBASE_DIST


#define STARTPOLL  DISCPOLL//#define SWITCHBASE_DIST


#define SWITCHBASE_ZHUANDIAN


#define SWITCHBASE_DIST


enum enumtagstate


@@ -81,7 +81,7 @@


    SINGLEPOLL,


}tag_state=STARTPOLL;


static dwt_config_t config = {


   3,               /* Channel number. */


   2,               /* Channel number. */


   DWT_PRF_64M,     /* Pulse repetition frequency. */


   DWT_PLEN_128,    /* Preamble length. */


   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */


@@ -117,8 +117,9 @@


uint32_t g_UWB_com_interval = 0; 


float dis_after_filter;            //当前距离值


LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器


uint16_t current_slotpos = 0;






uint8_t rec_anc_sync_sec,tagpoll_sync_sec,current_sync_sec,flag_tagnewpos;




static uint64_t get_tx_timestamp_u64(void)


{


@@ -474,6 +475,7 @@






#define SINGLEPOLL_BASENUM 5


#define ANCHORTYPE_NUM   3


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




@@ -496,7 +498,7 @@


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


    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      


   frame_seq_nb++;


   for(uint8_t singlepoll_i=0;singlepoll_i<10;singlepoll_i++)


   for(uint8_t singlepoll_i=0;singlepoll_i<ANCHORTYPE_NUM;singlepoll_i++)


   {


       




@@ -515,7 +517,7 @@


 //   memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*6],&intheight,2);


   send_buffer[MESSAGE_TYPE_IDX] = DISCOVERPOLL;   


//   singlepoll_basedist[singlepoll_i]= 0x1ffff;


   send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	


   //send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	


   send_buffer[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;   


    send_buffer[ANCHOR_ID_IDX] = singlepoll_i;


   memcpy(&send_buffer[TAG_ID_IDX],&dev_id,2);   


@@ -573,7 +575,7 @@


                memcpy(&mindist_ancid,&rx_buffer[ANCHOR_ID_IDX],2);


                


            }


            send_buffer[MESSAGE_TYPE_IDX] = FINAL;	


            send_buffer[MESSAGE_TYPE_IDX] = DISCOVERFINAL;	


         /* Compute final message transmission time. See NOTE 9 below. */


         final_tx_time = (resp_rx_ts + (600 * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2


         dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5


@@ -617,15 +619,8 @@


   


   if(getsync_flag==0)


   {


//   tagslotpos--;


//         if(tagslotpos==0||tagslotpos>max_slotpos)


//      {		


//         tagslotpos=max_slotpos;	


//      }


//      tyncpoll_time=(tagslotpos-1)*slottime;


   }


//   mainbase_id=minid;


//   mainbase_id = 0x4;




}


int16_t target_offsettime = 850;


//#define TAGET_OFFSETTIME 940


@@ -640,7 +635,7 @@


u16 delaycount;


int16_t poll_offsettime=-6900,clockoffset;


extern u16 slotpos_intoatl, slotpos;


void NextPollDelay(u32 anchor_time)


void NextPollDelay(u32 anchor_time)  //根据当前基站的时间，计算下一次需要工作的时间的延迟。然后设定定时器到这个时间。


{


      //tagslotpos = 0;


      error_time = anchor_time-expect_anctime;


@@ -683,7 +678,7 @@


      }


    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);


      last_lpcount = lpcount;


   }


}




void NextSlotDelayMs(int16_t delayms)


{


@@ -707,7 +702,7 @@


         __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);


            last_lpcount = lpcount;


}




uint16_t rec_tagperiod;


u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;


void GetNearMsg(void)


{


@@ -721,13 +716,13 @@


//mainbase_id = 0x9818;


   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] = NEAR_MSG;


	


   tx_near_msg[MESSAGE_TYPE_IDX] = POS_MSG;


   memcpy(&tx_near_msg[TAGMSG_CURRENTPOS_IDX],&current_slotpos,2);


    


   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);//设置超宽带发送数据长度


    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);	


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


   dwt_writetxfctrl(14, 0);//设置超宽带发送数据长度


      current_count=HAL_LPTIM_ReadCounter(&hlptim1);


      while(current_count<poll_startcount||current_count>poll_startcount+16384)


      {


@@ -752,24 +747,54 @@


      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] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据


      if (rx_buffer[MESSAGE_TYPE_IDX] == POS_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据


      {


           


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


         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;	


            memcpy(&current_slotpos,&rx_buffer[ANCMSG_TAGSLOTPOS_IDX],2);


            memcpy(&rec_tagperiod,&rx_buffer[ANCMSG_TAGPERIODMS_IDX],2);


            tagpoll_sync_sec = current_slotpos/200;


            tagslotpos = current_slotpos%200;


            max_slotpos = rec_tagperiod*20;


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


//}


uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;


uint8_t motor_flag;


uint16_t rec_ancpos;


void NearPoll(void)


{


   


@@ -777,7 +802,7 @@


    static u8 notenoughdist_count=0;


   uint32_t final_tx_time;


   u32 start_poll;


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


   u8 i,j,getsync_flag=0,timeout,get_newdist,pos_nochange=1;


   // USART_putc(current_slotnum);


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


    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      


@@ -794,25 +819,32 @@




   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;


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




    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] = NEAR_POLL;	


	


   tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;	


   tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;	


   tx_near_msg[MESSAGE_TYPE_IDX] = POS_POLL;	


    


   tx_near_msg[NEARP_TAGFREQ_INDEX] = rec_tagperiod;	


   tx_near_msg[NEARP_TAGNEWPOSFLAG_INDEX] = flag_tagnewpos;	


   memcpy(&tx_near_msg[NEARP_TAGSLOTPOS_INDEX],&current_slotpos,2);	




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


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


@@ -869,12 +901,32 @@


                     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] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据


                     {   u16 rec_nearbaseid,rec_nearbasepos;


                     if (rx_buffer[MESSAGE_TYPE_IDX] == POS_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据


                     {   u16 rec_nearbaseid,rec_nearbasepos,next_slotpos = current_slotpos;


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


                                memcpy(&rec_ancpos,&rx_buffer[PR_TAGPOS_INDEX],2);


                                if(rec_ancpos<max_slotpos)  //接收到基站回复的时间片位置后，第一个不一样就改变，后续的不一样就找最大的时间片


                                {


                                    if(pos_nochange)


                                    {


                                        if(rec_ancpos!=next_slotpos)


                                        {   


                                            flag_tagnewpos = 1;


                                            next_slotpos = rec_ancpos;


                                            pos_nochange =0;


                                        }


                                    }else{


                                         if(rec_ancpos>next_slotpos)


                                         {


                                             flag_tagnewpos = 1;


                                             next_slotpos = rec_ancpos;


                                         }


                                    }                                 


                                }




                        if(rec_nearbaseid==mainbase_id)


                        {


                           //////////////////////////////////时间同步


@@ -887,7 +939,7 @@


                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据


                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+nearbase_num*4], resp_rx_ts);


                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);


                           tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;


                           tx_near_msg[MESSAGE_TYPE_IDX]=POS_FINAL;


                           rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;


                        if(rec_remotepara_state&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2))


                           {


@@ -918,9 +970,10 @@


                                         memcpy(&rec_anc_signalpower[0],&rx_buffer[ANC_SIGNALPOWER],2);


                                         if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)


                                         {


                                             if(mainbase_dist!=0x1ffff)


                                             if(outrange_times++>OUTRANGE_RESTARTTIMES)


                                             {


                                               // tag_state = STARTPOLL;


                                                tag_state = STARTPOLL;


                                             }


                                         }else{


                                             outrange_times = 0;


@@ -940,11 +993,18 @@


                           if(tmp_time>999)


                           {


                              tmp_time-=999;


                              sync_timer++;


                              if(sync_timer>=1000)


                                 {sync_timer=0;}


                              sync_timer++;                                       


                           }


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


                                    rec_anc_sync_sec = sync_timer/1000;


                                    current_sync_sec = rec_anc_sync_sec+1;


                                    if(current_sync_sec>=rec_tagperiod/10)


                                    {


                                        current_sync_sec = 0;


                                    }


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


                                    current_slotpos = next_slotpos;


                                    tagpoll_sync_sec = current_slotpos/200;


                                    tagslotpos = current_slotpos%200;


                           SetLPTimer(ancsync_time);


                           if(result==0)


                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成


@@ -975,7 +1035,7 @@


                             {


                                            mainbase_id = rec_nearbaseid;


                                            tag_state = GETNEARMSG;


                                                                  trygetnearmsg_times = 0;


                                 trygetnearmsg_times = 0;


                                        }


                                    #endif




@@ -1014,17 +1074,21 @@


               changemainbase_count = 0;


            }


         #endif


         }else if(mainbase_lost_count>5*tag_frequency)


         }else if(mainbase_lost_count>5)


         {


            tag_state = DISCPOLL;


                current_slotpos = start_count%max_slotpos;  //如果丢失连接超过5次就随机换个时间片


                tagpoll_sync_sec = current_slotpos/200;


                tagslotpos = current_slotpos%200;


                flag_tagnewpos = 0;


         }   


         if(mainbase_lost_count!=0)


         {


            if(mainbase_lost_count<=tag_frequency*1)


            {NextSlotDelayMs(0);


            }else{


               NextSlotDelayMs(0);


            }


//            if(mainbase_lost_count<=tag_frequency*1)


//            {NextSlotDelayMs(0);


//            }else{


//               NextSlotDelayMs(0);


//            }


         }   




         if(para_update)


@@ -1037,7 +1101,7 @@


            {


               if( pack_index == MOTOR_ONTIME_INDEX)


               {


                        if(motor_keeptime==0)


                       // if(motor_keeptime==0)


                            motor_keeptime = rec_remotepara[3];


               }else{


                        if(pack_index<200)


@@ -1071,6 +1135,11 @@


                           nearbaseid_list0[i]=1;


                }


         }


            if(pos_nochange)


            {


                flag_tagnewpos = 0;


            }


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




@@ -1089,14 +1158,14 @@


            tagslotpos=TOTAL_SLOTNUM;


      }


      //SetNextPollTime(tagslotpos);


      NextSlotDelayMs(-2);


      //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_POLL;   


      tx_near_msg[REGP_FREQUENCY_INDEX] = 1000/g_com_map[COM_INTERVAL];


      tx_near_msg[REGP_FREQUENCY_INDEX] =rec_tagperiod;


      tx_near_msg[REGP_POWER_INDEX] = module_power;


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


@@ -1131,32 +1200,20 @@


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


//                           last_slotnum=current_slotnum;


//                           temptimer = sync_timer-10;


//                           if(temptimer<0)


//                           {temptimer+=1000;}


//                           current_slotnum=((float)temptimer/(SLOTTIME_MS*bigslot_num))+1;


//                           if(current_slotnum==last_slotnum-1)


//                           {flag_getresponse=1;}


//                           lastsync_timer=sync_timer;


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


//                           SetNextPollTime(tagslotpos);


//                           tmp_time=tmp_time+450;


//                           if(tmp_time>999)


//                           {


//                              tmp_time-=999;


//                              sync_timer++;					


//                           }


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


//                           SetLPTimer(ancsync_time);


                           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)


                           tag_state = DISCPOLL;


//                           if(tagslotpos!=255)


//                           tag_state = DISCPOLL;


                        }


                     }


}


@@ -1165,7 +1222,7 @@


extern float Height;


void Tag_App(void)//发送模式(TAG标签)


{


	


   static uint8_t diccount = 0;


   //LED0_ON;


   SPIx_CS_GPIO->BRR = SPIx_CS;


   delay_us(700);


@@ -1191,14 +1248,26 @@


         LED_LG_OFF;


         break;


      case DISCPOLL:


         LED_LG_ON;		


         DiscPoll();


         LED_LG_ON;	


        


         //DiscPoll();


            mainbase_lost_count = 0;


            tagpoll_sync_sec = 0;


            rec_tagperiod = 10;


            mainbase_id = 0xffff;


            GetNearMsg();


            if(diccount++%10==0)


            {


                Registor_Poll();


            }        


            NextSlotDelayMs(-5);


         LED_LG_OFF;


         break;


      case GETNEARMSG:


         trygetnearmsg_times++;   


         LED_LG_ON;


            GetNearMsg();	 


     


            GetNearMsg();	 


      LED_LG_OFF;


      if(trygetnearmsg_times>5)


      {


@@ -1207,6 +1276,7 @@


         break;


      case NEARPOLL:   


         NearPoll();


       


         //GetPressAndHeight();


         //intheight = Height*100;


         break;