提交硬件2.0版本的适配阈值

motion/detect_zero_vel.c

@@ -8,7 +8,7 @@
 
 //#include "hal_imu.h"
 
-#define MAG_THRESHHOLD 2000.f
+#define MAG_THRESHHOLD 500.f
 #define SAMPLE_C 4
 
 int16_t front_zero_tmp = 0;
@@ -114,7 +114,7 @@ int isLongTimeUpTrend(int16_t *mag_window, int16_t length, int16_t up_threshhold
 	*min_val = window_min_val;
 	
 	if((max_index > min_index && window_max_val - window_min_val > up_threshhold &&  window_max_val - mag_window[length - 1] < 10 )
-		|| (mag_window[length - 1] - window_min_val > up_threshhold + 500 && window_max_val - mag_window[length - 1] < 10))
+		|| (mag_window[length - 1] - window_min_val > up_threshhold && window_max_val - mag_window[length - 1] < 10))
 	{
 		return 1;
 	}
@@ -322,11 +322,11 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
 	}
 	else
 	{
-		front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, 3000, &front_min_val);
+		front_up_trend = isLongTimeUpTrend(front_mag_window, WINDOW_SIZE, 1000, &front_min_val);
 	}
 	
 	//缓慢踩地
-	if(front_mag_window[WINDOW_SIZE - 1] - front_mag_window[WINDOW_SIZE - 1 - SAMPLE_C] >-200)
+	if(front_mag_window[WINDOW_SIZE - 1] - front_mag_window[WINDOW_SIZE - 1 - SAMPLE_C] >-50)
 	{
 		if(continue_up_min_val > front_mag_window[WINDOW_SIZE - 1])
 		{
@@ -338,7 +338,7 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
 		continue_up_min_val = front_mag_window[WINDOW_SIZE - 1];
 	}
 	
-	if(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 2000)
+	if(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 1000)
 	{
 		front_up_trend = 1;
 	}
@@ -348,9 +348,9 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
 
 	// 自定义前后脚压力均上升，则重置压力等待时间，press_up_wait用于判断产生剧烈抖动时，则为触地
 	// special_press_up_wait 用于判断延续触地逻辑，使其阈值变大(解决触地剧烈抖动的情况)
-	if ((back_mag_window[WINDOW_SIZE - 1] - back_min_val > 2000 && back_max_val - back_mag_window[WINDOW_SIZE - 1] < 1000) ||
-		(front_mag_window[WINDOW_SIZE - 1] - front_min_val > 2000 && front_max_val - front_mag_window[WINDOW_SIZE - 1] < 1000) ||
-		(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 2000))
+	if ((back_mag_window[WINDOW_SIZE - 1] - back_min_val > 1000 && back_max_val - back_mag_window[WINDOW_SIZE - 1] < 500) ||
+		(front_mag_window[WINDOW_SIZE - 1] - front_min_val > 1000 && front_max_val - front_mag_window[WINDOW_SIZE - 1] < 500) ||
+		(front_mag_window[WINDOW_SIZE - 1] - continue_up_min_val > 1000))
 	{
 		press_up_wait = 20 * SAMPLE_C;
 		

motion/detect_zero_vel.h

@@ -13,4 +13,6 @@ void detect_zero_vel(int16_t front[3], int16_t back[3], int16_t acc[3],
 
 void start_cal_step(int16_t front[3], int16_t back[3], int16_t acc[3]);
 
+int32_t get_var_acc(void);
+
 #endif

motion/press_down_detect - 副本.c

@@ -0,0 +1,1451 @@
+
+#include "press_down_detect.h"
+
+#define down_thresh 2000.0f
+
+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 + 2000.0f
+		&& right_max_index < right_min_index && right_max_val > right_min_val + 2000.0f)
+	{
+		return 1;
+	}
+	
+	if(left_max_index < left_min_index && left_max_val > left_min_val + 2000.0f
+		&& right_max_index > right_min_index && right_max_val > right_min_val + 2000.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] > 2000.0f
+		&& right_max_front_val - right_min_front_val < 500.f && right_min_front_val > 15000.f
+		&& back_mag_right_buff[left_index] -  back_mag_right_buff[9] > 2000.f)
+	{
+		return 1;
+	}
+	
+	/*
+	* 左脚前掌往下压，右脚前掌往下压
+	*/
+	
+	if(left_index < 6 && front_mag_left_buff[9] - front_mag_left_buff[left_index] > 2000.0f
+		&& right_index < 6 && front_mag_right_buff[9] - front_mag_right_buff[right_index] > 2000.0f)
+	{
+		return 1;
+	}
+	
+	return 0;
+	
+	
+	
+	
+}
+
+int special_stay_by_long_time_down(float* front_mag_left, float* front_mag_right, int length)
+{
+	
+	float front_mag_left_max = front_mag_left[length - 1];
+	
+	float front_mag_right_max =  front_mag_right[length - 1];
+	
+	int up_count = 0;
+	
+	for(int i = length - 1; i > 1; i--)
+	{
+		
+		if(front_mag_left[i] - front_mag_left[i-1] > 10.f && front_mag_right[i] - front_mag_right[i-1] > 10.f)
+		{
+			up_count ++;
+
+			if (front_mag_left[i] > front_mag_left_max)
+			{
+				front_mag_left_max = front_mag_left[i];
+			}
+
+			if (front_mag_right[i] > front_mag_right_max)
+			{
+				front_mag_right_max = front_mag_right[i];
+			}
+		}
+
+		if(up_count > 7 && front_mag_left_max - front_mag_left[i] > 2000.f && front_mag_right_max - front_mag_right[i] >2000.f
+			&& front_mag_left_max - front_mag_left[length - 1] < 1000.f && front_mag_right_max - front_mag_right[length - 1] < 1000.f)
+		{
+			return 1;
+		}
+		
+	}
+
+	return 0;
+}
+
+int stage_class_up_or_down(float *mag_buff, int length)
+{
+	if(((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 50.f < (mag_buff[length - 6] + mag_buff[length - 5] + mag_buff[length - 4])*0.334f)
+		|| ((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 50.f < (mag_buff[length - 3] + mag_buff[length - 2] + mag_buff[length - 1])*0.334f))
+		
+	{
+		return 1;
+	}
+	
+	if(((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 100.f > (mag_buff[length - 6] + mag_buff[length - 5] + mag_buff[length - 4])*0.334f)
+		|| ((mag_buff[length - 10] + mag_buff[length - 9] + mag_buff[length - 8])*0.334f   + 100.f > (mag_buff[length - 3] + mag_buff[length - 2] + mag_buff[length - 1])*0.334f))
+		
+	{
+		return -1;
+	}
+	
+	
+	return 0;
+	
+}
+
+int back_down_by_long_time_big_trend(float* left_mag,  float* right_mag, int length)
+{
+	float left_down_sum = 0.0f;
+	float left_up_sum = 0.0f;
+	
+	float right_down_sum = 0.0f;
+	float right_up_sum = 0.0f;
+	
+	//先寻找到最小值下标
+	int left_mag_min_index = length - 1;
+	int right_mag_min_index  = length - 1;
+	
+	float left_mag_min_val = left_mag[length - 1];
+	float right_mag_min_val  = right_mag[length - 1];
+	
+	int left_mag_max_index = length - 1;
+	int right_mag_max_index  = length - 1;
+	
+	float left_mag_max_val = left_mag[length - 1];
+	float right_mag_max_val  = right_mag[length - 1];
+	
+	
+	for(int i = length - 1; i > 0; i--)
+	{
+			if(left_mag_min_val > left_mag[i])
+			{
+				left_mag_min_val = left_mag[i];
+				left_mag_min_index = i;
+			}
+			
+			if(right_mag_min_val > right_mag[i])
+			{
+				right_mag_min_val = right_mag[i];
+				right_mag_min_index = i;
+			}
+			
+			if(left_mag_max_val < left_mag[i])
+			{
+				left_mag_max_val = left_mag[i];
+				left_mag_max_index = i;
+			}
+			
+			if(right_mag_max_val < right_mag[i])
+			{
+				right_mag_max_val = right_mag[i];
+				right_mag_max_index = i;
+			}
+			
+	}
+	
+	int index = left_mag_min_index > right_mag_min_index ? left_mag_min_index : right_mag_min_index;
+	
+	//寻找差异值
+	float left_diff;
+	
+	float right_diff;
+	
+	for(int i = index; i < length - 1; i++)
+	{
+		 left_diff = left_mag[i] - left_mag[i + 1];
+		
+		 if(left_diff > 0.0f)
+		 {
+			 left_down_sum += left_diff; 
+		 }
+		 else
+		 {
+			 left_up_sum -= left_diff; 
+		 }
+		 
+		 right_diff = right_mag[i] - right_mag[i + 1];
+		
+		 if(right_diff > 0.0f)
+		 {
+			 right_down_sum += right_diff; 
+		 }
+		 else
+		 {
+			 right_up_sum  -= right_diff; 
+		 }
+
+	}
+	
+	if(left_mag_min_val + 2000.f < left_mag[length - 1] && right_mag_min_val + 2000.f < right_mag[length - 1] 
+		&& right_down_sum < 0.25f *right_up_sum && left_down_sum < 0.25f * left_up_sum)
+	//if(left_mag_min_val + 2000.f < left_mag[length - 1] && right_mag_min_val + 2000.f < right_mag[length - 1] )
+	
+	{
+		return 1;
+	}
+	
+	return 0;
+	
+}
+int back_down_by_long_time(float* front_mag_left, float* front_mag_right, float* back_mag_left, float* back_mag_right, int length)
+{
+	static float max_front_left_val;
+	static float max_front_right_val;
+	
+	static float min_back_left_val;
+	static float min_back_right_val;
+	
+	static int up_count;
+	
+	up_count ++;
+	
+	static int continue_up_count = 0;
+	
+	if(up_count == 0)
+	{
+		max_front_left_val = front_mag_left[length - 1];
+		max_front_right_val = front_mag_right[length - 1];
+		
+		min_back_left_val = back_mag_left[length - 1];
+		min_back_right_val = back_mag_right[length - 1];
+	}
+	
+//	if(stage_class_up_or_down(front_mag_left, 15) == -1 && stage_class_up_or_down(front_mag_right, 15) == -1)
+////	if( stage_class_up_or_down(back_mag_left, 15) == 1 && stage_class_up_or_down(back_mag_right, 15) == 1 )
+//	{
+//		float tmp = (front_mag_left[0] + front_mag_left[1] + front_mag_left[2]) *0.334f;
+//			
+//		max_front_left_val = tmp > max_front_left_val ? tmp : max_front_left_val;
+//		
+//		tmp = (front_mag_right[0] + front_mag_right[1] + front_mag_right[2]) *0.334f;
+//			
+//		max_front_right_val = tmp > max_front_right_val ? tmp : max_front_left_val;
+//				
+//	}
+//	else
+//	{
+//		max_front_left_val = front_mag_left[length - 1];
+//		max_front_right_val = front_mag_right[length - 1];
+//		
+//	}
+	
+	if(stage_class_up_or_down(back_mag_left, length) == 1 && stage_class_up_or_down(back_mag_right, length) == 1)
+	{
+		float tmp = (back_mag_left[0] + back_mag_left[1] + back_mag_left[2]) *0.334f;
+			
+		min_back_left_val = tmp > min_back_left_val ? min_back_left_val : tmp;
+		
+		tmp = (back_mag_right[0] + back_mag_right[1] + back_mag_right[2]) *0.334f;
+			
+		min_back_right_val = tmp > min_back_right_val ? min_back_right_val : tmp;
+		
+		continue_up_count ++;
+	}
+	else
+	{
+		min_back_left_val = back_mag_left[length - 1];
+		min_back_right_val = back_mag_right[length - 1];
+		
+		continue_up_count = 0;
+	}
+	
+	if(back_mag_left[length - 1] - min_back_left_val > 2000.f && back_mag_right[length - 1] - min_back_right_val > 2000.f && continue_up_count > 10)
+	{
+		return 1;
+	}
+	
+	if(back_mag_left[length - 1] - min_back_left_val > 4000.f && back_mag_right[length - 1] - min_back_right_val > 4000.f)
+	{
+		return 1;
+	}
+	
+	
+	return 0;
+}
+
+#define BIG_WINDOW_SIZE 20
+
+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 back_mag_norm_left[PRESS_COUNT_MAX];
+    static float back_mag_norm_right[PRESS_COUNT_MAX];
+	
+	  static float front_mag_left_big_buff[BIG_WINDOW_SIZE];
+    static float front_mag_right_big_buff[BIG_WINDOW_SIZE];
+	
+		static float back_mag_left_big_buff[BIG_WINDOW_SIZE];
+    static float back_mag_right_big_buff[BIG_WINDOW_SIZE];
+	
+	
+		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, (BIG_WINDOW_SIZE - 1) * sizeof(float));
+		memcpy(front_mag_right_big_buff, front_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * 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, (BIG_WINDOW_SIZE - 1) * sizeof(float));
+		memcpy(back_mag_right_big_buff, back_mag_right_big_buff + 1, (BIG_WINDOW_SIZE - 1) * 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[(BIG_WINDOW_SIZE - 1)] = front_mag_left;
+		front_mag_right_big_buff[(BIG_WINDOW_SIZE - 1)] = 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;
+		
+		
+		back_mag_left_big_buff[(BIG_WINDOW_SIZE - 1)] = back_mag_left;
+		back_mag_right_big_buff[(BIG_WINDOW_SIZE - 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);
+		
+	
+		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, BIG_WINDOW_SIZE))
+		{
+			left_back_up_min  =  back_mag_left;
+			right_back_up_min  =  back_mag_right;
+		}
+		
+		if(front_mag_left - left_front_up_min > 3000.f && front_mag_right - right_front_up_min > 3000.f)
+		{
+			*front_down = 1;
+		}
+
+		if(back_mag_different_trend(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
+		{
+			*front_down = 0;
+		}
+
+		//新增一个长时间判断蹲的，2020/01/08
+		if (special_stay_by_long_time_down(front_mag_left_big_buff, front_mag_right_big_buff, BIG_WINDOW_SIZE))
+		{
+			*front_down = 1;
+		}
+
+	  
+		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(back_down_by_long_time(front_mag_left_big_buff, front_mag_right_big_buff, back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
+//		{
+//		    *back_down = 1;
+//		}
+		
+		//2022/01/17
+		/* if(back_down_by_long_time_big_trend(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
+		  
+		 {
+				*back_down = 1;
+		 }*/
+
+		if (special_stay_by_long_time_down(back_mag_left_big_buff, back_mag_right_big_buff, BIG_WINDOW_SIZE))
+		{
+			*back_down = 1;
+		}
+
+    
+    return 0;
+}
+
+short pos_jump_detect(int *h_pos, int *s_pos, int left_zupt, int right_zupt)
+{
+	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;
+
+	
+	static int left_zupt_count;
+	static int right_zupt_count;
+	
+	if(left_zupt)
+	{
+		left_zupt_count = 20;
+	}
+	else
+	{
+		left_zupt_count --;
+	}
+	
+	if(right_zupt)
+	{
+		right_zupt_count = 20;
+	}
+	else
+	{
+		right_zupt_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 < 15
+		&& right_zupt_count > 0 && left_zupt_count > 0 &&
+	((right_up_count > 2 && left_up_count > 2) ||
+	 (right_up_count > 0 && left_up_count > 0 && h_pos[2] >1 && s_pos[2] > 1)))
+	{
+		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 = 20;
+	}
+
+	/*
+	* 检测压力下降用的逻辑
+	*/
+	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 < 50)
+	{
+		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 < 50)
+	{
+		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.0f && min_val_right < 0.f && max_val_left-min_val_left > 1.0f && min_val_left < 0.f
+
+		&& (wait > 0 && left_zupt == 0 && right_zupt == 0))
+		//if(left_max_press  - left_front_press > 3000 && right_max_press  - right_front_press > 3000)
+	{
+		return 1;
+
+	}
+
+	return 0;
+
+}
+
+int att_jump_detect(float left_pitch, float right_pitch, int left_zupt, int right_zupt)
+{
+	
+	 static int num;
+	
+	 static float left_pitch_window[8];
+	 static float right_pitch_window[8];
+	
+	 //用俯仰角判断好了, 现在的IMU方向 下标1为俯仰角
+	 memcpy(left_pitch_window,  left_pitch_window + 1,  7 * sizeof(float));
+	 memcpy(right_pitch_window, right_pitch_window + 1, 7 * sizeof(float));
+	
+	 left_pitch_window[7]  = left_pitch;
+	 right_pitch_window[7] = right_pitch;
+	
+	 num ++;
+	 
+	 if(num < 8)
+	 {
+		 return 0;
+	 }
+	 else
+	 {
+		 num = 8;
+	 }
+	 //要求起跳要同步
+	 
+	 static int zupt_count  = 0;
+	 if(left_zupt && right_zupt)
+	 {
+		 zupt_count = 21;
+	 }
+	 
+	 if(zupt_count > 0)
+	 {
+		 zupt_count --;
+	 }
+	 
+	 for(int i = 7; i > 1; i--)
+	 {
+		 if((left_pitch_window[i] > left_pitch_window[i-1] || left_pitch_window[i] > left_pitch_window[i-2]) 
+			 && (right_pitch_window[i] > right_pitch_window[i-1] || right_pitch_window[i] > right_pitch_window[i-2]))
+		 {
+			 if((left_pitch > left_pitch_window[i - 1] + 0.1f || left_pitch > left_pitch_window[i - 2] + 0.1f )
+				 && (right_pitch > right_pitch_window[i - 1] + 0.1f || right_pitch > right_pitch_window[i - 2] + 0.1f )
+				 &&  zupt_count > 0 )
+			 {
+				 return 1;
+			 }
+			 
+		 }
+		 else
+		 {
+			 return 0;
+		 }
+	 }
+	 
+	 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(float *press_buff, int length)
+//{
+//	
+//	float max_diff_val = press_buff[0] - press_buff[1];
+//	
+//	float temp_diff_val = 0.0f;
+//	
+//	int max_index = 0;
+//	
+//	for(int i = 1; i < length; i++)
+//	{
+//		 if(press_buff[i-1] - press_buff[i] > -50.f)
+//		 {
+//			 temp_diff_val += (press_buff[i-1] - press_buff[i]);
+//		 }
+//		 else
+//		 {
+//			 
+//			 temp_diff_val = 0.0f;
+//		 }
+//		 
+
+//		 if(max_diff_val < temp_diff_val)
+//		 {
+//			 max_diff_val = temp_diff_val;
+//			 
+//			 if(max_diff_val > 4000.f)
+//			 {
+//				 max_index = i;
+//			 }
+//		}
+//		 
+//	}
+//	
+//	float stable_max_val = press_buff[length - 4];
+//	float 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 < 1000.f &&  max_diff_val > 3000.f
+//		&& max_index > length - 5)
+//	{
+//		return 1;
+//	}
+//	
+//	return 0;
+//}
+
+
+
+//int acc_z_down_trend(float *left_acc_buff, float *right_acc_buff, int length)
+//{
+//	int left_max_index = 0;
+//	int left_min_index = 0;
+//	
+//	int right_max_index = 0;
+//	int right_min_index = 0;
+//	
+//	
+//	float left_max_val = left_acc_buff[0];
+//	float left_min_val = left_acc_buff[0];
+//	
+//	float right_max_val = right_acc_buff[0];
+//	float right_min_val = right_acc_buff[0];
+//	
+//	
+//	for(int i = 0; i < length; i++)
+//	{
+//		if(left_max_val < left_acc_buff[i])
+//		{
+//			left_max_val = left_acc_buff[i];
+//			left_max_index = i;
+//		}
+//		
+//		if(left_min_val > left_acc_buff[i])
+//		{
+//			left_min_val = left_acc_buff[i];
+//			left_min_index = i;
+//		}
+//		
+//		if(left_max_val < left_acc_buff[i])
+//		{
+//			left_max_val = left_acc_buff[i];
+//			left_max_index = i;
+//		}
+//		
+//		if(right_min_val > right_acc_buff[i])
+//		{
+//			right_min_val = right_acc_buff[i];
+//			right_min_index = i;
+//		}
+//		
+//		if(right_max_val < right_acc_buff[i])
+//		{
+//			right_max_val = right_acc_buff[i];
+//			right_max_index = i;
+//		}
+//		
+//	}
+//	
+//	if(right_max_index < right_min_index && right_max_val > right_min_val + 1.5f 
+//		&& left_max_index < left_min_index && left_max_val > left_min_val + 1.5f 
+//			&& abs(right_max_index - left_max_index < 4) && abs(left_max_index - left_min_index < 4))
+//	{
+//		return 1;
+//	}
+//	
+//	return 0;
+//	
+//}
+
+//int acc_little_shake(float *left_acc, float *right_acc, int length)
+//{
+//	float left_max_val = left_acc[0];
+//	float right_max_val = right_acc[0];
+//	
+//	float left_min_val = left_acc[0];
+//	float right_min_val = right_acc[0];
+//	
+//	for(int i = 0; i < length; i++)
+//	{
+//		if(left_max_val  < left_acc[i])
+//		{
+//			left_max_val = left_acc[i];
+//		}
+//		
+//		if(left_min_val > left_acc[i])
+//		{
+//			left_min_val = left_acc[i];
+//		}
+//		
+//		if(right_max_val  < left_acc[i])
+//		{
+//			right_max_val = left_acc[i];
+//		}
+//		
+//		if(right_min_val > left_acc[i])
+//		{
+//			right_min_val = left_acc[i];
+//		}
+
+//		
+//	}
+//	
+//	if(left_max_val - left_min_val > 0.4f && right_max_val - right_min_val > 0.4f)
+//	{
+//		return 1;
+//	}
+//	
+//	return 0;
+//	
+//	
+//}
+
+//int normal_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt, 
+//	float left_front_press, float right_front_press, float left_back_press, float right_back_press)
+//{
+
+////	static float right_data_press[15];
+////	static float left_data_press[15];
+//	
+//	static float right_front_data_press[20];
+//	static float left_front_data_press[20];
+//	
+//	static float  left_acc_z_buff[15];
+//	static float  right_acc_z_buff[15];
+//	
+//	
+//	static int left_wait;
+//	static int right_wait;
+//	static int wait;
+//	
+//	static int last_left_zupt;
+//	static int last_right_zupt;
+//	
+//	static int press_jump_count = 0;
+//		
+//	/*
+//	* 存储数据
+//	*/
+//	
+////	memcpy(right_data_press, right_data_press + 1, 14 * sizeof(float));
+////	
+////	memcpy(left_data_press, left_data_press + 1, 14 * sizeof(float));
+//	
+//	memcpy(right_front_data_press, right_front_data_press + 1, 19 * sizeof(float));
+//	
+//	memcpy(left_front_data_press, left_front_data_press + 1, 19 * sizeof(float));
+//	
+//	memcpy(left_acc_z_buff, left_acc_z_buff + 1, 14 * sizeof(float));
+//	
+//	memcpy(right_acc_z_buff, right_acc_z_buff + 1, 14 * sizeof(float));
+//	
+//	
+////	right_data_press[14] = right_back_press; left_data_press[14] = left_back_press; 
+//	
+//	right_front_data_press[19] = right_front_press; left_front_data_press[19] = left_front_press; 
+//	
+//	left_acc_z_buff[14] = left_acc_z; right_acc_z_buff[14] = 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 = 15;
+//	}
+//	
+//	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 && left_zupt == 0 && right_zupt == 0)
+////	{
+////		return 1;
+////	}
+//	
+//	if(detect_L_line(right_front_data_press, 20) && detect_L_line(left_front_data_press, 20) && wait > 0 && wait < 14 && left_zupt == 0 && right_zupt == 0)
+//	{
+//		 press_jump_count ++;
+//		 if(press_jump_count > 2)
+//		 {
+//			return 1;
+//		 }
+//	}
+//	else
+//	{
+//		press_jump_count =0;
+//	}
+////	
+////	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, right_acc_z_buff, 15)&& wait > 0 && wait < 14 && left_zupt == 0 && right_zupt == 0 )
+//	{
+//		return 1;
+//	}
+//	
+//	
+//	return 0;
+//	
+//	
+//}
+//short press_jump_detect(float left_acc_z, float right_acc_z, int left_zupt, int right_zupt,
+//	float left_front_press, float right_front_press, float left_back_press, float 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);
+//	
+//	if(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 1;
+//	}
+
+//	
+//	return 0;
+//	
+//}
+
+

motion/press_down_detect.c

@@ -53,9 +53,10 @@ void dual_foot_detect_up_trend(uint16_t* left_mag_up_min, uint16_t*left_up_count
 		}
 	}
 
-	if ((left_max_val - left_min_val > 100 && left_max_index > left_min_index && left_max_val - left_mag_window[window_size - 1] < 500) &&
-		(right_max_val - right_min_val > 100 && right_max_index > right_min_index && right_max_val - right_mag_window[window_size - 1] < 500)
-		&&((RESET_SIGNAL && right_mag_window[window_size - 1] < right_mag_window[window_size - 2] + 1000) &&(left_mag_window[window_size - 1]  < left_mag_window[window_size - 2] + 1000)
+	if ((left_max_val - left_min_val > 100 && left_max_index > left_min_index && left_max_val - left_mag_window[window_size - 1] < 100) &&
+		(right_max_val - right_min_val > 100 && right_max_index > right_min_index && right_max_val - right_mag_window[window_size - 1] < 100)
+		&&((RESET_SIGNAL && right_mag_window[window_size - 1] < right_mag_window[window_size - 2] + 500 
+		&& left_mag_window[window_size - 1]  < left_mag_window[window_size - 2] + 500)
 			|| !RESET_SIGNAL))
 	{
 		if (*left_up_count > 0)
@@ -116,8 +117,8 @@ void dual_foot_detect_up_trend(uint16_t* left_mag_up_min, uint16_t*left_up_count
 		*right_up_count = 0;
 
 		//ÖŘÖĂ´°żÚĘýžÝ
-		if (RESET_SIGNAL &&(right_mag_window[window_size - 1] > right_mag_window[window_size - 2] + 2000)
-			|| (left_mag_window[window_size - 1] > left_mag_window[window_size - 2] + 2000))
+		if (RESET_SIGNAL &&((right_mag_window[window_size - 1] > right_mag_window[window_size - 2] + 1000)
+			|| (left_mag_window[window_size - 1] > left_mag_window[window_size - 2] + 1000)))
 		{
 			memset(right_mag_window, right_mag_window[window_size - 1], window_size * sizeof(uint16_t));
 			memset(left_mag_window, left_mag_window[window_size - 1], window_size * sizeof(uint16_t));
@@ -282,7 +283,7 @@ int press_down_detect_new(int index, uint16_t front_mag_left, uint16_t back_mag_
 		&right_back_up_min, &right_back_up_count, back_mag_right_big_buff, BIG_WINDOW_SIZE, 0);
 
 
-	if (front_mag_left - left_front_up_min >2000.f && front_mag_right - right_front_up_min > 2000.f)
+	if (front_mag_left - left_front_up_min >750 && front_mag_right - right_front_up_min > 750)
 	{
 		*front_down = 1;
 
@@ -291,7 +292,7 @@ int press_down_detect_new(int index, uint16_t front_mag_left, uint16_t back_mag_
 
 
 
-	if (back_mag_left - left_back_up_min > 3000.0f && back_mag_right - right_back_up_min > 3000.0f)
+	if (back_mag_left - left_back_up_min > 2000.0f && back_mag_right - right_back_up_min > 2000.0f)
 	{
 		*back_down = 1;
 

motion/process_result.c

@@ -193,8 +193,6 @@ void process_motion(uint32_t time_stamp, int16_t _acc[3], int16_t _gry[3],  int1
 	/*
 		½âÎöÊý¾Ý
 	*/
-	
-
 	memcpy(accel, _acc, 3 * sizeof(int16_t));
 	
 	memcpy(gyro , _gry, 3 * sizeof(int16_t));

motion/trajAlgorithm/ekfPDR.c

@@ -229,7 +229,9 @@ void State_covariance_matrix_update(float *P, float *acc_n, float dt)
 	for (int i = 6; i < 9; i++)
 	{
 		P[i * 9 + i] += noise*100.f ;
+		//P[i * 9 + i] += noise*100.f ;
 	}
+
 }
 void Kalfman_gain_angle(float *P, float *Temporary_array, float *Temporary_array1, float *K)
 {

motion/trajAlgorithm/footPDR.c

@@ -56,9 +56,6 @@ float vel_mean;
 int zupt_count;
 
 
-
-
-
 void set_pdr_status()
 {
 	frame_index = 0;
@@ -656,7 +653,7 @@ void estimate_gyr_bias(float *gyr)
 	num_peak++;
 
 	//在线估计陀螺仪的零偏， 6050的零偏偏大
-	if (num_peak == 1500)
+	if (num_peak == 2000)
 	{
 		if (isStandCon(gyr_extreme))
 		{
@@ -665,7 +662,7 @@ void estimate_gyr_bias(float *gyr)
 			for(int i = 0; i < 3; i++)
 			{
 				//gyrBias[i]     = gyr_mean[i] *  0.0033f;
-				gyrBias[i]	= gyr_mean[i] * 0.00066667f;
+				gyrBias[i]	= gyr_mean[i] * 0.0005f;
 			}
 			DEBUG_LOG("gyrBias has cor!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n");			
 		
@@ -702,15 +699,14 @@ unsigned char footPDR(uint32_t num, float *gyr, float *acc, uint16_t front_press
 	unsigned char movement_e = 0;
 	
 	dt = (float)(num - last_timestamp) * 0.000001f;
-	SEGGER_RTT_printf( 0,"num : %d\n", num);
-	SEGGER_RTT_printf( 0,"last_timestamp : %d\n", last_timestamp);
-	SEGGER_RTT_printf( 0,"num - last_timestamp : %d\n", num - last_timestamp);
 	
-	if(num  < last_timestamp)
+	if(num  < last_timestamp || dt > 0.006f)
 	{
-		dt =0.00245f;
+		//dt =(float)(4294967295 - last_timestamp + num) * 0.000001f;
+		dt = 0.0024f;
 	}
 	
+	
 	last_timestamp = num;
 
 	for (int i = 0; i < 3; i++)
@@ -735,13 +731,9 @@ unsigned char footPDR(uint32_t num, float *gyr, float *acc, uint16_t front_press
 		
 		frame_index = 1;
 		
-		DEBUG_LOG( "PDR INIT");
-		
 		return movement_e;
 	}
 	
-	DEBUG_LOG( "PDR Process....");
-	
 
 	//惯导解算: 姿态矩阵更新
 	attitude_matrix_update(C, Temporary_array1, Temporary_array2, gyr, dt);
@@ -820,7 +812,14 @@ unsigned char footPDR(uint32_t num, float *gyr, float *acc, uint16_t front_press
 
 			memcpy(last_pos_n, pos_n, 3 * sizeof(float));
 		
-
+		zupt_count ++;
+	}
+	else
+	{
+		if(zupt_count > 0)
+		{
+			zupt_count--;
+		}
 	}
 
 	
@@ -843,6 +842,5 @@ unsigned char footPDR(uint32_t num, float *gyr, float *acc, uint16_t front_press
 	pos_res[1] = (int32_t) (pos_offset[1] * 1000.0f);
 	pos_res[2] = (int32_t) (pos_offset[2] * 1000.0f);
 	
-
 	return movement_e;
 }