国产UWB芯片工程的仓库
blame | 历史 | 补丁 | 提交 | 提交对比 | 忽略空白
chen
2025-07-25 44812eee65f5e9eb47c2b01b6cf86c101083eb61 
 keil/uwb_app.c


@@ -11,7 +11,7 @@


#include "board.h"


#include "CommMap.h"


#include "lib_aoa.h"




#include "lib_kf.h"


extern int simple_main(void);


extern int temp_main(void);


void Calibration_Time(void);


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


}


@@ -384,7 +384,6 @@


                 temp_count= phy_timer_count_get();


        memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T));


        rx_length = 0;




    }


}




@@ -498,11 +497,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 +532,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());


    


}


@@ -829,7 +830,24 @@


   }




}


void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type)


{


    if (angle == NULL)


    {


        return;


    }




#if KF_EN


    float post_angle;


    float angle_meas = mk_q7_to_f32(*angle);


    // call filter


    loc_kf_filter(angle_meas, (enum KF_DATA_TYPE_T)type, mac_addr, &post_angle);


    // update angle


    *angle = mk_f32_to_q7(post_angle);


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


@@ -888,8 +906,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();


@@ -908,6 +927,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);


@@ -930,13 +957,19 @@


      {


            aoa_calculate(&elevation, &azimuth);


            aoa_fom_get(NULL, &fom);


            angle_result_filter(NULL, &azimuth, KF_DATA_TYPE_AZIMUTH);


    


//             LOG_INFO(TRACE_MODULE_APP, "Azimuth %d Elevation %d Azimuth FoM %d\r\n",


//                        mk_q7_to_s16(azimuth), mk_q7_to_s16(elevation),fom);


				


//            float pdoa[3];


//            pdoa[0] = pdoa_select_get(0, 3);


//            pdoa[1] = pdoa_select_get(1, 3);


//            pdoa[2] = pdoa_select_get(2, 3);


//            LOG_INFO(TRACE_MODULE_APP, "PDOA: %f %f %f\r\n", pdoa[0], pdoa[1], pdoa[2]);


               angle_temp=angle_calculate();


               sts_rssi = sts_rssi_output_get();


               //angle_temp=angle_calculate();


            angle_temp=-mk_q7_to_s16(azimuth);


               //sts_rssi = sts_rssi_output_get();


         }


      


                  recev_error_num=0;


@@ -950,11 +983,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++;


@@ -985,6 +1026,11 @@


         


         return 0;


}


#define KF_SUPPORT_NUM 3


#define KF_TIMEOUT_MS 2000


static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM];


static struct KF_CHANNEL_CACHE_T kf_channel_cache[KF_SUPPORT_NUM];


static struct KF_MAT_VALUE_CACHE_T kf_mat_value_cache[KF_SUPPORT_NUM];


void Uwb_init(void)


{


     uwb_open();


@@ -1030,6 +1076,8 @@


      


      phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);


      #endif


       loc_post_kf_config(100, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM,


                           KF_TIMEOUT_MS);


}




//主函数绑定接受逻辑