记步算法 重新设计

motion/detect_step_by_mag.c

@@ -246,140 +246,113 @@ uint8_t RealTimeStep(int16_t front[3], int16_t back[3], int16_t acc[3])
 
 uint8_t detect_step_by_mag(int16_t *mag, int16_t acc_z)
 {
-	static uint8_t up_flag = 1;
-	int32_t mag_temp[3];
-	static int16_t accZ_buf[3];
-	static uint8_t acc_index = 0;
-	static int mag_buf[3];
-	static uint8_t mag_index = 0;
-	int step;
-	int calculate_flag = 0;
-  static int16_t LiftPoint;
-	static int16_t DropPoint;
-	static int16_t FrontTouchFlag=0;
-	static int16_t LiftDistance;
-	static int16_t counter1=0;
-	static int16_t counter2=0;
-	
-	
-	for(int i = 0; i < 3; i ++)
-	{
-		mag_temp[i] = (int32_t) (mag[i]);
-	}
-	mag_sqrt = abs(mag_temp[2]);
-	/*读入磁力计数据*/
-	if(mag_index >= 3)
-	{
-		mag_buf[0] = mag_buf[1];
-		mag_buf[1] = mag_buf[2];
-		mag_index = 2;
-		calculate_flag = 1;
-	}
-	mag_buf[mag_index++] = mag_sqrt;
+		//利用前脚磁力计以及后脚来判断
+
+	static int32_t  mag_front[5] = {40000};
+	static int16_t  acc_front_z[5];
+
+	static int32_t  mag_cur_min = 40000;
 	
-  /*读入xyZ轴加速度数据*/
-	if(acc_index >= 3)
-	{
-		accZ_buf[0] = accZ_buf[1];
-		accZ_buf[1] = accZ_buf[2];
-		acc_index = 2;
-	}
-	accZ_buf[acc_index++] = -acc_z;
+	static int time_count;
+
+	static int last_floor_status = 1;
 
+	static int press_up_wait_acc_time = 0;
+
+	int on_floor = 0;
 	
-	/*开始计算*/
-	if (calculate_flag == 1)
+	memcpy(mag_front, mag_front + 1, 4 * sizeof(int32_t));
+	memcpy(acc_front_z, acc_front_z + 1, 4 * sizeof(int16_t)); 
+
+	mag_front[4] = abs(mag[2]);
+	acc_front_z[4] = acc_z;
+
+
+	if (time_count > 4)
 	{
-		
-//		DEBUG_LOG("LiftPoint==:%d...\n",LiftPoint);
-//		DEBUG_LOG("DropPoint==:%d...\n",DropPoint);
-//		DEBUG_LOG("LiftDistance==:%d...\n",LiftDistance);
-						 
-		if(mag_buf[2] - mag_buf[1]>=0)
-	  {
-			if(FrontTouchFlag==0)
+		int16_t  max_acc_z = acc_front_z[0], min_acc_z = acc_front_z[0];
+		int32_t  max_mag_front = mag_front[0];
+		int32_t  min_mag_front = mag_front[0];
+		int16_t  max_mag_index = 0, min_mag_index = 0;
+
+		for (int i = 1; i < 5; i++)
+		{
+			if (min_mag_front > mag_front[i])
 			{
-       DropPoint = mag_buf[2];
+				min_mag_front = mag_front[i];
+				min_mag_index = i;
 			}
-			else
+
+			if (max_mag_front < mag_front[i])
 			{
-				if(mag_buf[2]>DropPoint)
-				{
-					DropPoint = mag_buf[2];
-				}
+				max_mag_front = mag_front[i];
+				max_mag_index = i;
 			}
- 	  }
-    else
-    {
-			if(FrontTouchFlag==0)
+
+			if (max_acc_z < acc_front_z[i])
 			{
-       LiftPoint = mag_buf[2];
+				max_acc_z = acc_front_z[i];
 			}
-			else
+
+			if (min_acc_z > acc_front_z[i])
 			{
-				;
+				min_acc_z = acc_front_z[i];
 			}
-	  }
-		
-		/*判断前后脚掌的状态*/
-    if(mag_buf[2] - LiftPoint>800)
-	  {
-		  FrontTouchFlag = 1;
-			if(mag_buf[2] - LiftPoint>LiftDistance)
-			{
-			  LiftDistance = mag_buf[2] - LiftPoint;
-			}			
-	  }
-	  if((mag_buf[2] - LiftPoint<=800)||((DropPoint-mag_buf[2]>0.4*LiftDistance)&&(abs(mag_buf[2]-mag_buf[1])<400)))
-	  {
-		  FrontTouchFlag = 0;
-			LiftDistance = 800;
-	  }
 
-		/*计算步数*/
-	  if (up_flag == 0)
-	  {
-			 counter1=0;
-			 if(counter2<300)
-			 {
-				counter2++;
-			 }
-			 if((FrontTouchFlag==1)&&(counter2>=8))
-			 {
-				 step = 1;
-				 up_flag = 1;
-				 DEBUG_LOG("step==%d\n",step);
-			 }
-			 else
-			 {
-				 step = 0;
-				 up_flag = 0;
-			 }
+		}
+
+
+		//寻找上升沿
+		if (min_mag_index < max_mag_index && max_mag_front > min_mag_front + 100)
+		{
+			mag_cur_min = mag_cur_min > min_mag_front ? min_mag_front : mag_cur_min;
+		}
+		else if (min_mag_index > max_mag_index && max_mag_front > min_mag_front + 100)
+		{
+			mag_cur_min = 40000;
+		}
+
+		if (mag_front[4] > mag_cur_min + 1000)
+		{
+			on_floor = 1;
+		}
+
+
+		if (last_floor_status == 1 && abs(acc_front_z[4] - acc_front_z[3]) < 105)
+		{
+			on_floor = 1;
+		}
+
+
+		if (on_floor)
+		{
+			press_up_wait_acc_time = 3; //拖延3个触地状态
+		}
+		else if (press_up_wait_acc_time > 0)
+		{
+			on_floor = 1;
+		}
 		
-	  }
-		else
+		if (press_up_wait_acc_time > 0)
 		{
-			step = 0;
-			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;
-			}	
+			press_up_wait_acc_time--;
 		}
 
+		
 	}
 	else
 	{
-		step = 0;
-		LiftPoint = mag_buf[2];
-		DropPoint = mag_buf[2];
+		time_count++;
 	}
 	
+	uint8_t step = 0;
+	
+	if(last_floor_status == 0 && on_floor == 1)
+	{
+		step = 1;
+	}
 	
+	last_floor_status = on_floor;
 
 	return step;
 }