From fed6a447e90051027925217fae8c6a5d57054989 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期一, 08 八月 2022 09:32:35 +0800
Subject: [PATCH] 1.66

---
 源码/核心板/Src/application/dw_app.c |   91 +++++++++++++++++++++++----------------------
 1 files changed, 46 insertions(+), 45 deletions(-)

diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
index 67acfdd..3b22bba 100644
--- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
+++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c"
@@ -313,7 +313,7 @@
 	}
 return max_slotpos-1;
 }
-u8 test=0;
+u8 test=0,dissucc_flag1,dissucc_flag2,lastdist1,lastdist2;
 u8 anclost_times=0,usartoutput_count,usartoutput_notenough=0;
 u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0,rec_extratag_battary;
 u16 temp_sync_timer1,temp_sync_timer2,rec_extratag_id;
@@ -422,23 +422,23 @@
 									memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
 									
 									
-									if(g_com_map[DEV_ROLE]!=0)
-									{
-									sync_timer=temp_sync_timer1;
-////									memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);		
+//									if(g_com_map[DEV_ROLE]!=0)
+//									{
+//									sync_timer=temp_sync_timer1;
+//////									memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);		
+////									
 //									
-									
-										tmp_time=tmp_time+650;
-									if(tmp_time>999)
-									{
-										tmp_time-=999;
-										sync_timer++;
-										if(sync_timer>=1000)
-											{sync_timer=0;}
-									}
-									TIM3->CNT=tmp_time;
-								}
-									current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL];
+//										tmp_time=tmp_time+650;
+//									if(tmp_time>999)
+//									{
+//										tmp_time-=999;
+//										sync_timer++;
+//										if(sync_timer>=1000)
+//											{sync_timer=0;}
+//									}
+//									TIM3->CNT=tmp_time;
+//								}
+//									current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL];
 //									if(tagslotpos>max_slotpos)
 //										tagslotpos=tagslotpos%(max_slotpos+1);
 //										tyncpoll_time=(tagslotpos-1)*slottime;
@@ -471,22 +471,19 @@
 									dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//将发送数据写入DW1000
 									dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
 									memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
-								if(temp_dist!=0x1ffff)
-									{
+                                if(temp_dist!=0x1ffff&&lastdist1!=temp_dist&&!dissucc_flag1)//没有成功测距,且距离和上一次不相等
+                                {
+                                    dissucc_flag1 = 1;
+                                    lastdist1 = temp_dist;
+	
 										g_Resttimer=0;
 										IWDG_Feed();
 										#ifdef USART_SINGLE_OUTPUT
 										usart_send[2] = 1;//正常模式
 										usart_send[3] = 17;//数据段长度
 										usart_send[4] = frame_seq_nb;//数据段长度
-										if(g_com_map[DEV_ROLE])
-										{
 										memcpy(&usart_send[5],&dev_id,2);
-										memcpy(&usart_send[7],&rec_nearbaseid,2);		
-										}else{
-										memcpy(&usart_send[5],&rec_nearbaseid,2);
-										memcpy(&usart_send[7],&dev_id,2);	
-										}											
+										memcpy(&usart_send[7],&rec_nearbaseid,2);												
 										memcpy(&usart_send[9],&temp_dist,4);
 										usart_send[13] = rx_buffer[DIST_IDX+3];
 										usart_send[14] = button;
@@ -495,21 +492,15 @@
 										UART_PushFrame(usart_send,21);
                                         usartoutput_count++;
 										#endif
-									}
+									
 									if(rec_extratag_dist!=0x1ffff&&rec_extratag_dist!=0&&rx_buffer[GROUP_ID_IDX]!=supergroupid)
 									{
 									#ifdef USART_SINGLE_OUTPUT
 										usart_send[2] = 1;//正常模式
 										usart_send[3] = 17;//数据段长度
 										usart_send[4] = frame_seq_nb;//数据段长度
-										if(g_com_map[DEV_ROLE])
-										{
 										memcpy(&usart_send[5],&rec_extratag_id,2);
-										memcpy(&usart_send[7],&rec_nearbaseid,2);		
-										}else{
-										memcpy(&usart_send[5],&rec_nearbaseid,2);
-										memcpy(&usart_send[7],&rec_extratag_id,2);	
-										}											
+										memcpy(&usart_send[7],&rec_nearbaseid,2);									
 										memcpy(&usart_send[9],&rec_extratag_dist,4);
 										usart_send[13] = rx_buffer[EXTRA_TAGDIST_INDEX+3];
 										usart_send[14] = button;
@@ -518,7 +509,7 @@
 										UART_PushFrame(usart_send,21);
 										#endif
 									}
-		
+                                }
 									
 								}else{
 									rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
@@ -545,8 +536,11 @@
 									LED0_BLINK;
 //									memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
 //									tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
-									if(temp_dist!=0x1ffff)
-									{
+						 if(temp_dist!=0x1ffff&&lastdist2!=temp_dist&&!dissucc_flag2)//没有成功测距,且距离和上一次不相等
+                                {
+                                    dissucc_flag2 = 1;
+                                    lastdist2 = temp_dist;
+                                    
 										#ifdef USART_SINGLE_OUTPUT
 										usart_send[2] = 1;//正常模式
 										usart_send[3] = 17;//数据段长度
@@ -571,7 +565,7 @@
 									//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
-								}
+								
 									if(rec_extratag_dist!=0x1ffff&&rec_extratag_dist!=0&&rx_buffer[GROUP_ID_IDX]!=supergroupid)
 									{
 									#ifdef USART_SINGLE_OUTPUT
@@ -594,6 +588,7 @@
 										UART_PushFrame(usart_send,21);
 										#endif
 									}
+                                }
 							}									
 							}
 						}else{
@@ -706,7 +701,8 @@
         {
             usartoutput_notenough = 0;
         }else{
-            if(usartoutput_notenough++>10)
+            sync_timer += time32_incr%5;
+            if(usartoutput_notenough++>50)
             {
                 SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader   
             }
@@ -791,7 +787,7 @@
 			break;
 	}
 
-	SetNextPollTime(tyncpoll_time);
+//	SetNextPollTime(tyncpoll_time);
 	g_start_send_flag = 0;		
 	GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);	
 }
@@ -802,6 +798,7 @@
 u16 tagid_list[TAG_NUM_IN_SYS];
 u8 tagofflinetime[TAG_NUM_IN_SYS];
 int32_t tagdist_list[TAG_NUM_IN_SYS];
+u8 tagseq_list[TAG_NUM_IN_SYS];
 void TagListUpdate(void)
 {
 	u16 i,j=0,temp[TAG_NUM_IN_SYS];
@@ -955,7 +952,10 @@
 			}else{
 				motorstate =0;
 			}
-			
+//			if(frame_seq_nb2!=tagseq_list[taglist_pos]+1)
+//            {
+//                tagdist_list[taglist_pos]=0x1ffff;
+//            }
 			if(new_tagid)
 			{
 				tagdist_list[taglist_pos]=0x1ffff;
@@ -1013,7 +1013,7 @@
 					uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
 					double Ra, Rb, Da, Db;
 					int64_t tof_dtu;
-          u8 si;
+                    u8 si;
 					resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
 					final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6
 					final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1,T4,T5
@@ -1059,13 +1059,13 @@
 						#ifdef TDFILTER
 						NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
 						filter_dist = pos_predict[taglist_pos]/10;
-                        filter_speed = vel_predict[taglist_pos]/10;
+                        //filter_speed = vel_predict[taglist_pos];
                         newmeasure = 1;
 						#else
 						filter_dist=hex_dist/10;
 						#endif
 						misdist_num[taglist_pos]=0;
-					tagdist_list[taglist_pos] = filter_dist;	
+					tagdist_list[taglist_pos] = filter_dist;
 					his_dist[taglist_pos]=hex_dist;	
                         if(hex_dist>0)
 						{
@@ -1073,6 +1073,7 @@
                         }else{
                             g_Tagdist[taglist_pos]= 1;
                         }
+                        tagseq_list[taglist_pos] = frame_seq_nb2;
 				#ifdef USART_SINGLE_OUTPUT
 					usart_send[2] = 1;//正常模式 
 					usart_send[3] = 17;//数据段长度
@@ -1082,7 +1083,7 @@
                             if(tagdist_list[taglist_pos]<=0)
                                 tagdist_list[taglist_pos]=10;
 					memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
-														tagdist_list[taglist_pos] =tagdist_list[taglist_pos]| (battary<<24);
+					tagdist_list[taglist_pos] =tagdist_list[taglist_pos]| (battary<<24);
 					usart_send[13] = battary;
 					usart_send[14] = button;
 					usart_send[15] = firstpath_power;

--
Gitblit v1.9.3