#include "mk_trace.h" #include "mk_uwb.h" #include "mk_phy.h" #include "mk_misc.h" #include "mk_power.h" #include "mk_sleep_timer.h" #include "lib_ranging.h" #include "lib_aoa.h" #include "dw_app_anchor.h" #include "global_param.h" #include "board.h" #include "lib_aoa.h" #include "lib_kf.h" #include "MK8000_kf_top.h" #if PDOA_3D_EN #include "lib_pdoa_3d.h" #endif extern int simple_main(void); extern int temp_main(void); void Calibration_Time(void); void TagListUpdate(void); void TagListUpdate_person_num(void); void buffer_message_send(void); void buffer_485_send(uint8_t* show_buffer); void Uwb_init(void); void OpenUWB(void); void CloseUWB(void); int Anchor_App(void); extern void IO_LED_control_change(uint8_t data); extern void IO_control_init(void); extern uint8_t loc_kf_filter(float data_meas, enum KF_DATA_TYPE_T data_type, uint8_t *mac_addr, float *data_post); extern void updata_led_power_state(void); /*receive buffer*/ static uint8_t rx_buf[150]; static uint8_t uwb_sendbuffer[150]; static volatile uint16_t rx_state; static volatile uint16_t rx_length; //resp±äÁ¿ static uint8_t frame_seq_nb2,battary,button,rec_nearbase_num,ancidlist_num; static uint16_t ancidlist_rec[TAG_NUM_IN_SYS],ancidlist_send[TAG_NUM_IN_SYS],rec_ancidlist[TAG_NUM_IN_SYS]; uint8_t show_location[TAG_NUM_IN_SYS]; void buffer_485_construct(uint8_t* show_buffer); static int16_t rec_ancdistlist[TAG_NUM_IN_SYS]; void update_show_buffer(void); extern uint8_t group_id; static uint16_t anc_id_recv; static uint16_t tag_id_recv; static int16_t rec_antdelay; extern uint32_t dev_id; static uint32_t taglist_pos,tmp_time; extern uint16_t tag_frequency; extern uint16_t disoffset; static uint8_t frame_len,recpoll_len,current_syncid,new_tagid,seize_anchor,result,g_start_sync_flag; extern uint8_t gps_power_state,motor_power_state,uwb_state,air780_power_state,gps_success_state,red_charge_state,green_charge_state,air780_success_state; int poll_rx_num,resp_tx_num; //respº¯Êý void PushAnchorDataArray(uint16_t ancid,int16_t dist,uint8_t battary);//ÕÒµ½×Ô¼ºµÄidÐÅÏ¢¶ÔӦλÖøüÐÂ×Ô¼ºµÄ½»»¥ÐÅÏ¢ static void resp_msg_set_ts(uint8_t *ts_field, int64_t ts);//ÓÃÀ´¶ÔӦλÖ÷ÅÈëʱ¼ä´Á static uint16_t tagid_list[TAG_NUM_IN_SYS]; uint8_t tag_seq[TAG_NUM_IN_SYS]; uint16_t angle_azimuth[TAG_NUM_IN_SYS]; uint16_t angle_elevation[TAG_NUM_IN_SYS]; uint16_t angle_num; uint32_t fliter_tagid_list[TAG_NUM_IN_SYS]; uint16_t CmpTagInList(uint16_t tagid); uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos); static uint8_t send_buffer[100]; static uint8_t tagofflinetime[TAG_NUM_IN_SYS]; uint32_t temp_count=0; uint32_t temp_count1=0; uint32_t temp_count2=0; uint32_t temp_count3=0; uint32_t temp_count7=0; uint32_t temp_internal=0; int16_t elevation = 0; int16_t azimuth = 0; uint8_t fom = 0; int32_t distance; uint8_t taglist_num; float *sts_rssi=NULL; extern uint8_t recev_error_num; uint8_t uwb_rx_flag; extern Operation_step UWB_work_state; void find_in_tag_id_authorization_list(uint32_t authorization_list_num); extern int16_t first_search_flag; typedef enum { SEARCH, CLOSE, RANGE, }enumwltagstate; /* Ranging period */ #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 550U //yuan550¼«ÏÞ #define RESP_TX_TO_FINAL_RX_DLY_US 500U /* RX sync window size 50 ms*/ #define RX_SYNC_WIN_US 5000U //yuan1000 7000success /* RX sync window size 50 ms*/ #define RX_SYNC_WIN_US_TEMP 2000000U //yuan1000 7000success /* Receive poll timeout 500us*/ #define POLL_RX_TIMEOUT_US 500 /* Receive final timeout 500us */ #define FINAL_RX_TIMEOUT_US 500 /* RX window open in advance */ #define RX_WIN_IN_ADVANCE_US (150) #define DELAY_BETWEEN_TWO_FRAME_UUS 500 //yuan1400 /* Field index in frame */ #define MSG_SEQ_NUM_IDX 2 #define FINAL_MSG_POLL_TX_TS_IDX 10 #define FINAL_MSG_RESP_RX_TS_IDX 14 #define FINAL_MSG_FINAL_TX_TS_IDX 18 #define DELAY_DEFAULT 1000 #define HALF_SECOND_TIME 624000000 /* Length of the common part of the message */ #define MSG_COMMON_LEN 10 #define UWB_DELAY_TIME_US 496 static uint8_t receive_flag=0; struct mk_uwb_configure { uint8_t phy_work_mode; /* PHY_TX / PHY_RX / PHT_TX|PHY_RX */ struct UWB_CONFIG_T phy_cfg; }; ///* Default communication configuration. */ //static struct mk_uwb_configure config = {//Ô­À´µÄ»ùÕ¾·Ç110k´úÂë // .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX), // .phy_cfg.ch_num = 5, /* Channel number. */ // .phy_cfg.code_index = 9, /* TX preamble code. */ // .phy_cfg.mean_prf = MEAN_PRF_64M, /* Data rate 6.8M */ // .phy_cfg.data_bit_rate = DATA_BR_6M8, /* data rate 6.8M. */ // .phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */ // .phy_cfg.sfd_sym = BPRF_NSFD2_8, /* Identifier for SFD sequence */ // .phy_cfg.ranging_bit = 1, /* ranging bit set. */ // .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF, /* IEEE802.15.4z - BPRF mode */ // .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0, /* SP0 Frame */ // .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */ // .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */ // .phy_cfg.rx_ant_id = UWB_RX_ANT_3, /* UWB RX antenna port */ //}; #ifdef DW1000 static struct mk_uwb_configure config = { .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX), .phy_cfg.ch_num = 5, /* Channel number. */ .phy_cfg.code_index = 9, /* TRX preamble code */ .phy_cfg.mean_prf = MEAN_PRF_64M, /* Mean prf 64/128/256M */ .phy_cfg.data_bit_rate = DATA_BR_6M8, /* Data rate 6.8M */ .phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */ .phy_cfg.sfd_sym = NON_STD_NSFD5_8, /* Identifier for SFD sequence */ .phy_cfg.ranging_bit = 1, /* ranging bit set 1 */ .phy_cfg.trx_mode = TRX_MODE_15_4A, /* IEEE802.15.4z - BPRF mode */ .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0, /* SP0 Frame */ .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */ .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */ .phy_cfg.rx_main_ant = UWB_RX_ANT_3, /* UWB RX main antenna port */ .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode */ .phy_cfg.pulse_shape = 0x2, /* 0x0: CCC pulse, 0x2: FiRa pulse */ }; #elif defined STS_MODE static struct mk_uwb_configure config = { .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX), .phy_cfg.ch_num = 5, /* Channel number. */ .phy_cfg.code_index = 9, /* TRX preamble code */ .phy_cfg.mean_prf = MEAN_PRF_64M, /* Mean prf 64/128/256M */ .phy_cfg.data_bit_rate = DATA_BR_6M8, /* Data rate 6.8M */ .phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */ .phy_cfg.sfd_sym = BPRF_NSFD2_8, /* Identifier for SFD sequence */ .phy_cfg.ranging_bit = 1, /* ranging bit set 1 */ .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF, /* IEEE802.15.4z - BPRF mode */ .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_1, /* SP1 Frame */ .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */ .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */ .phy_cfg.rx_main_ant = UWB_RX_ANT_3, /* UWB RX main antenna port */ .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode */ .phy_cfg.pulse_shape = 0x2, /* 0x0: CCC pulse, 0x2: FiRa pulse */ }; /* Use the default key and IV specified in the IEEE 802.15.4z attachment */ static struct UWB_STS_KEY_CONFIG_T sts_iv_key = { .sts_vcounter = 0x1F9A3DE4, .sts_vupper0 = 0xD37EC3CA, .sts_vupper1 = 0xC44FA8FB, .sts_vupper2 = 0x362EEB34, .sts_key0 = 0x14EB220F, .sts_key1 = 0xF86050A8, .sts_key2 = 0xD1D336AA, .sts_key3 = 0x14148674, }; #elif defined MK_MODE static struct mk_uwb_configure config = { .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX), .phy_cfg.ch_num = 9, /* Channel number. */ .phy_cfg.code_index = 9, /* TX preamble code. */ .phy_cfg.mean_prf = MEAN_PRF_64M, /* Data rate 6.8M */ .phy_cfg.data_bit_rate = DATA_BR_6M8, /* data rate 6.8M. */ .phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */ .phy_cfg.sfd_sym = BPRF_NSFD2_8, /* Identifier for SFD sequence */ .phy_cfg.ranging_bit = 1, /* ranging bit set. */ .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF, /* IEEE802.15.4z - BPRF mode */ .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_0, /* SP0 Frame */ .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */ .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */ .phy_cfg.rx_main_ant = UWB_RX_ANT_3, /* UWB RX main antenna port */ .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode */ .phy_cfg.pulse_shape = 0x2, /* 0x0: CCC pulse, 0x2: FiRa pulse */ }; #elif defined STS_MODE_SQUARE static struct mk_uwb_configure config = { .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX), .phy_cfg.ch_num = 5, /* Channel number. */ .phy_cfg.code_index = 9, /* TRX preamble code */ .phy_cfg.mean_prf = MEAN_PRF_64M, /* Mean prf 64/128/256M */ .phy_cfg.data_bit_rate = DATA_BR_6M8, /* Data rate 6.8M */ .phy_cfg.sync_sym = PREAM_LEN_128, /* Preamble duration, length of preamble 128 */ .phy_cfg.sfd_sym = BPRF_NSFD2_8, /* Identifier for SFD sequence */ .phy_cfg.ranging_bit = 1, /* ranging bit set 1 */ .phy_cfg.trx_mode = TRX_MODE_15_4Z_BPRF, /* IEEE802.15.4z - BPRF mode */ .phy_cfg.sts_pkt_cfg = STS_PKT_CFG_1, /* SP1 Frame */ .phy_cfg.sts_segnum = STS_SEGNUM_BPRF_1, /* Number of STS segments in the frame */ .phy_cfg.sts_seglen = STS_SEGLEN_BPRF_64, /* Number of symbols in an STS segment */ .phy_cfg.rx_main_ant = UWB_RX_ANT_3, /* UWB RX main antenna port */ .phy_cfg.rx_ant_mode = RX_ANT_PORTS_COMBINATION, /* UWB RX antenna mode */ .phy_cfg.pulse_shape = 0x2, /* 0x0: CCC pulse, 0x2: FiRa pulse */ }; /* Use the default key and IV specified in the IEEE 802.15.4z attachment */ static struct UWB_STS_KEY_CONFIG_T sts_iv_key = { .sts_vcounter = 0x1F9A3DE4, .sts_vupper0 = 0xD37EC3CA, .sts_vupper1 = 0xC44FA8FB, .sts_vupper2 = 0x362EEB34, .sts_key0 = 0x14EB220F, .sts_key1 = 0xF86050A8, .sts_key2 = 0xD1D336AA, .sts_key3 = 0x14148674, }; #endif static struct anchor_id_car{ uint16_t anchor_new_id; uint16_t change_num; }; /* Buffer to store received frame */ /* Frames used in the ranging process * Poll message: * - byte 0 - 1: 0x8841 to indicate a data frame using 16-bit addressing. * - byte 2: sequence number, incremented for each new frame. * - byte 3 - 4: PAN Id 0x4B4d * - byte 5 - 6: Destination address * - byte 7 - 8: Source address * - byte 9: Message type (0x02 RANGING_POLL / 0x03 RANGING_RESPONSE / 0x04 RANGING_FINAL) * Response message: * - byte 10: activity code (0x07 to tell the initiator to go on with the ranging exchange) * Final message: * - byte 10 - 13: poll message transmission timestamp. * - byte 14 - 17: response message reception timestamp. * - byte 18 - 21: final message transmission timestamp. */ static uint8_t rx_poll_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x02}; static uint8_t tx_resp_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x4D, 0x49, 0x53, 0x45, 0x03, 0x07}; static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0x4D, 0x4B, 0x53, 0x45, 0x4D, 0x49, 0x04, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; /* Count value of phy counter when transmitting and receiving frames */ static uint32_t poll_rx_en_start_u32; static uint32_t resp_tx_en_start_u32; static uint32_t resp_tx_timeout; int64_t temp_resp_i64; /* 41 bits timestamps of frames transmission/reception. */ int64_t poll_rx_ts_i64; int64_t resp_tx_ts_i64; int64_t final_rx_ts_i64; /* Frame sequence number, incremented after each transmission. */ static uint8_t frame_seq_nb = 0; /* MAC report data structure */ static struct MAC_HW_REPORT_T rx_rpt; enum SIMPLE_FSM_T { SIMPLE_IDLE = 0, SIMPLE_POLL = 1, SIMPLE_RESPONSE = 2, SIMPLE_FINAL = 3, }; static enum SIMPLE_FSM_T state = SIMPLE_IDLE; /** * @brief Correct TX timestamp of the ranging frame. * * @param[in] timestamp PHY timer count of TX * @return TX timestamp (unit: 15.65ps) */ static int64_t ranging_tx_time_correct(uint32_t timestamp) { int64_t tx_timestamp = ranging_tx_time(timestamp); // correct antenna delay (TX using the same antenna as RX) tx_timestamp += ranging_ant_delays_get(config.phy_cfg.rx_main_ant) / 2; return tx_timestamp; } /** * @brief Correct RX timestamp of the ranging frame. * * @param[in] ind MAC RX report * @return RX timestamp (unit: 15.65ps) */ static int64_t ranging_rx_time_correct(const struct MAC_HW_REPORT_T *ind) { int64_t rx_timestamp = ranging_rx_time(ind); // correct antenna delay rx_timestamp -= ranging_ant_delays_get(config.phy_cfg.rx_main_ant) / 2; return rx_timestamp; } /* RX done process handler. */ int8_t rssi; uint32_t range_timeout_us = 2000000;//yuan5000 uint8_t flag_temp2,flag_temp1; uint16_t uwb_losttimer; //uint8_t mac_error; uint8_t allow_flag; uint8_t resp_tx_flag,poll_rx_error_num; uint8_t tag_rssi[TAG_NUM_IN_SYS]; uint8_t tag_show_location[TAG_NUM_IN_SYS]; void construct_show_buffer(uint8_t*tag_show_location,uint32_t tag_pos) { //if(angle_azimuth[tag_pos]) //{ // //}elseif(angle_azimuth[tag_pos]) //{ //}elseif(angle_azimuth[tag_pos]) //{ //}elseif(angle_azimuth[tag_pos]) //{ //} } float temp1,temp2; static void angle_result_filter(uint8_t *mac_addr, int16_t *angle, uint8_t type) { if (angle == NULL) { return; } float post_angle; float angle_meas = mk_q7_to_f32(*angle); temp1=angle_meas; // call filter loc_kf_filter(angle_meas, (enum KF_DATA_TYPE_T)type, mac_addr, &post_angle); temp2=post_angle; // update angle *angle = mk_f32_to_q7(post_angle); temp1=*angle; // LOG_INFO(TRACE_MODULE_APP, "Peer %X, $%d %d;\r\n", READ_SHORT(mac_addr), (int16_t)angle_meas,(int16_t)post_angle); } static void rx_int_callback(struct MAC_HW_REPORT_T *rx_report) { uint8_t valid_sts=0; // Power off radio power_off_radio(); sts_lsp_store_stop(); /** UWB RX success */ if (rx_report->err_code == UWB_RX_OK) { // /* Received data does not contain FCS */ // rx_length = rx_report->pkt_len; // memcpy(rx_buf, rx_report->pkt_data, rx_length); /* Received data does not contain FCS */ rx_length = rx_report->pkt_len; memcpy(rx_buf, rx_report->pkt_data, rx_length); memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T)); /* Calculate rx timestamp */ temp_count= phy_timer_count_get(); poll_rx_en_start_u32 = rx_rpt.timestamp - phy_shr_duration(); poll_rx_ts_i64 = ranging_rx_time_correct(&rx_rpt); poll_rx_num++; rssi = rx_report->rssi; //receive_flag=1; Anchor_App(); // // PDoA caculation if (sts_valid_check())//ΪÁ˱ÜÃâÓ°ÏìʱÐò·Åµ½Á˻ذüºó½øÐвÙ×÷ { pdoa_3d_calculate(0, &elevation, &azimuth); pdoa_fom_get(NULL, &fom); if(allow_flag) { tag_rssi[taglist_pos]=rssi; uint8_t mac_addr=0; angle_result_filter(&mac_addr,&azimuth, KF_DATA_TYPE_AZIMUTH); angle_result_filter(&mac_addr,&elevation, KF_DATA_TYPE_ELEVATION); angle_azimuth[taglist_pos]=(uint16_t)mk_q7_to_s16(azimuth); angle_elevation[taglist_pos]=(uint16_t)mk_q7_to_s16(elevation); //buffer_message_send(); //int16_t azimth=mk_q7_to_s16(azimuth),elevate=mk_q7_to_s16(elevation); // // filter process LOG_INFO(TRACE_MODULE_APP, "PDoA 0x%04x Azimuth %d Elevation %d FoM %u area%#x\r\n",tag_id_recv, mk_q7_to_s16(azimuth), mk_q7_to_s16(elevation), fom,show_location[taglist_pos]); allow_flag=0; } } 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]); // sts_rssi = sts_rssi_output_get(); // } // #endif } else { /* UWB_PLD_ERR payload error */ /* UWB_PHR_ERR PHR error */ /* UWB_SFD_ERR Sfd error */ /* UWB_BD_ERR Preamble detection error */ /* UWB_TO_ERR Receive timeout */ /* UWB_STS_ERR STS error */ temp_count= phy_timer_count_get(); memcpy(&rx_rpt, rx_report, sizeof(struct MAC_HW_REPORT_T)); rx_length = 0; OpenUWB();//ÔٴοªÆôUWB½ÓÊÕ } //OpenUWB();//ÔٴοªÆôUWB½ÓÊÕ } /* TX done process handler. */ static void tx_int_callback(struct MAC_HW_REPORT_T *tx_report) { // Power off radio power_off_radio(); /** UWB TX success */ if (tx_report->err_code == UWB_TX_OK) { temp_count= phy_timer_count_get(); temp_internal=temp_count; resp_tx_num++; 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; } } uint32_t start_receive_count,end_receive_count,poll_timeout,current_count,temp_resp; uint16_t anchordata_id[TAG_NUM_IN_SYS],anchordata_dist[TAG_NUM_IN_SYS]; extern uint32_t tag_id_authorization_list[1024]; static uint8_t anchordata_bat[TAG_NUM_IN_SYS]; uint8_t anchordata_num = 0; static int16_t tagdist_list[TAG_NUM_IN_SYS]; uint8_t rssi_quality[TAG_NUM_IN_SYS]; uint16_t state_button[TAG_NUM_IN_SYS]; uint16_t pressure[TAG_NUM_IN_SYS]; uint16_t random_time; //anchor int32_t hist_dist; int16_t temp_recdist_before_offset; int16_t dist_temp; void PushAnchorDataArray(uint16_t ancid,int16_t dist,uint8_t battary) { uint8_t i; for(i=0; i0) // if(temp_recdist_before_offset!=0&&distance!=0x1ffff) // { // distance=dist*0.5+distance*0.5; // }else{ // distance=dist; // } } //uint16_t CmpTagInList(uint16_t tagid) //{ uint16_t i; // for(i=0; i>= 8; } } uint8_t tt5=0; //void TagListUpdate(void) //{ // uint8_t i,j=0; // for(i=0; i tagdist_list[j + 1]) { // ½»»»¾àÀëÖµ uint16_t temp_dist = tagdist_list[j]; tagdist_list[j] = tagdist_list[j + 1]; tagdist_list[j + 1] = temp_dist; // ͬ²½½»»»±êÇ©ID uint32_t temp_id = tagid_list[j]; tagid_list[j] = tagid_list[j + 1]; tagid_list[j + 1] = temp_id; //ͬ²½½»»»µçÁ¿ uint32_t temp_bat = anchordata_bat[j]; anchordata_bat[j]=anchordata_bat[j+1];//µçÁ¿ËæÖ®¸üРanchordata_bat[j+1]=temp_bat; //ͬ²½½»»»ÔÚÏßʱ¼ä uint32_t temp_tag_offlinetime = tagofflinetime[j]; tagofflinetime[j]=tagofflinetime[j+1];//µçÁ¿ËæÖ®¸üРtagofflinetime[j+1]=temp_tag_offlinetime; //ˮƽ½Ç¶È½»»» uint16_t temp_tag_azimuth=angle_azimuth[j]; angle_azimuth[j]=angle_azimuth[j+1]; angle_azimuth[j+1]=temp_tag_azimuth; //´¹Ö±½Ç¶È½»»» uint16_t temp_tag_elevation=angle_elevation[j]; angle_elevation[j]=angle_elevation[j+1]; angle_elevation[j+1]=temp_tag_elevation; //ÐźÅÇ¿¶È½»»» uint8_t rssi_temp=tag_rssi[j]; tag_rssi[j]=tag_rssi[j+1]; tag_rssi[j+1]=rssi_temp; //ÐźÅÖÊÁ¿½»»» uint8_t temp_quality=rssi_quality[j]; rssi_quality[j]=rssi_quality[j+1]; rssi_quality[j+1]=temp_quality; //É豸״̬½»»» uint16_t temp_state=state_button[j]; state_button[j]=state_button[j+1]; state_button[j+1]=temp_state; //Æøѹ½»»» uint16_t temp_pressure=pressure[j]; pressure[j]=pressure[j+1]; pressure[j+1]=temp_pressure; //°üÐò½»»» uint8_t seq_temp=tag_seq[j]; tag_seq[j]=tag_seq[j+1]; tag_seq[j+1]=seq_temp; //showbuffer½»»» uint8_t show_location_temp=show_location[j]; show_location[j]=show_location[j+1]; show_location[j+1]=show_location_temp; // //ͬ²½½»»»ÊÚȨ±í // uint8_t temp_tag_authorized = tag_authorized_List[j]; // tag_authorized_List[j]=tag_authorized_List[j+1];//µçÁ¿ËæÖ®¸üР// tag_authorized_List[j+1]=temp_tag_authorized; } } } } void change_button_state(void) { if(tag_authorized_List[0]==1&&taglist_num>=1)//Ö»ÓÐÔÚÓÐЧÅж¨·¶Î§ÄÚÓÐÊÚȨµÄ±êÇ©²Å»á¿ªÆô£¬·ñÔò¶¼ÊÇ¹Ø±Õ { if(tagdist_list[0]> 8;//¼ÆËãResponse·¢ËÍʱ¼äT3¡£ // // (resp_tx_en_start_u32) is the moment when TX enable // 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+rec_nearbase_num*DELAY_BETWEEN_TWO_FRAME_UUS);//ºóÃæµÄÐèÒª¸ù¾ÝÒÑÓлùÕ¾ÊýÁ¿½øÐиü¸Ä£¬Èç¹ûÊÇÇÀÕ¼×Ô¼º×îºóÒ»¸ö»Ø¸´£¬ÒªÓе×Êý // // } else { // //resp_tx_time = (poll_rx_ts + ((rec_nearbase_num*20+POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbasepos*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//¼ÆËãResponse·¢ËÍʱ¼äT3¡£ // 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() position=ancrec_nearbasepos; resp_tx_en_start_u32 = poll_rx_en_start_u32+US_TO_PHY_TIMER_COUNT(POLL_RX_TO_RESP_TX_DLY_US)+ancrec_nearbasepos*US_TO_PHY_TIMER_COUNT(DELAY_BETWEEN_TWO_FRAME_UUS);//¼ÓÈëÖ¡¼ä¸ô 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Ï൱ÓÚ»¹Ô­ÁË if(tagdist_list[ancrec_nearbasepos]<=change_by_frequency_distance&&tag_authorized_List[ancrec_nearbasepos]==1)//ÊÚȨ²Å¸Ä±äƵÂÊ·ñÔò²»¸Ä±ä { send_buffer[RESP_MSG_TAG_FREQUENCY]=tag_near_frequency;//мÓÈë¸Ä±ä±êÇ©²â¾àhz } else{ send_buffer[RESP_MSG_TAG_FREQUENCY]=1;//мÓÈë¸Ä±ä±êÇ©²â¾àhz } 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´óС resp_tx_flag=1;//ÏÞÖÆÖظ´¿ªÆô tagofflinetime[taglist_pos] = 0;//¸üбêǩͨÐÅ //phy_update_sts_iv_counter(0x00, sts_iv_key.sts_vcounter);//ÖØÖÃsts // temp_count1=phy_timer_count_get(); //while(mac_is_busy()); //gpio_pin_clr(SCL_PIN); } uint16_t uwb_searchcount; uint8_t flag_recsuccess; uint32_t start_receive_count_calibration; uint32_t current_count_calibration; uint32_t get_in_num,get_out_num; struct anchor_id_car anchor_id_in_car[TAG_NUM_IN_SYS],anchor_id_out_car[TAG_NUM_IN_SYS]; int16_t change_count[TAG_NUM_IN_SYS]; uint16_t anchor_id_in[TAG_NUM_IN_SYS],anchor_id_out[TAG_NUM_IN_SYS]; uint8_t secondtask_search_count,secondtask_search_flag; extern uint8_t g_start_send_flag,search_open_flag = 1,link_success_flag,flag_secondtask; enumwltagstate wltag_state=RANGE; uint32_t wltag_statetimer,wltag_uwbtimer; uint32_t uwbtasktimer=0,uwbtagsendtimer=0; uint16_t CmpCarInTable(uint16_t tagid) { uint16_t i; for(i=0; i=MS_TO_PHY_TIMER_COUNT(500)||current_count_calibration+(UINT32_MAX-start_receive_count_calibration)>=MS_TO_PHY_TIMER_COUNT(500)) &&!((end_receive_count-current_count_calibration<=MS_TO_PHY_TIMER_COUNT(200))||(end_receive_count+(UINT32_MAX-current_count_calibration)<=MS_TO_PHY_TIMER_COUNT(200)))) { start_receive_count_calibration=current_count_calibration;//¸üпªÊ¼Ê±¼ä if(secondtask_search_count++%2==0) { secondtask_search_flag = 1; }else{ secondtask_search_flag = 0; } if(secondtask_search_flag)//¸üÐÂSʱ¼äTICK { HIDO_TimerTick(); TagListUpdate(); // GPS_Poll(); // if(nomove_count<=g_com_map[NOMOVESLEEP_TIME])//·ÀÖ¹Òç³ö // nomove_count++; // else{ // nomove_count=g_com_map[NOMOVESLEEP_TIME]+1; // } } // update_led_power_state();//¸üеÈ״̬·ÀÖ¹Õ𶯿¨ËÀÔÚËÑË÷ } } uint16_t g_com_receive_id; void CloseUWB(void) { uwb_rx_force_off(1); uwb_rx_flag=0; //LOG_INFO(TRACE_MODULE_APP,"¹Ø±Õuwb_rx\r\n"); } void OpenUWB(void) { flag_temp1=uwb_rx(0, 0,range_timeout_us);//Ìá½»½ÓÊÕÇëÇó uwb_rx_flag=1; //LOG_INFO(TRACE_MODULE_APP,"´ò¿ªuwb_rx\r\n"); } //uint8_t temp_tag_num; void UWBOneSecondTask(void) { TagListUpdate();//¸üбêÇ©Êý uwb_losttimer++; if(uwb_losttimer>g_com_map[UWB_RNAGE_TIME]) uwb_losttimer=g_com_map[UWB_RNAGE_TIME]+1;//·ÀÖ¹Òç³ö } void UWBIdleTask(void) { if(HIDO_TimerGetTick()-uwbtasktimer >=1) { uwbtasktimer = HIDO_TimerGetTick(); UWBOneSecondTask(); } } int uwb_app_poll(void) { // switch(wltag_state) // { // case RANGE: // if(HIDO_TimerGetTick()-wltag_uwbtimer>2&&uwb_rx_flag==0) // { // wltag_uwbtimer = HIDO_TimerGetTick(); // OpenUWB(); // } // else if(HIDO_TimerGetTick()-wltag_uwbtimer>1&&uwb_rx_flag==1) // { // wltag_uwbtimer = HIDO_TimerGetTick(); // CloseUWB(); // } // if(uwb_losttimer>g_com_map[UWB_RNAGE_TIME]) // { // wltag_statetimer = HIDO_TimerGetTick(); // if(uwb_rx_flag==0)//ÈôÒªÇл»µ½ËÑË÷ģʽʱûÓпªÆôÔò¿ªÆôUWB // { // OpenUWB(); // } // wltag_state = SEARCH; // } // break; // case SEARCH: // if(HIDO_TimerGetTick()-wltag_statetimer>1) // { // wltag_statetimer = HIDO_TimerGetTick(); // wltag_state = CLOSE; // CloseUWB(); // } // break; // case CLOSE: // if(HIDO_TimerGetTick()-wltag_statetimer>g_com_map[UWB_WAIT_TIME]) // { // wltag_statetimer = HIDO_TimerGetTick(); // wltag_state = SEARCH; // OpenUWB(); // } // break; // } UWBIdleTask(); } extern uint8_t bind_flag; void bubble_sort(uint32_t *arr, uint32_t n) { if (n <= 1) return; // ÎÞÐèÅÅÐò uint32_t i, j; uint8_t swapped; // ±ê¼ÇÊÇ·ñ·¢Éú½»»» for (i = 0; i < n - 1; i++) { swapped = 0; // ÖØÖý»»»±ê¼Ç // ÿÂÖ½«×î´óÔªËØ"ðÅÝ"µ½Ä©Î² for (j = 0; j < n - i - 1; j++) { if (arr[j] > arr[j + 1]) { // ½»»»ÏàÁÚÔªËØ uint32_t temp = arr[j]; arr[j] = arr[j + 1]; arr[j + 1] = temp; swapped = 1; // ±ê¼Ç·¢ÉúÁ˽»»» } } // ÓÅ»¯£ºÈç¹û±¾ÂÖδ·¢Éú½»»»£¬ËµÃ÷Êý×éÒÑÅÅÐò if (!swapped) break; } } uint16_t tag_authorization_list_num_temp; void fliter_tag_id_authorization_list(uint16_t*tag_authorization_list_real_num) { uint32_t filtered_count = 0; // ±éÀúËùÓбêÇ©£¬½«¾àÀëСÓÚãÐÖµµÄID´æÈë½á¹ûÊý×é for (uint32_t i = 0; i < taglist_num; i++) { if (tagdist_list[i] < bind_distance&&tagid_list[i]!=0) { fliter_tagid_list[filtered_count] = tagid_list[i]; filtered_count++; } //fliter_tagid_list } *tag_authorization_list_real_num=filtered_count; } void copy_taglist_to_flash(void) { fliter_tag_id_authorization_list(&tag_authorization_list_num_temp);//¹ýÂ˵÷idΪ0µÄ±êÇ©ºÍ²»Âú×ã°ó¶¨¾àÀëµÄ±êÇ© memset(tag_id_authorization_list,0,sizeof(tag_id_authorization_list));//Çå¿ÕÉÏÒ»´ÎÊý×é·ÀÖ¹Öصþ memcpy(&tag_id_authorization_list,&fliter_tagid_list,tag_authorization_list_num_temp*4);//¸´ÖÆÂú×ãÌõ¼þÊý×é bubble_sort(&tag_id_authorization_list,tag_authorization_list_num_temp);//½øÐÐÅÅÐòºó´æÈë,Ϊ¶þ·Ö·¨×ö»ù´¡ save_taglist_map_to_flash();//´æÈëflash } void find_in_tag_id_authorization_list(uint32_t authorization_list_num) { // for (uint32_t i = 0; i < taglist_num; i++) { // uint32_t target = tagid_list[i]; // uint32_t left = 0; // uint32_t right = authorization_list_num - 1; // ÊÚȨÁбíµÄ×î´óË÷Òý // uint8_t found = 0; // // // ¶þ·Ö²éÕÒÑ­»· // while (left <= right) { // uint32_t mid = left + ((right - left) >> 1); // ·ÀÖ¹ÕûÊýÒç³ö // uint32_t mid_val = tag_id_authorization_list[mid]; // // if (mid_val == target) { // found = 1; // break; // ÕÒµ½Ä¿±êÖµ£¬Á¢¼´Í˳öÑ­»· // } else if (mid_val < target) { // left = mid + 1; // } else { // right = mid - 1; // } // LOG_INFO(TRACE_MODULE_APP, "DEBUG: i=%u, left=%d, right=%d, found=%u\n", i, left, right, found); // } //// //// // ´æ´¢²éÕÒ½á¹û //// tag_authorized_List[i] = found; // } { // Èç¹ûÊÚȨÁбíΪ¿Õ£¬Ö±½ÓÈ«²¿±ê¼ÇΪ0 if (authorization_list_num == 0) { for (int i = 0; i < taglist_num; i++) { tag_authorized_List[i] = 0; } return; } // ±éÀúÐèÒª²éÕÒµÄÿ¸öID for (int i = 0; i < taglist_num; i++) { if (tag_authorized_List[i] != 2) { continue; // Ìø¹ý²»ÐèÒª²éÕÒµÄÏî } int key = tagid_list[i]; int low = 0; int high = authorization_list_num - 1; int found = 0; // ±ê¼ÇÊÇ·ñÕÒµ½ // ÔÚÊÚȨÁбíÖÐÖ´Ðжþ·Ö²éÕÒ while (low <= high) { int mid = low + (high - low) / 2; // ·ÀÖ¹ÕûÊýÒç³ö if (tag_id_authorization_list[mid] == key) { found = 1; // ÕÒµ½Æ¥ÅäÏî break; } else if (tag_id_authorization_list[mid] < key) { low = mid + 1; // ÔÚÓҰ벿·Ö¼ÌÐø²éÕÒ } else { high = mid - 1; // ÔÚ×ó°ë²¿·Ö¼ÌÐø²éÕÒ } } // ¸ù¾Ý²éÕÒ½á¹ûÉèÖÃÊä³öÁбí tag_authorized_List[i] = found ? 1 : 0; } } } #define AREA_1 0xa1 #define AREA_2 0xa2 #define AREA_3 0xa3 #define AREA_4 0xa4 #define AREA_5 0xa5 #define AREA_6 0xa6 #define AREA_7 0xa7 #define AREA_8 0xa8 uint8_t change_by_distance(uint8_t position,uint16_t distance) { if(distanceg_com_map[ALARM_DISTANCE1]&&distanceg_com_map[ALARM_DISTANCE2]&&distance=-22.5&&(int16_t)angle_azimuth[i]<=22.5) { location_temp=change_by_distance(AREA_1,tagdist_list[i]); }else if((int16_t)angle_azimuth[i]>=22.5&&(int16_t)angle_azimuth[i]<=67.5){ location_temp=change_by_distance(AREA_8,tagdist_list[i]); }else if((int16_t)angle_azimuth[i]>=67.5&&(int16_t)angle_azimuth[i]<=112.5){ location_temp=change_by_distance(AREA_7,tagdist_list[i]); }else if((int16_t)angle_azimuth[i]>=112.5&&(int16_t)angle_azimuth[i]<=157.5){ location_temp=change_by_distance(AREA_6,tagdist_list[i]); }else if(((int16_t)angle_azimuth[i]>=157.5&&(int16_t)angle_azimuth[i]<=180)||((int16_t)angle_azimuth[i]>=-180&&(int16_t)angle_azimuth[i]<=-157.5)){ location_temp=change_by_distance(AREA_5,tagdist_list[i]); }else if((int16_t)angle_azimuth[i]>=-157.5&&(int16_t)angle_azimuth[i]<=-112.5){ location_temp=change_by_distance(AREA_4,tagdist_list[i]); }else if((int16_t)angle_azimuth[i]>=-112.5&&(int16_t)angle_azimuth[i]<=-67.5){ location_temp=change_by_distance(AREA_3,tagdist_list[i]); }else if((int16_t)angle_azimuth[i]>=-67.5&&(int16_t)angle_azimuth[i]<=-22.5){ location_temp=change_by_distance(AREA_2,tagdist_list[i]); } show_location[i]=location_temp; } } void buffer_485_send(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],&tagid_list,2*taglist_num);//tag_idlist memcpy(&usart_send[7+taglist_num+2*taglist_num],&tagdist_list,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); while(uart_is_busy(UART_ID0)); uart_send(UART_ID0, usart_send,13+5*taglist_num, NULL); } void buffer_message_send(void) { static uint8_t usart0_send[1024]; uint16_t checksum = 0; uint32_t yuliu=0; usart0_send[0] = 0x55; usart0_send[1] = 0xAA; usart0_send[2] = 0x51; usart0_send[3] = 8+16*taglist_num; //length memcpy(&usart0_send[4],&dev_id,2); //anchor_id for(uint8_t i=0;i=ANC_MAX_NUM) return 0; for(i=0; i=UINT32_MAX) {end_receive_count-=UINT32_MAX;} current_count=phy_timer_count_get(); while(current_countend_receive_count+HALF_SECOND_TIME)//Ñ­»·½ÓÊÜ°üÌå,ÈôΪ124.8KÔòÊÇ+62400000 { current_count=phy_timer_count_get(); while(mac_is_busy()) { Calibration_Time(); IdleTask(); current_count=phy_timer_count_get(); if(current_count>end_receive_count&¤t_count5) // { // recev_error_num=0; // UWB_work_state=SEARCH_DEV; // uwb_searchcount = 0; // search_open_flag = 1; // } //} return 0; } void Uwb_init(void)//uwbinitÐÞ¸ÄÁË { uwb_open(); // Set calibration parameters uwb_calibration_params_set(config.phy_cfg.ch_num); // set advanced parameters struct PHY_ADV_CONFIG_T adv_config = { .thres_fap_detect = 40, .nth_scale_factor = 1, .ranging_performance_mode = 0, #if RX_ANT_PORTS_NUM == 4 .skip_weakest_port_en = 1, #else .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); ranging_lib_init(config.phy_cfg.sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM); // Register rx interrupt callback function mac_register_process_handler(tx_int_callback, rx_int_callback); // Reduce inter frame spacing (IFS) for non encrypted packets mac_ifs_set(0, 0); } #if PDOA_3D_EN #define PDOA_3D_SUPPORT_NUM 10 #define PDOA_3D_TIMEOUT_MS 2000 static struct PDOA_3D_MAC_ADDR_T mac_addr_cache[PDOA_3D_SUPPORT_NUM]; static struct PDOA_3D_PDOA_DATA_T pdoa_data_cache[PDOA_3D_SUPPORT_NUM]; #endif #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 Anchor_uwb_aoa_square_init(void) { // Set calibration parameters uwb_calibration_params_set(config.phy_cfg.ch_num); // set advanced parameters struct PHY_ADV_CONFIG_T adv_config = { .thres_fap_detect = 40, .nth_scale_factor = 1, .ranging_performance_mode = 0, #if RX_ANT_PORTS_NUM == 4 .skip_weakest_port_en = 1, #else .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); ranging_lib_init(config.phy_cfg.sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM); // Register rx interrupt callback function mac_register_process_handler(tx_int_callback, rx_int_callback); // Reduce inter frame spacing (IFS) for non encrypted packets mac_ifs_set(0, 0); #if (ANT_PATTERN == ANT_PATTERN_SQUARE) angle_span_t aoa_span; aoa_span.Ndim = 2; aoa_span.el_low = 90; aoa_span.el_high = 90; aoa_span.el_step = 1; aoa_span.az_low = 0; aoa_span.az_high = 359; aoa_span.az_step = 1; aoa_angle_search_span_set(&aoa_span); #endif #if AOA_EN sts_param_config(config.phy_cfg.sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI_PDOA_AOA_FOM, STS_BUF_NUM, STS_BUF_SIZE); aoa_steering_vector_set((const float *)((uint32_t)((config.phy_cfg.ch_num == 9) ? svec_ch9_ptr : svec_ch5_ptr) | SRAM_BASE)); #else sts_param_config(config.phy_cfg.sts_pkt_cfg, STS_AUX_ANT_IQ_RSSI, STS_BUF_NUM, STS_BUF_SIZE); #endif #if PDOA_3D_EN pdoa_3d_param_config(ANT_PATTERN, ANT_LAYOUT, PDOA_3D_AMBIGUITY_LEVEL_NONE, mac_addr_cache, pdoa_data_cache, PDOA_3D_SUPPORT_NUM, PDOA_3D_TIMEOUT_MS); // pdoa_angle_reverse_set(1, 0); #endif // Set STS key and IV phy_sts_key_configure(&sts_iv_key); phy_rx_sts_switch_mode_set(config.phy_cfg.sts_pkt_cfg, STS_SWITCH_EVERY_4SYM, 0, 0); //³õʼ»¯Â˲¨¿Õ¼ä loc_post_kf_config(1000, kf_mac_addr_cache, kf_channel_cache, kf_mat_value_cache, KF_SUPPORT_NUM, KF_TIMEOUT_MS); } //Ö÷º¯Êý°ó¶¨½ÓÊÜÂß¼­ int UwbSearch(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(); #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);//а汾ÐÞ¸Ä ranging_lib_init(config.phy_cfg.sts_pkt_cfg, CE_AUX_CH_PWR_NLOS_FOM); #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); receive_flag = 0; temp_count3=phy_timer_count_get(); gpio_pin_set(SCL_PIN); sts_lsp_store(); flag_temp2=uwb_rx(0, 0, RX_SYNC_WIN_US_TEMP);//¿ªÆô½ÓÊÕ start_receive_count=phy_timer_count_get(); poll_timeout=US_TO_PHY_TIMER_COUNT(2000000);//¶àÒ»¸ö¶à0.4msĬÈÏ0.4ms¼ÆËãΪ0.125*4*100000,ĬÈÏ¿ªÆô1mss end_receive_count=start_receive_count+poll_timeout; if(end_receive_count>=UINT32_MAX) { end_receive_count-=UINT32_MAX; } current_count=phy_timer_count_get(); while(current_countend_receive_count+HALF_SECOND_TIME)//Ñ­»·½ÓÊÜ°üÌå,ÈôΪ124.8KÔòÊÇ+62400000 { while(mac_is_busy()) { IdleTask(); current_count=phy_timer_count_get(); if(current_count>end_receive_count&¤t_countSEARCH_TIMESTEMP) { uwb_searchcount=0; search_open_flag=1; } } //link_error_count+=g_com_map[COM_INTERVAL]; //if(link_error_count>=g_com_map[4G_INTERNAL]) //link_error_count=0; // update_led_power_state();//¸üеÆ״̬ } //LOG_INFO(TRACE_MODULE_APP,"UWB״̬£º%d\r\n",UWB_work_state); } uint8_t GetUWBBindState(void) { if(UWB_work_state == UN_BIND) { return 0; } return 1; }