shoe_mcu/motion/detect_step_by_mag - 副本.c

#include "detect_step_by_mag.h"
#include "math.h"
#include "stdlib.h"
#include "exception.h"
#include "app_flash.h"

#include "nrf_delay.h"

uint16_t mag_sqrt =0;

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 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 int mag_buf_front[3];
	static int mag_buf_back[3];
	int32_t mag_sqrt_front =0;
	int32_t mag_sqrt_back =0;
	int16_t step=0;
	static int FrontLiftDistance;
  static int BackLiftDistance;
	
	//DEBUG_LOG("Enter RealTimeStep!\n");
	//Tcounter++;
	
	for(int i = 0; i < 3; i ++)
	{
		mag_temp_front[i] = (int32_t) (front[i]);
	}
	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 = abs(mag_temp_back[2]);
	

	/*读入磁力计数据*/
	
	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;
	}
	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;
	}
	mag_buf_back[mag_index_back++] = mag_sqrt_back;//后脚掌数据
	
  /*读入xyZ轴加速度数据*/
	if(acc_index >= 3)
	{
		accZ_buf[0] = accZ_buf[1];
		accZ_buf[1] = accZ_buf[2];		
		acc_index = 2;
		calculate_flag = 1;
	}
	accZ_buf[acc_index++] = -acc[2];
	
	
	
	if(calculate_flag==1)
	{
//		DEBUG_LOG("mag_front3==:%d...\n",mag_buf_front[2]);
//		DEBUG_LOG("mag_back3==:%d...\n",mag_buf_back[2]);
//		DEBUG_LOG("FrontLiftPoint==:%d...\n",FrontLiftPoint);
//		DEBUG_LOG("BackLiftPoint==:%d...\n",BackLiftPoint);
//		DEBUG_LOG("FrontDropPoint==:%d...\n",FrontDropPoint);
//		DEBUG_LOG("BackDropPoint==:%d...\n",BackDropPoint);
//		DEBUG_LOG("FrontLiftDistance==:%d...\n",FrontLiftDistance);
//		DEBUG_LOG("BackLiftDistance==:%d...\n",BackLiftDistance);
		
	  /*不断更新前后脚掌磁力计的上升起始点或下降起始点*/
	  if(mag_buf_front[2] - mag_buf_front[1]>=0)
	  {
			if(FKEY==0)
			{
        FrontDropPoint = mag_buf_front[2];
			}
			else
			{
				if(mag_buf_front[2]>FrontDropPoint)
				{
					FrontDropPoint = mag_buf_front[2];
				}
			}
	  }
    else
    {
			if(FKEY==0)//FKEY为前脚波形状态变量，1代表波形进行中，0代表波形结束。该语句意思是前脚掌磁力计不处于波形进行阶段时，如常更新上升起点。
			{
       FrontLiftPoint = mag_buf_front[2];
			}
			else
			{
				;//当波形进行中，不改变初始上升起点，有助于过滤杂波。
			}
	  }

    if(mag_buf_back[2] - mag_buf_back[1]>=0)
	  {
			if(BKEY==0)
			{
        BackDropPoint = mag_buf_back[2];
			}
			else
			{
				if(mag_buf_back[2]>BackDropPoint)
				{
					BackDropPoint = mag_buf_back[2];
				}
			}
	  }
    else
    {
			if(BKEY==0)//BKEY为后脚波形状态变量，1代表波形进行中，0代表波形结束。该语句意思是后脚掌磁力计不处于波形进行阶段时，如常更新上升起点
			{
       BackLiftPoint = mag_buf_back[2];
			}
			else
			{
				;
			}
	  }

	  /*判断前后脚掌的状态*/
    if(mag_buf_front[2] - FrontLiftPoint>2200)
	  {
			FKEY = 1;
			if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance)
			{
			  FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint;
			}		
	  }
	  if(mag_buf_back[2] - BackLiftPoint>2200)
	  {
			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.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.4*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<400)))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
		{
			BKEY=0;
			BackLiftDistance = 2200;
		}
    
		
		/*跳出死循环*/
		if(FrontLiftPoint==0) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
		{
			FKEY=0;
		}
		
	  if(BackLiftPoint==0)//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
		{
			BKEY=0;
		}
		
		
	/*判断鞋子触地或离地*/
	  
	  if(ShoeTouchFlag==0)
	  {
			if(Ucounter<300)
			{
			 Ucounter++;
			}
			Rcounter=0;		
			if(((FKEY==1)||(BKEY==1))&&(Ucounter>=8))
		  {
			  step=1;
			  ShoeTouchFlag=1;
				//DEBUG_LOG("You       have           one             step!\n");
		  }
		  else
		  {
			  step=0;
			  ShoeTouchFlag=0;
		  }
	  }
	  else
	  {
		  step = 0;
			if(Rcounter<300)
			{
			 Rcounter++;
			}
			Ucounter=0;
			if((Rcounter>=6)&&(FKEY==0)&&((abs(accZ_buf[0]-2000)>300)||(abs(accZ_buf[1]-2000)>300)||(abs(accZ_buf[2]-2000)>300))&&(BKEY==0))
		  {
			  ShoeTouchFlag=0;
		  }
		  else
		  {
			  ShoeTouchFlag=1;
		  }
	  }
	
	
  }
	else
  {
		step=0;
		FrontLiftPoint = mag_buf_front[2];
		FrontDropPoint = mag_buf_front[2];
		BackLiftPoint = mag_buf_back[2];
		BackDropPoint = mag_buf_back[2];
  }
	
	return step;
}


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;
	
  /*读入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;

	
	/*开始计算*/
	if (calculate_flag == 1)
	{
		
//		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)
			{
       DropPoint = mag_buf[2];
			}
			else
			{
				if(mag_buf[2]>DropPoint)
				{
					DropPoint = mag_buf[2];
				}
			}
 	  }
    else
    {
			if(FrontTouchFlag==0)
			{
       LiftPoint = mag_buf[2];
			}
			else
			{
				;
			}
	  }
		
		/*判断前后脚掌的状态*/
    if(mag_buf[2] - LiftPoint>1000)
	  {
		  FrontTouchFlag = 1;
			if(mag_buf[2] - LiftPoint>LiftDistance)
			{
			  LiftDistance = mag_buf[2] - LiftPoint;
			}			
	  }
	  if((mag_buf[2] - LiftPoint<=1000)||((DropPoint-mag_buf[2]>0.4*LiftDistance)&&(abs(mag_buf[2]-mag_buf[1])<400)))
	  {
		  FrontTouchFlag = 0;
			LiftDistance = 1000;
	  }

		/*计算步数*/
		uint8_t displaybuf[30]={0};
		if(mFlash.isHost)sprintf((char *)displaybuf,"left step:%d\r\n",up_flag);
		else sprintf((char *)displaybuf,"right step:%d\r\n",up_flag);
		Except_Display(EXCEPT_DATEStep,(const char *)displaybuf);
	  if (up_flag == 0)
	  {
			 counter1=0;
			 if(counter2<300)
			 {
				counter2++;
			 }
			 if((FrontTouchFlag==1)&&(counter2>=6))
			 {
				 step = 1;
				 up_flag = 1;
				 
				 DEBUG_LOG("step==%d\n",step);
			 }
			 else
			 {
				 step = 0;
				 up_flag = 0;
			 }
		
	  }
		else
		{
			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;
			}	
		}

	}
	else
	{
		step = 0;
		LiftPoint = mag_buf[2];
		DropPoint = mag_buf[2];
	}
	
	

	return step;
}