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

detect_step_by_mag.c

@@ -6,89 +6,36 @@
 #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 FKEY=0;
   static int16_t BKEY=0;
 	static int FrontLiftPoint;
 	static int FrontDropPoint;
 	static int BackLiftPoint;
 	static int BackDropPoint;
-	*/
-	static int32_t SD;
-	//static int16_t Rcounter=0;
+	static uint8_t mag_index_front = 0;
+	static uint8_t mag_index_back = 0;
+	static int16_t Rcounter=0;
+	static int16_t Ucounter=0;//离地计时器
+	static int16_t ShoeTouchFlag=1;
+	static uint8_t acc_index = 0;
+	static int calculate_flag = 0;
 	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 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;
 	int32_t mag_sqrt_front =0;
 	int32_t mag_sqrt_back =0;
-	//static int calculate_flag = 0;
 	int16_t step=0;
-	float MV;
-	float D;
-	//static int16_t Ucounter=0;//离地计时器
-	//static int FrontLiftDistance;
-  //static int BackLiftDistance;
-	//static int16_t ShoeTouchFlag=1;
+	static int FrontLiftDistance;
+  static int BackLiftDistance;
 	
 	//SEGGER_RTT_printf(0,"Enter RealTimeStep!\n");
-	Tcounter++;
+	//Tcounter++;
 	
 	for(int i = 0; i < 3; i ++)
 	{
@@ -218,18 +165,14 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  }
 		/*判断是否满足FKEY=0和BKEY=0，即是否满足一个升降波形*/
 		
-		if((mag_buf_front[2] - FrontLiftPoint<=1500)||((FrontDropPoint-mag_buf_front[2]>0.45*FrontLiftDistance)&&(abs(mag_buf_front[2]-mag_buf_front[1])<500))) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
-		//if(mag_buf_front[2] - FrontLiftPoint<=1500)
-		//if(FrontDropPoint-mag_buf_front[2]>0.45*FrontLiftDistance)
+		if((mag_buf_front[2] - FrontLiftPoint<=1500)||((FrontDropPoint-mag_buf_front[2]>0.4*FrontLiftDistance)&&(abs(mag_buf_front[2]-mag_buf_front[1])<400))) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
 		{
 			FKEY=0;
 			FrontLiftDistance = 2200;
 		}
 		
 	  
-		if((mag_buf_back[2] - BackLiftPoint<=1500)||((BackDropPoint-mag_buf_back[2]>0.45*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<500)))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
-		//if(mag_buf_back[2] - BackLiftPoint<=1500)
-		//if(BackDropPoint-mag_buf_back[2]>0.45*BackLiftDistance)
+		if((mag_buf_back[2] - BackLiftPoint<=1500)||((BackDropPoint-mag_buf_back[2]>0.4*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<400)))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
 		{
 			BKEY=0;
 			BackLiftDistance = 2200;
@@ -286,18 +229,7 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 			  ShoeTouchFlag=1;
 		  }
 	  }
-		//SEGGER_RTT_printf(0,"Rcounter==:%d...\n",Rcounter);
-	  /*计算加速度的方差*/
-		/*
-	  MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;
-	  D=0;
-	  for(int i = 0; i < 3; i ++)
-	  {
-		  D = D + (accZ_buf[i]-MV)*(accZ_buf[i]-MV);
-	  }
-	  D = D*0.33333333;
-	  SD = sqrt((float)D);
-				*/
+	
 	
   }
 	else
@@ -307,75 +239,35 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 		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;
+	static uint8_t up_flag = 1;
 	int32_t mag_temp[3];
 	static int16_t accZ_buf[3];
 	static uint8_t acc_index = 0;
-	//static uint16_t mag_buf[3];
 	static int mag_buf[3];
 	static uint8_t mag_index = 0;
 	int step;
 	int calculate_flag = 0;
-  static int16_t FrontLiftPoint;
-	static int16_t FrontDropPoint;
+  static int16_t LiftPoint;
+	static int16_t DropPoint;
 	static int16_t FrontTouchFlag=0;
-	static int16_t PreFrontTouchFlag=0;
-	float MV;
-	float D;
-	static int32_t SD;
-	static int16_t counter=0;
+	static int16_t LiftDistance;
+	static int16_t counter1=0;
+	static int16_t counter2=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 ++)
 	{
 		mag_temp[i] = (int32_t) (mag[i]);
 	}
-	//mag_sqrt  = (uint16_t)(sqrt((float) (mag_temp[0] * mag_temp[0] + mag_temp[1] * mag_temp[1] + mag_temp[2] * mag_temp[2])));
 	mag_sqrt = abs(mag_temp[2]);
-//	SEGGER_RTT_printf(0,"mag_sqrt:%d...\n",mag_sqrt);
 	/*读入磁力计数据*/
 	if(mag_index >= 3)
 	{
@@ -400,124 +292,92 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	if (calculate_flag == 1)
 	{
 		
-		//static uint32_t time =0;
-		/*
-		if(TIME_GetTicks() - time >=1000){
-			time = TIME_GetTicks();
-		SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
-		SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf[1]);
-		SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf[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,"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)
+		SEGGER_RTT_printf(0,"LiftPoint==:%d...\n",LiftPoint);
+		SEGGER_RTT_printf(0,"DropPoint==:%d...\n",DropPoint);
+		SEGGER_RTT_printf(0,"LiftDistance==:%d...\n",LiftDistance);
+						 
+		if(mag_buf[2] - mag_buf[1]>=0)
 	  {
-       FrontDropPoint = mag_buf[1];
+			if(FrontTouchFlag==0)
+			{
+       DropPoint = mag_buf[2];
+			}
+			else
+			{
+				if(mag_buf[2]>DropPoint)
+				{
+					DropPoint = mag_buf[2];
+				}
+			}
  	  }
     else
     {
-       FrontLiftPoint = mag_buf[1];
+			if(FrontTouchFlag==0)
+			{
+       LiftPoint = mag_buf[2];
+			}
+			else
+			{
+				;
+			}
 	  }
 		
 		/*判断前后脚掌的状态*/
-    if(mag_buf[1] - FrontLiftPoint>2000)
+    if(mag_buf[2] - LiftPoint>2000)
 	  {
-		  FrontTouchFlag = 2;
+		  FrontTouchFlag = 1;
+			if(mag_buf[2] - LiftPoint>LiftDistance)
+			{
+			  LiftDistance = mag_buf[2] - LiftPoint;
+			}			
 	  }
-	  else if(mag_buf[1] - FrontDropPoint<-1000)
+	  if((mag_buf[2] - LiftPoint<=1500)||((DropPoint-mag_buf[2]>0.4*LiftDistance)&&(abs(mag_buf[2]-mag_buf[1])<400)))
 	  {
 		  FrontTouchFlag = 0;
-	  }
-	  else if((abs(mag_buf[1]-mag_buf[0])<500)&&(abs(mag_buf[1]-mag_buf[2])<500))
-	  {
-		  FrontTouchFlag = 1;
+			LiftDistance = 2200;
 	  }
 
 		/*计算步数*/
 	  if (up_flag == 0)
 	  {
-			 if(((FrontTouchFlag==2)&&(PreFrontTouchFlag!=2))&&(SD>60))
+			 counter1=0;
+			 if(counter2<300)
+			 {
+				counter2++;
+			 }
+			 if((FrontTouchFlag==1)&&(counter2>=6))
 			 {
 				 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]);
-		     SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf[1]);
-		     SEGGER_RTT_printf(0,"mag_front1==:%d...\n",mag_buf[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,"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);
-//				 SEGGER_RTT_printf(0,"step==%d\n",step);
-//				 nrf_delay_ms(1000);
-//				 while(1);
 			 }
 			 else
 			 {
 				 step = 0;
 				 up_flag = 0;
-//				 nrf_delay_ms(1000);
-//		     NVIC_SystemReset();
 			 }
 		
 	  }
 		else
 		{
 			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))
+			counter2=0;
+			if(counter1<300)
+			{
+			  counter1++;
+			}
+			if((FrontTouchFlag==0)&&((abs(accZ_buf[0]-2000)>300)||(abs(accZ_buf[1]-2000)>300)||(abs(accZ_buf[2]-2000)>300))&&(counter1>=6))
 			{
 				up_flag=0;
 			}	
 		}
-		
-		MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;
-	  D=0;
-	  for(int i = 0; i < 3; i ++)
-	  {
-		  D = D + (accZ_buf[i]-MV)*(accZ_buf[i]-MV);
-	  }
-	  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
 	{
 		step = 0;
+		LiftPoint = mag_buf[2];
+		DropPoint = mag_buf[2];
 	}
 	
 	

detect_step_by_mag.h

@@ -15,31 +15,5 @@
 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;
-
 
 #endif