liweiyan/shoe_mcu_2022_1_10/motion/press_down_detect.c

#include "press_down_detect.h"

int back_jump_stage = 0;

int cancel_down = 0;
int virus_flag  = 0;

float real_front_min_left = 50000.f;
float real_front_min_right = 50000.f;




void detect_up_trend(float *mag_up_min, float *mag_window, int window_size)
{
	//1、寻找最大值
	float max_val = mag_window[0];
	
	for(int i = 1; i < window_size; i++)
	{
		if(max_val < mag_window[i])
		{
			max_val = mag_window[i];
		}
	}
	
	if(max_val - mag_window[0] > 100.0f)
	{
		if(mag_window[0]  < *mag_up_min)
		{
			*mag_up_min = mag_window[0];
		}
	}
	else
	{
		 *mag_up_min = 40000.f;
	}
}

void avoid_down_apeear_dual_foot_run(float *mag_window,float *another_mag_window, int window_size, float *trend_min, float *trend_max, float *down_max, float *down_min)
{
	float max_val = mag_window[0];
	
	for(int i = 1; i < window_size; i++)
	{
		if(max_val < mag_window[i])
		{
			max_val = mag_window[i];
		}
	}
	
	if(max_val - mag_window[0] > 200.0f)
	{
		if(*trend_min > mag_window[0])
		{
			*trend_min = mag_window[0];
		}
		
		if( *trend_max < max_val)
		{
			*trend_max = max_val;
		}
		
		*down_max = another_mag_window[window_size - 1];
		*down_min = another_mag_window[window_size - 1];
	}
	else if(mag_window[window_size - 1] - max_val < -200.0f)
	{
		if(mag_window[window_size - 1] < *trend_max)
		{
			*trend_max = mag_window[window_size - 1];
		}
		
		if(max_val > *trend_min)
		{
			*trend_min = max_val;
		}
		
		if(*down_max < another_mag_window[window_size - 1])
		{
			*down_max = another_mag_window[window_size - 1];
		}
		
		if(*down_min > another_mag_window[window_size - 1])
		{
			*down_min = another_mag_window[window_size - 1];
		}
		
	}
}

int station_acc(float* acc_buff, int buff_size)
{
	float max_val = acc_buff[0];
	float min_val = acc_buff[0];
	
	for(int i = 1; i < buff_size; i++)
	{
		if(max_val < acc_buff[i] )
		{
			 max_val = acc_buff[i];
		}
		
		if(min_val > acc_buff[i] )
		{
			 min_val = acc_buff[i];
		}
		
	}
	
	if(max_val - min_val < 0.3f)
	{
		return 1;
	}
	
	return 0;
}


int check_dual_front_press_down(float *left_mag_window, float *right_mag_window, int length)
{
	float left_max_val = left_mag_window[0];
	
	float right_max_val = right_mag_window[0];
	
	float left_min_val = left_mag_window[0];
	
	float right_min_val = right_mag_window[0];
	
	int left_max_index = 0; int right_max_index = 0;
	
	int left_min_index = 0; int right_min_index = 0;
	
	for(int i = 1; i < length; i++)
	{
		if(left_max_val < left_mag_window[i])
		{
			left_max_val = left_mag_window[i];
			
			left_max_index = i;
		}
		
		if(right_max_val < right_mag_window[i])
		{
			right_max_val = right_mag_window[i];
			
			right_max_index = i;
		}
		
		if(left_min_val > left_mag_window[i])
		{
			left_min_val = left_mag_window[i];
			
			left_min_index = i;
		}
		
		if(right_min_val > right_mag_window[i])
		{
			right_min_val = right_mag_window[i];
			
			right_min_index = i;
		}
		
	}
	
	
	if(right_min_index > right_max_index && left_min_index > left_max_index 
		&& left_max_val > left_min_val + 2000.0f && right_max_val > right_min_val + 2000.0f)
	{
		return 1;
	}
	
	return 0;
	
}

int back_mag_different_trend(float *left_back_buff, float * right_back_buff, int length)
{
	int left_max_index = 0; int right_max_index = 0;
	
	int left_min_index = 0; int right_min_index = 0;
	
	float left_max_val  = left_back_buff[0];  float right_max_val  = right_back_buff[0]; 
	
	float left_min_val  = left_back_buff[0];  float right_min_val  = right_back_buff[0]; 
	
	for(int i = 0; i < length; i++)
	{
		if(left_max_val < left_back_buff[i])
		{
			left_max_val = left_back_buff[i];
			left_max_index = i;
		}
		
		if(left_min_val > left_back_buff[i])
		{
			left_min_val = left_back_buff[i];
			left_min_index = i;
		}
		
		if(right_max_val < right_back_buff[i])
		{
			right_max_val = right_back_buff[i];
			right_max_index = i;
		}
		
		if(right_min_val > right_back_buff[i])
		{
			right_min_val = right_back_buff[i];
			right_min_index = i;
		}
	}
	
	if(left_max_index > left_min_index && left_max_val > left_min_val + 500.0f
		&& right_max_index < right_min_index && right_max_val > right_min_val + 1000.0f)
	{
		return 1;
	}
	
	if(left_max_index < left_min_index && left_max_val > left_min_val + 1000.0f
		&& right_max_index > right_min_index && right_max_val > right_min_val + 500.0f)
	{
		return 1;
	}
	
	return 0;
	
}
//快速排序
void quick_sork(float arr[], int start, int end)
{
	if(start < end)
	{
		return;
	}
	
	int i = start;
	int j = end;
	float baseval = arr[start];
	
	while(i < j)
	{
		while(i < j && arr[j] >= baseval)
		{
			j--;
		}
		if(i < j)
		{
			arr[i] = arr[j];
			i++;
		}
		
		while(i < j && arr[i] < baseval)
		{
			i++;
		}
		if(i < j)
		{
			arr[j] = arr[i];
			j--;
		}
	}
	arr[i] = baseval;
	quick_sork(arr, start, i-1);
	quick_sork(arr, i+1, end);
	
}

float mid_val(float *mag_buff)
{
	static float src[10];
	
	
	memcpy(src, mag_buff, 10 * sizeof(float));
	
	//快速排序
	quick_sork(mag_buff, 0 , 9);
	
	return 0.5f *(mag_buff[4] + mag_buff[5]);
}

int special_stay_by_back_mag_down(float *left_back_mag, float *right_back_mag, int left_zupt, int right_zupt)
{
	static	float last_left_back_mag_val = 0.0f;
	static  float last_right_back_mag_val = 0.0f;
	
	static  float max_left_back;
	static  float min_left_back;
	
	static  float max_right_back;
	static  float min_right_back;
	
	float cur_left_back_mag_val = mid_val(left_back_mag);
	float cur_right_back_mag_val = mid_val(right_back_mag);
	
	if(last_left_back_mag_val < cur_left_back_mag_val - 0.00001f)
	{
		max_left_back = cur_left_back_mag_val;
		min_left_back = cur_left_back_mag_val;
		
		max_right_back = cur_right_back_mag_val;
		min_right_back = cur_right_back_mag_val;
	}
	else
	{
		 min_left_back = cur_left_back_mag_val;
	}
	
	if(last_right_back_mag_val < cur_right_back_mag_val - 0.00001f)
	{
		max_left_back = cur_left_back_mag_val;
		min_left_back = cur_left_back_mag_val;
		
		max_right_back = cur_right_back_mag_val;
		min_right_back = cur_right_back_mag_val;
	}
	else
	{
		 min_right_back = cur_right_back_mag_val;
	}
	
	
	last_left_back_mag_val  = cur_left_back_mag_val;
	last_right_back_mag_val = cur_right_back_mag_val;
	
	if(max_left_back > min_left_back +2000.f && max_right_back > min_right_back +2000.f
			 && left_zupt && right_zupt)
	 {
			return 1;
	 }
	 else
	 {
			return 0;
	 }
}

int special_stay_by_long_time_down_process(float *front_mag_left_buff, float *front_mag_right_buff, float *back_mag_left_buff, float *back_mag_right_buff)
{
	int  left_index = 9;
	int  right_index = 9;
	
	/*
	* 寻找第一个递增的点
	*/
	for(left_index = 9; left_index > 0 ; left_index --)
	{
		if(front_mag_left_buff[left_index] < front_mag_left_buff[left_index - 1])
		{
			break;
		}
	}
	
	for(right_index = 9; right_index > 0 ; right_index --)
	{
		if(front_mag_right_buff[right_index] < front_mag_right_buff[right_index - 1])
		{
			break;
		}
	}
	
	float right_max_front_val = front_mag_right_buff[0];
	float right_min_front_val = front_mag_right_buff[0];
	
	for(int i = 0; i < 10; i++)
	{
		right_max_front_val = right_max_front_val > front_mag_right_buff[i] ? right_max_front_val : front_mag_right_buff[i];
		
		right_min_front_val = right_min_front_val < front_mag_right_buff[i] ? right_min_front_val : front_mag_right_buff[i];
	}
	
	/*
	* 有些特殊的点，左脚前掌往下压，但是右脚前掌没啥变化, 只靠后脚掌数据递降来判断
	*/
	if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 1000.0f
		&& right_max_front_val - right_min_front_val < 500.f 
		&& back_mag_right_buff[left_index] -  back_mag_right_buff[9] > 1000.f)
	{
		return 1;
	}
	
	/*
	* 左脚前掌往下压，右脚前掌往下压
	*/
	
	if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 1000.0f
		&& right_index < 6 && front_mag_right_buff[9] - front_mag_right_buff[right_index] > 1000.0f)
	{
		return 1;
	}
	
	return 0;
	
	
	
	
}

int special_stay_by_long_time_down(float front_mag_left, float front_mag_right, float back_mag_left, float back_mag_right)
{
	static int time_count = 0;
	
	static float front_mag_left_buff[10];
	
	static float front_mag_right_buff[10]; 
	
	static float back_mag_left_buff[10];
	
	static float back_mag_right_buff[10];
	
	if(time_count == 8)
	{
		time_count = 0;
	}

	/*
	* 更新数据，8个数据为一组
	*/
	int long_time_down = 0;
	
	if(time_count == 0)
	{
		memcpy(front_mag_left_buff, front_mag_left_buff + 1, 9 * sizeof(float));
		
		memcpy(front_mag_right_buff, front_mag_right_buff + 1, 9 * sizeof(float));
		
		memcpy(back_mag_left_buff, back_mag_left_buff + 1, 9 * sizeof(float));
		
		memcpy(back_mag_right_buff, back_mag_right_buff + 1, 9 * sizeof(float));
		
		front_mag_left_buff[9] = front_mag_left;
		
		front_mag_right_buff[9] = front_mag_right;
		
		back_mag_left_buff[9] = back_mag_left;
		
		back_mag_right_buff[9] = back_mag_right;
		
		
		long_time_down = special_stay_by_long_time_down_process(front_mag_left_buff, front_mag_right_buff, back_mag_left_buff, back_mag_right_buff);
		
		if(long_time_down == 0)
		{
			long_time_down = special_stay_by_long_time_down_process(front_mag_right_buff, front_mag_left_buff, back_mag_right_buff, back_mag_left_buff);
		}
		
	}
	
	time_count ++;
	
	return long_time_down;
}


int press_down_detect_new(int index, float front_mag_left, float back_mag_left,
									float front_mag_right, float back_mag_right,int left_zupt, int right_zupt,
									float left_acc_x,float left_acc_y,  float left_acc_z, float right_acc_x, float right_acc_y, float right_acc_z, 
									int *front_down, int *back_down)
{
	  static float front_mag_norm_left[PRESS_COUNT_MAX];
    static float front_mag_norm_right[PRESS_COUNT_MAX];
	
	  static float front_mag_left_big_buff[15];
    static float front_mag_right_big_buff[15];
	
		static float back_mag_left_big_buff[10];
    static float back_mag_right_big_buff[10];
	
		static float back_mag_norm_left[PRESS_COUNT_MAX];
    static float back_mag_norm_right[PRESS_COUNT_MAX];
	
		static float acc_norm_left[10];
    static float acc_norm_right[10];
		
		static float left_front_up_min = 40000.0f;
		static float left_back_up_min = 40000.0f;
	
		static float right_front_up_min = 40000.0f;
		static float right_back_up_min = 40000.0f;
	
	
		memcpy(front_mag_norm_left, front_mag_norm_left + 1, 4 * sizeof(float));
		memcpy(front_mag_norm_right, front_mag_norm_right + 1, 4 * sizeof(float));
		
		memcpy(front_mag_left_big_buff, front_mag_left_big_buff + 1, 14 * sizeof(float));
		memcpy(front_mag_right_big_buff, front_mag_right_big_buff + 1, 14 * sizeof(float));
	
	  memcpy(back_mag_norm_left, back_mag_norm_left + 1, 4 * sizeof(float));
		memcpy(back_mag_norm_right, back_mag_norm_right + 1, 4 * sizeof(float));
		
		memcpy(acc_norm_left, acc_norm_left + 1, 9 * sizeof(float));
		memcpy(acc_norm_right, acc_norm_right + 1, 9 * sizeof(float));
		
		memcpy(back_mag_left_big_buff, back_mag_left_big_buff + 1, 9 * sizeof(float));
		memcpy(back_mag_right_big_buff, back_mag_right_big_buff + 1, 9 * sizeof(float));
	
	  front_mag_norm_left[PRESS_COUNT_MAX - 1] = front_mag_left;
		front_mag_norm_right[PRESS_COUNT_MAX - 1] = front_mag_right;
		
		front_mag_left_big_buff[14] = front_mag_left;
		front_mag_right_big_buff[14] = front_mag_right;
	
		back_mag_norm_left[PRESS_COUNT_MAX - 1] = back_mag_left;
		back_mag_norm_right[PRESS_COUNT_MAX - 1] = back_mag_right;
		
		acc_norm_left[9] =  sqrt(left_acc_x * left_acc_x + left_acc_y * left_acc_y + left_acc_z * left_acc_z);
		acc_norm_right[9] =  sqrt(right_acc_x * right_acc_x + right_acc_y * right_acc_y + right_acc_z * right_acc_z);
		
		back_mag_left_big_buff[9] = back_mag_left;
		back_mag_right_big_buff[9] = back_mag_right;
		
	
		detect_up_trend(&left_front_up_min, front_mag_norm_left, PRESS_COUNT_MAX);
		detect_up_trend(&right_front_up_min, front_mag_norm_right, PRESS_COUNT_MAX);
		
		detect_up_trend(&left_back_up_min, back_mag_norm_left, PRESS_COUNT_MAX);
		detect_up_trend(&right_back_up_min, back_mag_norm_right, PRESS_COUNT_MAX);
		
		if(left_zupt == 0)
		{
			left_front_up_min  =  front_mag_left;
			left_back_up_min  =  back_mag_left;
		}
		
		if(right_zupt == 0)
		{
			right_front_up_min  =  front_mag_right;
			right_back_up_min  =  back_mag_right;
		}
		//当检测到前脚掌的压力下降同时较快，将不会触发后脚掌下蹲的判断
		if(check_dual_front_press_down(front_mag_left_big_buff, front_mag_right_big_buff, 15))
		{
			left_back_up_min  =  back_mag_left;
			right_back_up_min  =  back_mag_right;
		}
		
		if(front_mag_left - left_front_up_min > 1000.0f && front_mag_right - right_front_up_min > 1000.0f )
		{
			*front_down = 1;
		}
		
		if(back_mag_different_trend(back_mag_left_big_buff, back_mag_right_big_buff, 10))
		{
			*front_down = 0;
		}
	  
		if(back_mag_left - left_back_up_min > 2000.0f && back_mag_right - right_back_up_min > 2000.0f )
		{
			*back_down = 1;
		}

		
		if(!station_acc(acc_norm_left, 10) || !station_acc(acc_norm_right, 10))
		{
			*front_down = 0;
			*back_down = 0;
		}
		
		if(special_stay_by_long_time_down(front_mag_left, front_mag_right, back_mag_left, back_mag_right))
		{
			*front_down = 1;
		}
		

    
    return 0;
}

//short press_jump_detect(int16_t *h_pos, int16_t *s_pos)
//{
//	static int last_h_z;
//	static int last_s_z;
//	static int left_up;
//	static int right_up;
//	
//	static int left_up_count;
//	static int right_up_count;

//	
//	if(h_pos[2] - last_h_z > 0 && h_pos[2]> 0)
//	{
//		left_up = 1;
//	}
//	else if((h_pos[2] - last_h_z < 0) || h_pos[2] <= 0)
//	{
//		left_up = -1;
//	}
//	
//	if(left_up == 1)
//	{
//		left_up_count ++;
//	}
//	else
//	{
//		left_up_count = 0;
//	}
//	
//	if(s_pos[2] - last_s_z > 0 && s_pos[2] > 0)
//	{
//		right_up = 1;
//	}
//	else if((s_pos[2] - last_s_z < 0) || s_pos[2] <= 0)
//	{
//		right_up = -1;
//	}
//	
//	if(right_up == 1)
//	{
//		right_up_count ++;
//	}
//	else
//	{
//		right_up_count = 0;
//	}
//	
//	last_h_z = h_pos[2];
//	last_s_z = s_pos[2];
//	
//	if(left_up == 1 && right_up == 1 && right_up_count < 15 && left_up_count < 20
//		&& right_up_count > 2 && left_up_count > 2)
//	{
//		return 1;
//	}
//	
//	return 0;

//}

//由于仅靠IMU来探测触底了，高度估计得不太对，所以用加速度来算起跳动作

void max_min_window(float *data, int length, float *max_val , int *max_index, float * min_val, int *min_index)
{
	 *max_val = data[0];
		
	 *max_index = 0;
	
	 *min_val = data[0];
	
	 *min_index = 0;
		
	 for(int i = 0; i < length; i++)
	 {
		if(*max_val < data[i])
		{
			*max_val = data[i];
			*max_index = i;
		}
		
		if(*min_val > data[i])
		{
			*min_val = data[i];
			*min_index = i;
		}
	 }
	
	
}

//short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int left_front_press, int right_front_press)
//{
//	
//	static float right_data_z_buff[10];
//	static float left_data_z_buff[10];
//	
//	static int last_right_press;
//	static int last_left_press;

//	static int left_max_press;
//	static int right_max_press;
//	
//	static int wait;
//		
//	/*
//	* 存储数据
//	*/
//	memcpy(right_data_z_buff, right_data_z_buff + 1, 9 * sizeof(float));
//	
//	memcpy(left_data_z_buff, left_data_z_buff + 1, 9 * sizeof(float));
//	
//	right_data_z_buff[9] = right_acc_z;

//	left_data_z_buff[9] = left_acc_z;
//	
//	if(left_zupt && right_zupt)
//	{
//		wait = 30;
//	}
//	
//	/*
//	* 检测压力下降用的逻辑
//	*/
//	if(last_left_press - left_front_press > 200)
//	{
//	   if(last_left_press > left_max_press)
//		 {
//			 left_max_press = last_left_press;
//		 }
//	}
//	else if(last_left_press - left_front_press < -200)
//	{
//		left_max_press = 0;
//	}
//	
//	last_left_press = left_front_press;
//	
//	if(last_right_press - right_front_press > 200)
//	{
//	   if(last_right_press > right_max_press)
//		 {
//			 right_max_press = last_right_press;
//		 }
//	}
//	else if(last_right_press - right_front_press < -200)
//	{
//		right_max_press = 0;
//	}
//	
//	last_right_press = right_front_press;
//	
//	/*
//	* 加速变化估计
//	*/
//	
//	float max_val_right, min_val_right, max_val_left, min_val_left;
//	
//	int max_index_right, min_index_right, max_index_left, min_index_left;
//	
//	
//	max_min_window(right_data_z_buff, 10, &max_val_right , &max_index_right, &min_val_right, &min_index_right);
//	
//	max_min_window(left_data_z_buff, 10, &max_val_left , &max_index_left, &min_val_left, &min_index_left);
//	
//	if(wait > 0)
//	{
//		wait --;
//	}
//	
//	/*
//	* 1.0f的阈值轻轻跳就可以达到了,现在改为1.3f的阈值
//	*/
//	if( max_index_right < min_index_right && max_index_left < min_index_left &&
//		
//		max_val_right - min_val_right > 1.3f && max_val_left - min_val_left > 1.3f && wait > 0 && left_zupt == 0 && right_zupt == 0 
//	
//		&& left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000 )
//	//if(left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000)
//	{
//		SEGGER_RTT_printf(0, "test\n");
//		SEGGER_RTT_printf(0," left_max_press  - left_front_press= %d , right_max_press  - right_front_press = %d\n", left_max_press  - left_front_press, right_max_press  - right_front_press);
//		return 1;

//	}
//	
//	return 0;
//	
//}

int little_jump_detect_process(int press, int *press_max, int*press_min, int *last_press)
{
	
	if(*last_press - press > 200)
	{
		if(*press_max < press)
		{
			*press_max = press;
		}
		
		if(*press_min > press)
		{
			*press_min = press;
		}
	}
	else if(*last_press - press < -200)
	{
		if(*press_max > press)
		{
			*press_max = press;
		}
		
		if(*press_min < press)
		{
			*press_min = press;
		}
	}
	
	*last_press = press;
	
	if(*press_max - press > 3000)
	{
		return 1;
	}
	
	if(*press_max - press < -1000)
	{
		return -1;
	}
	
	return 0;
}




int little_jump_detect( int left_front_press, int right_front_press, int left_back_press, int right_back_press, int left_zupt, int right_zupt)
{
	static int left_front_max = 0;
	static int right_front_max = 0;
	static int left_back_max = 0;
	static int right_back_max = 0;
	
	static int left_front_min = 40000;
	static int right_front_min = 40000;
	static int left_back_min = 40000;
	static int right_back_min = 40000;
	
	static int last_left_front_press = 0;
	static int last_right_front_press = 0;
	
	static int last_left_back_press = 0;
	static int last_right_back_press = 0;

	int left_front_down = little_jump_detect_process(left_front_press, &left_front_max, &left_front_min, &last_left_front_press);
	
	int right_front_down = little_jump_detect_process(right_front_press, &right_front_max, &right_front_min, &last_right_front_press);
	
	int left_back_down = little_jump_detect_process(left_back_press, &left_back_max, &left_back_min, &last_left_back_press);
	
	int right_back_down = little_jump_detect_process(right_back_press, &right_back_max, &right_back_min, &last_right_back_press);
	
//	if( left_back_down == -1)
//	{
//		left_front_max = left_front_press;
//		
//		left_front_min = left_front_press;

//	}
//	
//	if( right_back_down == -1)
//	{
//		right_front_max = right_front_press;
//		
//		right_front_min = right_front_press;

//	}
	
	if(left_front_down != 1)
	{
		right_front_max = right_front_press;
		
		right_front_min = right_front_press;
	}
	
	if(right_front_down != 1)
	{
		left_front_max = left_front_press;
		
		left_front_min = left_front_press;
	}

	
	if(left_front_down==1 && right_front_down==1 && left_zupt==0 && right_zupt==0)
	{
		return 1;
	}
	
	if(left_back_down==1 && right_back_down==1 && left_zupt==0 && right_zupt==0)
	{
		return 1;
	}
	
	return 0;
	
}

int detect_L_line(int32_t *press_buff, int length)
{
	
	int max_val = press_buff[0];
	
	int max_index = 0;
	
	for(int i = 1; i < length; i++)
	{
		 if(press_buff[i] > max_val)
		 {
			 max_val = press_buff[i];
			 
			 max_index = i;
		 }
		 
	}
	/*
	* 拐点太接近数组末端，舍弃掉
	*/
//	if(max_index >= length - 5)
//	{
//		return 0;
//	}
	
	int last_val = press_buff[max_index];
	
	int turn_point_index = max_index;
	
  for(int i = max_index + 1; i < length; i++)
	{
		if(last_val >= press_buff[i])
		{
			last_val = press_buff[i];
			
			turn_point_index = i;
			
		}
		else
		{
			break;
		}
	}
	
	/*
	* 检测到临时的拐点， 需要判断拐点倒数第五个之前
	*/

	
	last_val = press_buff[length - 4];
	
	int stable_max_val = press_buff[length - 4];
	int stable_min_val = press_buff[length - 4];
	
	for(int i= length - 4; i< length; i++)
	{
		if(stable_max_val < press_buff[i])
		{
			stable_max_val = press_buff[i];
		}
		
		if(stable_min_val > press_buff[i])
		{
			stable_min_val = press_buff[i];
		}
	}
	
	if(stable_max_val - stable_min_val < 200 && press_buff[max_index] -  press_buff[turn_point_index] > 4000
		&& turn_point_index > length - 5)
	{
		return 1;
	}
	
	return 0;
}

int detect_I_line(int32_t *press_buff, int length)
{
	
	int max_val = press_buff[0];
	
	int max_index = 0;
	
	for(int i = 1; i < length; i++)
	{
		 if(press_buff[i] >= max_val)
		 {
			 max_val = press_buff[i];
			 
			 max_index = i;
		 }
		 
	}
	/*
	* 拐点太接近数组末端，舍弃掉
	*/
//	if(max_index >= length - 5)
//	{
//		return 0;
//	}
	
	int last_val = press_buff[max_index];
	
	int turn_point_index = max_index;
	
  for(int i = max_index + 1; i < length; i++)
	{
		if(last_val >= press_buff[i])
		{
			last_val = press_buff[i];
			
			turn_point_index = i;
			
		}
		else
		{
			break;
		}
	}
	
	if(press_buff[max_index] -  press_buff[turn_point_index] > 4000 &&  abs(press_buff[turn_point_index] - press_buff[length -1]) < 500)
	{
		return 1;
	}

	
	return 0;
}

int little_up_jump_by_four_mag(int32_t *right_back_data_press, int32_t *left_back_data_press,  int32_t *right_front_data_press, int32_t *left_front_data_press, int length)
{
	int left_front_mag_max = left_front_data_press[0];
	int right_front_mag_max = right_front_data_press[0];
	
	int left_front_mag_min = left_front_data_press[0];
	int right_front_mag_min = right_front_data_press[0];
	
	int left_back_mag_max = left_back_data_press[0];
	int right_back_mag_max = right_back_data_press[0];
	
	int left_back_mag_min = left_back_data_press[0];
	int right_back_mag_min = right_back_data_press[0];
	
	int left_front_mag_max_index = 0; int right_front_mag_max_index = 0;
	
	int left_front_mag_min_index = 0; int right_front_mag_min_index = 0;
	
	int left_back_mag_max_index = 0; int right_back_mag_max_index = 0;
	
	int left_back_mag_min_index = 0; int right_back_mag_min_index = 0;
	
	for(int i = 1; i < length; i++)
	{
		if(left_front_mag_max < left_front_data_press[i])
		{
			left_front_mag_max = left_front_data_press[i];
			
			left_front_mag_max_index = i;
		}
		
		if(right_front_mag_max < right_front_data_press[i])
		{
			right_front_mag_max = right_front_data_press[i];
			
			right_front_mag_max_index = i;
		}
		
		if(left_front_mag_min > left_front_data_press[i])
		{
			left_front_mag_min = left_front_data_press[i];
			
			left_front_mag_min_index = i;
		}
		
		if(right_front_mag_min > right_front_data_press[i])
		{
			right_front_mag_min = right_front_data_press[i];
			
			right_front_mag_min_index = i;
		}
		
		
		if(left_back_mag_max < left_back_data_press[i])
		{
			left_back_mag_max = left_back_data_press[i];
			
			left_back_mag_max_index = i;
		}
		
		if(right_back_mag_max < right_back_data_press[i])
		{
			right_back_mag_max = right_back_data_press[i];
			
			right_back_mag_max_index = i;
		}
		
		if(left_back_mag_min > left_back_data_press[i])
		{
			left_back_mag_min = left_back_data_press[i];
			
			left_back_mag_min_index = i;
		}
		
		if(right_back_mag_min > right_back_data_press[i])
		{
			right_back_mag_min = right_back_data_press[i];
			
			right_back_mag_min_index = i;
		}
		
	}
	
	if(right_back_mag_min_index > right_back_mag_max_index && right_front_mag_min_index > right_front_mag_max_index
		&& left_back_mag_min_index > left_back_mag_max_index && left_front_mag_min_index > left_front_mag_max_index
		&& right_front_mag_max > right_front_mag_min + 2000 && right_back_mag_max > right_back_mag_min + 1000
		&& left_front_mag_max > left_front_mag_min + 2000 && left_back_mag_max > left_back_mag_min + 1000)
	{
		return 1;
	}
	
	return 0;
	
}

int acc_z_down_trend(float *acc_buff, int length)
{
	int max_index = 0;
	int min_index = 0;
	
	float max_val = acc_buff[0];
	float min_val = acc_buff[0];
	
	
	for(int i = 0; i < length; i++)
	{
		if(max_val < acc_buff[i])
		{
			max_val = acc_buff[i];
			max_index = i;
		}
		
		if(min_val > acc_buff[i])
		{
			min_val = acc_buff[i];
			min_index = i;
		}
	}
	
	if(max_index < min_index && max_val > min_val + 4.0f && max_val > 4.0f && min_val < 0.f)
	{
		return 1;
	}
	
	return 0;
	
}

int normal_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int32_t left_front_press, int32_t right_front_press, int32_t left_back_press, int32_t right_back_press)
{

	static int32_t right_data_press[15];
	static int32_t left_data_press[15];
	
	static int32_t right_front_data_press[15];
	static int32_t left_front_data_press[15];
	
	static float  left_acc_z_buff[10];
	static float  right_acc_z_buff[10];
	
	
	static int left_wait;
	static int right_wait;
	static int wait;
	
	static int last_left_zupt;
	static int last_right_zupt;
		
	/*
	* 存储数据
	*/
	
	memcpy(right_data_press, right_data_press + 1, 14 * sizeof(int32_t));
	
	memcpy(left_data_press, left_data_press + 1, 14 * sizeof(int32_t));
	
	memcpy(right_front_data_press, right_front_data_press + 1, 14 * sizeof(int32_t));
	
	memcpy(left_front_data_press, left_front_data_press + 1, 14 * sizeof(int32_t));
	
	memcpy(left_acc_z_buff, left_acc_z_buff + 1, 9 * sizeof(float));
	
	memcpy(right_acc_z_buff, right_acc_z_buff + 1, 9 * sizeof(float));
	
	
	right_data_press[14] = right_back_press; left_data_press[14] = left_back_press; 
	
	right_front_data_press[14] = right_front_press; left_front_data_press[14] = left_front_press; 
	
	left_acc_z_buff[9] = left_acc_z; right_acc_z_buff[9] = right_acc_z; 
	
//	if(left_zupt == 0 && last_left_zupt == 1)
//	{
//		left_wait = 10;
//	}
//	
//	if(right_zupt == 0 && last_right_zupt == 1)
//	{
//		right_wait = 10;
//	}
//	
	
//	if(left_wait > 0)
//	{
//		left_wait --;
//	}
//	
//	if(right_wait > 0)
//	{
//		right_wait --;
//	}

	if(left_zupt == 1 && right_zupt == 1)
	{
		wait = 13;
	}
	
	if(wait > 0)
	{
		wait --;
	}
	
	last_left_zupt = left_zupt;
	last_right_zupt = right_zupt;
	
	
	if(detect_L_line(right_data_press, 15) && detect_L_line(left_data_press, 15) && wait > 0 && wait < 12)
	{
		return 1;
	}
	
	if(detect_I_line(right_front_data_press, 15) && detect_I_line(left_front_data_press, 15) && wait > 0 && wait < 12)
	{
		return 1;
	}
	
	if(little_up_jump_by_four_mag(right_data_press+7, left_data_press+7, right_front_data_press+7, left_front_data_press+7, 8) && wait > 0 && wait < 12)
	{
		return 1;
	}
	
//	if(acc_z_down_trend(left_acc_z_buff, 10) && acc_z_down_trend(right_acc_z_buff, 10) )
//	{
//		return 1;
//	}
	
	
	return 0;
	
	
}
short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, int32_t left_front_press, int32_t right_front_press, int32_t left_back_press, int32_t right_back_press)
{
	
//	return little_jump_detect( left_front_press,  right_front_press,  left_back_press,  right_back_press, left_zupt, right_zupt) ||
//				normal_jump_detect( left_acc_z,  right_acc_z,  left_zupt,  right_zupt,  left_front_press,  right_front_press,  left_back_press,  right_back_press);
	return normal_jump_detect( left_acc_z,  right_acc_z,  left_zupt,  right_zupt,  left_front_press,  right_front_press,  left_back_press,  right_back_press);
	
}