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

detect_step_by_mag.c

@@ -19,15 +19,13 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	static int16_t FrontDropPoint;
 	static int16_t BackLiftPoint;
 	static int16_t BackDropPoint;
-	static int16_t FrontMagLevel=0;//前脚电平
-	static int16_t BackMagLevel=0;//后脚电平
 	static int32_t SD;
 	static int16_t counter;
 	static int16_t BeginToCount;
-	static int16_t FOB;//Front(1)orBack(2)
+	//static int16_t FOB;//Front(1)orBack(2)
 	int32_t mag_temp_front[3];
 	int32_t mag_temp_back[3];
-	static int16_t accZ_buf[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];
@@ -66,7 +64,7 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[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;//前脚掌数据
 	
@@ -75,16 +73,19 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[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 >= 3)
+	if(acc_index >= 4)
 	{
 		accZ_buf[0] = accZ_buf[1];
 		accZ_buf[1] = accZ_buf[2];
-		acc_index = 2;
+		accZ_buf[2] = accZ_buf[3];
+		
+		acc_index = 3;
+		calculate_flag = 1;
 	}
 	accZ_buf[acc_index++] = -acc[2];
 	
@@ -116,12 +117,10 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
     if(mag_buf_front[1] - FrontLiftPoint>2200)
 	  {
 		  FrontTouchFlag = 2;
-			//FrontMagLevel = 1;//high level
 	  }
 	  else if(mag_buf_front[1] - FrontDropPoint<-1000)
 	  {
 		  FrontTouchFlag = 0;
-			//FrontMagLevel = 0;//low level
 	  }
 	  else if((abs(mag_buf_front[1]-mag_buf_front[0])<500)&&(abs(mag_buf_front[1]-mag_buf_front[2])<500))
 	  {
@@ -131,12 +130,10 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  if(mag_buf_back[1] - BackLiftPoint>2200)
 	  {
 		  BackTouchFlag = 2;
-			//BackMagLevel = 1;//high level
 	  }
 	  else if(mag_buf_back[1] - BackDropPoint<-1000)
 	  {
 		  BackTouchFlag = 0;
-			//BackMagLevel = 0;//low level
 	  }
 	  else if((abs(mag_buf_back[1]-mag_buf_back[0])<500)&&(abs(mag_buf_back[1]-mag_buf_back[2])<500))
 	  {
@@ -147,22 +144,12 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	
 	  if(ShoeTouchFlag==0)
 	  {
-		  //if((FrontTouchFlag==2||BackTouchFlag==2)&&((accZ_buf[0]>2150)||accZ_buf[1]>2150||accZ_buf[2]>2150))
-			//if((((FrontTouchFlag==2)&&(FrontMagLevel==0))||((BackTouchFlag==2)&&(BackMagLevel==0)))&&(SD>60))
-			if(FrontTouchFlag==2||BackTouchFlag==2)
+			if((FrontTouchFlag==2||BackTouchFlag==2)&&(SD>60))
+			//if(FrontTouchFlag==2||BackTouchFlag==2)
 		  {
 			  step=1;
 			  ShoeTouchFlag=1;
 			  counter = 0;
-				BeginToCount = 0;
-				if(FrontTouchFlag==2)
-				{
-					FOB = 1;
-				}
-				if(BackTouchFlag==2)
-				{
-					FOB = 2;
-				}
 				/*
 				SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf_front[2]);
 		    SEGGER_RTT_printf(0,"mag_front2==:%d...\n",mag_buf_front[1]);
@@ -194,20 +181,15 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 	  else
 	  {
 		  step=0;
-			if(((FOB==1)&&(FrontTouchFlag!=2))||((FOB==2)&&(BackTouchFlag!=2)))
-			{
-			  BeginToCount=1;
-			}	
-			if(BeginToCount==1)
-			{
-				counter++;
-			}
-			if((FrontTouchFlag!=2)&&(BackTouchFlag!=2)&&(accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800)&&(counter>=2))
+			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))
 		  {
 			  ShoeTouchFlag=0;
-				FOB = 0;
-				//FrontMagLevel = 0;
-				//BackMagLevel = 0;
+				//FOB = 0;
 		  }
 		  else
 		  {
@@ -215,9 +197,9 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 		  }	
 	  }
 	  /*计算加速度的方差*/
-	  MV = (accZ_buf[0]+accZ_buf[1]+accZ_buf[2])/3;
+	  MV = (accZ_buf[1]+accZ_buf[2]+accZ_buf[3])/3;
 	  D=0;
-	  for(int i = 0; i < 3; i ++)
+	  for(int i = 1; i < 4; i ++)
 	  {
 		  D = D + (accZ_buf[i]-MV)*(accZ_buf[i]-MV);
 	  }
@@ -251,6 +233,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 	float MV;
 	float D;
 	static int32_t SD;
+	static int16_t counter=0;
 	
 	
 	//SEGGER_RTT_printf(0,"mag_sqrt==:%d...\n",1000);
@@ -331,6 +314,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 			 {
 				 step = 1;
 				 up_flag = 1;
+				 counter=0;
 				 SEGGER_RTT_printf(0,"step==%d\n",step);
 				 
 				 SEGGER_RTT_printf(0,"mag_front3==:%d...\n",mag_buf[2]);
@@ -339,7 +323,7 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		
 		
 	       SEGGER_RTT_printf(0,"acc3==:%d...\n",accZ_buf[2]);
-		     SEGGER_RTT_printf(0,"acc2==:%d...\n",accZ_buf[1]);
+		     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);
@@ -365,7 +349,8 @@ uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 		else
 		{
 			step = 0;
-			if((FrontTouchFlag!=2)&&(accZ_buf[0]<1800)&&(accZ_buf[1]<1800)&&(accZ_buf[2]<1800))
+			counter++;
+			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;
 			}