| | |
|---|
| | | #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); |
|---|
| | |
|---|
| | | } |
|---|
| | | |
|---|
| | | } |
|---|
| | | 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(); |
|---|
| | | //angle_temp=angle_calculate(); |
|---|
| | | angle_temp=-mk_q7_to_s16(azimuth); |
|---|
| | | sts_rssi = sts_rssi_output_get(); |
|---|
| | | } |
|---|
| | | |
|---|
| | |
|---|
| | | }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); |
|---|
| | | 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); |
|---|
| | |
|---|
| | | |
|---|
| | | 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); |
|---|
| | | } |
|---|
| | | |
|---|
| | | //主函数绑定接受逻辑 |
|---|
