初步将STS方形矩阵测角移植成功，能稳定和一个设备测360度角

chen

2025-07-05 76fc938c40ba7bdf3c6769d1f76a22328151ee90

 keil/uwb_app.c

@@ -334,8 +334,7 @@
//          /* Received data does not contain FCS */
//        rx_length = rx_report->pkt_len;
//        memcpy(rx_buf, rx_report->pkt_data, rx_length);
//          if (sts_valid_check())
//        {

        /* Received data does not contain FCS */
        rx_length = rx_report->pkt_len;
        memcpy(rx_buf, rx_report->pkt_data, rx_length);
@@ -348,32 +347,35 @@
        rssi = rx_report->rssi;
        //receive_flag=1;
            Anchor_App();
//         //    // PDoA caculation
//         pdoa_3d_calculate(0, &elevation, &azimuth);
//         pdoa_fom_get(NULL, &fom);
//         if(resp_tx_flag==0)
//         {
//           OpenUWB();
//         }
         //    // PDoA caculation
         if (sts_valid_check())//为了避免影响时序放到了回包后进行操作
      {
         pdoa_3d_calculate(0, &elevation, &azimuth);
         pdoa_fom_get(NULL, &fom);
         }
         if(resp_tx_flag==0)
         {
           OpenUWB();
         }
//            }else{
//               poll_rx_error_num++;
//            rx_report->err_code |= UWB_STS_ERR;
//            }
         #ifdef STS_MODE
            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]);
//         #ifdef STS_MODE
//            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();
         }
         #endif
//               sts_rssi = sts_rssi_output_get();
//         }
//         #endif
    }
    else
    {
@@ -386,9 +388,9 @@
                 temp_count= phy_timer_count_get();
        memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T));
        rx_length = 0;
//            OpenUWB();//再次开启UWB接收
            OpenUWB();//再次开启UWB接收
    }
   OpenUWB();//再次开启UWB接收
   //OpenUWB();//再次开启UWB接收
}

/* TX done process handler. */
@@ -403,8 +405,8 @@
        temp_count= phy_timer_count_get();
        temp_internal=temp_count;
        resp_tx_num++;
			
//            OpenUWB();//再次开启UWB接收
            resp_tx_flag=0;
            OpenUWB();//再次开启UWB接收
        //LOG_INFO(TRACE_MODULE_APP, "poll_rx_num is %d,resp_tx_num is %d\r\n",poll_rx_num,resp_tx_num);
    }else{
      mac_error=1;
@@ -895,7 +897,27 @@
    }
}
}

void buffer_485_construct(uint8_t* show_buffer)
{            static uint8_t usart_send[100];
            uint16_t checksum = 0;
        usart_send[0] = 0x55;
        usart_send[1] = 0xAA;
        usart_send[2] = 0x50;                        
        usart_send[3] = 9+5*taglist_num;      //length            
        memcpy(&usart_send[4],&dev_id,2); //anchor_id)
            usart_send[6]=taglist_num;       //区域数量
        memcpy(&usart_send[7],&show_buffer,taglist_num);//区域位置
        memcpy(&usart_send[7+taglist_num],&ancidlist_rec,2*taglist_num);//tag_idlist
        memcpy(&usart_send[7+taglist_num+2*taglist_num],&rec_ancdistlist,2*taglist_num); //tag_distlist
            usart_send[7+5*taglist_num]=0;//保留4位RESERVE1
            usart_send[7+5*taglist_num+1]=0;
            usart_send[7+5*taglist_num+2]=0;
            usart_send[7+5*taglist_num+3]=0;
	
        checksum = Checksum_u16(&usart_send[2],9+5*taglist_num);
        memcpy(&usart_send[7+5*taglist_num+4], &checksum, 2);
            uart_send(UART_ID0, usart_send,13+5*taglist_num, NULL);
}
int Anchor_App(void)
{
            uint8_t i;
@@ -961,10 +983,8 @@
                           Anchor_RecNearPoll(rec_nearbase_num);
                }
            }
//             //phy_update_sts_iv_counter(0x00, sts_iv_key.sts_vcounter);//新增sts
//             LOG_INFO(TRACE_MODULE_APP, "PDoA Azimuth %d Elevation %d FoM %u\r\n", mk_q7_to_s16(azimuth),
//                                 mk_q7_to_s16(elevation), fom);
				
                         LOG_INFO(TRACE_MODULE_APP, "PDoA Azimuth %d Elevation %d FoM %u\r\n", mk_q7_to_s16(azimuth),
                                 mk_q7_to_s16(elevation), fom);
}
int UwbRange(void)
{