shoes_project/touch_floor_by_imu/motion/detect_zero_vel.c

#include "detect_zero_vel.h"


int front_zero_tmp = 0;
int back_zero_tmp = 0;


  void detect_zero_vel(int16_t front, int16_t back, 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 = front;
  
  float back_val  = back;
  
  float acc_val  =  acc[2] / 2048.f;
		
	
	//滑动窗口更新数据
	
	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(acc_max_val - acc_min_val <  0.08f)
//	{
//		acc_zero_tmp = 1;
//	}
	
	/*
	* 判断前脚掌往下压
	*/
	if(front_max_index < front_min_index && front_max_val > front_min_val + 200.f)
	{
		front_zero_tmp = 1;
		
		front_zupt_wait = 10;
	}
	
	else if(front_max_index > front_min_index && front_max_val > front_min_val + 200.f)
	{
		front_zero_tmp = 0;
	}
	
	else if(front_zero_tmp > 0)
	{
		front_zero_tmp = 2;
	}
	
	/*
	* 判断后脚往下压
	*/
	if(back_max_index < back_min_index && back_max_val > back_min_val + 200.f)
	{
		back_zero_tmp = 1;
		
		back_zupt_wait = 10;
	}
	
	else if(back_max_index > back_min_index && back_max_val > back_min_val + 200.f)
	{
		back_zero_tmp = 0;
	}
	
	else if(back_zero_tmp > 0)
	{
		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((back_max_index > back_min_index && back_max_val > back_min_val + 500.f))
	{
		front_zero_tmp = 0;
		front_zupt_wait = 0;
		
		if((front_max_index > front_min_index && front_max_val > front_min_val + 500.f))
		{
			
			back_zero_tmp = 0;
			back_zupt_wait = 0;
		}
	}
	
	
	if(front_zero_tmp == 1)
	{
		*front_zero = 1;		
	}
	else if(front_zupt_wait > 0 && acc_max_val_x - acc_min_val_x < 0.025f && acc_max_val_y - acc_min_val_y < 0.025f )
	{
		*front_zero = 1;
			
		front_zupt_wait = 10;
			
	}
	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.025f && acc_max_val_y - acc_min_val_y < 0.025f )
	{
			*back_zero = 1;
		
			back_zupt_wait = 10;
	}
	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.015f
		&& fabsf(acc_y_window[WINDOW_SIZE - 2]  - acc_y_window[WINDOW_SIZE - 1]) < 0.015f)
	{
		*front_zero = 1;
		*back_zero  = 1;
		
		front_zero_tmp = 1;
		back_zero_tmp = 1;
		
		front_zupt_wait = 10;
		back_zupt_wait = 10;
	}
	
	last_front_zupt = front_zero_tmp;
	
	last_back_zupt  = back_zero_tmp;
  
  
}