From 76fc938c40ba7bdf3c6769d1f76a22328151ee90 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期六, 05 七月 2025 14:14:26 +0800
Subject: [PATCH] 初步将STS方形矩阵测角移植成功，能稳定和一个设备测360度角
---
keil/uwb_app.c | 84 ++++++++++++++++++++++++++----------------
1 files changed, 52 insertions(+), 32 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index afcb3bb..5624668 100644
--- a/keil/uwb_app.c
+++ b/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)
{
--
Gitblit v1.9.3