| | |
| | | #include "board.h" |
| | | #include "CommMap.h" |
| | | #include "lib_aoa.h" |
| | | |
| | | #include "lib_kf.h" |
| | | extern int simple_main(void); |
| | | extern int temp_main(void); |
| | | void Calibration_Time(void); |
| | |
| | | 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; |
| | |
| | | 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; |
| | |
| | | // 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]); |
| | |
| | | angle =angle*57.3; |
| | | //offset=10; |
| | | //angle+=offset; |
| | | if(angle>=90) |
| | | while(angle>=90) |
| | | angle-=180; |
| | | else if(angle<=-90) |
| | | while(angle<=-90) |
| | | { |
| | | angle+=180; |
| | | } |
| | |
| | | //{ |
| | | //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[10] = (uint8_t)rssi1; |
| | | usart_send[11] = 0; //BUTTON目前先不写 |
| | | usart_send[12] = bat_percent; //BATTARY目前先不写 |
| | | usart_send[13] = 0; //RESERVE1 |
| | | 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\n",tag_id,cnt_log++,angle1,distance_temp); |
| | | 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) |
| | |
| | | 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; |
| | | |
| | | } |
| | |
| | | |
| | | |
| | | uint8_t flag_temp2,flag_temp1; |
| | | uint8_t tx_num; |
| | | uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos)//根据自己是否为新基站定制消息去发送,根据是否抢占判断 |
| | | { |
| | | temp_count2=poll_rx_en_start_u32; |
| | |
| | | temp_count3= phy_timer_count_get(); |
| | | flag_temp2=uwb_tx(send_buffer, 60,1 ,resp_tx_en_start_u32);//立即发送测试size大小 |
| | | temp_count1=phy_timer_count_get(); |
| | | tx_num++; |
| | | while(mac_is_busy()); |
| | | |
| | | } |
| | |
| | | } |
| | | |
| | | } |
| | | void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type) |
| | | { |
| | | if (angle == NULL) |
| | | { |
| | | return; |
| | | } |
| | | |
| | | #if KF_EN |
| | | float post_angle; |
| | | float angle_meas = mk_q7_to_f32(*angle); |
| | | // call filter |
| | | loc_kf_filter(angle_meas, (enum KF_DATA_TYPE_T)type, mac_addr, &post_angle); |
| | | // update angle |
| | | *angle = mk_f32_to_q7(post_angle); |
| | | // LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle); |
| | | #endif |
| | | } |
| | | int UwbRange(void) |
| | | { |
| | | uint8_t i; |
| | |
| | | { |
| | | aoa_calculate(&elevation, &azimuth); |
| | | aoa_fom_get(NULL, &fom); |
| | | angle_result_filter(NULL, &azimuth, KF_DATA_TYPE_AZIMUTH); |
| | | |
| | | // LOG_INFO(TRACE_MODULE_APP, "Azimuth %d Elevation %d Azimuth FoM %d\r\n", |
| | | // mk_q7_to_s16(azimuth), mk_q7_to_s16(elevation),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(); |
| | | //angle_temp=angle_calculate(); |
| | | angle_temp=-mk_q7_to_s16(azimuth); |
| | | //sts_rssi = sts_rssi_output_get(); |
| | | } |
| | | |
| | | recev_error_num=0; |
| | |
| | | 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); |
| | | } |
| | | |
| | | |
| | | // while(angle_temp>=90) |
| | | // angle_temp-=180; |
| | | // while(angle_temp<=-90) |
| | | // { |
| | | // angle_temp+=180; |
| | | // } |
| | | 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++; |
| | |
| | | |
| | | return 0; |
| | | } |
| | | #define KF_SUPPORT_NUM 3 |
| | | #define KF_TIMEOUT_MS 2000 |
| | | static struct KF_MAC_ADDR_T kf_mac_addr_cache[KF_SUPPORT_NUM]; |
| | | static struct KF_CHANNEL_CACHE_T kf_channel_cache[KF_SUPPORT_NUM]; |
| | | static struct KF_MAT_VALUE_CACHE_T kf_mat_value_cache[KF_SUPPORT_NUM]; |
| | | void Uwb_init(void) |
| | | { |
| | | uwb_open(); |
| | |
| | | |
| | | phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0); |
| | | #endif |
| | | loc_post_kf_config(100, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM, |
| | | KF_TIMEOUT_MS); |
| | | } |
| | | |
| | | //主函数绑定接受逻辑 |