shoe_mcu/motion/detect_step_by_mag.c

#include "detect_step_by_mag.h"
#include "math.h"
#include "stdlib.h"
#include "system.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>1100)
	  {
			FKEY = 1;
			if(mag_buf_front[2] - FrontLiftPoint>FrontLiftDistance)
			{
			  FrontLiftDistance = mag_buf_front[2] - FrontLiftPoint;
			}		
	  }
	  if(mag_buf_back[2] - BackLiftPoint>1100)
	  {
			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<=750)||((FrontDropPoint-mag_buf_front[2]>0.4*FrontLiftDistance)&&(abs(mag_buf_front[2]-mag_buf_front[1])<100))) //当磁力计回降到小于上升起点+1000的数值时，FKEY归零，预示前脚波形结束。
		{
			FKEY=0;
			FrontLiftDistance = 1100;
		}
		
	  
		if((mag_buf_back[2] - BackLiftPoint<=750)||((BackDropPoint-mag_buf_back[2]>0.4*BackLiftDistance)&&(abs(mag_buf_back[2]-mag_buf_back[1])<100)))//当磁力计回降到小于上升起点+1000的数值时，BKEY归零，预示后脚波形结束。
		{
			BKEY=0;
			BackLiftDistance = 1100;
		}
    
		
		/*跳出死循环*/
		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 int32_t  mag_front[5] = {40000};
	static int16_t  acc_front_z[5];

	static int32_t  mag_cur_min = 40000;
	
	static int time_count;

	static int last_floor_status = 1;


	
	static int up_count = 0;
	

	
	static int max_acc_z = 0;
	static int min_acc_z = 0;

	uint8_t on_floor = 0;
	
	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("mag_front[3] : %d, mag_front[4] : %d , acc_front_z[4] : %d \n",  mag_front[3], mag_front[4], acc_front_z[4]);
		if (mag_front[4] >= mag_front[3])
		{
//			DEBUG_LOG("mag_front[4] >= mag_front[3] \n");
			mag_cur_min = mag_cur_min > mag_front[3] ? mag_front[3] : mag_cur_min;
//			DEBUG_LOG("mag_cur_min : %d, mag_front[4] : %d \n", mag_cur_min, mag_front[4]);
		}
		else
		{
//			DEBUG_LOG("else ------------------------> mag_front[4] >= mag_front[3] \n");
			mag_cur_min = 40000;
		}

		if (mag_front[4] > mag_cur_min + 500)
		{
			up_count ++;
		}
		else
		{
			up_count = 0;
		}
		
//		DEBUG_LOG("up_count : %d \n", up_count);
		
		if(up_count > 0)
		{
			on_floor = 1;
		}



		if (last_floor_status == 1 && abs(acc_front_z[4] - acc_front_z[3]) < 203)
		{
			on_floor = 1;
		}


//		if (on_floor)
//		{
//			press_up_wait_acc_time = 3; //拖延3个触地状态
//		}
//		else if (press_up_wait_acc_time > 0)
//		{
//			on_floor = 1;
//		}
//		
//		if (press_up_wait_acc_time > 0)
//		{
//			press_up_wait_acc_time--;
//		}
		
//		DEBUG_LOG("on_floor : %d \n", on_floor);
		
	}
	else
	{
		time_count++;
	}
	
	uint8_t step = 0;
	
	if(last_floor_status == 0 && on_floor == 1 && max_acc_z - min_acc_z >  512)
	{
		step = 1;
		DEBUG_LOG("step + 1, mag_cur_min : %d ,mag_front[4]: %d \n", mag_cur_min, mag_front[4])
	}
	
	if(on_floor == 0)
	{

		max_acc_z = max_acc_z > acc_z ? max_acc_z : acc_z;
		min_acc_z = min_acc_z < acc_z ? min_acc_z : acc_z;
	}
	else
	{
		max_acc_z = acc_z;
		min_acc_z = acc_z;
	}
	
	last_floor_status = on_floor;

	return step;
}