| | |
|---|
| | | #define FINAL_MSG_FINAL_TX_TS_IDX 18 |
|---|
| | | #define FINAL_MSG_TS_LEN 4 |
|---|
| | | |
|---|
| | | #define STARTPOLL REGPOLL//#define SWITCHBASE_DIST |
|---|
| | | #define STARTPOLL DISCPOLL//#define SWITCHBASE_DIST |
|---|
| | | #define SWITCHBASE_ZHUANDIAN |
|---|
| | | #define SWITCHBASE_DIST |
|---|
| | | enum enumtagstate |
|---|
| | |
|---|
| | | float dis_after_filter; //当前距离值 |
|---|
| | | LPFilter_Frac* p_Dis_Filter; //测距用的低通滤波器 |
|---|
| | | |
|---|
| | | |
|---|
| | | void NextSlotDelayMs(int16_t delayms); |
|---|
| | | void GetNearMsg(void); |
|---|
| | | void Registor_Poll(void); |
|---|
| | | static uint64_t get_tx_timestamp_u64(void) |
|---|
| | | { |
|---|
| | | uint8_t ts_tab[5]; |
|---|
| | |
|---|
| | | } |
|---|
| | | |
|---|
| | | u16 tag_time_recv[TAG_NUM_IN_SYS]; |
|---|
| | | u8 usart_send[25]; |
|---|
| | | u8 usart_send[40]; |
|---|
| | | u8 battary,button; |
|---|
| | | extern uint8_t g_pairstart; |
|---|
| | | void tag_sleep_configuraion(void) |
|---|
| | |
|---|
| | | u8 nearbase_num; |
|---|
| | | u16 mainbase_id; |
|---|
| | | int32_t mainbase_dist,base_mindist; |
|---|
| | | uint8_t trygetnearmsg_times; |
|---|
| | | uint8_t trygetnearmsg_times,try_reg_times; |
|---|
| | | u16 mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],nearbaseid_list0[MAX_NEARBASE_NUM],nearbaseid_list[MAX_NEARBASE_NUM],nearbaseid_list2[MAX_NEARBASE_NUM]; |
|---|
| | | u8 FindNearBasePos(u16 baseid) |
|---|
| | | { |
|---|
| | |
|---|
| | | int32_t minddist=0x1ffff,minid=-1,temp_dist; |
|---|
| | | u8 i,j,getsync_flag=0; |
|---|
| | | u16 mindist_ancid,random_delay_tim,tempval; |
|---|
| | | |
|---|
| | | NextSlotDelayMs(-2); |
|---|
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间 |
|---|
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); |
|---|
| | | frame_seq_nb++; |
|---|
| | |
|---|
| | | } |
|---|
| | | if(minddist!=0x1ffff&&minddist!=0) |
|---|
| | | { |
|---|
| | | trygetnearmsg_times = 0; |
|---|
| | | tag_state = GETNEARMSG; |
|---|
| | | mainbase_id = mindist_ancid; |
|---|
| | | GetNearMsg(); |
|---|
| | | try_reg_times = 0; |
|---|
| | | tag_state = REGPOLL; |
|---|
| | | mainbase_id = mindist_ancid; |
|---|
| | | Registor_Poll(); |
|---|
| | | //GetNearMsg(); |
|---|
| | | } |
|---|
| | | |
|---|
| | | if(getsync_flag==0) |
|---|
| | |
|---|
| | | { |
|---|
| | | |
|---|
| | | exsistbase_list[i]--; |
|---|
| | | clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ; |
|---|
| | | rtd_init = tag_resprx[i] - poll_tx_ts; |
|---|
| | | clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ; |
|---|
| | | rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff; |
|---|
| | | rtd_resp = anc_resptx[i] - anc_pollrx[i]; |
|---|
| | | tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS; |
|---|
| | | distance = tof * SPEED_OF_LIGHT; |
|---|
| | | if(distance>-1000&&distance<100000) |
|---|
| | | if(distance>-10&&distance<1000) |
|---|
| | | nearbase_distlist[i] = distance*100+anc_distoffset[i]; |
|---|
| | | }else{ |
|---|
| | | nearbase_distlist[i] = 0x1ffff; |
|---|
| | |
|---|
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); |
|---|
| | | |
|---|
| | | tx_near_msg[REGP_TAGSTATE_INDEX] = !GET_USERKEY<<2|imu_enable<<1|motor_enable; |
|---|
| | | tx_near_msg[MESSAGE_TYPE_IDX] = REG_POLL; |
|---|
| | | tx_near_msg[MESSAGE_TYPE_IDX] = REG_POLL2; |
|---|
| | | tx_near_msg[REGP_FREQUENCY_INDEX] = 1000/g_com_map[COM_INTERVAL]; |
|---|
| | | tx_near_msg[REGP_POWER_INDEX] = module_power; |
|---|
| | | memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2); |
|---|
| | | memcpy(&tx_near_msg[REGP_VERSION_INDEX],&g_com_map[VERSION],2); |
|---|
| | | memcpy(&tx_near_msg[REGP_IMUTHRES_INDEX],&g_com_map[IMU_THRES],2); |
|---|
| | | memcpy(&tx_near_msg[REGP_NOMOVESLEEPTIME_INDEX],&g_com_map[NOMOVESLEEP_TIME],2); |
|---|
| | |
|---|
| | | memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2); |
|---|
| | | memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2); |
|---|
| | | if(tagslotpos!=255) |
|---|
| | | tag_state = DISCPOLL; |
|---|
| | | { |
|---|
| | | trygetnearmsg_times = 0; |
|---|
| | | tag_state = GETNEARMSG; |
|---|
| | | GetNearMsg(); |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | | } |
|---|
| | |
|---|
| | | #ifdef QIYA_JIZHAN |
|---|
| | | QiyaJizhan_Send(); |
|---|
| | | #else |
|---|
| | | Dw1000_Init(); |
|---|
| | | // Dw1000_Init(); |
|---|
| | | dwt_forcetrxoff(); |
|---|
| | | error_times = 0; |
|---|
| | | switch(tag_state) |
|---|
| | | { |
|---|
| | | case REGPOLL: |
|---|
| | | LED_LG_ON; |
|---|
| | | Registor_Poll(); |
|---|
| | | Registor_Poll(); |
|---|
| | | if(try_reg_times++>5) |
|---|
| | | { |
|---|
| | | tag_state = STARTPOLL; |
|---|
| | | } |
|---|
| | | LED_LG_OFF; |
|---|
| | | // GetPressAndHeight(); |
|---|
| | | break; |
|---|
| | |
|---|
| | | LED_LG_OFF; |
|---|
| | | break; |
|---|
| | | case GETNEARMSG: |
|---|
| | | trygetnearmsg_times++; |
|---|
| | | |
|---|
| | | LED_LG_ON; |
|---|
| | | GetNearMsg(); |
|---|
| | | LED_LG_OFF; |
|---|
| | | if(trygetnearmsg_times>5) |
|---|
| | | if(trygetnearmsg_times++>5) |
|---|
| | | { |
|---|
| | | tag_state = STARTPOLL; |
|---|
| | | } |
|---|
| | | break; |
|---|
| | | case NEARPOLL: |
|---|
| | | NearPoll(); |
|---|
| | | GetPressAndHeight(); |
|---|
| | | intheight =Altitude*100; |
|---|
| | | // GetPressAndHeight(); |
|---|
| | | // intheight =Altitude*100; |
|---|
| | | break; |
|---|
| | | case SINGLEPOLL: |
|---|
| | | // Poll(); |
|---|
