shoe_mcu/motion/detect_zero_vel.c

//#include "detect_zero_vel.h"

//#include "hal_mt.h"
//#include "hal_flash.h"

//#define MAG_THRESHHOLD 1000.f

//int front_zero_tmp = 0;
//int back_zero_tmp = 0;

//void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3])
//{
//	int16_t front_zero = 0;
//	int16_t back_zero  = 0;
//	int16_t acc_zero   = 0;
//	
//	detect_zero_vel( front,  back, acc, &front_zero, &back_zero, &acc_zero);
//}

//void cal_step(int16_t front_zero, int16_t back_zero)
//{
//	static int step_calm_down;
//	
//	if(front_zero || back_zero)
//	{
//		if(step_calm_down == 0)
//		{
//			mFlash.mStep.stepCur[0] ++;
//		}

//		step_calm_down = 10;

//	}
//	
//	if(step_calm_down > 0)
//	{
//		step_calm_down --;
//	}
//}

//  void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3], 
//    int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero)
//{
//  static  float front_mag_window[WINDOW_SIZE];
//	
//  static  float back_mag_window[WINDOW_SIZE];    
//	
//  static  float acc_x_window[WINDOW_SIZE];
//	static  float acc_y_window[WINDOW_SIZE];
//	static  float acc_z_window[WINDOW_SIZE];
//	
//	static int last_front_zupt;
//	
//	static int last_back_zupt;
//	
//	static int front_zupt_wait;
//	
//	static int back_zupt_wait;
//	
//  
//  float front_val = sqrt((float) (front[0] * front[0] + front[1] * front[1] + front[2] * front[2]));
//  
//  float back_val  = sqrt((float) (back[0] * back[0] + back[1] * back[1] + back[2] * back[2]));
//		
//	
//	//滑动窗口更新数据
//	
//	memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
//	
//	memcpy(back_mag_window,  back_mag_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
//	
//	memcpy(acc_x_window,  acc_x_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
//	
//	memcpy(acc_y_window,  acc_y_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
//	
//	memcpy(acc_z_window,  acc_z_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
//  
//  front_mag_window[WINDOW_SIZE - 1] = front_val;
//   
//  back_mag_window[WINDOW_SIZE - 1]  = back_val;
//  
//  acc_x_window[WINDOW_SIZE - 1]  = acc[0] / 2048.f;
//	acc_y_window[WINDOW_SIZE - 1]  = acc[1] / 2048.f;
//	acc_z_window[WINDOW_SIZE - 1]  = acc[2] / 2048.f;


//	float front_max_val = front_val;
//	float front_min_val = front_val;
//	
//	uint16_t front_max_index = WINDOW_SIZE - 1;
//	uint16_t front_min_index = WINDOW_SIZE - 1;
//	
//	float back_max_val = back_val;
//	float back_min_val = back_val;
//	
//	uint16_t back_max_index = WINDOW_SIZE - 1;
//	uint16_t back_min_index = WINDOW_SIZE - 1;
//	

//	for(int i = 0 ; i < WINDOW_SIZE; i++ )
//	{
//		if(front_mag_window[i] > front_max_val)
//		{
//			front_max_val = front_mag_window[i];
//			front_max_index = i;
//		}
//		
//		if(front_mag_window[i] < front_min_val)
//		{
//			front_min_val = front_mag_window[i];
//			front_min_index = i;
//		}
//		
//		if(back_mag_window[i] > back_max_val)
//		{
//			back_max_val = back_mag_window[i];
//			back_max_index = i;
//		}
//		
//		if(back_mag_window[i] < back_min_val)
//		{
//			back_min_val = back_mag_window[i];
//			back_min_index = i;
//		}
//		
//	}
//	
//	/*
//	* 计算稳定的状态
//	*/
//		
//	float acc_max_val_x = acc_x_window[WINDOW_SIZE - 1] ;
//	float acc_min_val_x = acc_x_window[WINDOW_SIZE - 1] ;
//	
//	float acc_max_val_y = acc_y_window[WINDOW_SIZE - 1] ;
//	float acc_min_val_y = acc_y_window[WINDOW_SIZE - 1] ;
//	
//	float acc_max_val_z = acc_z_window[WINDOW_SIZE - 1];
//	float acc_min_val_z = acc_z_window[WINDOW_SIZE - 1];
//	
//	for(int i = WINDOW_SIZE - 5 ; i < WINDOW_SIZE; i++ )
//	{

//		if(acc_x_window[i] < acc_min_val_x)
//		{
//			acc_min_val_x = acc_x_window[i];
//		}
//		
//		if(acc_x_window[i] > acc_max_val_x)
//		{
//			acc_max_val_x = acc_x_window[i];
//		}
//		
//		if(acc_y_window[i] < acc_min_val_y)
//		{
//			acc_min_val_y = acc_y_window[i];
//		}
//		
//		if(acc_y_window[i] > acc_max_val_y)
//		{
//			acc_max_val_y = acc_y_window[i];
//		}
//		
//		if(acc_z_window[i] < acc_min_val_z)
//		{
//			acc_min_val_z = acc_z_window[i];
//		}
//		
//		if(acc_z_window[i] > acc_max_val_z)
//		{
//			acc_max_val_z = acc_z_window[i];
//		}
//		
//	}
//	
//	if(front_max_index > front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
//	{
//		front_zero_tmp = 1;
//		
//		front_zupt_wait = 10;
//	}
//	else if(front_max_index < front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
//	{
//		front_zero_tmp = 0;
//	}
//	else if(front_zero_tmp == 1)
//	{
//		front_zero_tmp = 2;
//	}
//	
//	
//	/*
//	* 判断后脚往下压
//	*/
//	
//	if(back_max_index > back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
//	{
//		back_zero_tmp  = 1;
//		
//		back_zupt_wait = 10;
//	}
//	
//	else if(back_max_index < back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
//	{
//		back_zero_tmp  = 0;
//	}
//	else if(back_zero_tmp == 1)
//	{
//		back_zero_tmp  = 2;
//	}
//	
//	/*
//	* 判断仅前踮 或 后垫
//	*/
//	if(back_zero_tmp == 2 && front_zero_tmp == 0)
//	{
//		back_zero_tmp = 0;
//	}
//	
//	if(front_zero_tmp == 2 && back_zero_tmp == 0)
//	{
//		front_zero_tmp = 0;
//	}
//	
//	//延迟等待稳定状态
////	if(*front_zero == 0 && front_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f)
////	{
////		*front_zero = 1;
////	}
//	
//	if(front_zero_tmp == 1)
//	{
//		*front_zero = 1;
//		
//	}
//	else if(front_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.07f && acc_max_val_y - acc_min_val_y < 0.07f && acc_max_val_z - acc_min_val_z < 0.07f)
//	{
//		*front_zero = 1;
//		front_zupt_wait = 20;
//	}

//	else
//	{
//		*front_zero = 0;
//	}
//	

//	
////	if(*back_zero == 0 && back_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f)
////	{
////		*back_zero = 1;
////	}
//	
//	if(back_zero_tmp == 1)
//	{
//		*back_zero = 1;
//	}
//	else if(back_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.07f && acc_max_val_y - acc_min_val_y < 0.07f && acc_max_val_z - acc_min_val_z < 0.07f )
//	{
//			*back_zero = 1;
//		
//			back_zupt_wait = 20;
//	}
//	else
//	{
//			*back_zero = 0;
//	}	
//	
//	
//	for(int i = 0 ; i < WINDOW_SIZE - 5; i++ )
//	{

//		if(acc_x_window[i] < acc_min_val_x)
//		{
//			acc_min_val_x = acc_x_window[i];
//		}
//		
//		if(acc_x_window[i] > acc_max_val_x)
//		{
//			acc_max_val_x = acc_x_window[i];
//		}
//		
//		if(acc_y_window[i] < acc_min_val_y)
//		{
//			acc_min_val_y = acc_y_window[i];
//		}
//		
//		if(acc_y_window[i] > acc_max_val_y)
//		{
//			acc_max_val_y = acc_y_window[i];
//		}
//		
//		if(acc_z_window[i] < acc_min_val_z)
//		{
//			acc_min_val_z = acc_z_window[i];
//		}
//		
//		if(acc_z_window[i] > acc_max_val_z)
//		{
//			acc_max_val_z = acc_z_window[i];
//		}
//		
//	}
//	
//	
//	if(acc_max_val_x - acc_min_val_x < 0.05f && acc_max_val_y - acc_min_val_y < 0.05f && acc_max_val_z - acc_min_val_z < 0.05f)
//	{
//		*acc_zero = 1;
//	}
//	else
//	{
//		*acc_zero = 0;
//	}
//	
//	
//	if(*front_zero == 0)
//	{
//		*front_zero = *back_zero;
//	}
//	
//	if(front_zupt_wait > 0)
//	{
//		front_zupt_wait --;
//	}
//	
//	if(back_zupt_wait > 0)
//	{
//		back_zupt_wait --;
//	}
//	
//	
//	//利用加速度延续
//	if((last_front_zupt == 1|| last_back_zupt == 1) 
//		&& fabsf(acc_x_window[WINDOW_SIZE - 2]  - acc_x_window[WINDOW_SIZE - 1]) < 0.05f
//		&& fabsf(acc_y_window[WINDOW_SIZE - 2]  - acc_y_window[WINDOW_SIZE - 1]) < 0.05f
//		&& fabsf(acc_z_window[WINDOW_SIZE - 2]  - acc_z_window[WINDOW_SIZE - 1]) < 0.05f)
//	{
//		*front_zero = 1;
//		*back_zero  = 1;
//		
//		front_zero_tmp = 1;
//		back_zero_tmp = 1;
//		
//		front_zupt_wait = 15;
//		back_zupt_wait = 15;
//	}
//	
//	
//	last_front_zupt = front_zero_tmp;
//	
//	last_back_zupt  = back_zero_tmp;
//  
//	/*
//	* 直接用这个来记步好了
//	*/
//  cal_step(*front_zero, *back_zero );
//}

#include "detect_zero_vel.h"

#include "hal_mt.h"
#include "hal_flash.h"
#include "hal_imu.h"


#define MAG_THRESHHOLD 1500.f

int front_zero_tmp = 0;
int back_zero_tmp = 0;

void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3])
{
	int16_t front_zero = 0;
	int16_t back_zero  = 0;
	int16_t acc_zero   = 0;
	
	detect_zero_vel( front,  back, acc, &front_zero, &back_zero, &acc_zero);
}

void cal_step(int16_t front_zero, int16_t back_zero, float acc_x, float acc_y, float acc_z)
{
	static int step_calm_down;
	
	static float acc_min[3];
	static float acc_max[3];
	
	if(!(front_zero || back_zero))
	{
		acc_max[2] = 1.0f;
		acc_min[2] = 1.0f;
		
		if(acc_x > acc_max[0])
		{
			acc_max[0] = acc_x;
		}
		if(acc_y > acc_max[1])
		{
			acc_max[1] = acc_y;
		}
		if(acc_z > acc_max[2])
		{
			acc_max[2] = acc_z;
		}
		
		if(acc_x < acc_min[0])
		{
			acc_min[0] = acc_x;
		}
		if(acc_y < acc_min[1])
		{
			acc_min[1] = acc_y;
		}
		if(acc_z < acc_min[2])
		{
			acc_min[2] = acc_z;
		}
	}
	
	if(front_zero || back_zero)
	{
		if(step_calm_down == 0 && (acc_max[0] - acc_min[0] > 0.8f || acc_max[1] - acc_min[1] > 0.8f || acc_max[2] - acc_min[2] > 0.8f))
		{
			mFlash.mStep.stepCur[0] ++;
		}

		//假设20ms相见为极限
		
		step_calm_down = 5;
		
		acc_max[0] = acc_x;
		acc_max[1] = acc_y;
		acc_max[2] = acc_z;
		
		acc_min[0] = acc_x;
		acc_min[1] = acc_y;
		acc_min[2] = acc_z;

	}
	
	if(step_calm_down > 0)
	{
		step_calm_down --;
	}
}


void setLongTimeUpTrend(int max_index, int min_index,  float max_val,  float min_val,  float *longTime_maxVal, float *longTime_minVal)
{
	
	if(max_index > min_index && max_val > min_val + 800.f)
	{
		*longTime_maxVal   = *longTime_maxVal > max_val ? *longTime_maxVal : max_val;
		*longTime_minVal   = *longTime_minVal < min_val ? *longTime_minVal : min_val;
		
	}
	else if(max_index < min_index && max_val > min_val + 800.f)
	{
		*longTime_maxVal = 0.0f;
		*longTime_minVal = 50000.0f;
	}

}

void setLongTimeDownTrend(int max_index, int min_index,  float max_val,  float min_val,  float *longTime_maxVal, float *longTime_minVal)
{
	
	if(max_index > min_index && max_val > min_val + 800.f)
	{
		*longTime_maxVal = 0.0f;
		*longTime_minVal = 50000.0f;
		
	}
	else if(max_index < min_index && max_val > min_val + 800.f)
	{		
		*longTime_maxVal   = *longTime_maxVal > max_val ? *longTime_maxVal : max_val;
		*longTime_minVal   = *longTime_minVal < min_val ? *longTime_minVal : min_val;
	}

}


  void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3], 
    int16_t *front_zero, int16_t *back_zero, int16_t *acc_zero)
{
  static  float front_mag_window[WINDOW_SIZE];
	
  static  float back_mag_window[WINDOW_SIZE];    
	
  static  float acc_x_window[WINDOW_SIZE];
	static  float acc_y_window[WINDOW_SIZE];
	static  float acc_z_window[WINDOW_SIZE];
	
	static int last_front_zupt;
	
	static int last_back_zupt;
	
	static int front_zupt_wait;
	
	static int back_zupt_wait;
	
	static int front_calm_down_wait;
	
	static int acc_zero_count;
	
	static float back_mag_max_up = 0.0f;
	
	static float back_mag_min_up = 50000.f;//磁力计均方和大小小于32000
	
	static float back_mag_max_down = 0.0f;
	
	static float back_mag_min_down = 50000.f;//磁力计均方和大小小于32000
  
  float front_val = sqrt((float) (front[0] * front[0] + front[1] * front[1] + front[2] * front[2]));
  
  float back_val  = sqrt((float) (back[0] * back[0] + back[1] * back[1] + back[2] * back[2]));
		
		
	
	//滑动窗口更新数据
	
	memcpy(front_mag_window, front_mag_window + 1, (WINDOW_SIZE - 1) * sizeof(float));
	
	memcpy(back_mag_window,  back_mag_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
	
	memcpy(acc_x_window,  acc_x_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
	
	memcpy(acc_y_window,  acc_y_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
	
	memcpy(acc_z_window,  acc_z_window  + 1, (WINDOW_SIZE - 1) * sizeof(float));
  
  front_mag_window[WINDOW_SIZE - 1] = front_val;
   
  back_mag_window[WINDOW_SIZE - 1]  = back_val;
  
  acc_x_window[WINDOW_SIZE - 1]  = acc[0] / 2048.f;
	acc_y_window[WINDOW_SIZE - 1]  = acc[1] / 2048.f;
	acc_z_window[WINDOW_SIZE - 1]  = acc[2] / 2048.f;


	float front_max_val = front_val;
	float front_min_val = front_val;
	
	uint16_t front_max_index = WINDOW_SIZE - 1;
	uint16_t front_min_index = WINDOW_SIZE - 1;
	
	float back_max_val = back_val;
	float back_min_val = back_val;
	
	uint16_t back_max_index = WINDOW_SIZE - 1;
	uint16_t back_min_index = WINDOW_SIZE - 1;
	

	for(int i = 0 ; i < WINDOW_SIZE; i++ )
	{
		if(front_mag_window[i] > front_max_val)
		{
			front_max_val = front_mag_window[i];
			front_max_index = i;
		}
		
		if(front_mag_window[i] < front_min_val)
		{
			front_min_val = front_mag_window[i];
			front_min_index = i;
		}
		
		if(back_mag_window[i] > back_max_val)
		{
			back_max_val = back_mag_window[i];
			back_max_index = i;
		}
		
		if(back_mag_window[i] < back_min_val)
		{
			back_min_val = back_mag_window[i];
			back_min_index = i;
		}
		
	}
	
	/*
	* 计算稳定的状态
	*/
		
	float acc_max_val_x = acc_x_window[WINDOW_SIZE - 1] ;
	float acc_min_val_x = acc_x_window[WINDOW_SIZE - 1] ;
	
	float acc_max_val_y = acc_y_window[WINDOW_SIZE - 1] ;
	float acc_min_val_y = acc_y_window[WINDOW_SIZE - 1] ;
	
	float acc_max_val_z = acc_z_window[WINDOW_SIZE - 1];
	float acc_min_val_z = acc_z_window[WINDOW_SIZE - 1];
	
	for(int i = WINDOW_SIZE - 10 ; i < WINDOW_SIZE; i++ )
	{

		if(acc_x_window[i] < acc_min_val_x)
		{
			acc_min_val_x = acc_x_window[i];
		}
		
		if(acc_x_window[i] > acc_max_val_x)
		{
			acc_max_val_x = acc_x_window[i];
		}
		
		if(acc_y_window[i] < acc_min_val_y)
		{
			acc_min_val_y = acc_y_window[i];
		}
		
		if(acc_y_window[i] > acc_max_val_y)
		{
			acc_max_val_y = acc_y_window[i];
		}
		
		if(acc_z_window[i] < acc_min_val_z)
		{
			acc_min_val_z = acc_z_window[i];
		}
		
		if(acc_z_window[i] > acc_max_val_z)
		{
			acc_max_val_z = acc_z_window[i];
		}
		
	}
	
	if(front_max_index > front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
	{
		front_zero_tmp = 1;
	}
	else if(front_max_index < front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
	{
		front_zero_tmp = 0;
	}
	else if(front_zero_tmp == 1)
	{
		front_zero_tmp = 2;
	}
	
	//当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态
	if(front_zero_tmp > 0)
	{
		front_zupt_wait  = 20;
	}

	/*
	* 判断后脚往下压
	*/
	
	if(back_max_index > back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
	{
		back_zero_tmp  = 1;
	}
	
	else if(back_max_index < back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
	{
		back_zero_tmp  = 0;
		
	}
	else if(back_zero_tmp == 1)
	{
		back_zero_tmp  = 2;
	}
	
	
	//当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态
	if(back_zero_tmp > 0)
	{
		back_zupt_wait  = 20;
	}
	
	
	/*过滤一下类似于穿拖鞋，后鞋垫与磁力计传感器之间的距离在振荡*/
	if( back_zero_tmp == 2 && front_max_index < front_min_index && front_max_val > front_min_val + MAG_THRESHHOLD)
	{
		back_zero_tmp = 0;
	}
	
	if(front_zero_tmp == 2 &&  back_max_index < back_min_index && back_max_val > back_min_val + MAG_THRESHHOLD)
	{
		front_zero_tmp = 0;
	}
	
	/*
	* 规避先脚掌下地，后脚掌在下地；接着是后脚先离地，再前脚离地的情况
	*/
	setLongTimeUpTrend(back_max_index, back_min_index, back_val, back_min_val, &back_mag_max_up, &back_mag_min_up);
	
	setLongTimeDownTrend(back_max_index, back_min_index, back_val, back_min_val, &back_mag_max_down, &back_mag_min_down);
	
	if(back_zero_tmp == 2 && back_mag_max_down > back_mag_min_down + 3000.f)
	{
		back_zero_tmp  = 0;
		
		back_mag_max_down = 0.0f;
		
		back_mag_min_down = 50000.0f;
	}

	
	
	if((front_zupt_wait > 0 || back_zupt_wait > 0) && acc_max_val_x - acc_min_val_x < 0.05f && acc_max_val_y - acc_min_val_y < 0.05f && acc_max_val_z - acc_min_val_z < 0.05f)
	{
		*back_zero = 1;
		*front_zero = 1;
	}
	

/*
* 判断仅前踮 或 后垫， IMU往前移动，就不需要这个了
*/
//	if(back_zero_tmp == 2 && front_zero_tmp == 0)
//	{
//		back_zero_tmp = 0;
//	}
//	
//	if(front_zero_tmp == 2 && back_zero_tmp == 0)
//	{
//		front_zero_tmp = 0;
//	}

	
//延迟等待稳定状态
//	if(*front_zero == 0 && front_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f)
//	{
//		*front_zero = 1;
//	}
	
	if(front_zero_tmp > 0)
	{
		/*
		* 如IMU放在前脚掌，就不用front_calm_down_wait == 0的判断了
		*/
		*front_zero = 1;

	}
	else
	{
		*front_zero = 0;
	}
	

	
//	if(*back_zero == 0 && back_zupt_wait > 0 && acc_max_val - acc_min_val < 0.08f)
//	{
//		*back_zero = 1;
//	}
	
	if(back_zero_tmp > 0)
	{
		*back_zero = 1;
	}
	else
	{
		*back_zero = 0;
	}	
	
	
	for(int i = 0 ; i < WINDOW_SIZE - 5; i++ )
	{

		if(acc_x_window[i] < acc_min_val_x)
		{
			acc_min_val_x = acc_x_window[i];
		}
		
		if(acc_x_window[i] > acc_max_val_x)
		{
			acc_max_val_x = acc_x_window[i];
		}
		
		if(acc_y_window[i] < acc_min_val_y)
		{
			acc_min_val_y = acc_y_window[i];
		}
		
		if(acc_y_window[i] > acc_max_val_y)
		{
			acc_max_val_y = acc_y_window[i];
		}
		
		if(acc_z_window[i] < acc_min_val_z)
		{
			acc_min_val_z = acc_z_window[i];
		}
		
		if(acc_z_window[i] > acc_max_val_z)
		{
			acc_max_val_z = acc_z_window[i];
		}
		
	}
	
	
	if(acc_max_val_x - acc_min_val_x < 0.03f && acc_max_val_y - acc_min_val_y < 0.03f && acc_max_val_z - acc_min_val_z < 0.03f )
	{
		*acc_zero = 1;
		 acc_zero_count ++;
	}
	else
	{
		*acc_zero = 0;
		acc_zero_count = 0;
	}
	
//	/*
//	* 强制大于1秒的零速均视为触地
//	*/
	if(acc_zero_count > 100)
	{
		*front_zero = 1;
		*back_zero = 1;
		
		front_zero_tmp = 1;
		back_zero_tmp = 1;
	}
	
		//利用加速度延续
	if((last_front_zupt == 1|| last_back_zupt == 1) 
		&& fabsf(acc_x_window[WINDOW_SIZE - 2]  - acc_x_window[WINDOW_SIZE - 1]) < 0.05f
		&& fabsf(acc_y_window[WINDOW_SIZE - 2]  - acc_y_window[WINDOW_SIZE - 1]) < 0.05f
		&& fabsf(acc_z_window[WINDOW_SIZE - 2]  - acc_z_window[WINDOW_SIZE - 1]) < 0.05f)
	{
		*front_zero = 1;
		
		*back_zero  = 1;
		
	}
	
	if(front_zupt_wait > 0)
	{
		front_zupt_wait --;
	}
	
	if(back_zupt_wait > 0)
	{
		back_zupt_wait --;
	}
	
	if(*front_zero || *back_zero)
	{
		front_zupt_wait = 20;
		
		
		back_zupt_wait = 20;
	}
	
	last_front_zupt = *front_zero ;
	
	last_back_zupt  = *back_zero;
  
	/*
	* 直接用这个来记步好了
	*/
	
	if(!IMU_GetGameMode())
		cal_step(*front_zero, *back_zero, acc_x_window[WINDOW_SIZE - 1], acc_y_window[WINDOW_SIZE - 1], acc_z_window[WINDOW_SIZE - 1]);
}