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 | 312 ++++++++++++++++++++++++++++++++++++++++++---------
1 files changed, 255 insertions(+), 57 deletions(-)
diff --git a/keil/uwb_app.c b/keil/uwb_app.c
index befa0c1..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);
@@ -69,7 +70,7 @@
#define RANGING_PERIOD_MS (1000)
/* This is the delay from Frame RX POLL frame to send RESP Frame */
-#define POLL_RX_TO_RESP_TX_DLY_US 4000U //yuan670 with urt 3000success
+#define POLL_RX_TO_RESP_TX_DLY_US 2000U //yuan670 with urt 3000success
#define RESP_TX_TO_FINAL_RX_DLY_US 500U
@@ -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
@@ -253,8 +254,93 @@
/* 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;
+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;
+//}
+return angle;
+}
+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] = 0; //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\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)
-{ uint8_t valid_sts=0;
+{
// Power off radio
power_off_radio();
@@ -272,19 +358,7 @@
poll_rx_num++;
rssi = rx_report->rssi;
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
{
@@ -432,20 +506,23 @@
// resp_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(20*rec_nearbase_num+POLL_RX_TO_RESP_TX_DLY_US+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS);//后面的需要根据已有基站数量进行更改，如果是抢占自己最后一个回复，要有底数
// //这里应该有问题问一下钟工
// }//此处设置绝对时间将poll u32改为phy_timer_count_get()
- resp_tx_en_start_u32 = phy_timer_count_get()+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US);
+ resp_tx_en_start_u32 = poll_rx_en_start_u32+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US);
resp_tx_ts_i64 = ranging_tx_time_correct(resp_tx_en_start_u32 + phy_shr_duration());//修正时间戳
/* Write all timestamps in the final message. See NOTE 8 below. */
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());
- gpio_pin_clr(SCL_PIN);
+
}
-uint32_t range_timeout_us = 5000;//yuan5000
+uint32_t range_timeout_us = 1000000;//yuan5000
uint16_t uwb_searchcount;
uint8_t flag_recsuccess;
@@ -506,17 +583,20 @@
anchordata_num=j;
}
+int success_num=0;
void in_table_log(void)
{
uint32_t u32LogLen,datalenth;
char acReadponse[200];
- u32LogLen = snprintf(acReadponse, sizeof(acReadponse), "车内：%d 人 ID:",get_in_num,get_out_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);
+ success_num=0;
}
void TagListUpdate_person_num_car(void)
{
@@ -652,28 +732,98 @@
if(rssi_ant0>rssi_ant2&&distance_from_tag<200)
{
change_count[position_anchor_exist]++;
+ if(change_count[position_anchor_exist]>0)
+ {
+ position_anchor_in_table=CmpCarInTable(receive_success_id);
+ add_in_car_table(position_anchor_in_table,receive_success_id);
+ }
if(change_count[position_anchor_exist]>=3)
{ change_count[position_anchor_exist]=3;
- position_anchor_in_table=CmpCarInTable(receive_success_id);
- add_in_car_table(position_anchor_in_table,receive_success_id);
+
}
}else if(rssi_ant0<rssi_ant2&&distance_from_tag<200)
{
change_count[position_anchor_exist]--;
- if(change_count[position_anchor_exist]<=-3)
- {change_count[position_anchor_exist]=-3;
+ if(change_count[position_anchor_exist]<0)
+ {
position_anchor_in_table=CmpCarInTable(receive_success_id);
delete_in_car_table(position_anchor_in_table);
+ }
+ if(change_count[position_anchor_exist]<=-3)
+ {change_count[position_anchor_exist]=-3;
}
}
}
+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)
{
uint8_t i;
uint16_t tempid;
// The following peripherals will be initialized in the uwb_open function
// phy/mac/aes/lsp/phy timers initialized
- uwb_open();
+
// // Set calibration parameters
// uwb_calibration_params_set(config.phy_cfg.ch_num);
@@ -687,7 +837,7 @@
// };
// phy_adv_params_configure(&adv_config);
-// // uwb configure
+// // uwb configure
// uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
// ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
@@ -700,6 +850,7 @@
temp_count3=phy_timer_count_get();
gpio_pin_set(SCL_PIN);
sts_lsp_store();
+
flag_temp1=uwb_rx(0, 0, range_timeout_us);//开启接收
// while(mac_is_busy());
@@ -710,25 +861,19 @@
if(end_receive_count>=UINT32_MAX)
{end_receive_count-=UINT32_MAX;}
current_count=phy_timer_count_get();
- while(current_count<end_receive_count||current_count>end_receive_count+HALF_SECOND_TIME)//循环接受包体,若为124.8K则是+62400000
- {
+
current_count=phy_timer_count_get();
while(mac_is_busy())
{
- Calibration_Time();
+ //Calibration_Time();
IdleTask();
- current_count=phy_timer_count_get();
- if(current_count>end_receive_count&¤t_count<end_receive_count+HALF_SECOND_TIME)
- {
- break;
- }
}
sts_lsp_store_stop();
if(receive_flag==1)//成功接收
{
-
+ gpio_pin_clr(SCL_PIN);
//if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL&&!memcmp(&rx_buf[TAG_ID_IDX],&g_com_map[BIND_DEV_ID],2))//判断是否是和自己是同一组通讯的且为poll包
if(group_id==rx_buf[GROUP_ID_IDX]&&rx_buf[MESSAGE_TYPE_IDX] == MBX_POLL)//判断是否是和自己是同一组通讯的且为poll包
{ flag_recsuccess = 1;
@@ -766,25 +911,49 @@
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 信号强度ANT0: %f,信号强度ANT2: %f\r\n",g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
- check_if_in_or_out_car(g_com_receive_id,distance,sts_rssi[0],sts_rssi[2]);
+ //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++;
-
- gpio_pin_clr(SCL_PIN);
+ //gpio_pin_clr(SCL_PIN);
//break;去掉break变为一对多
}
}
- sts_lsp_store();
-
- break;
- //失败或者接受被高发射机打断都会再次开启接收
- //flag_temp1=uwb_rx(0, 0, range_timeout_us);
- }
- delay_us(1);
- sts_lsp_store_stop();
- uwb_rx_force_off(1);
+// sts_lsp_store();
+ gpio_pin_clr(SCL_PIN);
+// sts_lsp_store_stop();
+// uwb_rx_force_off(1);
// if(!flag_recsuccess)
// {
@@ -805,37 +974,51 @@
}
void Uwb_init(void)
{
+ uwb_open();
+ #ifdef STS_MODE
// Set STS key and IV
phy_sts_key_configure(&sts_iv_key);
// which RX ports will be used for AoA/PDoA
phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
-
+ #endif
// Set calibration parameters
uwb_calibration_params_set(config.phy_cfg.ch_num);
-
+ #ifndef STS_MODE
// set advanced parameters
+ struct PHY_ADV_CONFIG_T adv_config = {
+ .thres_fap_detect = 40,
+ .nth_scale_factor = 4,
+ .ranging_performance_mode = 0,
+ .skip_weakest_port_en = 0,
+ };
+ #else
+ // set advanced parameters
struct PHY_ADV_CONFIG_T adv_config = {
.thres_fap_detect = 40,
.nth_scale_factor = 4,
.ranging_performance_mode = 3,
.skip_weakest_port_en = 0,
};
+ #endif
phy_adv_params_configure(&adv_config);
// uwb configure
uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
-
+ #ifdef STS_MODE
ranging_lib_init();
+ #endif
ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
-
+ #ifdef STS_MODE
aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
aoa_param_config();
phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+ #endif
}
+
//主函数绑定接受逻辑
int UwbSearch(void)
{
@@ -844,36 +1027,51 @@
// The following peripherals will be initialized in the uwb_open function
// phy/mac/aes/lsp/phy timers initialized
uwb_open();
+ #ifdef STS_MODE
// Set STS key and IV
phy_sts_key_configure(&sts_iv_key);
// which RX ports will be used for AoA/PDoA
phy_rx_ant_mode_set(RX_ANT_PORTS_COMBINATION);
-
+ #endif
// Set calibration parameters
uwb_calibration_params_set(config.phy_cfg.ch_num);
+ #ifndef STS_MODE
// set advanced parameters
+ struct PHY_ADV_CONFIG_T adv_config = {
+ .thres_fap_detect = 40,
+ .nth_scale_factor = 4,
+ .ranging_performance_mode = 0,
+ .skip_weakest_port_en = 0,
+ };
+ #else
+ // set advanced parameters
struct PHY_ADV_CONFIG_T adv_config = {
.thres_fap_detect = 40,
.nth_scale_factor = 4,
.ranging_performance_mode = 3,
.skip_weakest_port_en = 0,
};
+ #endif
phy_adv_params_configure(&adv_config);
// uwb configure
uwb_configure(config.phy_work_mode, board_param.tx_power_fcc[CALIB_CH(config.phy_cfg.ch_num)], &config.phy_cfg);
+ #ifdef STS_MODE
ranging_lib_init();
+ #endif
ranging_frame_type_set(config.phy_cfg.sts_pkt_cfg);
+ #ifdef STS_MODE
aoa_aux_info_set(AOA_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM);
aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE));
aoa_param_config();
phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0);
+ #endif
// Register rx interrupt callback function
mac_register_process_handler(tx_int_callback, rx_int_callback);
@@ -973,9 +1171,9 @@
case LINK_SUCCESS:
{ //连接成功进行轮询测距
- uwb_led_on();
+ //uwb_led_on();
UwbRange();
- uwb_led_off();
+ //uwb_led_off();
}
break;
--
Gitblit v1.9.3