jianbo_lib/c/motion/press_down_detect.c

#include "press_down_detect.h"


int press_down_detect_new(int index, uint16_t left_press, uint16_t right_press, float left_acc_z, float right_acc_z, float pitch_left, float pitch_right)
{
	 static int16_t valid_count = 0;
   static float left_acc_buff[10];
	 static float right_acc_buff[10];
	
   memcpy(left_acc_buff, left_acc_buff + 1, 9 * sizeof(float));
	
	 memcpy(right_acc_buff, right_acc_buff + 1, 9 * sizeof(float));
	
	 left_acc_buff[9] = left_acc_z;
	
	 right_acc_buff[9] = right_acc_z;
	
	 float max_val_left = left_acc_z;
	 float min_val_left = left_acc_z;
	
	 float max_val_right = right_acc_z;
	 float min_val_right = right_acc_z;
	
	 for(int i = 0; i < 10; i++)
	{
		if(max_val_left < left_acc_buff[i])
		{
			max_val_left = left_acc_buff[i];
		}
		
		if(min_val_left > left_acc_buff[i])
		{
			min_val_left = left_acc_buff[i];
		}
		
		if(max_val_right < right_acc_buff[i])
		{
			max_val_right = right_acc_buff[i];
		}
		
		if(min_val_right > right_acc_buff[i])
		{
			min_val_right = right_acc_buff[i];
		}
		
		
	}
	
	
	 if(fabsf(left_acc_z - 1.0f) < 0.2f && fabsf(right_acc_z - 1.0f) < 0.2f && pitch_left > -0.15f && pitch_right > -0.15f)
	 {
		 valid_count ++;
	 }
	 else
	 {
		 valid_count = 0;
	 }
	 
	 int16_t valid_laser = 0;
	 
	 if((left_press < 1000 && left_press > 200) || (right_press > 200 && right_press < 1000))
	 {
		 valid_laser = 1;
	 }
	 
   if(valid_count >= 10 && valid_laser == 1 && max_val_right - min_val_right < 0.1f && max_val_left - min_val_left < 0.1f) 
    return 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)
//	{
//		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)
//	{
//		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 < 30 && left_up_count < 30
//		&& right_up_count > 2 && left_up_count > 2 &&  s_pos[2] < 5 && h_pos[2] < 5 && 
//		s_pos[2] > 1 && h_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)
{
	
	static float right_data_z_buff[10];
	static float left_data_z_buff[10];
		
	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;
	
	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(max_index_right < min_index_right && max_index_left < min_index_left &&
		
		max_val_right - min_val_right > 1.0f && max_val_left - min_val_left > 1.0f &&
	
		min_val_left < 0.2f && min_val_right < 0.2f && max_val_left > 1.5f &&  max_val_right > 1.5f &&
	
		fabsf(max_val_right - max_val_left) < 2.0f && fabsf(min_val_right - min_val_left) < 2.0f &&
	
		abs(max_index_right- max_index_left) < 5 && abs(min_index_right- min_index_left) < 5)
	{
		return 1;
	}
	
	return 0;
	
}