加入snr打印逻辑

chen

2025-05-27 c6bf7d2728868250e6b219a55bdc7bf4245b8d7c

 keil/uwb_app.c

@@ -9,6 +9,7 @@
#include "dw_app_anchor.h"
#include "global_param.h"
#include "board.h"
#include "CommMap.h"
#include "lib_aoa.h"

extern int simple_main(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
@@ -253,8 +255,104 @@

/* RX done process handler. */
int8_t rssi;
#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; 
// 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]);
 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 * 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=10;
//angle+=offset;
if(angle>=90)
        angle-=180;
else if(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;
//}

//计算端口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;
        usart_send[0] = 0x55;
        usart_send[1] = 0xAA;
        usart_send[2] = 17+10;                        //length
        usart_send[3] = MSG_POS;                        //MSG_TYPE
        memcpy(&usart_send[4],&tag_id,2);                //tag_id)
        memcpy(&usart_send[6],&distance_temp,2);
        memcpy(&usart_send[8],&angle1,2);
        usart_send[10] = (uint8_t)rssi1;
        usart_send[11] = 0;  //BUTTON目前先不写
        usart_send[12] = bat_percent;         //BATTARY目前先不写
        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)
{      uint8_t valid_sts=0;
{		
    // Power off radio
    power_off_radio();

@@ -271,20 +369,9 @@
        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;
            valid_sts= sts_valid_check();
      if (valid_sts)
      {
            aoa_calculate(&elevation, &azimuth);
            aoa_fom_get(NULL, &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]);

               sts_rssi = sts_rssi_output_get();
         }
				
    }
    else
    {
@@ -438,9 +525,12 @@
    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();
    while(mac_is_busy());
    
@@ -511,11 +601,11 @@
{
   uint32_t u32LogLen,datalenth;
   char acReadponse[200];
   u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 测距成功数：%d ID: ",get_in_num,success_num);
	
   //u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 测距成功数：%d ID: ",get_in_num,success_num);
   u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "门内：%d 人 ID: ",get_in_num);
               for(uint16_t i=0;i<get_in_num;i++)
            {
                datalenth = sprintf((char*)&acReadponse[u32LogLen],",%04X",anchor_id_in[i]);
                datalenth = sprintf((char*)&acReadponse[u32LogLen],"%04X,",anchor_id_in[i]);
                u32LogLen += datalenth;
            }
      LOG_INFO(TRACE_MODULE_APP,"%s\r\n",acReadponse);
@@ -677,6 +767,68 @@
      }   
   }
}
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;
//            }
//         }
   }

}

int UwbRange(void)
{
@@ -772,12 +924,39 @@
                seize_anchor=1;   //抢占anchor
                Anchor_RecNearPoll(i);
            }
                  uint8_t valid_sts=0;
                  valid_sts= sts_valid_check();
      if (valid_sts)
      {
            aoa_calculate(&elevation, &azimuth);
            aoa_fom_get(NULL, &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();
         }
		
                  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]);
                check_if_in_or_out_car(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);
         if(calibration_start)
         {
         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);
         }
			
         //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++;
               
                  //gpio_pin_clr(SCL_PIN);
@@ -852,6 +1031,7 @@
      phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
      #endif
}

//主函数绑定接受逻辑
int UwbSearch(void)
{