| | |
| | | #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 |
| | |
| | | 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*/ |
| | |
| | | //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 uint32_t tag_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; |
| | |
| | | //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 uint32_t tagid_list[TAG_NUM_IN_SYS]; |
| | | 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); |
| | |
| | | #elif defined STS_MODE_SQUARE |
| | | static struct mk_uwb_configure config = { |
| | | .phy_work_mode = (uint8_t)(PHY_TX | PHY_RX), |
| | | .phy_cfg.ch_num = 9, /* Channel number. */ |
| | | .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 */ |
| | |
| | | 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 |
| | |
| | | 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\r\n",tag_id_recv, mk_q7_to_s16(azimuth), |
| | | mk_q7_to_s16(elevation), fom); |
| | | allow_flag=0; |
| | | } |
| | | } |
| | | if(resp_tx_flag==0) |
| | | { |
| | | OpenUWB(); |
| | |
| | | static uint8_t anchordata_bat[TAG_NUM_IN_SYS]; |
| | | uint8_t anchordata_num = 0; |
| | | static int32_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; |
| | |
| | | tagofflinetime[j]=tagofflinetime[j+1];//电量随之更新 |
| | | tagofflinetime[j+1]=temp_tag_offlinetime; |
| | | |
| | | //同步交换授权表 |
| | | 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; |
| | | // //同步交换授权表 |
| | | // 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; |
| | | } |
| | | } |
| | | } |
| | |
| | | } |
| | | taglist_num=j; |
| | | sort_tag_lists();//增加距离小的排到前面逻辑 |
| | | if(find_flag) |
| | | { |
| | | //if(tt5) |
| | | find_in_tag_id_authorization_list(tag_id_authorization_list_num); |
| | | find_flag=0; |
| | | } |
| | | change_button_state(); |
| | | buffer_message_send();//发送测距报文信息 |
| | | update_show_buffer();//根据角度和距离判断区域位置 |
| | | //delay_ms(10); |
| | | buffer_485_send(show_location);//发送显示信息 |
| | | } |
| | | |
| | | uint8_t position; |
| | |
| | | } |
| | | } |
| | | } |
| | | void buffer_485_construct(uint8_t* show_buffer) |
| | | #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(distance<1000){ |
| | | position=position; |
| | | }else if (distance>1000&&distance<2000){ |
| | | position=position+16; |
| | | }else if (distance>2000&&distance<3000){ |
| | | position=position+16; |
| | | } |
| | | return position; |
| | | } |
| | | void update_show_buffer(void) |
| | | { |
| | | uint8_t location_temp; |
| | | for(int i=0;i<taglist_num;i++) |
| | | { |
| | | if((int16_t)angle_azimuth[i]>=-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[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],&ancidlist_rec,2*taglist_num);//tag_idlist |
| | | memcpy(&usart_send[7+taglist_num+2*taglist_num],&rec_ancdistlist,2*taglist_num); //tag_distlist |
| | | 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; |
| | |
| | | |
| | | 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<taglist_num;i++) |
| | | { |
| | | memcpy(&usart0_send[6+i*16],tagid_list[i],2); |
| | | usart0_send[8+i*20] = tag_seq[i];//包序 |
| | | memcpy(&usart0_send[9+i*16],&tagdist_list[i],2); //distance |
| | | memcpy(&usart0_send[11+i*16],&angle_azimuth[i],2);//angle_azithum |
| | | memcpy(&usart0_send[13+i*16],&angle_elevation[i],2);//angle_elevation |
| | | memcpy(&usart0_send[15+i*16],&tag_rssi[i],1); //信号强度 |
| | | memcpy(&usart0_send[16+i*16],&rssi_quality[i],1); //信号质量预留 |
| | | usart0_send[17+i*20]=anchordata_bat[i];//标签电量 |
| | | memcpy(&usart0_send[18+i*16],&state_button[i],2); //设备状态 |
| | | memcpy(&usart0_send[20+i*16],&pressure[i],2); //气压 |
| | | } |
| | | memcpy(&usart0_send[6+taglist_num*16],&yuliu,4); //预留位4位 |
| | | checksum = Checksum_u16(&usart0_send[2],8+16*taglist_num); |
| | | memcpy(&usart0_send[10+16*taglist_num], &checksum, 2); |
| | | uart_send(UART_ID0, usart0_send,12+16*taglist_num, NULL); |
| | | } |
| | | int Anchor_App(void) |
| | | { |
| | |
| | | frame_seq_nb2 = rx_buf[SEQUENCE_IDX];//获取包序 |
| | | battary = rx_buf[BATTARY_IDX]; |
| | | rec_nearbase_num=rx_buf[FZ_NEARBASENUM_INDEX]; //标签传过来的他与基站交互的基站id数目 |
| | | memcpy(&tag_id_recv,&rx_buf[TAG_ID_IDX],4); |
| | | memcpy(&tag_id_recv,&rx_buf[TAG_ID_IDX],2); |
| | | memcpy(rec_ancidlist,&rx_buf[FZ_NEARBASEID_INDEX],2*rec_nearbase_num); |
| | | memcpy(rec_ancdistlist,&rx_buf[FZ_NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num); |
| | | memcpy(&rec_antdelay,&rx_buf[FZ_NEARBASEID_INDEX+rec_nearbase_num*4],2); |
| | | |
| | | allow_flag=1; |
| | | taglist_pos=CmpTagInList(tag_id_recv); |
| | | if(taglist_num>=ANC_MAX_NUM) |
| | | return 0; |
| | |
| | | Anchor_RecNearPoll(i); |
| | | tagdist_list[taglist_pos]=rec_ancdistlist[i]; |
| | | anchordata_bat[taglist_pos] = battary;//保存该基站电量 |
| | | // if(tag_authorized_List[taglist_pos]==1) |
| | | // { |
| | | // if(tagdist_list[taglist_pos]<button_determinate_distance) |
| | | // { |
| | | // gpio_pin_set(BUTTON_PIN); |
| | | // }else{ |
| | | // gpio_pin_clr(BUTTON_PIN); |
| | | // } |
| | | // } |
| | | tag_seq[taglist_pos]=frame_seq_nb2; |
| | | if(taglist_pos==taglist_num)// taglist_pos==taglist_num 说明这个基站不在当前列表中 |
| | | { //tempid==dev_id 说明基站下发测距报文有这个标签的信息 |
| | | taglist_num++; //满足上述两种情况才会添加基站ID进入列表中,否则会出现标签不在基站列表中,标签也不响应的情况 |
| | |
| | | Anchor_RecNearPoll(rec_nearbase_num); |
| | | } |
| | | } |
| | | LOG_INFO(TRACE_MODULE_APP, "PDoA Azimuth %d Elevation %d FoM %u\r\n", mk_q7_to_s16(azimuth), |
| | | mk_q7_to_s16(elevation), fom); |
| | | |
| | | |
| | | } |
| | | int UwbRange(void) |
| | | { |
| | |
| | | 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) |
| | | { |
| | | uwb_open(); |
| | |
| | | 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); |
| | | } |
| | | |
| | | |