提交实时计步和日常计步最新的代码

detect_step_by_mag.c

@@ -6,171 +6,255 @@
 #include "nrf_delay.h"
 
 uint16_t mag_sqrt =0;
+//static int16_t ShoeTouchFlag=1;
+
+int16_t Get_ShoeTouchFlag(void)
+{
+	return ShoeTouchFlag;
+}
+
+//static int16_t Rcounter=0;
+
+int16_t Get_Rcounter(void)
+{
+	return Rcounter;
+}
+
+static int16_t Tcounter=0;
+
+int16_t Get_Tcounter(void)
+{
+	return Tcounter;
+}
+
+
+//static int16_t FKEY=0;
+//static int16_t BKEY=0;
+int16_t Get_FKEY(void)
+{
+	return FKEY;
+}
+
+int16_t Get_BKEY(void)
+{
+	return BKEY;
+}
+
+
+
+static int RealFrontLiftPoint;
+static int RealBackLiftPoint;
+int32_t Get_RealFrontLiftPoint(void)
+{
+	return RealFrontLiftPoint;
+}
+
+int32_t Get_RealBackLiftPoint(void)
+{
+	return RealBackLiftPoint;
+}
 
 
 uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 {
 	
 	
-	static int16_t ShoeTouchFlag=0;
-	static int16_t FrontTouchFlag=0;
-	static int16_t BackTouchFlag=0;
-	static int16_t FrontLiftPoint;
-	static int16_t FrontDropPoint;
-	static int16_t BackLiftPoint;
-	static int16_t BackDropPoint;
+
+	//static int FrontLiftPoint;
+	//static int FrontDropPoint;
+	//static int BackLiftPoint;
+	//static int BackDropPoint;
 	static int32_t SD;
-	static int16_t counter;
-	static int16_t BeginToCount;
-	//static int16_t FOB;//Front(1)orBack(2)
+	//static int16_t Rcounter;
 	int32_t mag_temp_front[3];
 	int32_t mag_temp_back[3];
 	static int16_t accZ_buf[4];
-	static uint8_t acc_index = 0;
-	//static uint16_t mag_buf_front[3];
-	//static uint16_t mag_buf_back[3];
+	//static uint8_t acc_index = 0;
 	static int mag_buf_front[3];
 	static int mag_buf_back[3];
-	static uint8_t mag_index_front = 0;
-	static uint8_t mag_index_back = 0;
+	//static uint8_t mag_index_front = 0;
+	//static uint8_t mag_index_back = 0;
 	int32_t mag_sqrt_front =0;
 	int32_t mag_sqrt_back =0;
-	int calculate_flag = 0;
-	int16_t step;
+	//static int calculate_flag = 0;
+	int16_t step=0;
 	float MV;
 	float D;
-	
+	//static int16_t Ucounter=0;//离地计时器
 	
 	SEGGER_RTT_printf(0,"Enter RealTimeStep!\n");
+	Tcounter++;
 	
 	for(int i = 0; i < 3; i ++)
 	{
 		mag_temp_front[i] = (int32_t) (front[i]);
 	}
-	//mag_sqrt_front  = (uint16_t)(sqrt((float) (mag_temp_front[0] * mag_temp_front[0] + mag_temp_front[1] * mag_temp_front[1] + mag_temp_front[2] * mag_temp_front[2])));
 	mag_sqrt_front = 	abs(mag_temp_front[2]);
 	for(int i = 0; i < 3; i ++)
 	{
 		mag_temp_back[i] = (int32_t) (back[i]);
 	}
-	//mag_sqrt_back  = (uint16_t)(sqrt((float) (mag_temp_back[0] * mag_temp_back[0] + mag_temp_back[1] * mag_temp_back[1] + mag_temp_back[2] * mag_temp_back[2])));
 	mag_sqrt_back = abs(mag_temp_back[2]);
 	
-//	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);
 
 	/*读入磁力计数据*/
+	
 	if(mag_index_front >= 3)
 	{
 		mag_buf_front[0] = mag_buf_front[1];
 		mag_buf_front[1] = mag_buf_front[2];
 		mag_index_front = 2;
-		//calculate_flag = 1;
+		calculate_flag = 1;
+		
 	}
 	mag_buf_front[mag_index_front++] = mag_sqrt_front;//前脚掌数据
 	
+	
 	if(mag_index_back >= 3)
 	{
 		mag_buf_back[0] = mag_buf_back[1];
 		mag_buf_back[1] = mag_buf_back[2];
 		mag_index_back = 2;
-		//calculate_flag = 1;
+		calculate_flag = 1;
 	}
 	mag_buf_back[mag_index_back++] = mag_sqrt_back;//后脚掌数据
 	
   /*读入xyZ轴加速度数据*/
-	if(acc_index >= 4)
+	if(acc_index >= 3)
 	{
 		accZ_buf[0] = accZ_buf[1];
-		accZ_buf[1] = accZ_buf[2];
-		accZ_buf[2] = accZ_buf[3];
-		
-		acc_index = 3;
+		accZ_buf[1] = accZ_buf[2];		
+		acc_index = 2;
 		calculate_flag = 1;
 	}
 	accZ_buf[acc_index++] = -acc[2];
 	
 	
+	//SEGGER_RTT_printf(0,"mag_index_front==:%d...\n",mag_index_front);
+	//SEGGER_RTT_printf(0,"mag_index_back==:%d...\n",mag_index_back);
+	//SEGGER_RTT_printf(0,"calculate_flag==:%d...\n",calculate_flag);
+	
 	if(calculate_flag==1)
 	{
-		
+		SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf_front[2]);
+		SEGGER_RTT_printf(0,"mag_back3==:%d...\n",mag_buf_back[2]);
+		SEGGER_RTT_printf(0,"FrontLiftPoint==:%d...\n",FrontLiftPoint);
+		SEGGER_RTT_printf(0,"BackLiftPoint==:%d...\n",BackLiftPoint);
 		
 	  /*不断更新前后脚掌磁力计的上升起始点或下降起始点*/
-	  if(mag_buf_front[1] - mag_buf_front[0]>=0)
+	  if(mag_buf_front[2] - mag_buf_front[1]>=0)
 	  {
-      FrontDropPoint = mag_buf_front[1];
+			//if(FKEY==0)
+			//{
+        FrontDropPoint = mag_buf_front[2];
+			//}
+			//else
+			//{
+				//if(mag_buf_front[2]>FrontDropPoint)
+				//{
+					//FrontDropPoint = mag_buf_front[2];
+				//}
+			//}
 	  }
     else
     {
-      FrontLiftPoint = mag_buf_front[1];
+			if(FKEY==0)//FKEY为前脚波形状态变量，1代表波形进行中，0代表波形结束。该语句意思是前脚掌磁力计不处于波形进行阶段时，如常更新上升起点。
+			{
+       FrontLiftPoint = mag_buf_front[2];
+			}
+			else
+			{
+				;//当波形进行中，不改变初始上升起点，有助于过滤杂波。
+			}
 	  }
 
-    if(mag_buf_back[1] - mag_buf_back[0]>=0)
+    if(mag_buf_back[2] - mag_buf_back[1]>=0)
 	  {
-      BackDropPoint = mag_buf_back[1];
+			//if(BKEY==0)
+			//{
+        BackDropPoint = mag_buf_back[2];
+			//}
+			//else
+			//{
+				//if(mag_buf_back[2]>BackDropPoint)
+				//{
+					//BackDropPoint = mag_buf_back[2];
+				//}
+			//}
 	  }
     else
     {
-      BackLiftPoint = mag_buf_back[1];
+			if(BKEY==0)//BKEY为后脚波形状态变量，1代表波形进行中，0代表波形结束。该语句意思是后脚掌磁力计不处于波形进行阶段时，如常更新上升起点
+			{
+       BackLiftPoint = mag_buf_back[2];
+			}
+			else
+			{
+				;
+			}
 	  }
 
 	  /*判断前后脚掌的状态*/
-    if(mag_buf_front[1] - FrontLiftPoint>2200)
-	  {
-		  FrontTouchFlag = 2;
-	  }
-	  else if(mag_buf_front[1] - FrontDropPoint<-1000)
+    if(mag_buf_front[2] - FrontLiftPoint>2200)
 	  {
-		  FrontTouchFlag = 0;
-	  }
-	  else if((abs(mag_buf_front[1]-mag_buf_front[0])<500)&&(abs(mag_buf_front[1]-mag_buf_front[2])<500))
-	  {
-		  FrontTouchFlag = 1;
-	  }
-	
-	  if(mag_buf_back[1] - BackLiftPoint>2200)
-	  {
-		  BackTouchFlag = 2;
-	  }
-	  else if(mag_buf_back[1] - BackDropPoint<-1000)
-	  {
-		  BackTouchFlag = 0;
+			FKEY = 1;
+			/*
+			if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance)
+			{
+			  FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint;
+			}
+			*/
 	  }
-	  else if((abs(mag_buf_back[1]-mag_buf_back[0])<500)&&(abs(mag_buf_back[1]-mag_buf_back[2])<500))
+	  if(mag_buf_back[2] - BackLiftPoint>2200)
 	  {
-		  BackTouchFlag = 1;
+			BKEY = 1;
+			/*
+			if(mag_buf_back[2] - BackLiftPoint>BackLiftDistance)
+			{
+			  BackLiftDistance = mag_buf_back[2] - BackLiftPoint;
+			}
+			*/
 	  }
-	
+		/*判断是否满足FKEY=0和BKEY=0，即是否满足一个升降波形*/
+		
+		//if((mag_buf_front[2] - FrontLiftPoint<=1500)||(FrontDropPoint-mag_buf_front[2]>0.68*FrontLiftDistance)) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
+		if(mag_buf_front[2] - FrontLiftPoint<=1500)
+		{
+			FKEY=0;
+			//FrontLiftDistance = 2200;
+		}
+		
+	  
+		//if((mag_buf_back[2] - BackLiftPoint<=1500)||(BackDropPoint-mag_buf_back[2]>0.68*BackLiftDistance))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
+		if(mag_buf_back[2] - BackLiftPoint<=1500)
+		{
+			BKEY=0;
+			//BackLiftDistance = 2200;
+		}
+    
+		
+		/*跳出死循环*/
+		if(FrontLiftPoint==0) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
+		{
+			FKEY=0;
+		}
+		
+	  if(BackLiftPoint==0)//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
+		{
+			BKEY=0;
+		}
+		
+		
 	/*判断鞋子触地或离地*/
-	
+	  
 	  if(ShoeTouchFlag==0)
 	  {
-			if((FrontTouchFlag==2||BackTouchFlag==2)&&(SD>60))
-			//if(FrontTouchFlag==2||BackTouchFlag==2)
+			Ucounter++;
+			Rcounter=0;		
+			if(((FKEY==1)||(BKEY==1))&&(Ucounter>=6))
 		  {
 			  step=1;
 			  ShoeTouchFlag=1;
-			  counter = 0;
-				/*
-				SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf_front[2]);
-		    SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf_front[1]);
-		    SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf_front[0]);
-		
-	      SEGGER_RTT_printf(0,"mag_back3==:%d...\n",mag_buf_back[2]);
-		    SEGGER_RTT_printf(0,"mag_back2==:%d...\n",mag_buf_back[1]);
-		    SEGGER_RTT_printf(0,"mag_back1==:%d...\n",mag_buf_back[0]);
-		
-	      SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
-				
-		    SEGGER_RTT_printf(0,"acc2==:%d...\n",accZ_buf[1]);
-		    SEGGER_RTT_printf(0,"acc1==:%d...\n",accZ_buf[0]);
-				
-				SEGGER_RTT_printf(0,"FrontLiftPoint==:%d...\n",FrontLiftPoint);
-		    SEGGER_RTT_printf(0,"FrontDropPoint==:%d...\n",FrontDropPoint);
-				SEGGER_RTT_printf(0,"BackLiftPoint==:%d...\n",BackLiftPoint);
-		    SEGGER_RTT_printf(0,"BackDropPoint==:%d...\n",BackDropPoint);
-				
-				SEGGER_RTT_printf(0,"SD==:%d...\n",SD);
-        */				
 		  }
 		  else
 		  {
@@ -180,42 +264,78 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  }
 	  else
 	  {
-		  step=0;
-			counter++;
-			//if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(((accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))||((accZ_buf[0]>2300)&&(accZ_buf[1]>2300)&&(accZ_buf[2]>2300)))&&(counter>=2))
-			//if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&((accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))&&(counter>=2))
-			//if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&((accZ_buf[0]>2500)&&(accZ_buf[1]>2500)&&(accZ_buf[2]>2500)))
-		  //if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(((accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))||((accZ_buf[0]>3000)&&(accZ_buf[0]>accZ_buf[1])&&(accZ_buf[1]>accZ_buf[2])))&&(counter>=12))
-			if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(((accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))||((accZ_buf[0]>2500)&&(accZ_buf[1]>2500)&&(accZ_buf[2]>2500)))&&(counter>=12))
+		  step = 0;
+			Rcounter++;
+			Ucounter=0;
+			if((Rcounter>=6)&&(FKEY==0)&&((abs(accZ_buf[0]-2000)>250)||(abs(accZ_buf[1]-2000)>250)||(abs(accZ_buf[2]-2000)>250))&&(BKEY==0))
 		  {
 			  ShoeTouchFlag=0;
-				//FOB = 0;
+				Lock=0;
 		  }
 		  else
 		  {
 			  ShoeTouchFlag=1;
-		  }	
+		  }
+
+      if((Rcounter>3*100)&&(Lock==0))//防止卡死
+			{
+				FrontLiftPoint= FrontLiftPoint + 3000;
+				BackLiftPoint = BackLiftPoint + 3000;
+				Lock=1;
+			}			
+			
 	  }
+		
 	  /*计算加速度的方差*/
-	  MV = (accZ_buf[1]+accZ_buf[2]+accZ_buf[3])/3;
+		/*
+	  MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;
 	  D=0;
-	  for(int i = 1; i < 4; i ++)
+	  for(int i = 0; i < 3; i ++)
 	  {
 		  D = D + (accZ_buf[i]-MV)*(accZ_buf[i]-MV);
 	  }
-	  D = D/3;
+	  D = D*0.33333333;
 	  SD = sqrt((float)D);
-		
-		//SEGGER_RTT_printf(0,"step==:%d...\n",step);
+				*/
+	
   }
 	else
   {
-			step=0;
+		step=0;
+		FrontLiftPoint = mag_buf_front[2];
+		FrontDropPoint = mag_buf_front[2];
+		BackLiftPoint = mag_buf_back[2];
+		BackDropPoint = mag_buf_back[2];
+		SEGGER_RTT_printf(0,"Just Enter The Game! FrontLiftPoint==:%d...\n",FrontLiftPoint);
+		SEGGER_RTT_printf(0,"Just Enter The Game! BackLiftPoint==:%d...\n",BackLiftPoint);
   }
 	
+	RealFrontLiftPoint = FrontLiftPoint;
+	RealBackLiftPoint = BackLiftPoint;
 	return step;
 }
 
+void ClearRealTimeStep()
+{
+  FrontLiftPoint=30000;
+  FrontDropPoint=30000;
+  BackLiftPoint=30000;
+  BackDropPoint=30000;
+  calculate_flag=0;
+  Ucounter=0;
+  ShoeTouchFlag=1;
+  Rcounter=0;
+  FKEY=0;
+  BKEY=0;
+	acc_index = 0;
+  mag_index_front = 0;
+  mag_index_back = 0;
+	FrontLiftDistance = 2200;
+	BackLiftDistance = 2200;
+	Lock = 0;
+}
+
+
 uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 {
 	static uint8_t up_flag = 0;
@@ -230,11 +350,22 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
   static int16_t FrontLiftPoint;
 	static int16_t FrontDropPoint;
 	static int16_t FrontTouchFlag=0;
+	static int16_t PreFrontTouchFlag=0;
 	float MV;
 	float D;
 	static int32_t SD;
 	static int16_t counter=0;
 	
+	/*
+	static float K;
+	static float L;
+	static float a=0;
+	static float v=0;
+  static float s=0;	
+	
+	
+	K = 9.8*0.0001*0.0005;//单位分别为厘米和毫秒
+	*/
 	
 	//SEGGER_RTT_printf(0,"mag_sqrt==:%d...\n",1000);
 	for(int i = 0; i < 3; i ++)
@@ -268,7 +399,8 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	if (calculate_flag == 1)
 	{
 		
-		static uint32_t time =0;
+		//static uint32_t time =0;
+		/*
 		if(TIME_GetTicks() - time >=1000){
 			time = TIME_GetTicks();
 		SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
@@ -282,7 +414,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		SEGGER_RTT_printf(0,"f_mx=%d,f_my=%d,f_mz=%d,acc_z:%d\r\n",mag[0],mag[1],mag[2],acc_z);
 		
 		}
-		
+		*/
 				 
 		if(mag_buf[1] - mag_buf[0]>=0)
 	  {
@@ -294,7 +426,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	  }
 		
 		/*判断前后脚掌的状态*/
-    if(mag_buf[1] - FrontLiftPoint>1500)
+    if(mag_buf[1] - FrontLiftPoint>2000)
 	  {
 		  FrontTouchFlag = 2;
 	  }
@@ -306,15 +438,19 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	  {
 		  FrontTouchFlag = 1;
 	  }
-		
+
 		/*计算步数*/
 	  if (up_flag == 0)
 	  {
-			 if((FrontTouchFlag==2)&&(SD>60))
+			 if(((FrontTouchFlag==2)&&(PreFrontTouchFlag!=2))&&(SD>60))
 			 {
 				 step = 1;
 				 up_flag = 1;
 				 counter=0;
+				 //a=0;
+				 //v=0;
+				 //s=0;
+				 /*
 				 SEGGER_RTT_printf(0,"step==%d\n",step);
 				 
 				 SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
@@ -330,6 +466,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		     SEGGER_RTT_printf(0,"FrontDropPoint==:%d...\n",FrontDropPoint);
 	 
 		     SEGGER_RTT_printf(0,"SD==:%d...\n",SD);
+				 */
 //				 static char string[50];
 //				 sprintf(string,"1 min=%f,mag_buf=%d\n",min,mag_buf[2]);
 //				 SEGGER_RTT_printf(0,"%s",string);
@@ -350,6 +487,9 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		{
 			step = 0;
 			counter++;
+			//a = (accZ_buf[2]-L)*K;
+			//v = v + a*10;
+			//s = s + v*10;
 			if((FrontTouchFlag!=2)&&(((accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))||((accZ_buf[0]>2500)&&(accZ_buf[1]>2500)&&(accZ_buf[2]>2500)))&&(counter>=12))
 			{
 				up_flag=0;
@@ -364,8 +504,15 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	  }
 	  D = D/3;
 	  SD = sqrt((float)D);
-		
-		
+		PreFrontTouchFlag = FrontTouchFlag;
+		/*
+		static char string[100];                                                                                        
+		sprintf(string,"=====>time consuming:%02f us\r\n",(float)a); 
+		SEGGER_RTT_printf(0,"%s",string);
+		SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
+		sprintf(string,"=====>time consuming:%02f us\r\n",(float)v); 
+		SEGGER_RTT_printf(0,"%s",string);
+		*/
 	}
 	else
 	{

detect_step_by_mag.h

@@ -14,9 +14,30 @@
 //uint8_t detect_step_by_mag(int16_t *mag);
 uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z);
 uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3]);
-
-
-
+int16_t Get_ShoeTouchFlag(void);
+int16_t Get_Rcounter(void);
+int16_t Get_Tcounter(void);
+int16_t Get_FKEY(void);
+int16_t Get_BKEY(void);
+int32_t Get_RealFrontLiftPoint(void);
+int32_t Get_RealBackLiftPoint(void);
+void ClearRealTimeStep();
+static int FrontLiftPoint;
+static int FrontDropPoint;
+static int BackLiftPoint;
+static int BackDropPoint;
+static int calculate_flag;
+static int16_t Ucounter=0;
+static int16_t ShoeTouchFlag=1;
+static int16_t Rcounter=0;
+static int16_t FKEY=0;
+static int16_t BKEY=0;
+static uint8_t acc_index;
+static uint8_t mag_index_front;
+static uint8_t mag_index_back;
+static int FrontLiftDistance;
+static int BackLiftDistance;
+static int Lock=0;
 
 
 #endif