V2.7 固定时间片9标签20HZ测试通过

zhyinch

2020-10-31 fc057b986700bbb5dd6bac94c40b4093f2751f7f

 Ô´Âë/ºËÐÄ°å/Src/application/dw_app.c

@@ -176,14 +176,14 @@

extern uint8_t g_start_send_flag;
u8 g_start_sync_flag;
void SyncPoll(u8 sync_seq,uint32_t sync_baseid)
void SyncPoll(u8 sync_seq)
{u8 result;
   g_start_sync_flag=1;   //中断模式，退出终端后，需要重新来过
   dwt_forcetrxoff();      //关闭接收，以防在RX ON çŠ¶æ€
   
   
   tx_sync_msg[SYNC_SEQ_IDX]=sync_seq;
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&sync_baseid,4);
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&dev_id,4);
   dwt_writetxdata(sizeof(tx_sync_msg), tx_sync_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE);
@@ -309,11 +309,30 @@
   }
return max_slotpos-1;
}
dwt_rxdiag_t d1;
u8 test=0;
u8 anclost_times=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u16 temp_sync_timer1,temp_sync_timer2;
 double firstpath_power, rx_power;
  double f1, f2, r1, r2;
uint16_t F1,F2,F3,N,C;
double B = 131072;
double A = 121.74;
double min_power;
double LOS(dwt_rxdiag_t *dia) {
     F1 = dia->firstPathAmp1;
     F2 = dia->firstPathAmp2;
     F3 = dia->firstPathAmp3;
     N = dia->rxPreamCount;
     C = dia->maxGrowthCIR;
    
    firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A;
    rx_power=10*log10(C*B/(N*N))-A;

   // min_power =  - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
    return min_power;
  }
void Poll(void)
{
   uint32_t frame_len;
@@ -896,6 +915,7 @@
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
         }
}
uint32_t time_monitor[10];
extern uint16_t configremotetagID;
extern u8 remotetag_paralen;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse;
@@ -913,7 +933,7 @@
         dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
         dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置发送完成后开启接收延迟时间
         dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间
			
         dwt_readdiagnostics(&d1);
         if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1])
         {
            motorstate =0;
@@ -1003,6 +1023,10 @@
               int64_t tof_dtu;
               resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
               final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6

//               dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
//               dwt_rxenable(0);//打开接收
					
               final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1，T4，T5
               final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_NEARBASE_IDX+ancrec_nearbasepos*4], &resp_rx_ts);
               final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
@@ -1010,6 +1034,7 @@
               poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
               resp_tx_ts_32 = (uint32_t)resp_tx_ts;
               final_rx_ts_32 = (uint32_t)final_rx_ts;
               time_monitor[1] = sync_timer*1000+TIM3->CNT;	
               Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
               Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
               Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
@@ -1018,10 +1043,14 @@
               tof = tof_dtu * DWT_TIME_UNITS;
               distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
               dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
               dist_cm = dist_no_bias * 100; //dis ä¸ºå•ä½ä¸ºcm的距离				
               dist_cm = dist_no_bias * 100; //dis ä¸ºå•ä½ä¸ºcm的距离		
               dwt_readdiagnostics(&d1);
//               time_monitor[2] = sync_timer*1000+TIM3->CNT;	
               LOS(&d1);		
               time_monitor[3] = sync_timer*1000+TIM3->CNT;					
               /*--------------------------以下为非测距逻辑------------------------*/
               //dist_cm=33000;
					
               GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);		
               LED0_BLINK; //每成功一次通讯则闪烁一次
               dis_after_filter=dist_cm;
               hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
@@ -1043,6 +1072,8 @@
               memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
               usart_send[13] = battary;
               usart_send[14] = button;
               usart_send[15] = firstpath_power;
               usart_send[16] = (rx_power-firstpath_power)*10;
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
@@ -1079,8 +1110,9 @@
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
         }
}
extern u16 synclost_count;
uint32_t current_syncid=0xffffffff,synclost_timer;
extern u8 flag_syncbase;
extern u8 flag_syncbase,waitsync_flag;
u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
u16 ancidlist_rec[20],ancidlist_send[20];
void Anchor_App(void)
@@ -1104,7 +1136,7 @@
   if (status_reg & SYS_STATUS_RXFCG)//成功接收
   { u16 tag_recv_interval;
      float temp_tagpos;
	
   time_monitor[0] = sync_timer*1000+TIM3->CNT;
      g_Resttimer=0;
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
@@ -1120,28 +1152,17 @@
            break;
         case SYNC:         
            memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
            if(rec_syncid<current_syncid)
            {
               current_syncid=rec_syncid;
         if(anc_id_recv==g_com_map[SYNCBASEID])
         {
               synclost_count = 0;
               flag_syncbase=0;
               sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
               TIM3->CNT = sync_seq*325%1000+15;
               sync_timer = sync_seq*325/1000;
               TIM3->CNT = (sync_seq*325%1000)-80;
               sync_timer = (sync_seq*325/1000);
               synclost_timer=0;
               //SyncPoll(sync_seq,rec_syncid);			
               }else if(rec_syncid==current_syncid)
            {
               if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)		
                  {
                     flag_syncbase=0;
                     sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
                     TIM3->CNT = sync_seq*325%1000+5;
                     sync_timer = sync_seq*325/1000+995;
                     synclost_timer=0;
                     //SyncPoll(sync_seq,rec_syncid);
                  }
				
            }
            if(g_com_map[BASESYNCSEQ]==2)
               SyncPoll(sync_seq);
         }
            break;
         case NEAR_MSG:
            if(anc_id_recv==dev_id)
@@ -1160,7 +1181,8 @@
            }
            break;
      
         case NEAR_POLL:			
         case NEAR_POLL:		
      //   GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);					
            memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
            taglist_pos=CmpTagInList(tag_id_recv);
            if(taglist_pos==taglist_num)
@@ -1180,6 +1202,7 @@

         if(anc_id_recv==dev_id)
         {
            GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);	
            Anchor_RecNearPoll(0);   
         }else{
               
@@ -1188,6 +1211,7 @@
                memcpy(&tempid,&rx_buffer[NEARBASEID_INDEX+i*2],2);
               if(tempid==dev_id)
               {
						
                  seize_anchor=0;  //非抢占。已存在列表中
                  Anchor_RecNearPoll(i+1);   
                  break;
@@ -1202,7 +1226,10 @@
//               }

            
            break;		
            break;
          default:
				
            break;
      }   
   }else{
       #ifdef DEBUG_INF