国产UWB芯片工程的仓库
blame | 历史 | 补丁 | 提交 | 提交对比 | 忽略空白
chen
8 天以前 945f0cc0210d2ea3339870f0e38d9be72314c4e2 
 keil/uwb_app.c


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


#include "dw_app_anchor.h"


#include "global_param.h"


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


@@ -61,6 +62,7 @@


int32_t distance;


uint8_t taglist_num;


float *sts_rssi=NULL;


float *sts_rssi1=NULL;


extern uint8_t recev_error_num;




extern Operation_step UWB_work_state;


@@ -99,7 +101,7 @@


#define HALF_SECOND_TIME 624000000


/* Length of the common part of the message */


#define MSG_COMMON_LEN 10




#define calib_len 60


#define UWB_DELAY_TIME_US 496


static uint8_t receive_flag=0;


struct mk_uwb_configure


@@ -256,8 +258,11 @@


#define MSG_POS          0x01


#define M_PI      3.1415927


uint8_t usart_send[40];


uint8_t usart_send1[40];


extern uint8_t bat_percent;


double angle_temp;


float sts_first_rssi_result[4];


float rssi_result[4];


double angle_calculate(void)


{


 double offset; 


@@ -267,37 +272,45 @@


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


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


 float *iq1 = sts_first_path_iq_get();




//LOG_INFO(TRACE_MODULE_APP, "ANT0 IQ: %f %f\r\n", iq1[0], iq1[1]);


//LOG_INFO(TRACE_MODULE_APP, "ANT1 IQ: %f %f\r\n", iq1[2], iq1[3]);


//LOG_INFO(TRACE_MODULE_APP, "ANT2 IQ: %f %f\r\n", iq1[4], iq1[5]);


//LOG_INFO(TRACE_MODULE_APP, "ANT3 IQ: %f %f\r\n", iq1[6], iq1[7]);


float iq_re = iq1[2 * 0] * iq1[2 * 1] + iq1[2 * 0 + 1] * iq1[2 * 1 + 1];


float iq_im = iq1[2 * 0] * iq1[2 * 1 + 1] - iq1[2 * 0 + 1] * iq1[2 * 1];


float iq_re = iq1[2 * 0] * iq1[2 * 3] + iq1[2 * 0 + 1] * iq1[2 * 3 + 1];


float iq_im = iq1[2 * 0] * iq1[2 * 3 + 1] - iq1[2 * 0 + 1] * iq1[2 * 3];


double result=atan2f(iq_im, iq_re)/ M_PI;


double angle=asin(result);


angle =angle*57.3;


offset=30;


angle+=offset;


if(angle>=90)


//offset=10;


//angle+=offset;


while(angle>=90)


        angle-=180;


else if(angle<=-90)


while(angle<=-90)


{


angle+=180;


}


//LOG_INFO(TRACE_MODULE_APP, "ANGLE_X: %lf \r\n", angle);


float iq_re2 = iq1[2 * 2] * iq1[2 * 3] + iq1[2 * 2 + 1] * iq1[2 * 3 + 1];


float iq_im2 = iq1[2 * 2] * iq1[2 * 3 + 1] - iq1[2 * 2 + 1] * iq1[2 * 3];


double result2=atan2f(iq_im2, iq_re2)/ M_PI;


double angle2=asin(result2);


angle2 =angle2*57.3;


if(angle2>=90)


        angle2-=180;


else if(angle2<=-90)


{


angle2+=180;


}


//float iq_re2 = iq1[2 * 2] * iq1[2 * 3] + iq1[2 * 2 + 1] * iq1[2 * 3 + 1];


//float iq_im2 = iq1[2 * 2] * iq1[2 * 3 + 1] - iq1[2 * 2 + 1] * iq1[2 * 3];


//double result2=atan2f(iq_im2, iq_re2)/ M_PI;


//double angle2=asin(result2);


//angle2 =angle2*57.3;


//if(angle2>=90)


//        angle2-=180;


//else if(angle2<=-90)


//{


//angle2+=180;


//}




//计算端口0和3 rssi和STS 第一路径rssi


sts_first_rssi_result[0]=iq1[0]*iq1[0]+iq1[1]*iq1[1];//计算天线0的sts rssi


sts_first_rssi_result[1]=iq1[6]*iq1[6]+iq1[7]*iq1[7];//计算天线3的sts rssi


rssi_result[0]=20 * log10(sts_first_rssi_result[0] / sts_first_rssi_result[1]) + rssi;//根据相对rssi计算天线0的rssi


rssi_result[1]=rssi;


return angle;


}


int8_t snr_result[2];


void buffer_construct(uint16_t tag_id,uint16_t distance_temp,int16_t angle1,uint8_t rssi1)


{


        uint16_t checksum = 0;


@@ -311,10 +324,32 @@


        usart_send[10] = (uint8_t)rssi1;


        usart_send[11] = 0;  //BUTTON目前先不写


        usart_send[12] = bat_percent;         //BATTARY目前先不写


        usart_send[13] = 0;  //RESERVE1


        usart_send[13] = (uint8_t)snr_result[1];  //RESERVE1


        usart_send[14] = 0;  //RESERVE2


        checksum = Checksum_u16(&usart_send[2], 13+10);


        memcpy(&usart_send[15+10], &checksum, 2);


}


 char log_buf[300];


 uint32_t u32LogLen1,dataloglenth;


 uint32_t cnt_log;




void buffer_log_send(uint16_t tag_id,uint16_t distance_temp,int16_t angle1)


{


   dataloglenth = snprintf((char*)&log_buf,sizeof(log_buf), "DATA,%x,%d,%d,%d,%d,%d\n",tag_id,cnt_log++,angle1,distance_temp,(int32_t)rssi_result[1],snr_result[1]);


   uart_send(TRACE_PORT_UART0,log_buf,dataloglenth,NULL);


}


void buffer_construct_user(uint16_t tag_id)


{					


        uint16_t checksum = 0;


        usart_send1[0] = 0x55;


        usart_send1[1] = 0xAA;


        usart_send1[2] = 0x0d;                        //length


        usart_send1[3] = 5+10;                        //MSG_TYPE


        memcpy(&usart_send1[4],&tag_id,2);                //tag_id)


        usart_send1[6] = 10;


        //用户数据段


        checksum = Checksum_u16(&usart_send1[2], 5+10);


        memcpy(&usart_send1[7+10], &checksum, 2);


}


static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report)


{      


@@ -334,6 +369,7 @@


        poll_rx_ts_i64 = ranging_rx_time_correct(&rx_rpt);


        poll_rx_num++;


        rssi = rx_report->rssi;


            snr_result[1]=rx_report->snr;


        receive_flag=1;


            


    }


@@ -462,6 +498,7 @@






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;


@@ -489,10 +526,14 @@


    resp_msg_set_ts(&send_buffer[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts_i64);


    resp_msg_set_ts(&send_buffer[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts_i64);//此处时间戳int64直接转换为uint64不知道会不会有错误


    memcpy(&send_buffer[RESP_MSG_ANC_DISTOFFSET],&disoffset,2);//差个修正offset,修正有符号但是这个com表为无符号的,传过去直接赋给Int16_t相当于还原了


      //放入用户控制参数


      buffer_construct_user(tag_id_recv);


      memcpy(&send_buffer[40],usart_send1,20);


    temp_resp_i64=resp_tx_ts_i64;


    temp_count3= phy_timer_count_get();


    flag_temp2=uwb_tx(send_buffer, 40,1 ,resp_tx_en_start_u32);//立即发送测试size大小


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


    


}


@@ -728,7 +769,85 @@


      }   


   }


}


extern uint8_t calibration_start;


 float pd_offset,offset;


  float divide_param = 1.97; //3.25为分立天线的值


void CalibratePdOffset(int16_t angle)


{


   uint16_t i;


   static uint8_t times=0,steady=0;


   static int16_t tmp,max,min,ave;


   static int32_t sum;


   static int16_t History[calib_len];


   History[times]=angle;


   times++;


   if(times >= calib_len)


   {


      max = -900;


      min = 900;


      times = 0;


      sum = 0;


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


      {


         tmp=History[i];


         sum += tmp;


         if (tmp > max)


            max = tmp;


         else if(tmp<min)


            min = tmp;	


      }


      ave = sum / calib_len;


		


//      if(steady)


//      {


         if (abs(ave) < 100)


         {


//            Save.pd_offset=pd_offset;


            pd_offset=-ave;


            memcpy(&g_com_map[PDOFFSET],&pd_offset,4);


            save_com_map_to_flash();


            i = 2;


            WriteCtrlPara(COMM_MAP_CALIB_OK,(uint8_t*)&i,1);


            calibration_start = 0;	


               //printf("Calibration Success.\r\n");


         }


//         else


//         {


//            steady = 0;


//         }


//		


//      }


//         else


//         {


//            if(max - min < 200)


//            {


//               offset =- ave * M_PI * divide_param/1800;


//               pd_offset += offset;


//               steady = 1;


//            }else{


//               pd_offset -= 1;


//            }


//         }


   }




}


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


}


int UwbRange(void)


{


    uint8_t i;


@@ -829,22 +948,45 @@


      {


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


         }


       buffer_construct(tag_id_recv,distance,(int16_t)angle_temp,rssi);


		


                  recev_error_num=0;


              //range_timeout_us=5000;//恢复为5000进入range后


           //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 包序%d\r\n",g_com_receive_id,distance,frame_seq_nb2);


                  //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0:%f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);


                  //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d 信号强度ANT0:%f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[1],sts_rssi[2]);


                  //LOG_INFO(TRACE_MODULE_APP,"标签ID：%X,距离: %d\r\n",g_com_receive_id,distance);


         uart_send(TRACE_PORT_UART0,usart_send,10+17,NULL);


         if(calibration_start)


         {


         CalibratePdOffset(angle_temp);


         }else{


         angle_temp+=pd_offset;


			


			


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


@@ -875,6 +1017,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();


@@ -920,6 +1067,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);


}




//主函数绑定接受逻辑