wentingwei/detect_step_by_mag.c

#include "detect_step_by_mag.h"
#include "math.h"
#include "stdlib.h"
#include "hal_imu.h"

#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 int FrontLiftPoint;
	//static int FrontDropPoint;
	//static int BackLiftPoint;
	//static int BackDropPoint;
	static int32_t SD;
	//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 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;//离地计时器
	
	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 = 	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];
	
	
	//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[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.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)
	  {
			Ucounter++;
			Rcounter=0;		
			if(((FKEY==1)||(BKEY==1))&&(Ucounter>=6))
		  {
			  step=1;
			  ShoeTouchFlag=1;
		  }
		  else
		  {
			  step=0;
			  ShoeTouchFlag=0;
		  }
	  }
	  else
	  {
		  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;
				Lock=0;
		  }
		  else
		  {
			  ShoeTouchFlag=1;
		  }

      if((Rcounter>3*100)&&(Lock==0))//防止卡死
			{
				FrontLiftPoint= FrontLiftPoint + 3000;
				BackLiftPoint = BackLiftPoint + 3000;
				Lock=1;
			}			
			
	  }
		
	  /*计算加速度的方差*/
		/*
	  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
  {
		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;
	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 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 ++)
	{
		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)
	{
		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)
	{
		
		//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)
	  {
       FrontDropPoint = mag_buf[1];
 	  }
    else
    {
       FrontLiftPoint = mag_buf[1];
	  }
		
		/*判断前后脚掌的状态*/
    if(mag_buf[1] - FrontLiftPoint>2000)
	  {
		  FrontTouchFlag = 2;
	  }
	  else if(mag_buf[1] - FrontDropPoint<-1000)
	  {
		  FrontTouchFlag = 0;
	  }
	  else if((abs(mag_buf[1]-mag_buf[0])<500)&&(abs(mag_buf[1]-mag_buf[2])<500))
	  {
		  FrontTouchFlag = 1;
	  }

		/*计算步数*/
	  if (up_flag == 0)
	  {
			 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]);
		     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))
			{
				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;
	}
	
	

	return step;
}