飞机测试初步完成，采集第一次数据

zhyinch

2020-10-20 8440ff4c3ee00bf9f478ec111efb19eaf02d25fc

 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,14 +617,9 @@
                              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{
@@ -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)
            changemainbase_count++;		
         }else{
            changemainbase_count = 0;
         }
         if(changemainbase_count>3)
            {
               memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);
               memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);
               report_num++;
            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:

         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);