jianbo_lib/c/motion/detect_step_by_acc_gyr.c

#include "detect_step_by_acc_gyr.h"


static int ACC_WINDOW_SIZE = 15;
static float acc_y_buff[15];

static float acc_z_buff[15];
static float acc_x_buff[15];

static float gyr_y_buff[15];

//static int acc_y_wait;
//static int acc_z_wait;
//static int acc_x_wait;

static int acc_y_status = 1;
static int acc_z_status = 1;
static int acc_x_status = 1;

static int last_zupt;

static int last_acc_x_status;
static int last_acc_y_status;
static int last_acc_z_status;



//static int can_detect_zupt = 1;
//static int stop_out_zupt = 0;

static int acc_diff_wait_count = 0;

static int zupt_wait_cout = 0;

static int special_acc_wait_time = 0;



int detect_turning_point(float *data_buff, int length, int last_status)
{
	//1、寻找窗口的最大值的下标
	int max_index  = 0;
	float max_val  = data_buff[0];
	
	for(int i = 0; i < length; i++)
	{
			if(max_val < data_buff[i])
			{
				max_index = i;
				
				max_val = data_buff[i];
			}
			
	}
	//2、以最大值下标把窗口两部分，右边部分判断出骤减及减缓趋势的时候，则判断为触地
	int right_length = length - max_index - 1;
	
	if(right_length > 3)
	{
		//当窗口长度大于5的时候，则进行趋势判断, 再对半分窗口
		int right_index = max_index;
		
		//寻找最小值，确定中间点
		int mid_index = max_index;
		float min_val = data_buff[right_index];
		
		for(int i = max_index; i < length; i++)
		{
			if(min_val > data_buff[i])
			{
				min_val = data_buff[i];
				mid_index = i;
			}
		}
		
		float right_diff_sum =  data_buff[right_index] - data_buff[mid_index];
		
		float max_right_diff = 0.0f;
		
		for(int i = right_index + 1; i <= mid_index ; i++)
		{
			if(data_buff[i-1] - data_buff[i] > max_right_diff)
			{
				max_right_diff = data_buff[i-1] - data_buff[i];
			}
		}
		
		
		float left_diff_sum = 0.0f;
		
		for(int i = mid_index + 1; i < length; i++)
		{
			left_diff_sum += fabsf(data_buff[i] - data_buff[i-1]);
		}
		
		//if(right_diff_sum > 3 * left_diff_sum && max_val > data_buff[length - 1] + 0.5f)
		SEGGER_RTT_printf(0, "right_diff_sum :  %d, left_diff_sum : %d!\n", (int)(right_diff_sum * 1000), (int)(left_diff_sum * 1000));
		
		//衰减过于激烈，肯定为触发条件
	  if(right_index + 2 < length && data_buff[right_index] - data_buff[right_index + 2] > 1.5f && data_buff[length-1] - min_val < 0.2f)
		{
			return 1;
		}
		
		if(right_diff_sum > 5 * left_diff_sum && max_val > data_buff[length - 1] + 0.8f && mid_index < length - 2 &&
			max_right_diff > 0.3f)
		{
			return 1;
		}
		
	}
	
	if(last_status == 1 && max_val > data_buff[length - 1] + 0.5f)
	{
		return 1;
	}
	
	float max_val_right = data_buff[length - 1];
	
	float min_val_right  = data_buff[length - 1];
	
	for(int i = length - 6; i < length - 1; i++)
	{
		if(max_val_right < data_buff[i])
		{
			max_val_right = data_buff[i];
		}
		
		if(min_val_right > data_buff[i])
		{
			min_val_right = data_buff[i];
		}
	}
	
	if(last_status == 1 && max_val_right - min_val_right < 0.15f)
	{
		return 1;
	}
	
	
	for(int i = 0; i < length - 6; i++)
	{
		if(max_val_right < data_buff[i])
		{
			max_val_right = data_buff[i];
		}
		
		if(min_val_right > data_buff[i])
		{
			min_val_right = data_buff[i];
		}
	}
	
	if( max_val_right - min_val_right < 0.1f)
	{
		return 2;
	}

	return  0;
} 

float var_acc_2(float *acc, int length)
{
	if(length < 10)
	{
		return 0.0f;
	}
	
	float mean_x = 0.0f;
	
	float sum_x  = 0.0f;
	
	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;
}

int detect_turn_point_by_gyr()
{
	int min_index = 0;
	int max_index = 0;
	
	float max_val = gyr_y_buff[0];
	float min_val = gyr_y_buff[0];
	
	for(int i = 0; i < ACC_WINDOW_SIZE; i++)
	{
		if(min_val > gyr_y_buff[i])
		{
			min_val = gyr_y_buff[i];
			min_index = i;
		}
		
		if(max_val < gyr_y_buff[i])
		{
			max_val = gyr_y_buff[i];
			max_index = i;
		}
	}
	
	if(max_index < min_index && max_val > min_val + 2.0f && gyr_y_buff[10] - min_val < 0.5f &&
		gyr_y_buff[14] - min_val < 0.5f && max_val > 2.0f && min_val > -0.5f)
	{
		return 1;
	}
	
	return 0;
}

int special_detect_acc_down()
{
	int min_index = ACC_WINDOW_SIZE-10;
	int max_index = ACC_WINDOW_SIZE-10;
	
	float max_val = acc_z_buff[min_index];
	float min_val = acc_z_buff[min_index];
	
	for(int i = ACC_WINDOW_SIZE-10; i < ACC_WINDOW_SIZE; i++)
	{
		if(min_val > acc_z_buff[i])
		{
			min_val = acc_z_buff[i];
			min_index = i;
		}
		
		if(max_val < acc_z_buff[i])
		{
			max_val = acc_z_buff[i];
			max_index = i;
		}
	}
	
	if(max_index > min_index && max_val >  min_val + 1.0f && min_val < 0.5f &&max_val > 1.2f)
	{
		special_acc_wait_time = 20;
		return 1;
	}
	
	return 0;
}



void step_detect_by_acc_gyr(float *acc, float *gyr, int *x_zero, int *y_zero, int *z_zero, int *zupt)
{
	
	//缓存加速度的滑动窗口
	
	memcpy(acc_x_buff, acc_x_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(float));
	
	acc_x_buff[(ACC_WINDOW_SIZE - 1)] = fabsf(acc[0]);
	
	memcpy(acc_y_buff, acc_y_buff + 1, (ACC_WINDOW_SIZE - 1) * sizeof(float));
	
	acc_y_buff[(ACC_WINDOW_SIZE - 1)] = fabsf(acc[1]);
	
	memcpy(acc_z_buff, acc_z_buff + 1, (ACC_WINDOW_SIZE - 1)  * sizeof(float));
	
	acc_z_buff[(ACC_WINDOW_SIZE - 1)] = acc[2];
	
	memcpy(gyr_y_buff, gyr_y_buff + 1, (ACC_WINDOW_SIZE - 1)  * sizeof(float));
	
	gyr_y_buff[(ACC_WINDOW_SIZE - 1)] = gyr[1] * 0.0175f;
	
	//检测拐点
	
//	int acc_y_zero, acc_z_zero , acc_x_zero;
	
	acc_y_status = detect_turning_point(acc_y_buff, ACC_WINDOW_SIZE, acc_y_status);
	
	acc_z_status = detect_turning_point(acc_z_buff, ACC_WINDOW_SIZE, acc_z_status);
	
	acc_x_status = detect_turning_point(acc_x_buff, ACC_WINDOW_SIZE, acc_x_status);
	
	if(acc_x_status == 2 || acc_y_status == 2 || acc_z_status == 2)
	{
		if(acc_x_status == 1 || acc_y_status == 1 || acc_z_status == 1)
		{
			if(acc_x_status == 2)
			{
				acc_x_status = 1;
			}
			
			if(acc_y_status == 2)
			{
				acc_y_status = 1;
			}
			
			if(acc_z_status == 2)
			{
				acc_z_status = 1;
			}
		}
	}		
	
//	if(sqrt(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]) > 1.5f* 57.2958f)
////	if(fabs(sqrt(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]) - 1.0f) > 0.5f )
//触底室，一般都不会X Y Z轴不会出现低概率的值
//	if(acc[2] < 0.7f  || acc[1] < -0.8f || acc[0] < -0.8f)
//	{
//		acc_x_status = 0;
//		acc_y_status = 0;
//		acc_z_status = 0;
//	}
//	
//经验值过滤, 但是发现触地的时候出现很大抖动，需要知道ACC的最大值

	float max_acc = acc_z_buff[ACC_WINDOW_SIZE - 10];
	int max_index = ACC_WINDOW_SIZE - 10;
	
	float min_acc = acc_z_buff[ACC_WINDOW_SIZE - 10];
	int min_index = ACC_WINDOW_SIZE - 10;
	
	for(int i = ACC_WINDOW_SIZE - 10; i < ACC_WINDOW_SIZE; i++)
	{
		if(acc_z_buff[i] > max_acc)
		{
			max_acc = acc_z_buff[i];
			max_index =  i;
		}
		
		if(acc_z_buff[i] < min_acc)
		{
			min_acc = acc_z_buff[i];
			min_index =  i;
		}
	}
	
	if(max_index == ACC_WINDOW_SIZE - 1 && max_acc - min_acc > 3.0f)
	{
		acc_diff_wait_count = 15;
	}
	
	//标志垫着脚跑的变量
	int special_zupt = 0;
	if((var_acc_2(acc_z_buff, ACC_WINDOW_SIZE) > 0.5f && acc[2] > 0.5f) || detect_turn_point_by_gyr())
	{
		acc_x_status = 1;
		acc_y_status = 1;
		acc_z_status = 1;
		special_zupt = 1;
	}
	
	//判断普通跑步的变量
	if(special_zupt == 0 &&((acc[2] < 0.7f && acc_diff_wait_count  == 0) || acc[2] < 0 || sqrt(gyr[0]*gyr[0] + gyr[1]*gyr[1] + gyr[2]*gyr[2])* 0.0175f > 1.2f))
	{
		acc_x_status = 0;
		acc_y_status = 0;
		acc_z_status = 0;
	}
	
	
	//一般都是在剧烈抖动踩触发的踩，但是会出现断开的现象，添加Z轴加速度下降及平缓来避免这种断开的现象
	
	special_detect_acc_down();
	
	if(acc[2] > 1.0f && special_acc_wait_time > 0)
	{
	  acc_x_status = 1;
		acc_y_status = 1;
		acc_z_status = 1;
	}
	
	if(special_acc_wait_time > 0)
	{
		special_acc_wait_time --;
	}
	
	if(acc_diff_wait_count > 0 )
	{
		acc_diff_wait_count --;
	}
	
		if((last_acc_x_status == 1 && acc_x_status == 0) || (last_acc_y_status == 1 && acc_y_status == 0))
	{
		*zupt = 0;
	}
	
	if((last_acc_x_status == 0 && acc_x_status == 1) || (last_acc_y_status == 0 && acc_y_status == 1))
	{
		*zupt = 1;
		zupt_wait_cout = ACC_WINDOW_SIZE;
	}
	
	if((acc_x_status == 1 && acc_y_status == 1) ||((last_zupt == 1 && (acc_x_status == 1 || acc_y_status == 1))))
	{
		*zupt = 1;
		zupt_wait_cout = ACC_WINDOW_SIZE;
	}
	
	
	if(zupt_wait_cout > 0)
	{
		zupt_wait_cout --;
	}
	
	last_acc_x_status = acc_x_status;
	last_acc_y_status = acc_y_status;
	last_acc_z_status = acc_z_status;
	
	*x_zero = acc_x_status;
	*y_zero = acc_y_status;
	*z_zero = acc_z_status;
	
}