V2.14 1.修复下发震动BUG

zhyinch

2021-01-28 92b9bc4ba9b0e75055cffa06ecf7316137d8d14b

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

@@ -166,7 +166,7 @@

u16 tag_time_recv[TAG_NUM_IN_SYS];
u8 usart_send[100],usart_send_anc[100];
u8 battary,button;
u8 battary,button,tag_frequency,tag_slotpos;
extern uint8_t g_pairstart;
void tag_sleep_configuraion(void)
{
@@ -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);
@@ -918,12 +918,13 @@
uint32_t time_monitor[10];
extern uint16_t configremotetagID;
extern u8 remotetag_paralen;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse;
u8 finalsend=0;
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base
{
   u8 motorstate;
   time_monitor[0] = sync_timer*1000+TIM3->CNT;
	
         tmp_time=TIM3->CNT;
         memcpy(&tx_nearresp_msg[NR_NEARSWITCH_DISTANCE],&g_com_map[NEARSWITCH_DISTANCE_INDEX],2);
         memcpy(&tx_nearresp_msg[ANCTIMEMS],&sync_timer,2);
         memcpy(&tx_nearresp_msg[ANCTIMEUS],&tmp_time,2);
         memcpy(&tx_nearresp_msg[TAGSLOTPOS],&taglist_pos,2);
@@ -935,18 +936,6 @@
         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;
         }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2])
         {
            motorstate =2;
         }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3])
         {
            motorstate =1;
         }else{
            motorstate =0;
         }
         
         if(new_tagid)
         {
@@ -959,6 +948,7 @@
         tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
         tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
         waittagconfig_reponse=0;
            finalsend = 0;
         if(pwtag.remain_time>0)
            {uint8_t i;
               for(i=0;i<pwtag.groupnum;i++)
@@ -969,20 +959,23 @@
                     tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
                     remotetag_paralen = 2+3;
                     remotetag_para[0] = 2;
                     remotetag_para[1] = 6;
                     remotetag_para[1] = pwtag.index;
                     remotetag_para[2] = 2;
                     memcpy(&remotetag_para[3],&pwtag.group_interval[i],2);
                     memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
                     dwt_writetxdata(22+remotetag_paralen, tx_nearresp_msg, 0);//写入发送数据
                     dwt_writetxfctrl(22+remotetag_paralen, 0);//设定发送长度		
                     dwt_writetxdata(24+remotetag_paralen, tx_nearresp_msg, 0);//写入发送数据
                     dwt_writetxfctrl(24+remotetag_paralen, 0);//设定发送长度		
                     remotesend_state=0;
                            finalsend = 1;
                     break;
                  }            
               }
                }
               
            }else{
                  dwt_writetxdata(22, tx_nearresp_msg, 0);//写入发送数据
                  dwt_writetxfctrl(22, 0);//设定发送长度
            if(finalsend == 0)
                {
                  dwt_writetxdata(24, tx_nearresp_msg, 0);//写入发送数据
                  dwt_writetxfctrl(24, 0);//设定发送长度
            }
               
//          if(remotesend_state&&tag_id_recv==configremotetagID)
@@ -998,6 +991,8 @@
         battary = rx_buffer[BATTARY_IDX];
         button = rx_buffer[BUTTON_IDX];
         frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
         tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
         tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
         if(result==0)
         {
            while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///不断查询芯片状态直到接收成功或者出现错误
@@ -1025,8 +1020,8 @@
               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);//打开接收
//               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);
@@ -1045,13 +1040,13 @@
               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的距离      
//               dwt_readdiagnostics(&d1);
               dwt_readdiagnostics(&d1);
//               time_monitor[2] = sync_timer*1000+TIM3->CNT;   
//               LOS(&d1);		
//               time_monitor[3] = 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];
@@ -1073,6 +1068,10 @@
               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;
               usart_send[17] = tag_frequency;
               usart_send[18] = tag_slotpos;
               checksum = Checksum_u16(&usart_send[2],17);
               memcpy(&usart_send[19],&checksum,2);
               UART_PushFrame(usart_send,21);
@@ -1089,9 +1088,9 @@
                  misdist_num[taglist_pos]++;
               }
            }
               if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1&&waittagconfig_reponse)
               if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1)
               {
                  waittagconfig_reponse = 0;
					
                  remotesend_state = 0;
                  usart_send[2] = 7;//正常模式 
                  usart_send[3] = 5;//数据段长度
@@ -1109,8 +1108,10 @@
            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)
@@ -1134,7 +1135,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;//获得接收数据长度
@@ -1144,34 +1145,24 @@
      memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
      switch(rx_buffer[MESSAGE_TYPE_IDX])
      {
         uint16_t checksum;
         case POLL:
            if (anchor_type == rx_buffer[ANC_TYPE_IDX])
            Anchor_RecPoll();
            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)
@@ -1183,14 +1174,51 @@
               ancidlist_num=g_com_map[NEARBASE_NUM];
               tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
               memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
               send_len=11+ancidlist_num*2+2;
               send_len=21+ancidlist_num*2;
               dwt_writetxdata(send_len, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
               dwt_writetxfctrl(send_len, 0);//设置超宽带发送数据长度
               dwt_starttx(DWT_START_TX_IMMEDIATE);
            }
            break;
      case REG_POLL:	
         for(i=0;i<MAX_REGTAGNUM;i++)
      {
         if(tag_id_recv==regtag_map.tagid[i]&&regtag_map.remain_time[i]>0)
         {
               tx_near_msg[REGR_TAGSLOTPOS_INDEX]=regtag_map.tag_slotpos[i];
               tx_near_msg[REGR_TAGFREQ_INDEX] = regtag_map.tag_frequency[i];
               memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
               memcpy(&tx_near_msg[ANCHOR_ID_IDX],&dev_id,2);
               tx_near_msg[MESSAGE_TYPE_IDX] = REG_RESPONSE;
               ancidlist_num=g_com_map[NEARBASE_NUM];
               tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
               memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
               send_len=21+ancidlist_num*2;
               dwt_writetxdata(send_len, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
               dwt_writetxfctrl(send_len, 0);//设置超宽带发送数据长度
               dwt_starttx(DWT_START_TX_IMMEDIATE);
               regtag_map.tagid[i] = 0;
            break;
         }
      
         case NEAR_POLL:			
      }
      if(i==MAX_REGTAGNUM)
      {
         dwt_readdiagnostics(&d1);
         LOS(&d1);	
         usart_send[2]=0x0a;
         usart_send[3]=20;
         memcpy(&usart_send[4],&dev_id,2);
         memcpy(&usart_send[6],&tag_id_recv,2);
         memcpy(&usart_send[8],&rx_buffer[10],9);
         usart_send[17] = firstpath_power;
         checksum = Checksum_u16(&usart_send[2],20);
         memcpy(&usart_send[22],&checksum,2);
         UART_PushFrame(usart_send,24);
      }
      break;
         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)
@@ -1207,10 +1235,10 @@
            tagpos_rec[(u8)temp_tagpos]=1;
            rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
         

GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);	
         if(anc_id_recv==dev_id)
         {
            Anchor_RecNearPoll(0);	
            Anchor_RecNearPoll(rec_nearbase_num);	
         }else{
               
            for(i=0;i<rec_nearbase_num;i++)
@@ -1218,8 +1246,9 @@
                memcpy(&tempid,&rx_buffer[NEARBASEID_INDEX+i*2],2);
               if(tempid==dev_id)
               {
						
                  seize_anchor=0;  //非抢占。已存在列表中
                  Anchor_RecNearPoll(i+1);	
                  Anchor_RecNearPoll(i);	
                  break;
               }                  
            }
@@ -1232,7 +1261,10 @@
//               }

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