成功加入一对一配对逻辑

chen

2025-07-16 4f6558e8b65cefd5f58a21a9426d4754395de885

 keil/uwb_app.c

@@ -32,7 +32,7 @@
static uint8_t frame_seq_nb2,battary,button,rec_nearbase_num,ancidlist_num;
static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS];
static int16_t rec_ancdistlist[TAG_NUM_IN_SYS];
extern uint8_t group_id;
extern uint16_t group_id;
static uint16_t anc_id_recv,tag_id_recv;
static int16_t rec_antdelay;
extern uint32_t dev_id;
@@ -284,9 +284,9 @@
angle =angle*57.3;
//offset=10;
//angle+=offset;
if(angle>=90)
while(angle>=90)
        angle-=180;
else if(angle<=-90)
while(angle<=-90)
{
angle+=180;
}
@@ -498,11 +498,12 @@


uint8_t flag_temp2,flag_temp1;
uint8_t tx_num;
uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos)//根据自己是否为新基站定制消息去发送，根据是否抢占判断
{
    temp_count2=poll_rx_en_start_u32;

    memcpy(&send_buffer[GROUP_ID_IDX],&group_id,1);//组id
    memcpy(&send_buffer[GROUP_ID_IDX],&group_id,2);//组id变为2字节
    memcpy(&send_buffer[ANCHOR_ID_IDX],&dev_id,2);//自身id
    memcpy(&send_buffer[TAG_ID_IDX],&tag_id_recv,2);//接受者id
    send_buffer[MESSAGE_TYPE_IDX]=MBX_RESPONSE;
@@ -532,6 +533,7 @@
    temp_count3= phy_timer_count_get();
    flag_temp2=uwb_tx(send_buffer, 60,1 ,resp_tx_en_start_u32);//立即发送测试size大小
    temp_count1=phy_timer_count_get();
      tx_num++;
    while(mac_is_busy());
    
}
@@ -846,6 +848,7 @@
    // LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle);
#endif
}
uint16_t temp_group_id;
int UwbRange(void)
{
    uint8_t i;
@@ -904,8 +907,9 @@
    {
      gpio_pin_clr(SCL_PIN);
        //if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL&&!memcmp(&rx_buf[TAG_ID_IDX],&g_com_map[BIND_DEV_ID],2))//判断是否是和自己是同一组通讯的且为poll包
         if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
         if(memcmp(&group_id,&rx_buf[GROUP_ID_IDX],2)==0&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
        {    flag_recsuccess = 1;

               memcpy(&g_com_receive_id,&rx_buf[TAG_ID_IDX],2);
//                sleep_timer_start(__MS_TO_32K_CNT(UWB_DELAY_TIME_US));//测试
                 temp_count2=phy_timer_count_get();
@@ -924,6 +928,14 @@
                    PushAnchorDataArray(tag_id_recv,rec_ancdistlist[i],battary);
                }
            }
                                 if(g_com_map[BIND_START_INDEX])//如果开启配对
                                 {
                                    if(distance<=200)//小于200m的才可配对
                                    {
                                       memcpy(&temp_group_id,&rx_buf[BIND_GROUP],2);

                                    }
                                 }
            for(i=0; i<rec_nearbase_num; i++)
            {
                memcpy(&tempid,&rx_buf[FZ_NEARBASEID_INDEX+i*2],2);
@@ -958,7 +970,7 @@
//            LOG_INFO(TRACE_MODULE_APP, "PDOA: %f %f %f\r\n", pdoa[0], pdoa[1], pdoa[2]);
               //angle_temp=angle_calculate();
            angle_temp=-mk_q7_to_s16(azimuth);
               sts_rssi = sts_rssi_output_get();
               //sts_rssi = sts_rssi_output_get();
         }
      
                  recev_error_num=0;
@@ -972,11 +984,19 @@
         CalibratePdOffset(angle_temp);
         }else{
         angle_temp+=pd_offset;
         buffer_construct(tag_id_recv,distance,(int16_t)angle_temp,rssi);
         uart_send(TRACE_PORT_UART0,usart_send,10+17,NULL);
         //buffer_log_send(tag_id_recv,distance,(int16_t)angle_temp);
         }
         
			
//         while(angle_temp>=90)
//         angle_temp-=180;
//         while(angle_temp<=-90)
//         {
//         angle_temp+=180;
//         }
         buffer_construct(tag_id_recv,distance,(int16_t)angle_temp,rssi);
                  uart_send(TRACE_PORT_UART0,usart_send,10+17,NULL);
         //buffer_log_send(tag_id_recv,distance,(int16_t)angle_temp);
                  }

         //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 角度 :%lf\r\n",g_com_receive_id,distance,angle_temp);
                //check_if_in_or_out_car(g_com_receive_id,distance,sts_rssi[1],sts_rssi[2]);
               success_num++;