From 8440ff4c3ee00bf9f478ec111efb19eaf02d25fc Mon Sep 17 00:00:00 2001 From: zhyinch <zhyinch@gmail.com> Date: 星期二, 20 十月 2020 14:03:21 +0800 Subject: [PATCH] 飞机测试初步完成,采集第一次数据 --- Src/application/dw_app.c | 107 ++++++++++++++++++++++------------------------------- 1 files changed, 44 insertions(+), 63 deletions(-) diff --git a/Src/application/dw_app.c b/Src/application/dw_app.c index 2fe757e..3aca82a 100644 --- a/Src/application/dw_app.c +++ b/Src/application/dw_app.c @@ -448,6 +448,7 @@ tyncpoll_time = (tagslotpos--%max_slotpos)*slottime; } } +uint8_t changemainbase_count=0; void NearPoll(void) { @@ -465,8 +466,7 @@ next_nearbase_num = MAX_NEARBASE_NUM-1; } HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET); - last_nearbase_num=next_nearbase_num; - nearbase_num=next_nearbase_num; + recbase_num=0; motor_state=0; @@ -535,14 +535,7 @@ resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4 recbase_num++; memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2); - if(last_nearbase_num==0) - { - get_newbase=1; - nearbaseid_list[0]=rec_nearbaseid; - nearbase_num=1; - memcpy(&tx_near_msg[ANCHOR_ID_IDX],&rec_nearbaseid,2); - } - if(rec_nearbaseid==nearbaseid_list[0]) + if(rec_nearbaseid==mainbase_id) { //////////////////////////////////时间同步 memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2); @@ -562,10 +555,9 @@ // tagslotpos=tagslotpos%(max_slotpos+1); // tyncpoll_time=(tagslotpos-1)*slottime; //////////////////////////// - rec_nearbasepos=0; - exsistbase_list[rec_nearbasepos]=KEEP_TIMES; + memcpy(&temp_dist,&rx_buffer[DIST_IDX],4); - nearbase_distlist[rec_nearbasepos]=temp_dist; + mainbase_dist=temp_dist; if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3) motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf; rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4; @@ -606,35 +598,9 @@ offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset; SetNextPollTime(tyncpoll_time); - - // count_offset=sync_count-current_count-143; - // current_slottimes=(ancsync_time-10000)/(g_com_map[COM_INTERVAL]*1000); - // nextpoll_delaytime=tyncpoll_time*1000+g_com_map[COM_INTERVAL]*1000-((ancsync_time)%(g_com_map[COM_INTERVAL]*1000))-4300; -// if(abs(ancsync_time-910000)<1000) -// { -// nextpoll_delaytime+=10000; -// } -// if(nextpoll_delaytime<2000) -// { -// nextpoll_delaytime+=g_com_map[COM_INTERVAL]*1000; -// } -// lastpoll_count= current_count+(nextpoll_delaytime)/LPTIMER_LSB; -// if(lastpoll_count>LPTIMER_1S_COUNT) -// lastpoll_count-=LPTIMER_1S_COUNT; -// __HAL_LPTIM_COMPARE_SET(&hlptim1, lastpoll_count); - ///////////////////////////////////////// -// dwt_setdelayedtrxtime(final_tx_time); -// result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送 }else{ rec_nearbasepos=FindNearBasePos(rec_nearbaseid); - - if(rec_nearbasepos>=last_nearbase_num) //发现新的基站 - { - get_newbase=1; - nearbase_num++; - nearbaseid_list[rec_nearbasepos] = rec_nearbaseid; - memcpy(&tx_near_msg[ANCHOR_ID_IDX],&rec_nearbaseid,2); - } + exsistbase_list[rec_nearbasepos]=KEEP_TIMES; memcpy(&temp_dist,&rx_buffer[DIST_IDX],4); @@ -651,15 +617,10 @@ para_len = frame_len-22; memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],para_len); } - final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts); + final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], resp_rx_ts); dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000 dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度 - - - //dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5 - // result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送 - //dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4);//将发送数据写入DW1000 - } + } } }else{ dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR); @@ -670,7 +631,7 @@ // dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR); } dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR); - nearbase_num=recbase_num; + j=0; if(exsistbase_list[0]==0) { @@ -687,7 +648,7 @@ } // tyncpoll_time=0; next_nearbase_num=0; - for(i=0;i<last_nearbase_num+get_newbase;i++) + for(i=0;i<nearbase_num;i++) { if(exsistbase_list[i]>0) { @@ -722,19 +683,34 @@ } } } - - report_num=0; - for (i=0;i<last_nearbase_num;i++) + if(true_nearbase_distlist[0]<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST) { - nearbaseid_list[i]=true_nearbase_idlist[i]; - nearbase_distlist[i]=true_nearbase_distlist[i]; - if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==0xa) - { - memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2); - memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4); - report_num++; - } + changemainbase_count++; + }else{ + changemainbase_count = 0; } + if(changemainbase_count>3) + { + mainbase_id = true_nearbase_idlist[0]; + tag_state = GETNEARMSG; + } + if(mainbase_lost_count>3) + { + tag_state = DISCPOLL; + } +//串口输出距离 +// report_num=0; +// for (i=0;i<last_nearbase_num;i++) +// { +// nearbaseid_list[i]=true_nearbase_idlist[i]; +// nearbase_distlist[i]=true_nearbase_distlist[i]; +// if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==0xa) +// { +// memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2); +// memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4); +// report_num++; +// } +// } for(i=0;i<MAX_NEARBASE_NUM;i++) { nearbase_distlist[i]=0x1ffff; @@ -778,9 +754,14 @@ Poll(); break; case GETNEARMSG: - - GetNearMsg(); - + trygetnearmsg_times--; + LED_LG_ON; + GetNearMsg(); + LED_LG_OFF; + if(trygetnearmsg_times==0) + { + tag_state = DISCPOLL; + } break; case NEARPOLL: //GPIO_WriteBit(EU_TX_GPIO, EU_RX_PIN, Bit_SET); -- Gitblit v1.9.3