From c8c1cf5223f0576d4c378ac627d6eeed6caf04d5 Mon Sep 17 00:00:00 2001
From: chen <15335560115@163.com>
Date: 星期三, 07 五月 2025 15:38:06 +0800
Subject: [PATCH] 成功加入log打印和校准逻辑
---
keil/uwb_app.c | 133 ++++++++++++++++++++++++++++++++++++++------
1 files changed, 115 insertions(+), 18 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index 0edee1b..78c6121 100644
--- a/keil/uwb_app.c
+++ b/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);
@@ -99,7 +100,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,6 +257,7 @@
#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;
double angle_calculate(void)
@@ -271,13 +273,13 @@
//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;
+//offset=10;
+//angle+=offset;
if(angle>=90)
angle-=180;
else if(angle<=-90)
@@ -285,17 +287,17 @@
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;
+//}
return angle;
}
void buffer_construct(uint16_t tag_id,uint16_t distance_temp,int16_t angle1,uint8_t rssi1)
@@ -315,6 +317,27 @@
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\n",tag_id,cnt_log++,angle1,distance_temp);
+ 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)
{
@@ -489,9 +512,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());
@@ -728,6 +754,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)
{
@@ -837,14 +925,23 @@
angle_temp=angle_calculate();
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);
+ 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++;
--
Gitblit v1.9.3