| | |
| | | LPFilter_Frac* p_Dis_Filter; //测距用的低通滤波器 |
| | | |
| | | |
| | | |
| | | void GetNearMsg(void); |
| | | static uint64_t get_tx_timestamp_u64(void) |
| | | { |
| | | uint8_t ts_tab[5]; |
| | |
| | | trygetnearmsg_times = 0; |
| | | tag_state = GETNEARMSG; |
| | | mainbase_id = mindist_ancid; |
| | | GetNearMsg(); |
| | | } |
| | | |
| | | if(getsync_flag==0) |
| | |
| | | } |
| | | nearbase_distlist[rec_nearbasepos+1]=temp_dist; // nearbase_distlist[1]对应 rec_nearbaseid[0]的距离 |
| | | final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts); |
| | | if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]==1&&temp_dist>0) |
| | | if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]!=0&&(nearbase_switchdistlist[rec_nearbasepos]==1||temp_dist<nearbase_switchdistlist[rec_nearbasepos])) |
| | | { |
| | | salvebase_mindist = temp_dist; |
| | | mindist_slavebaseid = rec_nearbaseid; |
| | | } |
| | | #ifdef SWITCHBASE_ZHUANDIAN |
| | | if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]>1&&temp_dist>0) |
| | | { |
| | | mainbase_id = rec_nearbaseid; |
| | | tag_state = GETNEARMSG; |
| | | trygetnearmsg_times = 0; |
| | | } |
| | | // if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]>1&&temp_dist>0) |
| | | // { |
| | | // mainbase_id = rec_nearbaseid; |
| | | // tag_state = GETNEARMSG; |
| | | // trygetnearmsg_times = 0; |
| | | // } |
| | | #endif |
| | | |
| | | } |
| | |
| | | if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST ) |
| | | { |
| | | changemainbase_count++; |
| | | if(changemainbase_count>5) |
| | | if(changemainbase_count>2) |
| | | { |
| | | changemainbase_count = 0; |
| | | mainbase_id = mindist_slavebaseid; |
| | | tag_state = GETNEARMSG; |
| | | trygetnearmsg_times = 0; |
| | | GetNearMsg(); |
| | | } |
| | | }else{ |
| | | changemainbase_count = 0; |
| | |
| | | { |
| | | mainbase_lost_count = 0; |
| | | tag_state = DISCPOLL; |
| | | DiscPoll(); |
| | | } |
| | | if(mainbase_lost_count!=0) |
| | | { |
| | |
| | | if(nearbaseid_list[i]!=nearbaseid_list2[i]) |
| | | { |
| | | tag_state = GETNEARMSG; |
| | | GetNearMsg(); |
| | | trygetnearmsg_times = 0; |
| | | nearbaseid_list0[i]=1; |
| | | } |