F103基站代码
blame | 历史 | 补丁 | 提交 | 提交对比 | 忽略空白
zhyinch
2021-05-03 f34d8a58e94582091a28649d758591b705cae387 
 Ô´Âë/ºËÐÄ°å/Src/application/dw_app.c


@@ -47,7 +47,7 @@


uint32_t g_UWB_com_interval = 0; 


float dis_after_filter;            //当前距离值


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


uint16_t g_Tagdist[TAG_NUM_IN_SYS];


int32_t g_Tagdist[TAG_NUM_IN_SYS];


uint8_t g_flag_Taggetdist[256];




static uint64_t get_tx_timestamp_u64(void)


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


        *ts += ts_field[i] << (i * 8);


    }


}


extern int32_t tagdist_list[TAG_NUM_IN_SYS];


void TagDistClear(void)


{


   static uint16_t clear_judge_cnt;


@@ -109,7 +110,7 @@


         g_flag_Taggetdist[i]++;


         if(g_flag_Taggetdist[i]>=2)


         {


            g_Tagdist[i]=0xffff;


            tagdist_list[i]=0x1ffff;


         }


      }


   }


@@ -165,7 +166,7 @@


}




u16 tag_time_recv[TAG_NUM_IN_SYS];


u8 usart_send[100],usart_send_anc[100];


u8 usart_send[150],usart_send_anc[100];


u8 battary,button,tag_frequency,tag_slotpos;


extern uint8_t g_pairstart;


void tag_sleep_configuraion(void)


@@ -839,6 +840,7 @@


   }


   return i;


}


u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;


uint32_t frame_len;


uint32_t resp_tx_time;


uint8_t rec_nearbase_num,anc_report_num;


@@ -863,8 +865,7 @@


         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度


         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收




         battary = rx_buffer[BATTARY_IDX];


         button = rx_buffer[BUTTON_IDX];


			


         frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];


         if(result==0)


         {


@@ -905,11 +906,44 @@


               LED0_BLINK; //每成功一次通讯则闪烁一次


               dis_after_filter=dist_cm;


               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];


               if(abs(hex_dist-his_dist[taglist_pos])<1000)


                  {


                     tagdist_list[taglist_pos] = hex_dist;


                        }


                  his_dist[taglist_pos]=hex_dist;							


if(hex_dist>-1000&&hex_dist<200000)


               {


                        g_flag_Taggetdist[taglist_pos]=0;


               if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)


               {


                  misdist_num[taglist_pos]=0;


               tagdist_list[taglist_pos] = hex_dist;	


               his_dist[taglist_pos]=hex_dist;	


                  g_Tagdist[taglist_pos]=hex_dist;	


            #ifndef USART_INTEGRATE_OUTPUT


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


               usart_send[3] = 17;//数据段长度


               usart_send[4] = frame_seq_nb2;//数据段长度


               memcpy(&usart_send[5],&tag_id_recv,2);


               memcpy(&usart_send[7],&dev_id,2);				


               memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);


               usart_send[13] = battary;


               usart_send[14] = button;


               usart_send[15] = firstpath_power;


               usart_send[16] = (rx_power-firstpath_power)*10;


               usart_send[17] = tag_frequency;


               usart_send[18] = tag_slotpos;


               checksum = Checksum_u16(&usart_send[2],17);


               memcpy(&usart_send[19],&checksum,2);


            //   UART_PushFrame(usart_send,21);


            #else		


               memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);


               memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);


               anc_report_num++;


            #endif


             #ifdef DEBUG_INF


             printf("序号：%d\r\n",frame_seq_nb2);


             #endif


               }else{


               //   printf("%d",hex_dist);


                  misdist_num[taglist_pos]++;


               }


            }						


            }


         }else{


            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);


@@ -918,7 +952,8 @@


uint32_t time_monitor[10];


extern uint16_t configremotetagID;


extern u8 remotetag_paralen;


u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;


u8 finalsend=0;




u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base


{


   


@@ -934,7 +969,7 @@


         dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3


         dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置发送完成后开启接收延迟时间


         dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间


         dwt_readdiagnostics(&d1);


         //dwt_readdiagnostics(&d1);


         


         if(new_tagid)


         {


@@ -947,6 +982,7 @@


         tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;


         tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;


         waittagconfig_reponse=0;


            finalsend = 0;


         if(pwtag.remain_time>0)


            {uint8_t i;


               for(i=0;i<pwtag.groupnum;i++)


@@ -964,11 +1000,14 @@


                     dwt_writetxdata(24+remotetag_paralen, tx_nearresp_msg, 0);//写入发送数据


                     dwt_writetxfctrl(24+remotetag_paralen, 0);//设定发送长度      


                     remotesend_state=0;


                            finalsend = 1;


                     break;


                  }            


               }


                }


               


            }else{


            if(finalsend == 0)


                {


                  dwt_writetxdata(24, tx_nearresp_msg, 0);//写入发送数据


                  dwt_writetxfctrl(24, 0);//设定发送长度


            }


@@ -983,11 +1022,6 @@




         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收




         battary = rx_buffer[BATTARY_IDX];


         button = rx_buffer[BUTTON_IDX];


         frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];


         tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];


         tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];


         if(result==0)


         {


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


@@ -1045,9 +1079,10 @@


               LED0_BLINK; //每成功一次通讯则闪烁一次


               dis_after_filter=dist_cm;


               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];


               g_flag_Taggetdist[taglist_pos]=0;


					


               if(hex_dist>-1000&&hex_dist<200000)


               {


                        g_flag_Taggetdist[taglist_pos]=0;


               if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)


               {


                  misdist_num[taglist_pos]=0;


@@ -1069,7 +1104,7 @@


               usart_send[18] = tag_slotpos;


               checksum = Checksum_u16(&usart_send[2],17);


               memcpy(&usart_send[19],&checksum,2);


               UART_PushFrame(usart_send,21);


            //   UART_PushFrame(usart_send,21);


            #else      


               memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);


               memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);


@@ -1103,12 +1138,15 @@


            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);


         }


}




extern int32_t intheight;


extern u16 synclost_count;


uint32_t current_syncid=0xffffffff,synclost_timer;


extern u8 flag_syncbase,waitsync_flag;


u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;


u16 ancidlist_rec[20],ancidlist_send[20];


u16 recnearbaselist_id[30];


int32_t recnearbaselist_dist[30];


int16_t rec_tagheight;


void Anchor_App(void)


{


   


@@ -1141,9 +1179,42 @@


      switch(rx_buffer[MESSAGE_TYPE_IDX])


      {


         uint16_t checksum;


         case POLL:


            if (anchor_type == rx_buffer[ANC_TYPE_IDX])


         case SPOLL:


            //if (anchor_type == rx_buffer[ANC_TYPE_IDX])


            if(dev_id==anc_id_recv)


            {


                rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];


                battary = rx_buffer[BATTARY_IDX];


                button = rx_buffer[BUTTON_IDX];


                frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];


         tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];


         tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];


      ////////////////基站汇总模式数据          


               memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],2);


                memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);


                memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4+4);


                usart_send[2] = 0x0c;//正常模式 


               usart_send[3] = 15+8*(rec_nearbase_num+1);//数据段长度


               memcpy(&usart_send[4],&tag_id_recv,2);


                    usart_send[6] = rx_buffer[SEQUENCE_IDX];


                    usart_send[7] = rx_buffer[SEQUENCEH_IDX];


               usart_send[8] = battary;


                    usart_send[9] = button;


                    memcpy(&usart_send[10],&rec_tagheight,2);


               usart_send[12] = tag_frequency;


                    usart_send[13] = tag_slotpos;


                    usart_send[14] = 0;


                    usart_send[15] = 0;


               usart_send[16] = rec_nearbase_num+1;


                   


                    memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num+2);


                    memcpy(&usart_send[19+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num+4);


					


               checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);


               memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);


               UART_PushFrame(usart_send,27+8*rec_nearbase_num);


            Anchor_RecPoll();


            }


            break;


         case SYNC:         


            memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);


@@ -1213,7 +1284,8 @@


      }


      break;


         case NEAR_POLL:      


      //   GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);					


      //   GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);	


               


            memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);


            taglist_pos=CmpTagInList(tag_id_recv);


            if(taglist_pos==taglist_num)


@@ -1226,10 +1298,43 @@


               new_tagid=0;


            }


            tagofflinetime[taglist_pos]=0;


          temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);


                temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);


            tagpos_rec[(u8)temp_tagpos]=1;


            rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];


			


                battary = rx_buffer[BATTARY_IDX];


                button = rx_buffer[BUTTON_IDX];


                frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];


         tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];


         tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];


      ////////////////基站汇总模式数据          


                memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],2);


                memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);


                memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4+4);


                usart_send[2] = 0x0c;//正常模式 


               usart_send[3] = 15+8*(rec_nearbase_num+1);//数据段长度


               memcpy(&usart_send[4],&tag_id_recv,2);


                    usart_send[6] = rx_buffer[SEQUENCE_IDX];


                    usart_send[7] = rx_buffer[SEQUENCEH_IDX];


               usart_send[8] = battary;


                    usart_send[9] = button;




                    memcpy(&usart_send[10],&rec_tagheight,2);


               usart_send[12] = tag_frequency;


                    usart_send[13] = tag_slotpos;


                    usart_send[14] = 0;


                    usart_send[15] = 0;


               usart_send[16] = rec_nearbase_num+1;


                    memcpy(&usart_send[17],&anc_id_recv,2);


                    memcpy(&usart_send[19],&recnearbaselist_id,2*rec_nearbase_num);


                    memcpy(&usart_send[19+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num+4);


					


               checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);


               memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);


               UART_PushFrame(usart_send,27+8*rec_nearbase_num);




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


                


                


GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);   


         if(anc_id_recv==dev_id)


         {


@@ -1254,7 +1359,8 @@


//                  seize_anchor=1;   //抢占anchor


//                  Anchor_RecNearPoll(i);   


//               }




//                    if(tag_id_recv==0x4008)


//                    printf("标签高度: %d,基站高度: %d,高度差： %d. \r\n",rec_tagheight,intheight,intheight-rec_tagheight);


            


            break;


          default: