liweiyan/shoe_mcu2.1.1/motion/detect_zero_vel.c

#include "system.h"
//#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 "app_flash.h"

//#include "hal_imu.h"

#define MAG_THRESHHOLD 2000.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;
	
	DEBUG_LOG("detect_zero_vel( front,  back, acc, &front_zero, &back_zero, &acc_zero); \n");
	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))
		{
	
		}

		//假设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 --;
	}
}

float var_acc_f(float* acc, int length)
{
	if (length < 10)
	{
		return 0;
	}

	float mean_x = 0;

	float sum_x = 0;

	for (int i = length - 10; i < length; i++)
	{
		sum_x += acc[i];
	}

	mean_x = sum_x *0.1f;

	sum_x = 0.0f;

	for (int i = length - 10; i < length; i++)
	{
		sum_x += ((acc[i] - mean_x) * (acc[i] - mean_x));
	}


	return sum_x *0.1f;
}


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;
	}

}

int press_down_front(float *press_buff, int length)
{

	static float max_val = 0.0f;
	
	if(press_buff[0] - press_buff[length - 1] > 100.0f)
	{
		max_val =  press_buff[0]  > max_val ? press_buff[0]  : max_val;
	}
	else
	{
		max_val = 0;
	}

	
	if(max_val > press_buff[length - 1] + 2000.0f)
	{
		return 1;
	}
	
	return 0;
	
}

int press_down_back(float *press_buff, int length)
{
	static float max_val = 0.0f;
	
	if(press_buff[0] - press_buff[length - 1] > 100.0f)
	{
		max_val =  press_buff[0]  > max_val ? press_buff[0]  : max_val;
	}
	else
	{
		max_val = 0;
	}

	
	if(max_val > press_buff[length - 1] + 2000.0f)
	{
		return 1;
	}
	
	return 0;
	
}

//2020/01/02
//长趋势判断压力上升
int isLongTimeUpTrend(float *mag_window, int length, float up_threshhold, float *min_val)
{
	//上下沿判断
	int max_index = 0; int min_index = 0;
	float window_max_val = mag_window[0]; float window_min_val = mag_window[0]; 
	
	for(int i = 1; i < length; i++)
	{
		if(window_max_val < mag_window[i])
		{
			window_max_val = mag_window[i];
			max_index = i;
		}
		
		if(window_min_val > mag_window[i])
		{
			window_min_val = mag_window[i];
			min_index = i;
		}
	}
	
	*min_val = window_min_val;
	
	if((max_index > min_index && window_max_val - window_min_val > 1500.0f &&  window_max_val - mag_window[length - 1] < 10.f )
		|| (mag_window[length - 1] - window_min_val > 2000.0f && window_max_val - mag_window[length - 1] < 10.f))
	{
		return 1;
	}
	
	
	return 0;
	
}

int isLongTimeDownTrend(float *mag_window, int length, float up_threshhold, float *max_val)
{

		//上下沿判断
	int max_index = 0; int min_index = 0;
	float window_max_val = mag_window[0]; float window_min_val = mag_window[0]; 
	
	for(int i = 1; i < length; i++)
	{
		if(window_max_val < mag_window[i])
		{
			window_max_val = mag_window[i];
			max_index = i;
		}
		
		if(window_min_val > mag_window[i])
		{
			window_min_val = mag_window[i];
			min_index = i;
		}
	}
	
	*max_val = window_max_val;
	
	if((max_index < min_index && window_max_val - window_min_val >  2000.0f && mag_window[length - 1] - window_min_val < 1000.f)
		|| window_max_val - mag_window[length - 1] > 1000.f)
	{
		return 1;
	}
	
	
	return 0;
	
}


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 press_wait;



	static int acc_zero_count;
	
	
	static float front_min_val;
	static float back_min_val;
	
	static float front_max_val;
	static float back_max_val;
	
  
  float front_val = (float) (abs(front[2]));
  
  float back_val  = (float) (abs(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 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];
		}
		
	}
	
	int front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_min_val);
	int front_down_trend = isLongTimeDownTrend(front_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &front_max_val);
	
	int back_up_trend = isLongTimeUpTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_min_val);
	int back_down_trend = isLongTimeDownTrend(back_mag_window, WINDOW_SIZE, MAG_THRESHHOLD, &back_max_val);

	
	if (back_down_trend == 1 && front_down_trend == 1)
	{
		front_up_trend = 0;
		back_up_trend = 0;
	}
	

	
	if(front_up_trend)
	{
		front_zero_tmp = 1;
	}
	else if(front_down_trend)
	{
		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_up_trend)
	{
		back_zero_tmp  = 1;
	}
	
	else if(back_down_trend)
	{
		back_zero_tmp  = 0;
		
	}
	else if(back_zero_tmp == 1)
	{
		back_zero_tmp  = 2;
	}

	float var_acc_temp = var_acc_f(acc_z_window, 10);
	
	if (front_up_trend || back_up_trend)
	{
		if (var_acc_temp > 0.5f)
		{
			press_wait = 20;
		}
	}
	
	//当触发了压力上升及平稳的时候，需要用加速度倒计时延续状态
	if(back_zero_tmp > 0)
	{
		back_zupt_wait  = 20;
	}
	
	
	/*过滤一下类似于穿拖鞋，后鞋垫与磁力计传感器之间的距离在振荡*/
	if( back_zero_tmp != 0 && front_down_trend)
	{
		back_zero_tmp = 0;
	}

	
	if(front_zero_tmp == 2 && back_down_trend)
	{
		front_zero_tmp = 0;
	}

	//if (fabsf(acc_x_window[WINDOW_SIZE - 2] - acc_x_window[WINDOW_SIZE - 1]) > 0.15f ||
	//	fabsf(acc_y_window[WINDOW_SIZE - 2] - acc_y_window[WINDOW_SIZE - 1]) > 0.15f ||
	//	fabsf(acc_z_window[WINDOW_SIZE - 2] - acc_z_window[WINDOW_SIZE - 1]) > 0.15f)
	//{
	//	if (front_zero_tmp == 2)
	//	{
	//		front_zero_tmp = 0;
	//	}

	//	if (back_zero_tmp == 2)
	//	{
	//		back_zero_tmp = 0;
	//	}

	//}


	if (front_zero_tmp == 1)
	{
		*front_zero = 1;

	}
	else if(front_zero_tmp == 2  &&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;
	}
	else
	{
		*front_zero = 0;
	}


	if (back_zero_tmp == 1)
	{
		*back_zero = 1;
	}
	else if (back_zero_tmp == 2 && 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)
	{
		*back_zero = 1;
	}
	else 
	{
		*back_zero = 0;
	}
	
	
	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)
	{
		if (front_zupt_wait > 0)
		{
			*front_zero = 1;

			if (front_zero_tmp == 0)
			{
				front_zero_tmp = 2;
			}
		}
		if (back_zupt_wait > 0)
		{
			*back_zero = 1;

			if (back_zero_tmp == 0)
			{
				back_zero_tmp = 2;
			}
		}
	}
	

	
	
	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;
	}

	//if (acc_max_val_x - acc_min_val_x < 0.1f && acc_max_val_y - acc_min_val_y < 0.1f && acc_max_val_z - acc_min_val_z < 0.1f)
	//{
	//	*front_zero = 1;
	//	*back_zero = 1;

	//	front_zero_tmp = 2;
	//	back_zero_tmp = 2;

	//	printf("fabsf(acc_max_val_z - 1.0f) < 0.1f && fabsf(acc_min_val_z - 1.0f) < 0.1f \n");
	//}

	
//	/*
//	* 强制大于1秒的零速均视为触地
//	*/
	if(acc_zero_count > 100)
	{
		*front_zero = 1;
		*back_zero = 1;
		
		front_zero_tmp = 1;
		back_zero_tmp = 1;
	}
	
		//利用加速度延续
	float continue_thresh = 0.05f;

	if (press_wait > 0)
	{
		continue_thresh = 0.5f;
	}

	if((last_front_zupt == 1|| last_back_zupt == 1) 
		&& fabsf(acc_x_window[WINDOW_SIZE - 2]  - acc_x_window[WINDOW_SIZE - 1]) < continue_thresh
		&& fabsf(acc_y_window[WINDOW_SIZE - 2]  - acc_y_window[WINDOW_SIZE - 1]) < continue_thresh
		&& fabsf(acc_z_window[WINDOW_SIZE - 2]  - acc_z_window[WINDOW_SIZE - 1]) < continue_thresh)
	{
		if (last_front_zupt == 1)
		{
			*front_zero = 1;

			if (front_zero_tmp == 0)
			{
				front_zero_tmp = 2;
			}
		}

		if (last_back_zupt == 1)
		{
			*back_zero = 1;

			if (back_zero_tmp == 0)
			{
				back_zero_tmp = 2;
			}
		}

	}
	
	if(front_zupt_wait > 0)
	{
		front_zupt_wait --;
	}
	
	if(back_zupt_wait > 0)
	{
		back_zupt_wait --;
	}

	if (press_wait > 0)
	{
		press_wait--;
	}
	
	if (*front_zero)
	{
		front_zupt_wait = 20;
	}

	if(*back_zero)
	{
		back_zupt_wait = 20;
	}
	
	last_front_zupt = *front_zero;
	
	last_back_zupt  = *back_zero;
  
	/*
	* 直接用这个来记步好了
	*/

	//cal_step(*front_zero, *back_zero, acc_x_window[WINDOW_SIZE - 1], acc_y_window[WINDOW_SIZE - 1], acc_z_window[WINDOW_SIZE - 1]);
}